bullet: Update to current svn, r2636
[blender.git] / extern / bullet2 / src / BulletDynamics / Dynamics / btRigidBody.h
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 #ifndef BT_RIGIDBODY_H
17 #define BT_RIGIDBODY_H
18
19 #include "LinearMath/btAlignedObjectArray.h"
20 #include "LinearMath/btTransform.h"
21 #include "BulletCollision/BroadphaseCollision/btBroadphaseProxy.h"
22 #include "BulletCollision/CollisionDispatch/btCollisionObject.h"
23
24 class btCollisionShape;
25 class btMotionState;
26 class btTypedConstraint;
27
28
29 extern btScalar gDeactivationTime;
30 extern bool gDisableDeactivation;
31
32 #ifdef BT_USE_DOUBLE_PRECISION
33 #define btRigidBodyData btRigidBodyDoubleData
34 #define btRigidBodyDataName     "btRigidBodyDoubleData"
35 #else
36 #define btRigidBodyData btRigidBodyFloatData
37 #define btRigidBodyDataName     "btRigidBodyFloatData"
38 #endif //BT_USE_DOUBLE_PRECISION
39
40
41 enum    btRigidBodyFlags
42 {
43         BT_DISABLE_WORLD_GRAVITY = 1,
44         ///The BT_ENABLE_GYROPSCOPIC_FORCE can easily introduce instability
45         ///So generally it is best to not enable it. 
46         ///If really needed, run at a high frequency like 1000 Hertz:   ///See Demos/GyroscopicDemo for an example use
47         BT_ENABLE_GYROPSCOPIC_FORCE = 2
48 };
49
50
51 ///The btRigidBody is the main class for rigid body objects. It is derived from btCollisionObject, so it keeps a pointer to a btCollisionShape.
52 ///It is recommended for performance and memory use to share btCollisionShape objects whenever possible.
53 ///There are 3 types of rigid bodies: 
54 ///- A) Dynamic rigid bodies, with positive mass. Motion is controlled by rigid body dynamics.
55 ///- B) Fixed objects with zero mass. They are not moving (basically collision objects)
56 ///- C) Kinematic objects, which are objects without mass, but the user can move them. There is on-way interaction, and Bullet calculates a velocity based on the timestep and previous and current world transform.
57 ///Bullet automatically deactivates dynamic rigid bodies, when the velocity is below a threshold for a given time.
58 ///Deactivated (sleeping) rigid bodies don't take any processing time, except a minor broadphase collision detection impact (to allow active objects to activate/wake up sleeping objects)
59 class btRigidBody  : public btCollisionObject
60 {
61
62         btMatrix3x3     m_invInertiaTensorWorld;
63         btVector3               m_linearVelocity;
64         btVector3               m_angularVelocity;
65         btScalar                m_inverseMass;
66         btVector3               m_linearFactor;
67
68         btVector3               m_gravity;      
69         btVector3               m_gravity_acceleration;
70         btVector3               m_invInertiaLocal;
71         btVector3               m_totalForce;
72         btVector3               m_totalTorque;
73         
74         btScalar                m_linearDamping;
75         btScalar                m_angularDamping;
76
77         bool                    m_additionalDamping;
78         btScalar                m_additionalDampingFactor;
79         btScalar                m_additionalLinearDampingThresholdSqr;
80         btScalar                m_additionalAngularDampingThresholdSqr;
81         btScalar                m_additionalAngularDampingFactor;
82
83
84         btScalar                m_linearSleepingThreshold;
85         btScalar                m_angularSleepingThreshold;
86
87         //m_optionalMotionState allows to automatic synchronize the world transform for active objects
88         btMotionState*  m_optionalMotionState;
89
90         //keep track of typed constraints referencing this rigid body
91         btAlignedObjectArray<btTypedConstraint*> m_constraintRefs;
92
93         int                             m_rigidbodyFlags;
94         
95         int                             m_debugBodyId;
96         
97
98 protected:
99
100         ATTRIBUTE_ALIGNED16(btVector3           m_deltaLinearVelocity);
101         btVector3               m_deltaAngularVelocity;
102         btVector3               m_angularFactor;
103         btVector3               m_invMass;
104         btVector3               m_pushVelocity;
105         btVector3               m_turnVelocity;
106
107
108 public:
109
110
111         ///The btRigidBodyConstructionInfo structure provides information to create a rigid body. Setting mass to zero creates a fixed (non-dynamic) rigid body.
112         ///For dynamic objects, you can use the collision shape to approximate the local inertia tensor, otherwise use the zero vector (default argument)
113         ///You can use the motion state to synchronize the world transform between physics and graphics objects. 
114         ///And if the motion state is provided, the rigid body will initialize its initial world transform from the motion state,
115         ///m_startWorldTransform is only used when you don't provide a motion state.
116         struct  btRigidBodyConstructionInfo
117         {
118                 btScalar                        m_mass;
119
120                 ///When a motionState is provided, the rigid body will initialize its world transform from the motion state
121                 ///In this case, m_startWorldTransform is ignored.
122                 btMotionState*          m_motionState;
123                 btTransform     m_startWorldTransform;
124
125                 btCollisionShape*       m_collisionShape;
126                 btVector3                       m_localInertia;
127                 btScalar                        m_linearDamping;
128                 btScalar                        m_angularDamping;
129
130                 ///best simulation results when friction is non-zero
131                 btScalar                        m_friction;
132                 ///the m_rollingFriction prevents rounded shapes, such as spheres, cylinders and capsules from rolling forever.
133                 ///See Bullet/Demos/RollingFrictionDemo for usage
134                 btScalar                        m_rollingFriction;
135                 ///best simulation results using zero restitution.
136                 btScalar                        m_restitution;
137
138                 btScalar                        m_linearSleepingThreshold;
139                 btScalar                        m_angularSleepingThreshold;
140
141                 //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
142                 //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
143                 bool                            m_additionalDamping;
144                 btScalar                        m_additionalDampingFactor;
145                 btScalar                        m_additionalLinearDampingThresholdSqr;
146                 btScalar                        m_additionalAngularDampingThresholdSqr;
147                 btScalar                        m_additionalAngularDampingFactor;
148
149                 btRigidBodyConstructionInfo(    btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0)):
150                 m_mass(mass),
151                         m_motionState(motionState),
152                         m_collisionShape(collisionShape),
153                         m_localInertia(localInertia),
154                         m_linearDamping(btScalar(0.)),
155                         m_angularDamping(btScalar(0.)),
156                         m_friction(btScalar(0.5)),
157                         m_rollingFriction(btScalar(0)),
158                         m_restitution(btScalar(0.)),
159                         m_linearSleepingThreshold(btScalar(0.8)),
160                         m_angularSleepingThreshold(btScalar(1.f)),
161                         m_additionalDamping(false),
162                         m_additionalDampingFactor(btScalar(0.005)),
163                         m_additionalLinearDampingThresholdSqr(btScalar(0.01)),
164                         m_additionalAngularDampingThresholdSqr(btScalar(0.01)),
165                         m_additionalAngularDampingFactor(btScalar(0.01))
166                 {
167                         m_startWorldTransform.setIdentity();
168                 }
169         };
170
171         ///btRigidBody constructor using construction info
172         btRigidBody(    const btRigidBodyConstructionInfo& constructionInfo);
173
174         ///btRigidBody constructor for backwards compatibility. 
175         ///To specify friction (etc) during rigid body construction, please use the other constructor (using btRigidBodyConstructionInfo)
176         btRigidBody(    btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia=btVector3(0,0,0));
177
178
179         virtual ~btRigidBody()
180         { 
181                 //No constraints should point to this rigidbody
182                 //Remove constraints from the dynamics world before you delete the related rigidbodies. 
183                 btAssert(m_constraintRefs.size()==0); 
184         }
185
186 protected:
187
188         ///setupRigidBody is only used internally by the constructor
189         void    setupRigidBody(const btRigidBodyConstructionInfo& constructionInfo);
190
191 public:
192
193         void                    proceedToTransform(const btTransform& newTrans); 
194         
195         ///to keep collision detection and dynamics separate we don't store a rigidbody pointer
196         ///but a rigidbody is derived from btCollisionObject, so we can safely perform an upcast
197         static const btRigidBody*       upcast(const btCollisionObject* colObj)
198         {
199                 if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
200                         return (const btRigidBody*)colObj;
201                 return 0;
202         }
203         static btRigidBody*     upcast(btCollisionObject* colObj)
204         {
205                 if (colObj->getInternalType()&btCollisionObject::CO_RIGID_BODY)
206                         return (btRigidBody*)colObj;
207                 return 0;
208         }
209         
210         /// continuous collision detection needs prediction
211         void                    predictIntegratedTransform(btScalar step, btTransform& predictedTransform) ;
212         
213         void                    saveKinematicState(btScalar step);
214         
215         void                    applyGravity();
216         
217         void                    setGravity(const btVector3& acceleration);  
218
219         const btVector3&        getGravity() const
220         {
221                 return m_gravity_acceleration;
222         }
223
224         void                    setDamping(btScalar lin_damping, btScalar ang_damping);
225
226         btScalar getLinearDamping() const
227         {
228                 return m_linearDamping;
229         }
230
231         btScalar getAngularDamping() const
232         {
233                 return m_angularDamping;
234         }
235
236         btScalar getLinearSleepingThreshold() const
237         {
238                 return m_linearSleepingThreshold;
239         }
240
241         btScalar getAngularSleepingThreshold() const
242         {
243                 return m_angularSleepingThreshold;
244         }
245
246         void                    applyDamping(btScalar timeStep);
247
248         SIMD_FORCE_INLINE const btCollisionShape*       getCollisionShape() const {
249                 return m_collisionShape;
250         }
251
252         SIMD_FORCE_INLINE btCollisionShape*     getCollisionShape() {
253                         return m_collisionShape;
254         }
255         
256         void                    setMassProps(btScalar mass, const btVector3& inertia);
257         
258         const btVector3& getLinearFactor() const
259         {
260                 return m_linearFactor;
261         }
262         void setLinearFactor(const btVector3& linearFactor)
263         {
264                 m_linearFactor = linearFactor;
265                 m_invMass = m_linearFactor*m_inverseMass;
266         }
267         btScalar                getInvMass() const { return m_inverseMass; }
268         const btMatrix3x3& getInvInertiaTensorWorld() const { 
269                 return m_invInertiaTensorWorld; 
270         }
271                 
272         void                    integrateVelocities(btScalar step);
273
274         void                    setCenterOfMassTransform(const btTransform& xform);
275
276         void                    applyCentralForce(const btVector3& force)
277         {
278                 m_totalForce += force*m_linearFactor;
279         }
280
281         const btVector3& getTotalForce() const
282         {
283                 return m_totalForce;
284         };
285
286         const btVector3& getTotalTorque() const
287         {
288                 return m_totalTorque;
289         };
290     
291         const btVector3& getInvInertiaDiagLocal() const
292         {
293                 return m_invInertiaLocal;
294         };
295
296         void    setInvInertiaDiagLocal(const btVector3& diagInvInertia)
297         {
298                 m_invInertiaLocal = diagInvInertia;
299         }
300
301         void    setSleepingThresholds(btScalar linear,btScalar angular)
302         {
303                 m_linearSleepingThreshold = linear;
304                 m_angularSleepingThreshold = angular;
305         }
306
307         void    applyTorque(const btVector3& torque)
308         {
309                 m_totalTorque += torque*m_angularFactor;
310         }
311         
312         void    applyForce(const btVector3& force, const btVector3& rel_pos) 
313         {
314                 applyCentralForce(force);
315                 applyTorque(rel_pos.cross(force*m_linearFactor));
316         }
317         
318         void applyCentralImpulse(const btVector3& impulse)
319         {
320                 m_linearVelocity += impulse *m_linearFactor * m_inverseMass;
321         }
322         
323         void applyTorqueImpulse(const btVector3& torque)
324         {
325                         m_angularVelocity += m_invInertiaTensorWorld * torque * m_angularFactor;
326         }
327         
328         void applyImpulse(const btVector3& impulse, const btVector3& rel_pos) 
329         {
330                 if (m_inverseMass != btScalar(0.))
331                 {
332                         applyCentralImpulse(impulse);
333                         if (m_angularFactor)
334                         {
335                                 applyTorqueImpulse(rel_pos.cross(impulse*m_linearFactor));
336                         }
337                 }
338         }
339
340         void clearForces() 
341         {
342                 m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
343                 m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
344         }
345         
346         void updateInertiaTensor();    
347         
348         const btVector3&     getCenterOfMassPosition() const { 
349                 return m_worldTransform.getOrigin(); 
350         }
351         btQuaternion getOrientation() const;
352         
353         const btTransform&  getCenterOfMassTransform() const { 
354                 return m_worldTransform; 
355         }
356         const btVector3&   getLinearVelocity() const { 
357                 return m_linearVelocity; 
358         }
359         const btVector3&    getAngularVelocity() const { 
360                 return m_angularVelocity; 
361         }
362         
363
364         inline void setLinearVelocity(const btVector3& lin_vel)
365         { 
366                 m_linearVelocity = lin_vel; 
367         }
368
369         inline void setAngularVelocity(const btVector3& ang_vel) 
370         { 
371                 m_angularVelocity = ang_vel; 
372         }
373
374         btVector3 getVelocityInLocalPoint(const btVector3& rel_pos) const
375         {
376                 //we also calculate lin/ang velocity for kinematic objects
377                 return m_linearVelocity + m_angularVelocity.cross(rel_pos);
378
379                 //for kinematic objects, we could also use use:
380                 //              return  (m_worldTransform(rel_pos) - m_interpolationWorldTransform(rel_pos)) / m_kinematicTimeStep;
381         }
382
383         void translate(const btVector3& v) 
384         {
385                 m_worldTransform.getOrigin() += v; 
386         }
387
388         
389         void    getAabb(btVector3& aabbMin,btVector3& aabbMax) const;
390
391
392
393
394         
395         SIMD_FORCE_INLINE btScalar computeImpulseDenominator(const btVector3& pos, const btVector3& normal) const
396         {
397                 btVector3 r0 = pos - getCenterOfMassPosition();
398
399                 btVector3 c0 = (r0).cross(normal);
400
401                 btVector3 vec = (c0 * getInvInertiaTensorWorld()).cross(r0);
402
403                 return m_inverseMass + normal.dot(vec);
404
405         }
406
407         SIMD_FORCE_INLINE btScalar computeAngularImpulseDenominator(const btVector3& axis) const
408         {
409                 btVector3 vec = axis * getInvInertiaTensorWorld();
410                 return axis.dot(vec);
411         }
412
413         SIMD_FORCE_INLINE void  updateDeactivation(btScalar timeStep)
414         {
415                 if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == DISABLE_DEACTIVATION))
416                         return;
417
418                 if ((getLinearVelocity().length2() < m_linearSleepingThreshold*m_linearSleepingThreshold) &&
419                         (getAngularVelocity().length2() < m_angularSleepingThreshold*m_angularSleepingThreshold))
420                 {
421                         m_deactivationTime += timeStep;
422                 } else
423                 {
424                         m_deactivationTime=btScalar(0.);
425                         setActivationState(0);
426                 }
427
428         }
429
430         SIMD_FORCE_INLINE bool  wantsSleeping()
431         {
432
433                 if (getActivationState() == DISABLE_DEACTIVATION)
434                         return false;
435
436                 //disable deactivation
437                 if (gDisableDeactivation || (gDeactivationTime == btScalar(0.)))
438                         return false;
439
440                 if ( (getActivationState() == ISLAND_SLEEPING) || (getActivationState() == WANTS_DEACTIVATION))
441                         return true;
442
443                 if (m_deactivationTime> gDeactivationTime)
444                 {
445                         return true;
446                 }
447                 return false;
448         }
449
450
451         
452         const btBroadphaseProxy*        getBroadphaseProxy() const
453         {
454                 return m_broadphaseHandle;
455         }
456         btBroadphaseProxy*      getBroadphaseProxy() 
457         {
458                 return m_broadphaseHandle;
459         }
460         void    setNewBroadphaseProxy(btBroadphaseProxy* broadphaseProxy)
461         {
462                 m_broadphaseHandle = broadphaseProxy;
463         }
464
465         //btMotionState allows to automatic synchronize the world transform for active objects
466         btMotionState*  getMotionState()
467         {
468                 return m_optionalMotionState;
469         }
470         const btMotionState*    getMotionState() const
471         {
472                 return m_optionalMotionState;
473         }
474         void    setMotionState(btMotionState* motionState)
475         {
476                 m_optionalMotionState = motionState;
477                 if (m_optionalMotionState)
478                         motionState->getWorldTransform(m_worldTransform);
479         }
480
481         //for experimental overriding of friction/contact solver func
482         int     m_contactSolverType;
483         int     m_frictionSolverType;
484
485         void    setAngularFactor(const btVector3& angFac)
486         {
487                 m_angularFactor = angFac;
488         }
489
490         void    setAngularFactor(btScalar angFac)
491         {
492                 m_angularFactor.setValue(angFac,angFac,angFac);
493         }
494         const btVector3&        getAngularFactor() const
495         {
496                 return m_angularFactor;
497         }
498
499         //is this rigidbody added to a btCollisionWorld/btDynamicsWorld/btBroadphase?
500         bool    isInWorld() const
501         {
502                 return (getBroadphaseProxy() != 0);
503         }
504
505         virtual bool checkCollideWithOverride(const  btCollisionObject* co) const;
506
507         void addConstraintRef(btTypedConstraint* c);
508         void removeConstraintRef(btTypedConstraint* c);
509
510         btTypedConstraint* getConstraintRef(int index)
511         {
512                 return m_constraintRefs[index];
513         }
514
515         int getNumConstraintRefs() const
516         {
517                 return m_constraintRefs.size();
518         }
519
520         void    setFlags(int flags)
521         {
522                 m_rigidbodyFlags = flags;
523         }
524
525         int getFlags() const
526         {
527                 return m_rigidbodyFlags;
528         }
529
530         btVector3 computeGyroscopicForce(btScalar maxGyroscopicForce) const;
531
532         ///////////////////////////////////////////////
533
534         virtual int     calculateSerializeBufferSize()  const;
535
536         ///fills the dataBuffer and returns the struct name (and 0 on failure)
537         virtual const char*     serialize(void* dataBuffer,  class btSerializer* serializer) const;
538
539         virtual void serializeSingleObject(class btSerializer* serializer) const;
540
541 };
542
543 //@todo add m_optionalMotionState and m_constraintRefs to btRigidBodyData
544 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
545 struct  btRigidBodyFloatData
546 {
547         btCollisionObjectFloatData      m_collisionObjectData;
548         btMatrix3x3FloatData            m_invInertiaTensorWorld;
549         btVector3FloatData              m_linearVelocity;
550         btVector3FloatData              m_angularVelocity;
551         btVector3FloatData              m_angularFactor;
552         btVector3FloatData              m_linearFactor;
553         btVector3FloatData              m_gravity;      
554         btVector3FloatData              m_gravity_acceleration;
555         btVector3FloatData              m_invInertiaLocal;
556         btVector3FloatData              m_totalForce;
557         btVector3FloatData              m_totalTorque;
558         float                                   m_inverseMass;
559         float                                   m_linearDamping;
560         float                                   m_angularDamping;
561         float                                   m_additionalDampingFactor;
562         float                                   m_additionalLinearDampingThresholdSqr;
563         float                                   m_additionalAngularDampingThresholdSqr;
564         float                                   m_additionalAngularDampingFactor;
565         float                                   m_linearSleepingThreshold;
566         float                                   m_angularSleepingThreshold;
567         int                                             m_additionalDamping;
568 };
569
570 ///do not change those serialization structures, it requires an updated sBulletDNAstr/sBulletDNAstr64
571 struct  btRigidBodyDoubleData
572 {
573         btCollisionObjectDoubleData     m_collisionObjectData;
574         btMatrix3x3DoubleData           m_invInertiaTensorWorld;
575         btVector3DoubleData             m_linearVelocity;
576         btVector3DoubleData             m_angularVelocity;
577         btVector3DoubleData             m_angularFactor;
578         btVector3DoubleData             m_linearFactor;
579         btVector3DoubleData             m_gravity;      
580         btVector3DoubleData             m_gravity_acceleration;
581         btVector3DoubleData             m_invInertiaLocal;
582         btVector3DoubleData             m_totalForce;
583         btVector3DoubleData             m_totalTorque;
584         double                                  m_inverseMass;
585         double                                  m_linearDamping;
586         double                                  m_angularDamping;
587         double                                  m_additionalDampingFactor;
588         double                                  m_additionalLinearDampingThresholdSqr;
589         double                                  m_additionalAngularDampingThresholdSqr;
590         double                                  m_additionalAngularDampingFactor;
591         double                                  m_linearSleepingThreshold;
592         double                                  m_angularSleepingThreshold;
593         int                                             m_additionalDamping;
594         char    m_padding[4];
595 };
596
597
598
599 #endif //BT_RIGIDBODY_H
600