Bullet Collision Detection & Physics Library
btMultiBodySliderConstraint.cpp
Go to the documentation of this file.
1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2013 Erwin Coumans http://bulletphysics.org
4 
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10 
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15 
17 
23 
24 #define BTMBSLIDERCONSTRAINT_DIM 5
25 #define EPSILON 0.000001
26 
27 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* body, int link, btRigidBody* bodyB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
28  :btMultiBodyConstraint(body,0,link,-1,BTMBSLIDERCONSTRAINT_DIM,false),
29  m_rigidBodyA(0),
30  m_rigidBodyB(bodyB),
31  m_pivotInA(pivotInA),
32  m_pivotInB(pivotInB),
33  m_frameInA(frameInA),
34  m_frameInB(frameInB),
35  m_jointAxis(jointAxis)
36 {
37  m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses
38 }
39 
40 btMultiBodySliderConstraint::btMultiBodySliderConstraint(btMultiBody* bodyA, int linkA, btMultiBody* bodyB, int linkB, const btVector3& pivotInA, const btVector3& pivotInB, const btMatrix3x3& frameInA, const btMatrix3x3& frameInB, const btVector3& jointAxis)
41  :btMultiBodyConstraint(bodyA,bodyB,linkA,linkB,BTMBSLIDERCONSTRAINT_DIM,false),
42  m_rigidBodyA(0),
43  m_rigidBodyB(0),
44  m_pivotInA(pivotInA),
45  m_pivotInB(pivotInB),
46  m_frameInA(frameInA),
47  m_frameInB(frameInB),
48  m_jointAxis(jointAxis)
49 {
50  m_data.resize(BTMBSLIDERCONSTRAINT_DIM);//at least store the applied impulses
51 }
52 
54 {
55  //not implemented yet
56  btAssert(0);
57 }
58 
60 {
61 }
62 
63 
65 {
66  if (m_rigidBodyA)
67  return m_rigidBodyA->getIslandTag();
68 
69  if (m_bodyA)
70  {
72  if (col)
73  return col->getIslandTag();
74  for (int i=0;i<m_bodyA->getNumLinks();i++)
75  {
76  if (m_bodyA->getLink(i).m_collider)
77  return m_bodyA->getLink(i).m_collider->getIslandTag();
78  }
79  }
80  return -1;
81 }
82 
84 {
85  if (m_rigidBodyB)
86  return m_rigidBodyB->getIslandTag();
87  if (m_bodyB)
88  {
90  if (col)
91  return col->getIslandTag();
92 
93  for (int i=0;i<m_bodyB->getNumLinks();i++)
94  {
95  col = m_bodyB->getLink(i).m_collider;
96  if (col)
97  return col->getIslandTag();
98  }
99  }
100  return -1;
101 }
102 
104 {
105  // Convert local points back to world
106  btVector3 pivotAworld = m_pivotInA;
107  btMatrix3x3 frameAworld = m_frameInA;
108  btVector3 jointAxis = m_jointAxis;
109  if (m_rigidBodyA)
110  {
114 
115  } else if (m_bodyA) {
116  pivotAworld = m_bodyA->localPosToWorld(m_linkA, m_pivotInA);
117  frameAworld = m_bodyA->localFrameToWorld(m_linkA, m_frameInA);
119  }
120  btVector3 pivotBworld = m_pivotInB;
121  btMatrix3x3 frameBworld = m_frameInB;
122  if (m_rigidBodyB)
123  {
126 
127  } else if (m_bodyB) {
128  pivotBworld = m_bodyB->localPosToWorld(m_linkB, m_pivotInB);
129  frameBworld = m_bodyB->localFrameToWorld(m_linkB, m_frameInB);
130  }
131 
132  btVector3 constraintAxis[2];
133  for (int i = 0; i < 3; ++i)
134  {
135  constraintAxis[0] = frameAworld.getColumn(i).cross(jointAxis);
136  if (constraintAxis[0].safeNorm() > EPSILON)
137  {
138  constraintAxis[0] = constraintAxis[0].normalized();
139  constraintAxis[1] = jointAxis.cross(constraintAxis[0]);
140  constraintAxis[1] = constraintAxis[1].normalized();
141  break;
142  }
143  }
144 
145  btMatrix3x3 relRot = frameAworld.inverse()*frameBworld;
146  btVector3 angleDiff;
148 
149  int numDim = BTMBSLIDERCONSTRAINT_DIM;
150  for (int i=0;i<numDim;i++)
151  {
152  btMultiBodySolverConstraint& constraintRow = constraintRows.expandNonInitializing();
153  constraintRow.m_orgConstraint = this;
154  constraintRow.m_orgDofIndex = i;
155  constraintRow.m_relpos1CrossNormal.setValue(0,0,0);
156  constraintRow.m_contactNormal1.setValue(0,0,0);
157  constraintRow.m_relpos2CrossNormal.setValue(0,0,0);
158  constraintRow.m_contactNormal2.setValue(0,0,0);
159  constraintRow.m_angularComponentA.setValue(0,0,0);
160  constraintRow.m_angularComponentB.setValue(0,0,0);
161 
162  constraintRow.m_solverBodyIdA = data.m_fixedBodyId;
163  constraintRow.m_solverBodyIdB = data.m_fixedBodyId;
164 
165  if (m_rigidBodyA)
166  {
167  constraintRow.m_solverBodyIdA = m_rigidBodyA->getCompanionId();
168  }
169  if (m_rigidBodyB)
170  {
171  constraintRow.m_solverBodyIdB = m_rigidBodyB->getCompanionId();
172  }
173 
174  btVector3 constraintNormalLin(0,0,0);
175  btVector3 constraintNormalAng(0,0,0);
176  btScalar posError = 0.0;
177  if (i < 2) {
178  constraintNormalLin = constraintAxis[i];
179  posError = (pivotAworld-pivotBworld).dot(constraintNormalLin);
180  fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
181  constraintNormalLin, pivotAworld, pivotBworld,
182  posError,
183  infoGlobal,
185  );
186  }
187  else { //i>=2
188  constraintNormalAng = frameAworld.getColumn(i%3);
189  posError = angleDiff[i%3];
190  fillMultiBodyConstraint(constraintRow, data, 0, 0, constraintNormalAng,
191  constraintNormalLin, pivotAworld, pivotBworld,
192  posError,
193  infoGlobal,
195  );
196  }
197  }
198 }
199 
201 {
202  btTransform tr;
203  tr.setIdentity();
204 
205  if (m_rigidBodyA)
206  {
208  tr.setOrigin(pivot);
209  drawer->drawTransform(tr, 0.1);
210  }
211  if (m_bodyA)
212  {
214  tr.setOrigin(pivotAworld);
215  drawer->drawTransform(tr, 0.1);
216  }
217  if (m_rigidBodyB)
218  {
219  // that ideally should draw the same frame
221  tr.setOrigin(pivot);
222  drawer->drawTransform(tr, 0.1);
223  }
224  if (m_bodyB)
225  {
227  tr.setOrigin(pivotBworld);
228  drawer->drawTransform(tr, 0.1);
229  }
230 }
void setOrigin(const btVector3 &origin)
Set the translational element.
Definition: btTransform.h:150
static bool matrixToEulerXYZ(const btMatrix3x3 &mat, btVector3 &xyz)
btVector3 localPosToWorld(int i, const btVector3 &vec) const
int getNumLinks() const
Definition: btMultiBody.h:164
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
const btMultiBodyLinkCollider * getBaseCollider() const
Definition: btMultiBody.h:134
void setValue(const btScalar &_x, const btScalar &_y, const btScalar &_z)
Definition: btVector3.h:652
const btTransform & getCenterOfMassTransform() const
Definition: btRigidBody.h:359
btMultiBodyConstraint * m_orgConstraint
void setIdentity()
Set this transformation to the identity.
Definition: btTransform.h:172
#define btAssert(x)
Definition: btScalar.h:131
#define BTMBSLIDERCONSTRAINT_DIM
This file was written by Erwin Coumans.
btQuaternion getOrientation() const
btVector3 quatRotate(const btQuaternion &rotation, const btVector3 &v)
Definition: btQuaternion.h:917
btVector3 normalized() const
Return a normalized version of this vector.
Definition: btVector3.h:964
btMatrix3x3 transpose() const
Return the transpose of the matrix.
Definition: btMatrix3x3.h:958
btVector3 getColumn(int i) const
Get a column of the matrix as a vector.
Definition: btMatrix3x3.h:134
btMatrix3x3 localFrameToWorld(int i, const btMatrix3x3 &mat) const
virtual void debugDraw(class btIDebugDraw *drawer)
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
The btIDebugDraw interface class allows hooking up a debug renderer to visually debug simulations...
Definition: btIDebugDraw.h:29
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btMultiBodySliderConstraint(btMultiBody *body, int link, btRigidBody *bodyB, const btVector3 &pivotInA, const btVector3 &pivotInB, const btMatrix3x3 &frameInA, const btMatrix3x3 &frameInB, const btVector3 &jointAxis)
btAlignedObjectArray< btScalar > m_data
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
The btTransform class supports rigid transforms with only translation and rotation and no scaling/she...
Definition: btTransform.h:34
virtual void createConstraintRows(btMultiBodyConstraintArray &constraintRows, btMultiBodyJacobianData &data, const btContactSolverInfo &infoGlobal)
int getIslandTag() const
btVector3 localDirToWorld(int i, const btVector3 &vec) const
virtual void drawTransform(const btTransform &transform, btScalar orthoLen)
Definition: btIDebugDraw.h:166
void resize(int newsize, const T &fillData=T())
The btMatrix3x3 class implements a 3x3 rotation matrix, to perform linear algebra in combination with...
Definition: btMatrix3x3.h:48
btScalar dot(const btQuaternion &q1, const btQuaternion &q2)
Calculate the dot product between two quaternions.
Definition: btQuaternion.h:878
btScalar fillMultiBodyConstraint(btMultiBodySolverConstraint &solverConstraint, btMultiBodyJacobianData &data, btScalar *jacOrgA, btScalar *jacOrgB, const btVector3 &constraintNormalAng, const btVector3 &constraintNormalLin, const btVector3 &posAworld, const btVector3 &posBworld, btScalar posError, const btContactSolverInfo &infoGlobal, btScalar lowerLimit, btScalar upperLimit, bool angConstraint=false, btScalar relaxation=1.f, bool isFriction=false, btScalar desiredVelocity=0, btScalar cfmSlip=0)
btMatrix3x3 inverse() const
Return the inverse of the matrix.
Definition: btMatrix3x3.h:1003
int getCompanionId() const
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292