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