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