Finalised support for compiling BULLET on linux.
[blender.git] / extern / bullet / BulletDynamics / Dynamics / RigidBody.cpp
1 #include "RigidBody.h"
2 #include "MassProps.h"
3 #include "CollisionShapes/ConvexShape.h"
4 #include "GEN_MinMax.h"
5 #include <SimdTransformUtil.h>
6
7 float gRigidBodyDamping = 0.5f;
8 static int uniqueId = 0;
9
10 RigidBody::RigidBody( const MassProps& massProps,SimdScalar linearDamping,SimdScalar angularDamping,SimdScalar friction,SimdScalar restitution)
11 : m_collisionShape(0),
12         m_activationState1(1),
13         m_hitFraction(1.f),
14         m_gravity(0.0f, 0.0f, 0.0f),
15         m_linearDamping(0.f),
16         m_angularDamping(0.5f),
17         m_totalForce(0.0f, 0.0f, 0.0f),
18         m_totalTorque(0.0f, 0.0f, 0.0f),
19         m_linearVelocity(0.0f, 0.0f, 0.0f),
20         m_angularVelocity(0.f,0.f,0.f),
21         m_restitution(restitution),
22         m_friction(friction)
23
24 {
25         m_debugBodyId = uniqueId++;
26         
27         setMassProps(massProps.m_mass, massProps.m_inertiaLocal);
28     setDamping(linearDamping, angularDamping);
29         m_worldTransform.setIdentity();
30         updateInertiaTensor();
31
32 }
33
34 void RigidBody::setLinearVelocity(const SimdVector3& lin_vel)
35
36
37         m_linearVelocity = lin_vel; 
38 }
39
40
41 void RigidBody::predictIntegratedTransform(SimdScalar timeStep,SimdTransform& predictedTransform) const
42 {
43         SimdTransformUtil::IntegrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
44 }
45
46         
47 void    RigidBody::getAabb(SimdVector3& aabbMin,SimdVector3& aabbMax) const
48 {
49         m_collisionShape ->GetAabb(m_worldTransform,aabbMin,aabbMax);
50 }
51
52 void    RigidBody::SetCollisionShape(CollisionShape* mink)
53 {
54         m_collisionShape = mink;
55         SimdTransform ident;
56         ident.setIdentity();
57         SimdVector3 aabbMin,aabbMax;
58         m_collisionShape ->GetAabb(ident,aabbMin,aabbMax);
59         SimdVector3 diag = (aabbMax-aabbMin)*0.5f;
60 }
61
62
63 void RigidBody::setGravity(const SimdVector3& acceleration) 
64 {
65         if (m_inverseMass != 0.0f)
66         {
67                 m_gravity = acceleration * (1.0f / m_inverseMass);
68         }
69 }
70
71 bool RigidBody::mergesSimulationIslands() const
72 {
73         return ( getInvMass() != 0) ;
74 }
75
76 void RigidBody::SetActivationState(int newState) 
77
78         m_activationState1 = newState;
79 }
80
81
82
83 void RigidBody::setDamping(SimdScalar lin_damping, SimdScalar ang_damping)
84 {
85         m_linearDamping = GEN_clamped(lin_damping, 0.0f, 1.0f);
86         m_angularDamping = GEN_clamped(ang_damping, 0.0f, 1.0f);
87 }
88
89
90
91 #include <stdio.h>
92
93 void RigidBody::applyForces(SimdScalar step)
94 {
95         
96         applyCentralForce(m_gravity);   
97         
98         m_linearVelocity *= GEN_clamped((1.f - step * m_linearDamping), 0.0f, 1.0f);
99         m_angularVelocity *= GEN_clamped((1.f - step * m_angularDamping), 0.0f, 1.0f);
100         
101 }
102
103 void RigidBody::proceedToTransform(const SimdTransform& newTrans)
104 {
105         setCenterOfMassTransform( newTrans );
106 }
107         
108
109 void RigidBody::setMassProps(SimdScalar mass, const SimdVector3& inertia)
110 {
111         m_inverseMass = mass != 0.0f ? 1.0f / mass : 0.0f;
112         m_invInertiaLocal.setValue(inertia[0] != 0.0f ? 1.0f / inertia[0]: 0.0f,
113                                                    inertia[1] != 0.0f ? 1.0f / inertia[1]: 0.0f,
114                                                    inertia[2] != 0.0f ? 1.0f / inertia[2]: 0.0f);
115
116 }
117
118         
119
120 void RigidBody::updateInertiaTensor() 
121 {
122         m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
123 }
124
125
126 void RigidBody::integrateVelocities(SimdScalar step) 
127 {
128         m_linearVelocity += m_totalForce * (m_inverseMass * step);
129         m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
130
131 #define MAX_ANGVEL SIMD_HALF_PI
132         /// clamp angular velocity. collision calculations will fail on higher angular velocities       
133         float angvel = m_angularVelocity.length();
134         if (angvel*step > MAX_ANGVEL)
135         {
136                 m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
137         }
138
139         clearForces();
140 }
141
142 SimdQuaternion RigidBody::getOrientation() const
143 {
144                 SimdQuaternion orn;
145                 m_worldTransform.getBasis().getRotation(orn);
146                 return orn;
147 }
148         
149         
150 void RigidBody::setCenterOfMassTransform(const SimdTransform& xform)
151 {
152         m_worldTransform = xform;
153         
154 //      m_worldTransform.getBasis().getRotation(m_orn1);
155         updateInertiaTensor();
156 }
157
158
159