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