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