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
5btMultiBodyConstraint::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
20{
21 if (m_bodyA)
22 {
23 m_jacSizeA = (6 + m_bodyA->getNumDofs());
24 }
25
26 if (m_bodyB)
27 {
29 }
30 else
32}
33
35{
37
40}
41
43{
44}
45
46void 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
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
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..
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}
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
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:185
int getCompanionId() const
Definition: btMultiBody.h:515
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:114
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
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:519
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:272
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:60
btScalar getInvMass() const
Definition: btRigidBody.h:263
const btVector3 & getAngularVelocity() const
Definition: btRigidBody.h:402
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:532
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:265
const btVector3 & getLinearVelocity() const
Definition: btRigidBody.h:398
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