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