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 
346  /*
347  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
348  {
349  solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
350 
351  if (solverConstraint.m_appliedImpulse)
352  {
353  if (multiBodyA)
354  {
355  btScalar impulse = solverConstraint.m_appliedImpulse;
356  btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
357  multiBodyA->applyDeltaVee(deltaV,impulse);
358  applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
359  } else
360  {
361  if (rb0)
362  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
363  }
364  if (multiBodyB)
365  {
366  btScalar impulse = solverConstraint.m_appliedImpulse;
367  btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
368  multiBodyB->applyDeltaVee(deltaV,impulse);
369  applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
370  } else
371  {
372  if (rb1)
373  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
374  }
375  }
376  } else
377  */
378 
379  solverConstraint.m_appliedImpulse = 0.f;
380  solverConstraint.m_appliedPushImpulse = 0.f;
381 
382  {
383  btScalar positionalError = 0.f;
384  btScalar velocityError = desiredVelocity - rel_vel; // * damping;
385 
386  btScalar erp = infoGlobal.m_erp2;
387 
388  //split impulse is not implemented yet for btMultiBody*
389  //if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
390  {
391  erp = infoGlobal.m_erp;
392  }
393 
394  positionalError = -penetration * erp / infoGlobal.m_timeStep;
395 
396  btScalar penetrationImpulse = positionalError * solverConstraint.m_jacDiagABInv;
397  btScalar velocityImpulse = velocityError * solverConstraint.m_jacDiagABInv;
398 
399  //split impulse is not implemented yet for btMultiBody*
400 
401  // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
402  {
403  //combine position and velocity into rhs
404  solverConstraint.m_rhs = penetrationImpulse + velocityImpulse;
405  solverConstraint.m_rhsPenetration = 0.f;
406  }
407  /*else
408  {
409  //split position and velocity into rhs and m_rhsPenetration
410  solverConstraint.m_rhs = velocityImpulse;
411  solverConstraint.m_rhsPenetration = penetrationImpulse;
412  }
413  */
414 
415  solverConstraint.m_cfm = 0.f;
416  solverConstraint.m_lowerLimit = lowerLimit;
417  solverConstraint.m_upperLimit = upperLimit;
418  }
419 
420  return rel_vel;
421 }
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:523
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:103
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:34
btMultiBodySolverConstraint::m_appliedPushImpulse
btSimdScalar m_appliedPushImpulse
Definition: btMultiBodySolverConstraint.h:50
btSolverBody::m_originalBody
btRigidBody * m_originalBody
Definition: btSolverBody.h:120
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:59
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btMultiBodySolverConstraint.h:48
btMultiBodyConstraint::btMultiBodyConstraint
btMultiBodyConstraint(btMultiBody *bodyA, btMultiBody *bodyB, int linkA, int linkB, int numRows, bool isUnilateral)
Definition: btMultiBodyConstraint.cpp:5
btMultiBody::getCompanionId
int getCompanionId() const
Definition: btMultiBody.h:472
btContactSolverInfo
Definition: btContactSolverInfo.h:69
btRigidBody::getAngularVelocity
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:357
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:294
btMultiBodyConstraint::m_posOffset
int m_posOffset
Definition: btMultiBodyConstraint.h:52
btMultiBodySolverConstraint::m_deltaVelBindex
int m_deltaVelBindex
Definition: btMultiBodySolverConstraint.h:39
btMultiBodyJacobianData::m_jacobians
btAlignedObjectArray< btScalar > m_jacobians
Definition: btMultiBodyConstraint.h:30
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:380
btMultiBodySolverConstraint::m_rhsPenetration
btScalar m_rhsPenetration
Definition: btMultiBodySolverConstraint.h:60
btMultiBodySolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btMultiBodySolverConstraint.h:47
btMultiBodyPoint2Point.h
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:46
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:229
btRigidBody.h
btMultiBodySolverConstraint::m_deltaVelAindex
int m_deltaVelAindex
Definition: btMultiBodySolverConstraint.h:37
btMultiBodyConstraint::~btMultiBodyConstraint
virtual ~btMultiBodyConstraint()
Definition: btMultiBodyConstraint.cpp:42
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btMultiBodySolverConstraint::m_multiBodyB
btMultiBody * m_multiBodyB
Definition: btMultiBodySolverConstraint.h:74
btAssert
#define btAssert(x)
Definition: btScalar.h:133
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:28
btMultiBodySolverConstraint::m_linkA
int m_linkA
Definition: btMultiBodySolverConstraint.h:71
btMultiBody::calcAccelerationDeltasMultiDof
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
stepVelocitiesMultiDof is deprecated, use computeAccelerationsArticulatedBodyAlgorithmMultiDof instea...
Definition: btMultiBody.cpp:1424
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:167
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:210
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:41
btRigidBody::getAngularFactor
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:487
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:47
btMultiBodyConstraint::fillMultiBodyConstraint
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)
Definition: btMultiBodyConstraint.cpp:52
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:261
btMultiBodyJacobianData::scratch_r
btAlignedObjectArray< btScalar > scratch_r
Definition: btMultiBodyConstraint.h:33
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:80
btMultiBodySolverConstraint::m_jacBindex
int m_jacBindex
Definition: btMultiBodySolverConstraint.h:40
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:113
btMultiBody
Definition: btMultiBody.h:49
btRigidBody::getLinearVelocity
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:353
btMultiBody::setCompanionId
void setCompanionId(int id)
Definition: btMultiBody.h:476
btMultiBodySolverConstraint::m_multiBodyA
btMultiBody * m_multiBodyA
Definition: btMultiBodySolverConstraint.h:70
btContactSolverInfoData::m_erp
btScalar m_erp
Definition: btContactSolverInfo.h:46
btMultiBodyConstraint::allocateJacobiansMultiDof
void allocateJacobiansMultiDof()
Definition: btMultiBodyConstraint.cpp:34
btMultiBodyJacobianData::m_deltaVelocities
btAlignedObjectArray< btScalar > m_deltaVelocities
Definition: btMultiBodyConstraint.h:32
btMultiBodyJacobianData::m_solverBodyPool
btAlignedObjectArray< btSolverBody > * m_solverBodyPool
Definition: btMultiBodyConstraint.h:36
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:44
btMultiBodyConstraint::m_numRows
int m_numRows
Definition: btMultiBodyConstraint.h:49
btMultiBodySolverConstraint::m_linkB
int m_linkB
Definition: btMultiBodySolverConstraint.h:75
btMultiBodySolverConstraint::m_jacDiagABInv
btScalar m_jacDiagABInv
Definition: btMultiBodySolverConstraint.h:54
btMultiBodySolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btMultiBodySolverConstraint.h:69
btMultiBodySolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition: btMultiBodySolverConstraint.h:51
btMultiBodySolverConstraint::m_jacAindex
int m_jacAindex
Definition: btMultiBodySolverConstraint.h:38
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:43
btSolverBody::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:126
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:256
btMultiBodySolverConstraint::m_rhs
btScalar m_rhs
Definition: btMultiBodySolverConstraint.h:55
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:262
btMultiBodyConstraint::m_data
btAlignedObjectArray< btScalar > m_data
Definition: btMultiBodyConstraint.h:63
btMultiBodyConstraint::updateJacobianSizes
void updateJacobianSizes()
Definition: btMultiBodyConstraint.cpp:19
btMultiBodySolverConstraint::m_friction
btScalar m_friction
Definition: btMultiBodySolverConstraint.h:53
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btMultiBodySolverConstraint.h:73
btMultiBodyConstraint::m_jacSizeBoth
int m_jacSizeBoth
Definition: btMultiBodyConstraint.h:51
btMultiBodySolverConstraint::m_upperLimit
btScalar m_upperLimit
Definition: btMultiBodySolverConstraint.h:59
btMultiBodyConstraint.h
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:44
btMultiBodyConstraint::m_jacSizeA
int m_jacSizeA
Definition: btMultiBodyConstraint.h:50
btMultiBodySolverConstraint::m_lowerLimit
btScalar m_lowerLimit
Definition: btMultiBodySolverConstraint.h:58
btMultiBody::getBasePos
const btVector3 & getBasePos() const
Definition: btMultiBody.h:185
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:45
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:45
btMultiBodyJacobianData::scratch_m
btAlignedObjectArray< btMatrix3x3 > scratch_m
Definition: btMultiBodyConstraint.h:35
btContactSolverInfoData::m_erp2
btScalar m_erp2
Definition: btContactSolverInfo.h:47
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition: btMultiBodyConstraint.h:31
btAlignedObjectArray::at
const T & at(int n) const
Definition: btAlignedObjectArray.h:154
btMultiBodyConstraint::applyDeltaVee
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
Definition: btMultiBodyConstraint.cpp:46
btMultiBody::fillConstraintJacobianMultiDof
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
Definition: btMultiBody.cpp:1721
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:149
btMultiBodySolverConstraint::m_cfm
btScalar m_cfm
Definition: btMultiBodySolverConstraint.h:56