Bullet Collision Detection & Physics Library: btMultiBodyConstraint.cpp Source File

Bullet Collision Detection & Physics Library

- Main Page
- Namespaces
- Classes
- Files

- File List
- File Members

btMultiBodyConstraint.cpp
Go to the documentation of this file.
1 #include "btMultiBodyConstraint.h"
2 #include "BulletDynamics/Dynamics/btRigidBody.h"
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
22 void btMultiBodyConstraint::updateJacobianSizes()
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())
35 m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumDofs();
36 else
37 m_jacSizeBoth = m_jacSizeA + 6 + m_bodyB->getNumLinks();
38 }
39 else
40 m_jacSizeBoth = m_jacSizeA;
41 }
42
43 void btMultiBodyConstraint::allocateJacobiansMultiDof()
44 {
45 updateJacobianSizes();
46
47 m_posOffset = ((1 + m_jacSizeBoth)*m_numRows);
48 m_data.resize((2 + m_jacSizeBoth) * m_numRows);
49 }
50
51 btMultiBodyConstraint::~btMultiBodyConstraint()
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
61 btScalar btMultiBodyConstraint::fillMultiBodyConstraint( btMultiBodySolverConstraint& solverConstraint,
62 btMultiBodyJacobianData& data,
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
140 btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
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..
200 data.m_deltaVelocitiesUnitImpulse.resize(data.m_deltaVelocitiesUnitImpulse.size()+ndofB);
201 btAssert(data.m_jacobians.size() == data.m_deltaVelocitiesUnitImpulse.size());
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 }
btRigidBody::getInvMass
btScalar getInvMass() const
Definition: btRigidBody.h:270
btMultiBody::calcAccelerationDeltas
void calcAccelerationDeltas(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
Definition: btMultiBody.cpp:1796
SIMD_EPSILON
#define SIMD_EPSILON
Definition: btScalar.h:494
btMultiBodySolverConstraint::m_rhsPenetration
btScalar m_rhsPenetration
Definition: btMultiBodySolverConstraint.h:59
btMultiBodyConstraint::fillMultiBodyConstraint
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)
Definition: btMultiBodyConstraint.cpp:61
btMultiBody::getLink
const btMultibodyLink & getLink(int index) const
Definition: btMultiBody.h:118
btMultiBodySolverConstraint::m_relpos1CrossNormal
btVector3 m_relpos1CrossNormal
Definition: btMultiBodySolverConstraint.h:40
btContactSolverInfoData::m_erp
btScalar m_erp
Definition: btContactSolverInfo.h:47
btMultiBodySolverConstraint
1D constraint along a normal axis between bodyA and bodyB. It can be combined to solve contact and fr...
Definition: btMultiBodySolverConstraint.h:28
btMultiBodySolverConstraint::m_contactNormal2
btVector3 m_contactNormal2
Definition: btMultiBodySolverConstraint.h:43
btMultiBodyJacobianData::scratch_r
btAlignedObjectArray< btScalar > scratch_r
Definition: btMultiBodyConstraint.h:33
btMultiBodyJacobianData::m_deltaVelocities
btAlignedObjectArray< btScalar > m_deltaVelocities
Definition: btMultiBodyConstraint.h:32
btRigidBody::getAngularFactor
const btVector3 & getAngularFactor() const
Definition: btRigidBody.h:501
btMultiBodyJacobianData::m_solverBodyPool
btAlignedObjectArray< btSolverBody > * m_solverBodyPool
Definition: btMultiBodyConstraint.h:36
btAlignedObjectArray::at
const T & at(int n) const
Definition: btAlignedObjectArray.h:154
btMultiBodySolverConstraint::m_angularComponentA
btVector3 m_angularComponentA
Definition: btMultiBodySolverConstraint.h:46
btMultiBody::getNumLinks
int getNumLinks() const
Definition: btMultiBody.h:154
btMultiBodySolverConstraint::m_angularComponentB
btVector3 m_angularComponentB
Definition: btMultiBodySolverConstraint.h:47
btContactSolverInfoData::m_splitImpulse
int m_splitImpulse
Definition: btContactSolverInfo.h:50
btMultiBodyConstraint::m_bodyB
btMultiBody * m_bodyB
Definition: btMultiBodyConstraint.h:47
btAssert
#define btAssert(x)
Definition: btScalar.h:113
btMultiBodyConstraint.h
btMultiBodyConstraint::~btMultiBodyConstraint
virtual ~btMultiBodyConstraint()
Definition: btMultiBodyConstraint.cpp:51
btMultiBodyConstraint::m_posOffset
int m_posOffset
Definition: btMultiBodyConstraint.h:54
btMultiBody
Definition: btMultiBody.h:48
btRigidBody.h
btMultiBodyJacobianData::scratch_m
btAlignedObjectArray< btMatrix3x3 > scratch_m
Definition: btMultiBodyConstraint.h:35
btVector3::dot
btScalar dot(const btVector3 &v) const
Return the dot product.
Definition: btVector3.h:235
btMultiBodySolverConstraint::m_deltaVelBindex
int m_deltaVelBindex
Definition: btMultiBodySolverConstraint.h:37
btMultiBodySolverConstraint::m_deltaVelAindex
int m_deltaVelAindex
Definition: btMultiBodySolverConstraint.h:35
btRigidBody::getVelocityInLocalPoint
btVector3 getVelocityInLocalPoint(const btVector3 &rel_pos) const
Definition: btRigidBody.h:379
btMultiBodyJacobianData::m_deltaVelocitiesUnitImpulse
btAlignedObjectArray< btScalar > m_deltaVelocitiesUnitImpulse
Definition: btMultiBodyConstraint.h:31
btMultiBodySolverConstraint::m_jacBindex
int m_jacBindex
Definition: btMultiBodySolverConstraint.h:38
btAlignedObjectArray::size
int size() const
return the number of elements in the array
Definition: btAlignedObjectArray.h:149
btTransform::getOrigin
btVector3 & getOrigin()
Return the origin vector translation.
Definition: btTransform.h:117
btMultiBodySolverConstraint::m_multiBodyA
btMultiBody * m_multiBodyA
Definition: btMultiBodySolverConstraint.h:70
btMultiBodyPoint2Point.h
btMultiBody::setCompanionId
void setCompanionId(int id)
Definition: btMultiBody.h:517
btMultiBodyConstraint::m_jacSizeA
int m_jacSizeA
Definition: btMultiBodyConstraint.h:52
btVector3::cross
btVector3 cross(const btVector3 &v) const
Return the cross product between this and another vector.
Definition: btVector3.h:377
btMultiBodyConstraint::m_jacSizeBoth
int m_jacSizeBoth
Definition: btMultiBodyConstraint.h:53
btMultiBodySolverConstraint::m_multiBodyB
btMultiBody * m_multiBodyB
Definition: btMultiBodySolverConstraint.h:74
btRigidBody
The btRigidBody is the main class for rigid body objects.
Definition: btRigidBody.h:62
btMultiBodySolverConstraint::m_linkA
int m_linkA
Definition: btMultiBodySolverConstraint.h:71
btMultiBodySolverConstraint::m_jacAindex
int m_jacAindex
Definition: btMultiBodySolverConstraint.h:36
btContactSolverInfoData::m_erp2
btScalar m_erp2
Definition: btContactSolverInfo.h:48
btMultiBodyJacobianData
Definition: btMultiBodyConstraint.h:28
btMultiBodyConstraint::m_data
btAlignedObjectArray< btScalar > m_data
Definition: btMultiBodyConstraint.h:66
btMultiBodyJacobianData::m_jacobians
btAlignedObjectArray< btScalar > m_jacobians
Definition: btMultiBodyConstraint.h:30
btMultiBodySolverConstraint::m_rhs
btScalar m_rhs
Definition: btMultiBodySolverConstraint.h:54
btVector3
btVector3 can be used to represent 3D points and vectors.
Definition: btVector3.h:83
btMultiBodyJacobianData::scratch_v
btAlignedObjectArray< btVector3 > scratch_v
Definition: btMultiBodyConstraint.h:34
btContactSolverInfo
Definition: btContactSolverInfo.h:65
btMultiBodySolverConstraint::m_appliedImpulse
btSimdScalar m_appliedImpulse
Definition: btMultiBodySolverConstraint.h:50
btMultiBody::calcAccelerationDeltasMultiDof
void calcAccelerationDeltasMultiDof(const btScalar *force, btScalar *output, btAlignedObjectArray< btScalar > &scratch_r, btAlignedObjectArray< btVector3 > &scratch_v) const
Definition: btMultiBody.cpp:1633
btMultiBodySolverConstraint::m_solverBodyIdB
int m_solverBodyIdB
Definition: btMultiBodySolverConstraint.h:73
btSolverBody
The btSolverBody is an internal datastructure for the constraint solver. Only necessary data is packe...
Definition: btSolverBody.h:108
btMultiBody::getCompanionId
int getCompanionId() const
Definition: btMultiBody.h:513
btMultiBodySolverConstraint::m_upperLimit
btScalar m_upperLimit
Definition: btMultiBodySolverConstraint.h:58
btMultiBody::fillContactJacobianMultiDof
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
btAlignedObjectArray::resize
void resize(int newsize, const T &fillData=T())
Definition: btAlignedObjectArray.h:212
btSolverBody::m_originalBody
btRigidBody * m_originalBody
Definition: btSolverBody.h:124
btMultiBodyConstraint::m_numRows
int m_numRows
Definition: btMultiBodyConstraint.h:51
btMultiBodySolverConstraint::m_friction
btScalar m_friction
Definition: btMultiBodySolverConstraint.h:52
btContactSolverInfoData::m_splitImpulsePenetrationThreshold
btScalar m_splitImpulsePenetrationThreshold
Definition: btContactSolverInfo.h:51
btRigidBody::getInvInertiaTensorWorld
const btMatrix3x3 & getInvInertiaTensorWorld() const
Definition: btRigidBody.h:271
btMultiBodyConstraint::m_bodyA
btMultiBody * m_bodyA
Definition: btMultiBodyConstraint.h:46
btMultiBodySolverConstraint::m_linkB
int m_linkB
Definition: btMultiBodySolverConstraint.h:75
btMultiBody::fillContactJacobian
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
Definition: btMultiBody.cpp:2241
btMultiBodySolverConstraint::m_relpos2CrossNormal
btVector3 m_relpos2CrossNormal
Definition: btMultiBodySolverConstraint.h:42
btMultibodyLink::m_cachedWorldTransform
btTransform m_cachedWorldTransform
Definition: btMultiBodyLink.h:131
btContactSolverInfoData::m_linearSlop
btScalar m_linearSlop
Definition: btContactSolverInfo.h:53
btMultiBody::isMultiDof
bool isMultiDof()
Definition: btMultiBody.h:579
btMultiBodyConstraint::allocateJacobiansMultiDof
void allocateJacobiansMultiDof()
Definition: btMultiBodyConstraint.cpp:43
btSolverBody::getWorldTransform
const btTransform & getWorldTransform() const
Definition: btSolverBody.h:130
btMultiBodyConstraint::btMultiBodyConstraint
btMultiBodyConstraint(btMultiBody *bodyA, btMultiBody *bodyB, int linkA, int linkB, int numRows, bool isUnilateral)
Definition: btMultiBodyConstraint.cpp:7
btMultiBodySolverConstraint::m_appliedPushImpulse
btSimdScalar m_appliedPushImpulse
Definition: btMultiBodySolverConstraint.h:49
btMultiBodySolverConstraint::m_jacDiagABInv
btScalar m_jacDiagABInv
Definition: btMultiBodySolverConstraint.h:53
btContactSolverInfoData::m_timeStep
btScalar m_timeStep
Definition: btContactSolverInfo.h:42
btMultiBody::getBasePos
const btVector3 & getBasePos() const
Definition: btMultiBody.h:176
btMultiBodySolverConstraint::m_solverBodyIdA
int m_solverBodyIdA
Definition: btMultiBodySolverConstraint.h:69
btMultiBodySolverConstraint::m_contactNormal1
btVector3 m_contactNormal1
Definition: btMultiBodySolverConstraint.h:41
btMultiBodyConstraint::updateJacobianSizes
void updateJacobianSizes()
Definition: btMultiBodyConstraint.cpp:22
btMultiBodySolverConstraint::m_cfm
btScalar m_cfm
Definition: btMultiBodySolverConstraint.h:55
btMultiBody::getNumDofs
int getNumDofs() const
Definition: btMultiBody.h:155
btMultiBodyConstraint::applyDeltaVee
void applyDeltaVee(btMultiBodyJacobianData &data, btScalar *delta_vee, btScalar impulse, int velocityIndex, int ndof)
Definition: btMultiBodyConstraint.cpp:55
btMultiBody::getVelocityVector
const btScalar * getVelocityVector() const
Definition: btMultiBody.h:248
btScalar
float btScalar
The btScalar type abstracts floating point numbers, to easily switch between double and single floati...
Definition: btScalar.h:278
btMultiBodySolverConstraint::m_lowerLimit
btScalar m_lowerLimit
Definition: btMultiBodySolverConstraint.h:57
btMultiBodyConstraint::m_linkB
int m_linkB
Definition: btMultiBodyConstraint.h:49
btMultiBodyConstraint::m_linkA
int m_linkA
Definition: btMultiBodyConstraint.h:48