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