Bullet Collision Detection & Physics Library
btMultiBodyConstraint.cpp
Go to the documentation of this file.
3 #include "btMultiBodyPoint2Point.h" //for testing (BTMBP2PCONSTRAINT_BLOCK_ANGULAR_MOTION_TEST macro)
4 
5 btMultiBodyConstraint::btMultiBodyConstraint(btMultiBody* bodyA, btMultiBody* bodyB, int linkA, int linkB, int numRows, bool isUnilateral)
6  : m_bodyA(bodyA),
7  m_bodyB(bodyB),
8  m_linkA(linkA),
9  m_linkB(linkB),
10  m_numRows(numRows),
11  m_jacSizeA(0),
12  m_jacSizeBoth(0),
13  m_isUnilateral(isUnilateral),
14  m_numDofsFinalized(-1),
15  m_maxAppliedImpulse(100)
16 {
17 }
18 
20 {
21  if (m_bodyA)
22  {
23  m_jacSizeA = (6 + m_bodyA->getNumDofs());
24  }
25 
26  if (m_bodyB)
27  {
29  }
30  else
32 }
33 
35 {
37 
40 }
41 
43 {
44 }
45 
46 void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
47 {
48  for (int i = 0; i < ndof; ++i)
49  data.m_deltaVelocities[velocityIndex + i] += delta_vee[i] * impulse;
50 }
51 
54  btScalar* jacOrgA, btScalar* jacOrgB,
55  const btVector3& constraintNormalAng,
56  const btVector3& constraintNormalLin,
57  const btVector3& posAworld, const btVector3& posBworld,
58  btScalar posError,
59  const btContactSolverInfo& infoGlobal,
60  btScalar lowerLimit, btScalar upperLimit,
61  bool angConstraint,
62  btScalar relaxation,
63  bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
64 {
65  solverConstraint.m_multiBodyA = m_bodyA;
66  solverConstraint.m_multiBodyB = m_bodyB;
67  solverConstraint.m_linkA = m_linkA;
68  solverConstraint.m_linkB = m_linkB;
69 
70  btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
71  btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
72 
73  btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
74  btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
75 
76  btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
77  btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
78 
79  btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
80  if (bodyA)
81  rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
82  if (bodyB)
83  rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
84 
85  if (multiBodyA)
86  {
87  if (solverConstraint.m_linkA < 0)
88  {
89  rel_pos1 = posAworld - multiBodyA->getBasePos();
90  }
91  else
92  {
93  rel_pos1 = posAworld - multiBodyA->getLink(solverConstraint.m_linkA).m_cachedWorldTransform.getOrigin();
94  }
95 
96  const int ndofA = multiBodyA->getNumDofs() + 6;
97 
98  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
99 
100  if (solverConstraint.m_deltaVelAindex < 0)
101  {
102  solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
103  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
104  data.m_deltaVelocities.resize(data.m_deltaVelocities.size() + ndofA);
105  }
106  else
107  {
108  btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex + ndofA);
109  }
110 
111  //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
112  //resize..
113  solverConstraint.m_jacAindex = data.m_jacobians.size();
114  data.m_jacobians.resize(data.m_jacobians.size() + ndofA);
115  //copy/determine
116  if (jacOrgA)
117  {
118  for (int i = 0; i < ndofA; i++)
119  data.m_jacobians[solverConstraint.m_jacAindex + i] = jacOrgA[i];
120  }
121  else
122  {
123  btScalar* jac1 = &data.m_jacobians[solverConstraint.m_jacAindex];
124  //multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
125  multiBodyA->fillConstraintJacobianMultiDof(solverConstraint.m_linkA, posAworld, constraintNormalAng, constraintNormalLin, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
126  }
127 
128  //determine the velocity response of multibodyA to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
129  //resize..
130  data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size() + ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
132  btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
133  //determine..
134  multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex], delta, data.scratch_r, data.scratch_v);
135 
136  btVector3 torqueAxis0;
137  if (angConstraint)
138  {
139  torqueAxis0 = constraintNormalAng;
140  }
141  else
142  {
143  torqueAxis0 = rel_pos1.cross(constraintNormalLin);
144  }
145  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
146  solverConstraint.m_contactNormal1 = constraintNormalLin;
147  }
148  else //if(rb0)
149  {
150  btVector3 torqueAxis0;
151  if (angConstraint)
152  {
153  torqueAxis0 = constraintNormalAng;
154  }
155  else
156  {
157  torqueAxis0 = rel_pos1.cross(constraintNormalLin);
158  }
159  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld() * torqueAxis0 * rb0->getAngularFactor() : btVector3(0, 0, 0);
160  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
161  solverConstraint.m_contactNormal1 = constraintNormalLin;
162  }
163 
164  if (multiBodyB)
165  {
166  if (solverConstraint.m_linkB < 0)
167  {
168  rel_pos2 = posBworld - multiBodyB->getBasePos();
169  }
170  else
171  {
172  rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
173  }
174 
175  const int ndofB = multiBodyB->getNumDofs() + 6;
176 
177  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
178  if (solverConstraint.m_deltaVelBindex < 0)
179  {
180  solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
181  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
182  data.m_deltaVelocities.resize(data.m_deltaVelocities.size() + ndofB);
183  }
184 
185  //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
186  //resize..
187  solverConstraint.m_jacBindex = data.m_jacobians.size();
188  data.m_jacobians.resize(data.m_jacobians.size() + ndofB);
189  //copy/determine..
190  if (jacOrgB)
191  {
192  for (int i = 0; i < ndofB; i++)
193  data.m_jacobians[solverConstraint.m_jacBindex + i] = jacOrgB[i];
194  }
195  else
196  {
197  //multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
198  multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
199  }
200 
201  //determine velocity response of multibodyB to reaction impulses of this constraint (i.e. A[i,i] for i=1,...n_con: multibody's inverse inertia with respect to this 1D constraint)
202  //resize..
205  btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
206  //determine..
207  multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex], delta, data.scratch_r, data.scratch_v);
208 
209  btVector3 torqueAxis1;
210  if (angConstraint)
211  {
212  torqueAxis1 = constraintNormalAng;
213  }
214  else
215  {
216  torqueAxis1 = rel_pos2.cross(constraintNormalLin);
217  }
218  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
219  solverConstraint.m_contactNormal2 = -constraintNormalLin;
220  }
221  else //if(rb1)
222  {
223  btVector3 torqueAxis1;
224  if (angConstraint)
225  {
226  torqueAxis1 = constraintNormalAng;
227  }
228  else
229  {
230  torqueAxis1 = rel_pos2.cross(constraintNormalLin);
231  }
232  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld() * -torqueAxis1 * rb1->getAngularFactor() : btVector3(0, 0, 0);
233  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
234  solverConstraint.m_contactNormal2 = -constraintNormalLin;
235  }
236  {
237  btVector3 vec;
238  btScalar denom0 = 0.f;
239  btScalar denom1 = 0.f;
240  btScalar* jacB = 0;
241  btScalar* jacA = 0;
242  btScalar* deltaVelA = 0;
243  btScalar* deltaVelB = 0;
244  int ndofA = 0;
245  //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
246  if (multiBodyA)
247  {
248  ndofA = multiBodyA->getNumDofs() + 6;
249  jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
250  deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
251  for (int i = 0; i < ndofA; ++i)
252  {
253  btScalar j = jacA[i];
254  btScalar l = deltaVelA[i];
255  denom0 += j * l;
256  }
257  }
258  else if (rb0)
259  {
260  vec = (solverConstraint.m_angularComponentA).cross(rel_pos1);
261  if (angConstraint)
262  {
263  denom0 = constraintNormalAng.dot(solverConstraint.m_angularComponentA);
264  }
265  else
266  {
267  denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
268  }
269  }
270  //
271  if (multiBodyB)
272  {
273  const int ndofB = multiBodyB->getNumDofs() + 6;
274  jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
275  deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
276  for (int i = 0; i < ndofB; ++i)
277  {
278  btScalar j = jacB[i];
279  btScalar l = deltaVelB[i];
280  denom1 += j * l;
281  }
282  }
283  else if (rb1)
284  {
285  vec = (-solverConstraint.m_angularComponentB).cross(rel_pos2);
286  if (angConstraint)
287  {
288  denom1 = constraintNormalAng.dot(-solverConstraint.m_angularComponentB);
289  }
290  else
291  {
292  denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
293  }
294  }
295 
296  //
297  btScalar d = denom0 + denom1;
298  if (d > SIMD_EPSILON)
299  {
300  solverConstraint.m_jacDiagABInv = relaxation / (d);
301  }
302  else
303  {
304  //disable the constraint row to handle singularity/redundant constraint
305  solverConstraint.m_jacDiagABInv = 0.f;
306  }
307  }
308 
309  //compute rhs and remaining solverConstraint fields
310  btScalar penetration = isFriction ? 0 : posError;
311 
312  btScalar rel_vel = 0.f;
313  int ndofA = 0;
314  int ndofB = 0;
315  {
316  btVector3 vel1, vel2;
317  if (multiBodyA)
318  {
319  ndofA = multiBodyA->getNumDofs() + 6;
320  btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
321  for (int i = 0; i < ndofA; ++i)
322  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
323  }
324  else if (rb0)
325  {
326  rel_vel += rb0->getLinearVelocity().dot(solverConstraint.m_contactNormal1);
327  rel_vel += rb0->getAngularVelocity().dot(solverConstraint.m_relpos1CrossNormal);
328  }
329  if (multiBodyB)
330  {
331  ndofB = multiBodyB->getNumDofs() + 6;
332  btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
333  for (int i = 0; i < ndofB; ++i)
334  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
335  }
336  else if (rb1)
337  {
338  rel_vel += rb1->getLinearVelocity().dot(solverConstraint.m_contactNormal2);
339  rel_vel += rb1->getAngularVelocity().dot(solverConstraint.m_relpos2CrossNormal);
340  }
341 
342  solverConstraint.m_friction = 0.f; //cp.m_combinedFriction;
343  }
344 
345  solverConstraint.m_appliedImpulse = 0.f;
346  solverConstraint.m_appliedPushImpulse = 0.f;
347 
348  {
349  btScalar positionalError = 0.f;
350  btScalar velocityError = desiredVelocity - rel_vel; // * damping;
351 
352  btScalar erp = infoGlobal.m_erp2;
353 
354  //split impulse is not implemented yet for btMultiBody*
355  //if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
356  {
357  erp = infoGlobal.m_erp;
358  }
359 
360  positionalError = -penetration * erp / infoGlobal.m_timeStep;
361 
362  btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
363  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
364 
365  //split impulse is not implemented yet for btMultiBody*
366 
367  // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
368  {
369  //combine position and velocity into rhs
370  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
371  solverConstraint.m_rhsPenetration = 0.f;
372  }
373  /*else
374  {
375  //split position and velocity into rhs and m_rhsPenetration
376  solverConstraint.m_rhs = velocityImpulse;
377  solverConstraint.m_rhsPenetration = penetrationImpulse;
378  }
379  */
380 
381  solverConstraint.m_cfm = 0.f;
382  solverConstraint.m_lowerLimit = lowerLimit;
383  solverConstraint.m_upperLimit = upperLimit;
384  }
385 
386  return rel_vel;
387 }
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:314
#define SIMD_EPSILON
Definition: btScalar.h:543
#define btAssert(x)
Definition: btScalar.h:153
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
const T & at(int n) const
btAlignedObjectArray< btScalar > m_data
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
btMultiBodyConstraint(btMultiBody *bodyA, btMultiBody *bodyB, int linkA, int linkB, int numRows, bool isUnilateral)
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)
int getCompanionId() const
Definition: btMultiBody.h:515
const btVector3 & getBasePos() const
Definition: btMultiBody.h:185
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
int getNumDofs() const
Definition: btMultiBody.h:167
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
void fillConstraintJacobianMultiDof(int link, const btVector3 &contact_point, const btVector3 &normal_ang, const btVector3 &normal_lin, btScalar *jac, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v, btAlignedObjectArray< btMatrix3x3 > &scratch_m) const
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:272
void setCompanionId(int id)
Definition: btMultiBody.h:519
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:402
btScalar getInvMass() const
Definition: btRigidBody.h:263
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:398
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:532
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:82
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocities
btAlignedObjectArray< btScalar > m_jacobians
btAlignedObjectArray< btSolverBody > * m_solverBodyPool
btAlignedObjectArray< btScalar > scratch_r
btAlignedObjectArray< btMatrix3x3 > scratch_m
btAlignedObjectArray< btVector3 > scratch_v
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:105
btRigidBody * m_originalBody
Definition: btSolverBody.h:120
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:126