1 /*
2 Bullet Continuous Collision Detection and Physics Library
3 Copyright (c) 2003-2006 Erwin Coumans https://bulletphysics.org
4
5 This software is provided 'as-is', without any express or implied warranty.
6 In no event will the authors be held liable for any damages arising from the use of this software.
7 Permission is granted to anyone to use this software for any purpose,
8 including commercial applications, and to alter it and redistribute it freely,
9 subject to the following restrictions:
10
11 1. The origin of this software must not be misrepresented; you must not claim that you wrote the original software. If you use this software in a product, an acknowledgment in the product documentation would be appreciated but is not required.
12 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
13 3. This notice may not be removed or altered from any source distribution.
14 */
15
16 #include "btRigidBody.h"
17 #include "BulletCollision/CollisionShapes/btConvexShape.h"
18 #include "LinearMath/btMinMax.h"
19 #include "LinearMath/btTransformUtil.h"
20 #include "LinearMath/btMotionState.h"
21 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
22 #include "LinearMath/btSerializer.h"
23
24 //'temporarily' global variables
25 btScalar gDeactivationTime = btScalar(2.);
26 bool gDisableDeactivation = false;
27 static int uniqueId = 0;
28
btRigidBody(const btRigidBody::btRigidBodyConstructionInfo & constructionInfo)29 btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
30 {
31 setupRigidBody(constructionInfo);
32 }
33
btRigidBody(btScalar mass,btMotionState * motionState,btCollisionShape * collisionShape,const btVector3 & localInertia)34 btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia)
35 {
36 btRigidBodyConstructionInfo cinfo(mass, motionState, collisionShape, localInertia);
37 setupRigidBody(cinfo);
38 }
39
setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo & constructionInfo)40 void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
41 {
42 m_internalType = CO_RIGID_BODY;
43
44 m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
45 m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
46 m_angularFactor.setValue(1, 1, 1);
47 m_linearFactor.setValue(1, 1, 1);
48 m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
49 m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
50 m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
51 m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
52 setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
53
54 m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
55 m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
56 m_optionalMotionState = constructionInfo.m_motionState;
57 m_contactSolverType = 0;
58 m_frictionSolverType = 0;
59 m_additionalDamping = constructionInfo.m_additionalDamping;
60 m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
61 m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
62 m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
63 m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
64
65 if (m_optionalMotionState)
66 {
67 m_optionalMotionState->getWorldTransform(m_worldTransform);
68 }
69 else
70 {
71 m_worldTransform = constructionInfo.m_startWorldTransform;
72 }
73
74 m_interpolationWorldTransform = m_worldTransform;
75 m_interpolationLinearVelocity.setValue(0, 0, 0);
76 m_interpolationAngularVelocity.setValue(0, 0, 0);
77
78 //moved to btCollisionObject
79 m_friction = constructionInfo.m_friction;
80 m_rollingFriction = constructionInfo.m_rollingFriction;
81 m_spinningFriction = constructionInfo.m_spinningFriction;
82
83 m_restitution = constructionInfo.m_restitution;
84
85 setCollisionShape(constructionInfo.m_collisionShape);
86 m_debugBodyId = uniqueId++;
87
88 setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
89 updateInertiaTensor();
90
91 m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY;
92
93 m_deltaLinearVelocity.setZero();
94 m_deltaAngularVelocity.setZero();
95 m_invMass = m_inverseMass * m_linearFactor;
96 m_pushVelocity.setZero();
97 m_turnVelocity.setZero();
98 }
99
predictIntegratedTransform(btScalar timeStep,btTransform & predictedTransform)100 void btRigidBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform)
101 {
102 btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform);
103 }
104
saveKinematicState(btScalar timeStep)105 void btRigidBody::saveKinematicState(btScalar timeStep)
106 {
107 //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
108 if (timeStep != btScalar(0.))
109 {
110 //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
111 if (getMotionState())
112 getMotionState()->getWorldTransform(m_worldTransform);
113 btVector3 linVel, angVel;
114
115 btTransformUtil::calculateVelocity(m_interpolationWorldTransform, m_worldTransform, timeStep, m_linearVelocity, m_angularVelocity);
116 m_interpolationLinearVelocity = m_linearVelocity;
117 m_interpolationAngularVelocity = m_angularVelocity;
118 m_interpolationWorldTransform = m_worldTransform;
119 //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
120 }
121 }
122
getAabb(btVector3 & aabbMin,btVector3 & aabbMax) const123 void btRigidBody::getAabb(btVector3& aabbMin, btVector3& aabbMax) const
124 {
125 getCollisionShape()->getAabb(m_worldTransform, aabbMin, aabbMax);
126 }
127
setGravity(const btVector3 & acceleration)128 void btRigidBody::setGravity(const btVector3& acceleration)
129 {
130 if (m_inverseMass != btScalar(0.0))
131 {
132 m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
133 }
134 m_gravity_acceleration = acceleration;
135 }
136
setDamping(btScalar lin_damping,btScalar ang_damping)137 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
138 {
139 #ifdef BT_USE_OLD_DAMPING_METHOD
140 m_linearDamping = btMax(lin_damping, btScalar(0.0));
141 m_angularDamping = btMax(ang_damping, btScalar(0.0));
142 #else
143 m_linearDamping = btClamped(lin_damping, btScalar(0.0), btScalar(1.0));
144 m_angularDamping = btClamped(ang_damping, btScalar(0.0), btScalar(1.0));
145 #endif
146 }
147
148 ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
applyDamping(btScalar timeStep)149 void btRigidBody::applyDamping(btScalar timeStep)
150 {
151 //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
152 //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
153
154 #ifdef BT_USE_OLD_DAMPING_METHOD
155 m_linearVelocity *= btMax((btScalar(1.0) - timeStep * m_linearDamping), btScalar(0.0));
156 m_angularVelocity *= btMax((btScalar(1.0) - timeStep * m_angularDamping), btScalar(0.0));
157 #else
158 m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep);
159 m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep);
160 #endif
161
162 if (m_additionalDamping)
163 {
164 //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
165 //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
166 if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
167 (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
168 {
169 m_angularVelocity *= m_additionalDampingFactor;
170 m_linearVelocity *= m_additionalDampingFactor;
171 }
172
173 btScalar speed = m_linearVelocity.length();
174 if (speed < m_linearDamping)
175 {
176 btScalar dampVel = btScalar(0.005);
177 if (speed > dampVel)
178 {
179 btVector3 dir = m_linearVelocity.normalized();
180 m_linearVelocity -= dir * dampVel;
181 }
182 else
183 {
184 m_linearVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
185 }
186 }
187
188 btScalar angSpeed = m_angularVelocity.length();
189 if (angSpeed < m_angularDamping)
190 {
191 btScalar angDampVel = btScalar(0.005);
192 if (angSpeed > angDampVel)
193 {
194 btVector3 dir = m_angularVelocity.normalized();
195 m_angularVelocity -= dir * angDampVel;
196 }
197 else
198 {
199 m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.));
200 }
201 }
202 }
203 }
204
applyGravity()205 void btRigidBody::applyGravity()
206 {
207 if (isStaticOrKinematicObject())
208 return;
209
210 applyCentralForce(m_gravity);
211 }
212
clearGravity()213 void btRigidBody::clearGravity()
214 {
215 if (isStaticOrKinematicObject())
216 return;
217
218 applyCentralForce(-m_gravity);
219 }
220
proceedToTransform(const btTransform & newTrans)221 void btRigidBody::proceedToTransform(const btTransform& newTrans)
222 {
223 setCenterOfMassTransform(newTrans);
224 }
225
setMassProps(btScalar mass,const btVector3 & inertia)226 void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
227 {
228 if (mass == btScalar(0.))
229 {
230 m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
231 m_inverseMass = btScalar(0.);
232 }
233 else
234 {
235 m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
236 m_inverseMass = btScalar(1.0) / mass;
237 }
238
239 //Fg = m * a
240 m_gravity = mass * m_gravity_acceleration;
241
242 m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
243 inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
244 inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
245
246 m_invMass = m_linearFactor * m_inverseMass;
247 }
248
updateInertiaTensor()249 void btRigidBody::updateInertiaTensor()
250 {
251 m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
252 }
253
getLocalInertia() const254 btVector3 btRigidBody::getLocalInertia() const
255 {
256 btVector3 inertiaLocal;
257 const btVector3 inertia = m_invInertiaLocal;
258 inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
259 inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y() : btScalar(0.0),
260 inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z() : btScalar(0.0));
261 return inertiaLocal;
262 }
263
evalEulerEqn(const btVector3 & w1,const btVector3 & w0,const btVector3 & T,const btScalar dt,const btMatrix3x3 & I)264 inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt,
265 const btMatrix3x3& I)
266 {
267 const btVector3 w2 = I * w1 + w1.cross(I * w1) * dt - (T * dt + I * w0);
268 return w2;
269 }
270
evalEulerEqnDeriv(const btVector3 & w1,const btVector3 & w0,const btScalar dt,const btMatrix3x3 & I)271 inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt,
272 const btMatrix3x3& I)
273 {
274 btMatrix3x3 w1x, Iw1x;
275 const btVector3 Iwi = (I * w1);
276 w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]);
277 Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]);
278
279 const btMatrix3x3 dfw1 = I + (w1x * I - Iw1x) * dt;
280 return dfw1;
281 }
282
computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const283 btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const
284 {
285 btVector3 inertiaLocal = getLocalInertia();
286 btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
287 btVector3 tmp = inertiaTensorWorld * getAngularVelocity();
288 btVector3 gf = getAngularVelocity().cross(tmp);
289 btScalar l2 = gf.length2();
290 if (l2 > maxGyroscopicForce * maxGyroscopicForce)
291 {
292 gf *= btScalar(1.) / btSqrt(l2) * maxGyroscopicForce;
293 }
294 return gf;
295 }
296
computeGyroscopicImpulseImplicit_Body(btScalar step) const297 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const
298 {
299 btVector3 idl = getLocalInertia();
300 btVector3 omega1 = getAngularVelocity();
301 btQuaternion q = getWorldTransform().getRotation();
302
303 // Convert to body coordinates
304 btVector3 omegab = quatRotate(q.inverse(), omega1);
305 btMatrix3x3 Ib;
306 Ib.setValue(idl.x(), 0, 0,
307 0, idl.y(), 0,
308 0, 0, idl.z());
309
310 btVector3 ibo = Ib * omegab;
311
312 // Residual vector
313 btVector3 f = step * omegab.cross(ibo);
314
315 btMatrix3x3 skew0;
316 omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]);
317 btVector3 om = Ib * omegab;
318 btMatrix3x3 skew1;
319 om.getSkewSymmetricMatrix(&skew1[0], &skew1[1], &skew1[2]);
320
321 // Jacobian
322 btMatrix3x3 J = Ib + (skew0 * Ib - skew1) * step;
323
324 // btMatrix3x3 Jinv = J.inverse();
325 // btVector3 omega_div = Jinv*f;
326 btVector3 omega_div = J.solve33(f);
327
328 // Single Newton-Raphson update
329 omegab = omegab - omega_div; //Solve33(J, f);
330 // Back to world coordinates
331 btVector3 omega2 = quatRotate(q, omegab);
332 btVector3 gf = omega2 - omega1;
333 return gf;
334 }
335
computeGyroscopicImpulseImplicit_World(btScalar step) const336 btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const
337 {
338 // use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior.
339 // calculate using implicit euler step so it's stable.
340
341 const btVector3 inertiaLocal = getLocalInertia();
342 const btVector3 w0 = getAngularVelocity();
343
344 btMatrix3x3 I;
345
346 I = m_worldTransform.getBasis().scaled(inertiaLocal) *
347 m_worldTransform.getBasis().transpose();
348
349 // use newtons method to find implicit solution for new angular velocity (w')
350 // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0
351 // df/dw' = I + 1xIw'*step + w'xI*step
352
353 btVector3 w1 = w0;
354
355 // one step of newton's method
356 {
357 const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I);
358 const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I);
359
360 btVector3 dw;
361 dw = dfw.solve33(fw);
362 //const btMatrix3x3 dfw_inv = dfw.inverse();
363 //dw = dfw_inv*fw;
364
365 w1 -= dw;
366 }
367
368 btVector3 gf = (w1 - w0);
369 return gf;
370 }
371
integrateVelocities(btScalar step)372 void btRigidBody::integrateVelocities(btScalar step)
373 {
374 if (isStaticOrKinematicObject())
375 return;
376
377 m_linearVelocity += m_totalForce * (m_inverseMass * step);
378 m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
379
380 #define MAX_ANGVEL SIMD_HALF_PI
381 /// clamp angular velocity. collision calculations will fail on higher angular velocities
382 btScalar angvel = m_angularVelocity.length();
383 if (angvel * step > MAX_ANGVEL)
384 {
385 m_angularVelocity *= (MAX_ANGVEL / step) / angvel;
386 }
387 #if defined(BT_CLAMP_VELOCITY_TO) && BT_CLAMP_VELOCITY_TO > 0
388 clampVelocity(m_angularVelocity);
389 #endif
390 }
391
getOrientation() const392 btQuaternion btRigidBody::getOrientation() const
393 {
394 btQuaternion orn;
395 m_worldTransform.getBasis().getRotation(orn);
396 return orn;
397 }
398
setCenterOfMassTransform(const btTransform & xform)399 void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
400 {
401 if (isKinematicObject())
402 {
403 m_interpolationWorldTransform = m_worldTransform;
404 }
405 else
406 {
407 m_interpolationWorldTransform = xform;
408 }
409 m_interpolationLinearVelocity = getLinearVelocity();
410 m_interpolationAngularVelocity = getAngularVelocity();
411 m_worldTransform = xform;
412 updateInertiaTensor();
413 }
414
addConstraintRef(btTypedConstraint * c)415 void btRigidBody::addConstraintRef(btTypedConstraint* c)
416 {
417 ///disable collision with the 'other' body
418
419 int index = m_constraintRefs.findLinearSearch(c);
420 //don't add constraints that are already referenced
421 //btAssert(index == m_constraintRefs.size());
422 if (index == m_constraintRefs.size())
423 {
424 m_constraintRefs.push_back(c);
425 btCollisionObject* colObjA = &c->getRigidBodyA();
426 btCollisionObject* colObjB = &c->getRigidBodyB();
427 if (colObjA == this)
428 {
429 colObjA->setIgnoreCollisionCheck(colObjB, true);
430 }
431 else
432 {
433 colObjB->setIgnoreCollisionCheck(colObjA, true);
434 }
435 }
436 }
437
removeConstraintRef(btTypedConstraint * c)438 void btRigidBody::removeConstraintRef(btTypedConstraint* c)
439 {
440 int index = m_constraintRefs.findLinearSearch(c);
441 //don't remove constraints that are not referenced
442 if (index < m_constraintRefs.size())
443 {
444 m_constraintRefs.remove(c);
445 btCollisionObject* colObjA = &c->getRigidBodyA();
446 btCollisionObject* colObjB = &c->getRigidBodyB();
447 if (colObjA == this)
448 {
449 colObjA->setIgnoreCollisionCheck(colObjB, false);
450 }
451 else
452 {
453 colObjB->setIgnoreCollisionCheck(colObjA, false);
454 }
455 }
456 }
457
calculateSerializeBufferSize() const458 int btRigidBody::calculateSerializeBufferSize() const
459 {
460 int sz = sizeof(btRigidBodyData);
461 return sz;
462 }
463
464 ///fills the dataBuffer and returns the struct name (and 0 on failure)
serialize(void * dataBuffer,class btSerializer * serializer) const465 const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
466 {
467 btRigidBodyData* rbd = (btRigidBodyData*)dataBuffer;
468
469 btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
470
471 m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld);
472 m_linearVelocity.serialize(rbd->m_linearVelocity);
473 m_angularVelocity.serialize(rbd->m_angularVelocity);
474 rbd->m_inverseMass = m_inverseMass;
475 m_angularFactor.serialize(rbd->m_angularFactor);
476 m_linearFactor.serialize(rbd->m_linearFactor);
477 m_gravity.serialize(rbd->m_gravity);
478 m_gravity_acceleration.serialize(rbd->m_gravity_acceleration);
479 m_invInertiaLocal.serialize(rbd->m_invInertiaLocal);
480 m_totalForce.serialize(rbd->m_totalForce);
481 m_totalTorque.serialize(rbd->m_totalTorque);
482 rbd->m_linearDamping = m_linearDamping;
483 rbd->m_angularDamping = m_angularDamping;
484 rbd->m_additionalDamping = m_additionalDamping;
485 rbd->m_additionalDampingFactor = m_additionalDampingFactor;
486 rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
487 rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
488 rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
489 rbd->m_linearSleepingThreshold = m_linearSleepingThreshold;
490 rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
491
492 // Fill padding with zeros to appease msan.
493 #ifdef BT_USE_DOUBLE_PRECISION
494 memset(rbd->m_padding, 0, sizeof(rbd->m_padding));
495 #endif
496
497 return btRigidBodyDataName;
498 }
499
serializeSingleObject(class btSerializer * serializer) const500 void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
501 {
502 btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(), 1);
503 const char* structType = serialize(chunk->m_oldPtr, serializer);
504 serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, (void*)this);
505 }
506