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
7btMultiBodyConstraint::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 m_jacSizeA = (6 + m_bodyA->getNumDofs());
27 }
28
29 if(m_bodyB)
30 {
32 }
33 else
35}
36
38{
40
43}
44
46{
47}
48
49void 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& constraintNormalAng,
59 const btVector3& constraintNormalLin,
60 const btVector3& posAworld, const btVector3& posBworld,
61 btScalar posError,
62 const btContactSolverInfo& infoGlobal,
63 btScalar lowerLimit, btScalar upperLimit,
64 bool angConstraint,
65 btScalar relaxation,
66 bool isFriction, btScalar desiredVelocity, btScalar cfmSlip)
67{
68 solverConstraint.m_multiBodyA = m_bodyA;
69 solverConstraint.m_multiBodyB = m_bodyB;
70 solverConstraint.m_linkA = m_linkA;
71 solverConstraint.m_linkB = m_linkB;
72
73 btMultiBody* multiBodyA = solverConstraint.m_multiBodyA;
74 btMultiBody* multiBodyB = solverConstraint.m_multiBodyB;
75
76 btSolverBody* bodyA = multiBodyA ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdA);
77 btSolverBody* bodyB = multiBodyB ? 0 : &data.m_solverBodyPool->at(solverConstraint.m_solverBodyIdB);
78
79 btRigidBody* rb0 = multiBodyA ? 0 : bodyA->m_originalBody;
80 btRigidBody* rb1 = multiBodyB ? 0 : bodyB->m_originalBody;
81
82 btVector3 rel_pos1, rel_pos2; //these two used to be inited to posAworld and posBworld (respectively) but it does not seem necessary
83 if (bodyA)
84 rel_pos1 = posAworld - bodyA->getWorldTransform().getOrigin();
85 if (bodyB)
86 rel_pos2 = posBworld - bodyB->getWorldTransform().getOrigin();
87
88 if (multiBodyA)
89 {
90 if (solverConstraint.m_linkA<0)
91 {
92 rel_pos1 = posAworld - multiBodyA->getBasePos();
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);
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 torqueAxis0 = constraintNormalAng;
140 }
141 else {
142 torqueAxis0 = rel_pos1.cross(constraintNormalLin);
143
144 }
145 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
146 solverConstraint.m_contactNormal1 = constraintNormalLin;
147 }
148 else //if(rb0)
149 {
150 btVector3 torqueAxis0;
151 if (angConstraint) {
152 torqueAxis0 = constraintNormalAng;
153 }
154 else {
155 torqueAxis0 = rel_pos1.cross(constraintNormalLin);
156 }
157 solverConstraint.m_angularComponentA = rb0 ? rb0->getInvInertiaTensorWorld()*torqueAxis0*rb0->getAngularFactor() : btVector3(0,0,0);
158 solverConstraint.m_relpos1CrossNormal = torqueAxis0;
159 solverConstraint.m_contactNormal1 = constraintNormalLin;
160 }
161
162 if (multiBodyB)
163 {
164 if (solverConstraint.m_linkB<0)
165 {
166 rel_pos2 = posBworld - multiBodyB->getBasePos();
167 } else
168 {
169 rel_pos2 = posBworld - multiBodyB->getLink(solverConstraint.m_linkB).m_cachedWorldTransform.getOrigin();
170 }
171
172 const int ndofB = multiBodyB->getNumDofs() + 6;
173
174 solverConstraint.m_deltaVelBindex = multiBodyB->getCompanionId();
175 if (solverConstraint.m_deltaVelBindex <0)
176 {
177 solverConstraint.m_deltaVelBindex = data.m_deltaVelocities.size();
178 multiBodyB->setCompanionId(solverConstraint.m_deltaVelBindex);
180 }
181
182 //determine jacobian of this 1D constraint in terms of multibodyB's degrees of freedom
183 //resize..
184 solverConstraint.m_jacBindex = data.m_jacobians.size();
185 data.m_jacobians.resize(data.m_jacobians.size()+ndofB);
186 //copy/determine..
187 if(jacOrgB)
188 {
189 for (int i=0;i<ndofB;i++)
190 data.m_jacobians[solverConstraint.m_jacBindex+i] = jacOrgB[i];
191 }
192 else
193 {
194 //multiBodyB->fillContactJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalLin, &data.m_jacobians[solverConstraint.m_jacBindex], data.scratch_r, data.scratch_v, data.scratch_m);
195 multiBodyB->fillConstraintJacobianMultiDof(solverConstraint.m_linkB, posBworld, -constraintNormalAng, -constraintNormalLin, &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 multiBodyB->calcAccelerationDeltasMultiDof(&data.m_jacobians[solverConstraint.m_jacBindex],delta,data.scratch_r, data.scratch_v);
205
206 btVector3 torqueAxis1;
207 if (angConstraint) {
208 torqueAxis1 = constraintNormalAng;
209 }
210 else {
211 torqueAxis1 = rel_pos2.cross(constraintNormalLin);
212 }
213 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
214 solverConstraint.m_contactNormal2 = -constraintNormalLin;
215 }
216 else //if(rb1)
217 {
218 btVector3 torqueAxis1;
219 if (angConstraint) {
220 torqueAxis1 = constraintNormalAng;
221 }
222 else {
223 torqueAxis1 = rel_pos2.cross(constraintNormalLin);
224 }
225 solverConstraint.m_angularComponentB = rb1 ? rb1->getInvInertiaTensorWorld()*-torqueAxis1*rb1->getAngularFactor() : btVector3(0,0,0);
226 solverConstraint.m_relpos2CrossNormal = -torqueAxis1;
227 solverConstraint.m_contactNormal2 = -constraintNormalLin;
228 }
229 {
230
231 btVector3 vec;
232 btScalar denom0 = 0.f;
233 btScalar denom1 = 0.f;
234 btScalar* jacB = 0;
235 btScalar* jacA = 0;
236 btScalar* deltaVelA = 0;
237 btScalar* deltaVelB = 0;
238 int ndofA = 0;
239 //determine the "effective mass" of the constrained multibodyA with respect to this 1D constraint (i.e. 1/A[i,i])
240 if (multiBodyA)
241 {
242 ndofA = multiBodyA->getNumDofs() + 6;
243 jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
244 deltaVelA = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
245 for (int i = 0; i < ndofA; ++i)
246 {
247 btScalar j = jacA[i] ;
248 btScalar l = deltaVelA[i];
249 denom0 += j*l;
250 }
251 }
252 else if(rb0)
253 {
254 vec = ( solverConstraint.m_angularComponentA).cross(rel_pos1);
255 if (angConstraint) {
256 denom0 = rb0->getInvMass() + constraintNormalAng.dot(vec);
257 }
258 else {
259 denom0 = rb0->getInvMass() + constraintNormalLin.dot(vec);
260 }
261 }
262 //
263 if (multiBodyB)
264 {
265 const int ndofB = multiBodyB->getNumDofs() + 6;
266 jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
267 deltaVelB = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
268 for (int i = 0; i < ndofB; ++i)
269 {
270 btScalar j = jacB[i] ;
271 btScalar l = deltaVelB[i];
272 denom1 += j*l;
273 }
274
275 }
276 else if(rb1)
277 {
278 vec = ( -solverConstraint.m_angularComponentB).cross(rel_pos2);
279 if (angConstraint) {
280 denom1 = rb1->getInvMass() + constraintNormalAng.dot(vec);
281 }
282 else {
283 denom1 = rb1->getInvMass() + constraintNormalLin.dot(vec);
284 }
285 }
286
287 //
288 btScalar d = denom0+denom1;
289 if (d>SIMD_EPSILON)
290 {
291 solverConstraint.m_jacDiagABInv = relaxation/(d);
292 }
293 else
294 {
295 //disable the constraint row to handle singularity/redundant constraint
296 solverConstraint.m_jacDiagABInv = 0.f;
297 }
298 }
299
300
301 //compute rhs and remaining solverConstraint fields
302 btScalar penetration = isFriction? 0 : posError;
303
304 btScalar rel_vel = 0.f;
305 int ndofA = 0;
306 int ndofB = 0;
307 {
308 btVector3 vel1,vel2;
309 if (multiBodyA)
310 {
311 ndofA = multiBodyA->getNumDofs() + 6;
312 btScalar* jacA = &data.m_jacobians[solverConstraint.m_jacAindex];
313 for (int i = 0; i < ndofA ; ++i)
314 rel_vel += multiBodyA->getVelocityVector()[i] * jacA[i];
315 }
316 else if(rb0)
317 {
318 rel_vel += rb0->getVelocityInLocalPoint(rel_pos1).dot(solverConstraint.m_contactNormal1);
319 }
320 if (multiBodyB)
321 {
322 ndofB = multiBodyB->getNumDofs() + 6;
323 btScalar* jacB = &data.m_jacobians[solverConstraint.m_jacBindex];
324 for (int i = 0; i < ndofB ; ++i)
325 rel_vel += multiBodyB->getVelocityVector()[i] * jacB[i];
326
327 }
328 else if(rb1)
329 {
330 rel_vel += rb1->getVelocityInLocalPoint(rel_pos2).dot(solverConstraint.m_contactNormal2);
331 }
332
333 solverConstraint.m_friction = 0.f;//cp.m_combinedFriction;
334 }
335
336
338 /*
339 if (infoGlobal.m_solverMode & SOLVER_USE_WARMSTARTING)
340 {
341 solverConstraint.m_appliedImpulse = isFriction ? 0 : cp.m_appliedImpulse * infoGlobal.m_warmstartingFactor;
342
343 if (solverConstraint.m_appliedImpulse)
344 {
345 if (multiBodyA)
346 {
347 btScalar impulse = solverConstraint.m_appliedImpulse;
348 btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacAindex];
349 multiBodyA->applyDeltaVee(deltaV,impulse);
350 applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelAindex,ndofA);
351 } else
352 {
353 if (rb0)
354 bodyA->internalApplyImpulse(solverConstraint.m_contactNormal1*bodyA->internalGetInvMass()*rb0->getLinearFactor(),solverConstraint.m_angularComponentA,solverConstraint.m_appliedImpulse);
355 }
356 if (multiBodyB)
357 {
358 btScalar impulse = solverConstraint.m_appliedImpulse;
359 btScalar* deltaV = &data.m_deltaVelocitiesUnitImpulse[solverConstraint.m_jacBindex];
360 multiBodyB->applyDeltaVee(deltaV,impulse);
361 applyDeltaVee(data,deltaV,impulse,solverConstraint.m_deltaVelBindex,ndofB);
362 } else
363 {
364 if (rb1)
365 bodyB->internalApplyImpulse(-solverConstraint.m_contactNormal2*bodyB->internalGetInvMass()*rb1->getLinearFactor(),-solverConstraint.m_angularComponentB,-(btScalar)solverConstraint.m_appliedImpulse);
366 }
367 }
368 } else
369 */
370
371 solverConstraint.m_appliedImpulse = 0.f;
372 solverConstraint.m_appliedPushImpulse = 0.f;
373
374 {
375
376 btScalar positionalError = 0.f;
377 btScalar velocityError = desiredVelocity - rel_vel;// * damping;
378
379
380 btScalar erp = infoGlobal.m_erp2;
381
382 //split impulse is not implemented yet for btMultiBody*
383 //if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
384 {
385 erp = infoGlobal.m_erp;
386 }
387
388 positionalError = -penetration * erp/infoGlobal.m_timeStep;
389
390 btScalar penetrationImpulse = positionalError*solverConstraint.m_jacDiagABInv;
391 btScalar velocityImpulse = velocityError *solverConstraint.m_jacDiagABInv;
392
393 //split impulse is not implemented yet for btMultiBody*
394
395 // if (!infoGlobal.m_splitImpulse || (penetration > infoGlobal.m_splitImpulsePenetrationThreshold))
396 {
397 //combine position and velocity into rhs
398 solverConstraint.m_rhs = penetrationImpulse+velocityImpulse;
399 solverConstraint.m_rhsPenetration = 0.f;
400
401 }
402 /*else
403 {
404 //split position and velocity into rhs and m_rhsPenetration
405 solverConstraint.m_rhs = velocityImpulse;
406 solverConstraint.m_rhsPenetration = penetrationImpulse;
407 }
408 */
409
410 solverConstraint.m_cfm = 0.f;
411 solverConstraint.m_lowerLimit = lowerLimit;
412 solverConstraint.m_upperLimit = upperLimit;
413 }
414
415 return rel_vel;
416
417}
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:292
#define SIMD_EPSILON
Definition: btScalar.h:521
#define btAssert(x)
Definition: btScalar.h:131
int size() const
return the number of elements in the array
void resize(int newsize, const T &fillData=T())
btAlignedObjectArray< btScalar > m_data
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
btMultiBodyConstraint(btMultiBody *bodyA, btMultiBody *bodyB, int linkA, int linkB, int numRows, bool isUnilateral)
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)
const btVector3 & getBasePos() const
Definition: btMultiBody.h:186
int getCompanionId() const
Definition: btMultiBody.h:482
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:119
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
int getNumDofs() const
Definition: btMultiBody.h:165
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
void setCompanionId(int id)
Definition: btMultiBody.h:486
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:258
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:63
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:382
btScalar getInvMass() const
Definition: btRigidBody.h:273
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:504
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:274
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:84
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:389
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
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:109
btRigidBody * m_originalBody
Definition: btSolverBody.h:124
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:130