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 
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_maxAppliedImpulse(100)
17 {
19 }
20 
22 {
23  if(m_bodyA)
24  {
25  if(m_bodyA->isMultiDof())
26  m_jacSizeA = (6 + m_bodyA->getNumDofs());
27  else
28  m_jacSizeA = (6 + m_bodyA->getNumLinks());
29  }
30 
31  if(m_bodyB)
32  {
33  if(m_bodyB->isMultiDof())
35  else
37  }
38  else
40 
43 }
44 
46 {
47 }
48 
49 void btMultiBodyConstraint::applyDeltaVee(btMultiBodyJacobianData& data, btScalar* delta_vee, btScalar impulse, int velocityIndex, int ndof)
50 {
51  for (int i = 0; i < ndof; ++i)
52  data.m_deltaVelocities[velocityIndex+i] += delta_vee[i] * impulse;
53 }
54 
57  btScalar* jacOrgA, btScalar* jacOrgB,
58  const btVector3& contactNormalOnB,
59  const btVector3& posAworld, const btVector3& posBworld,
60  btScalar posError,
61  const btContactSolverInfo& infoGlobal,
62  btScalar lowerLimit, btScalar upperLimit,
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  const int ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
89 
90  solverConstraint.m_deltaVelAindex = multiBodyA->getCompanionId();
91 
92  if (solverConstraint.m_deltaVelAindex <0)
93  {
94  solverConstraint.m_deltaVelAindex = data.m_deltaVelocities.size();
95  multiBodyA->setCompanionId(solverConstraint.m_deltaVelAindex);
97  } else
98  {
99  btAssert(data.m_deltaVelocities.size() >= solverConstraint.m_deltaVelAindex+ndofA);
100  }
101 
102  //determine jacobian of this 1D constraint in terms of multibodyA's degrees of freedom
103  //resize..
104  solverConstraint.m_jacAindex = data.m_jacobians.size();
105  data.m_jacobians.resize(data.m_jacobians.size()+ndofA);
106  //copy/determine
107  if(jacOrgA)
108  {
109  for (int i=0;i<ndofA;i++)
110  data.m_jacobians[solverConstraint.m_jacAindex+i] = jacOrgA[i];
111  }
112  else
113  {
114  btScalar* jac1=&data.m_jacobians[solverConstraint.m_jacAindex];
115  if(multiBodyA->isMultiDof())
116  multiBodyA->fillContactJacobianMultiDof(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
117  else
118  multiBodyA->fillContactJacobian(solverConstraint.m_linkA, posAworld, contactNormalOnB, jac1, data.scratch_r, data.scratch_v, data.scratch_m);
119  }
120 
121  //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)
122  //resize..
123  data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofA); //=> each constraint row has the constrained tree dofs allocated in m_deltaVelocitiesUnitImpulse
125  btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
126  //determine..
127  if(multiBodyA->isMultiDof())
128  multiBodyA->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
129  else
130  multiBodyA->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacAindex],delta,data.scratch_r, data.scratch_v);
131  }
132  else if(rb0)
133  {
134  btVector3 torqueAxis0 = rel_pos1.cross(contactNormalOnB);
135  solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
136  solverConstraint.m_relpos1CrossNormal = torqueAxis0;
137  solverConstraint.m_contactNormal1 = contactNormalOnB;
138  }
139 
140  if (multiBodyB)
141  {
142  const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
143 
144  solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
145  if (solverConstraint.m_deltaVelBindex <0)
146  {
147  solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
148  multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
149  data.m_deltaVelocities.resize(data.m_deltaVelocities.size()+ndofB);
150  }
151 
152  //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
153  //resize..
154  solverConstraint.m_jacBindex = data.m_jacobians.size();
155  data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
156  //copy/determine..
157  if(jacOrgB)
158  {
159  for (int i=0;i<ndofB;i++)
160  data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
161  }
162  else
163  {
164  if(multiBodyB->isMultiDof())
165  multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
166  else
167  multiBodyB->fillContactJacobian(solverConstraint.m_linkB, posBworld, -contactNormalOnB, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
168  }
169 
170  //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)
171  //resize..
174  btScalar* delta = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
175  //determine..
176  if(multiBodyB->isMultiDof())
177  multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
178  else
179  multiBodyB->calcAccelerationDeltas(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
180 
181  }
182  else if(rb1)
183  {
184  btVector3 torqueAxis1 = rel_pos2.cross(contactNormalOnB);
185  solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
186  solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
187  solverConstraint.m_contactNormal2 = -contactNormalOnB;
188  }
189  {
190 
191  btVector3 vec;
192  btScalar denom0 = 0.f;
193  btScalar denom1 = 0.f;
194  btScalar* jacB = 0;
195  btScalar* jacA = 0;
196  btScalar* deltaVelA = 0;
197  btScalar* deltaVelB = 0;
198  int ndofA = 0;
199  //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
200  if (multiBodyA)
201  {
202  ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
203  jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
204  deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
205  for (int i = 0; i < ndofA; ++i)
206  {
207  btScalar j = jacA[i] ;
208  btScalar l = deltaVelA[i];
209  denom0 += j*l;
210  }
211  }
212  else if(rb0)
213  {
214  vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
215  denom0 = rb0->getInvMass() + contactNormalOnB.dot(vec);
216  }
217  //
218  if (multiBodyB)
219  {
220  const int ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
221  jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
222  deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
223  for (int i = 0; i < ndofB; ++i)
224  {
225  btScalar j = jacB[i] ;
226  btScalar l = deltaVelB[i];
227  denom1 += j*l;
228  }
229 
230  }
231  else if(rb1)
232  {
233  vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
234  denom1 = rb1->getInvMass() + contactNormalOnB.dot(vec);
235  }
236 
237  //
238  btScalar d = denom0+denom1;
239  if (d>SIMD_EPSILON)
240  {
241  solverConstraint.m_jacDiagABInv = relaxation/(d);
242  }
243  else
244  {
245  //disable the constraint row to handle singularity/redundant constraint
246  solverConstraint.m_jacDiagABInv = 0.f;
247  }
248  }
249 
250 
251  //compute rhs and remaining solverConstraint fields
252  btScalar penetration = isFriction? 0 : posError+infoGlobal.m_linearSlop;
253 
254  btScalar rel_vel = 0.f;
255  int ndofA = 0;
256  int ndofB = 0;
257  {
258  btVector3 vel1,vel2;
259  if (multiBodyA)
260  {
261  ndofA = (multiBodyA->isMultiDof() ? multiBodyA->getNumDofs() : multiBodyA->getNumLinks()) + 6;
262  btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
263  for (int i = 0; i < ndofA ; ++i)
264  rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
265  }
266  else if(rb0)
267  {
268  rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
269  }
270  if (multiBodyB)
271  {
272  ndofB = (multiBodyB->isMultiDof() ? multiBodyB->getNumDofs() : multiBodyB->getNumLinks()) + 6;
273  btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
274  for (int i = 0; i < ndofB ; ++i)
275  rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
276 
277  }
278  else if(rb1)
279  {
280  rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
281  }
282 
283  solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
284  }
285 
286 
288  /*
289  if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
290  {
291  solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
292 
293  if (solverConstraint.m_appliedImpulse)
294  {
295  if (multiBodyA)
296  {
297  btScalar impulse = solverConstraint.m_appliedImpulse;
298  btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
299  multiBodyA->applyDeltaVee(deltaV,impulse);
300  applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
301  } else
302  {
303  if (rb0)
304  bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
305  }
306  if (multiBodyB)
307  {
308  btScalar impulse = solverConstraint.m_appliedImpulse;
309  btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
310  multiBodyB->applyDeltaVee(deltaV,impulse);
311  applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
312  } else
313  {
314  if (rb1)
315  bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
316  }
317  }
318  } else
319  */
320 
321  solverConstraint.m_appliedImpulse = 0.f;
322  solverConstraint.m_appliedPushImpulse = 0.f;
323 
324  {
325 
326  btScalar positionalError = 0.f;
327  btScalar velocityError = desiredVelocity - rel_vel;// * damping;
328 
329 
330  btScalar erp = infoGlobal.m_erp2;
331  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
332  {
333  erp = infoGlobal.m_erp;
334  }
335 
336  positionalError = -penetration * erp/infoGlobal.m_timeStep;
337 
338  btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
339  btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
340 
341  if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
342  {
343  //combine position and velocity into rhs
344  solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
345  solverConstraint.m_rhsPenetration = 0.f;
346 
347  } else
348  {
349  //split position and velocity into rhs and m_rhsPenetration
350  solverConstraint.m_rhs = velocityImpulse;
351  solverConstraint.m_rhsPenetration = penetrationImpulse;
352  }
353 
354  solverConstraint.m_cfm = 0.f;
355  solverConstraint.m_lowerLimit = lowerLimit;
356  solverConstraint.m_upperLimit = upperLimit;
357  }
358 
359  return rel_vel;
360 
361 }
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)
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:145
#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:460
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:456
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:413
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:522
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:130
btMultiBodyConstraint(btMultiBody *bodyA, btMultiBody *bodyB, int linkA, int linkB, int numRows, bool isUnilateral)
int getNumDofs() const
Definition: btMultiBody.h:146
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:224
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278