update Bullet 2.x with latest changes, notice that the integration is not finished...
[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
22 float gLinearAirDamping = 1.f;
23 //'temporarily' global variables
24 float   gDeactivationTime = 2.f;
25 bool    gDisableDeactivation = false;
26
27 float gLinearSleepingThreshold = 0.8f;
28 float gAngularSleepingThreshold = 1.0f;
29 static int uniqueId = 0;
30
31 btRigidBody::btRigidBody(float mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
32
33         m_gravity(0.0f, 0.0f, 0.0f),
34         m_totalForce(0.0f, 0.0f, 0.0f),
35         m_totalTorque(0.0f, 0.0f, 0.0f),
36         m_linearVelocity(0.0f, 0.0f, 0.0f),
37         m_angularVelocity(0.f,0.f,0.f),
38         m_linearDamping(0.f),
39         m_angularDamping(0.5f),
40         m_optionalMotionState(motionState),
41         m_contactSolverType(0),
42         m_frictionSolverType(0)
43 {
44
45         motionState->getWorldTransform(m_worldTransform);
46
47         m_interpolationWorldTransform = m_worldTransform;
48         m_interpolationLinearVelocity.setValue(0,0,0);
49         m_interpolationAngularVelocity.setValue(0,0,0);
50         
51         //moved to btCollisionObject
52         m_friction = friction;
53         m_restitution = restitution;
54
55         m_collisionShape = collisionShape;
56         m_debugBodyId = uniqueId++;
57         
58         //m_internalOwner is to allow upcasting from collision object to rigid body
59         m_internalOwner = this;
60
61         setMassProps(mass, localInertia);
62     setDamping(linearDamping, angularDamping);
63         updateInertiaTensor();
64
65 }
66
67
68 btRigidBody::btRigidBody( float mass,const btTransform& worldTransform,btCollisionShape* collisionShape,const btVector3& localInertia,btScalar linearDamping,btScalar angularDamping,btScalar friction,btScalar restitution)
69
70         m_gravity(0.0f, 0.0f, 0.0f),
71         m_totalForce(0.0f, 0.0f, 0.0f),
72         m_totalTorque(0.0f, 0.0f, 0.0f),
73         m_linearVelocity(0.0f, 0.0f, 0.0f),
74         m_angularVelocity(0.f,0.f,0.f),
75         m_linearDamping(0.f),
76         m_angularDamping(0.5f),
77         m_optionalMotionState(0),
78         m_contactSolverType(0),
79         m_frictionSolverType(0)
80 {
81         
82         m_worldTransform = worldTransform;
83         m_interpolationWorldTransform = m_worldTransform;
84         m_interpolationLinearVelocity.setValue(0,0,0);
85         m_interpolationAngularVelocity.setValue(0,0,0);
86
87         //moved to btCollisionObject
88         m_friction = friction;
89         m_restitution = restitution;
90
91         m_collisionShape = collisionShape;
92         m_debugBodyId = uniqueId++;
93         
94         //m_internalOwner is to allow upcasting from collision object to rigid body
95         m_internalOwner = this;
96
97         setMassProps(mass, localInertia);
98     setDamping(linearDamping, angularDamping);
99         updateInertiaTensor();
100
101 }
102
103 #define EXPERIMENTAL_JITTER_REMOVAL 1
104 #ifdef EXPERIMENTAL_JITTER_REMOVAL
105 //Bullet 2.20b has experimental code to reduce jitter just before objects fall asleep/deactivate
106 //doesn't work very well yet (value 0 only reduces performance a bit, no difference in functionality)
107 float gClippedAngvelThresholdSqr = 0.f;
108 float   gClippedLinearThresholdSqr = 0.f;
109 #endif //EXPERIMENTAL_JITTER_REMOVAL
110
111 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
112 {
113
114 #ifdef EXPERIMENTAL_JITTER_REMOVAL
115         //clip to avoid jitter
116         if (m_angularVelocity.length2() < gClippedAngvelThresholdSqr)
117         {
118                 m_angularVelocity.setValue(0,0,0);
119                 printf("clipped!\n");
120         }
121         
122         if (m_linearVelocity.length2() < gClippedLinearThresholdSqr)
123         {
124                 m_linearVelocity.setValue(0,0,0);
125                 printf("clipped!\n");
126         }
127 #endif //EXPERIMENTAL_JITTER_REMOVAL
128
129         btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
130 }
131
132 void                    btRigidBody::saveKinematicState(btScalar timeStep)
133 {
134         //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
135         if (timeStep != 0.f)
136         {
137                 //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
138                 if (getMotionState())
139                         getMotionState()->getWorldTransform(m_worldTransform);
140                 btVector3 linVel,angVel;
141                 
142                 btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
143                 m_interpolationLinearVelocity = m_linearVelocity;
144                 m_interpolationAngularVelocity = m_angularVelocity;
145                 m_interpolationWorldTransform = m_worldTransform;
146                 //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
147         }
148 }
149         
150 void    btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
151 {
152         getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
153 }
154
155
156
157
158 void btRigidBody::setGravity(const btVector3& acceleration) 
159 {
160         if (m_inverseMass != 0.0f)
161         {
162                 m_gravity = acceleration * (1.0f / m_inverseMass);
163         }
164 }
165
166
167
168
169
170
171 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
172 {
173         m_linearDamping = GEN_clamped(lin_damping, 0.0f, 1.0f);
174         m_angularDamping = GEN_clamped(ang_damping, 0.0f, 1.0f);
175 }
176
177
178
179 #include <stdio.h>
180
181
182 void btRigidBody::applyForces(btScalar step)
183 {
184         if (isStaticOrKinematicObject())
185                 return;
186         
187         applyCentralForce(m_gravity);   
188         
189         m_linearVelocity *= GEN_clamped((1.f - step * gLinearAirDamping * m_linearDamping), 0.0f, 1.0f);
190         m_angularVelocity *= GEN_clamped((1.f - step * m_angularDamping), 0.0f, 1.0f);
191
192 #define FORCE_VELOCITY_DAMPING 1
193 #ifdef FORCE_VELOCITY_DAMPING
194         float speed = m_linearVelocity.length();
195         if (speed < m_linearDamping)
196         {
197                 float dampVel = 0.005f;
198                 if (speed > dampVel)
199                 {
200                         btVector3 dir = m_linearVelocity.normalized();
201                         m_linearVelocity -=  dir * dampVel;
202                 } else
203                 {
204                         m_linearVelocity.setValue(0.f,0.f,0.f);
205                 }
206         }
207
208         float angSpeed = m_angularVelocity.length();
209         if (angSpeed < m_angularDamping)
210         {
211                 float angDampVel = 0.005f;
212                 if (angSpeed > angDampVel)
213                 {
214                         btVector3 dir = m_angularVelocity.normalized();
215                         m_angularVelocity -=  dir * angDampVel;
216                 } else
217                 {
218                         m_angularVelocity.setValue(0.f,0.f,0.f);
219                 }
220         }
221 #endif //FORCE_VELOCITY_DAMPING
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 == 0.f)
234         {
235                 m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
236                 m_inverseMass = 0.f;
237         } else
238         {
239                 m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
240                 m_inverseMass = 1.0f / mass;
241         }
242         
243         m_invInertiaLocal.setValue(inertia[0] != 0.0f ? 1.0f / inertia[0]: 0.0f,
244                                    inertia[1] != 0.0f ? 1.0f / inertia[1]: 0.0f,
245                                    inertia[2] != 0.0f ? 1.0f / inertia[2]: 0.0f);
246
247 }
248
249         
250
251 void btRigidBody::updateInertiaTensor() 
252 {
253         m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
254 }
255
256
257 void btRigidBody::integrateVelocities(btScalar step) 
258 {
259         if (isStaticOrKinematicObject())
260                 return;
261
262         m_linearVelocity += m_totalForce * (m_inverseMass * step);
263         m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
264
265 #define MAX_ANGVEL SIMD_HALF_PI
266         /// clamp angular velocity. collision calculations will fail on higher angular velocities       
267         float angvel = m_angularVelocity.length();
268         if (angvel*step > MAX_ANGVEL)
269         {
270                 m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
271         }
272
273         clearForces();
274 }
275
276 btQuaternion btRigidBody::getOrientation() const
277 {
278                 btQuaternion orn;
279                 m_worldTransform.getBasis().getRotation(orn);
280                 return orn;
281 }
282         
283         
284 void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
285 {
286         m_interpolationWorldTransform = m_worldTransform;
287         m_interpolationLinearVelocity = getLinearVelocity();
288         m_interpolationAngularVelocity = getAngularVelocity();
289         m_worldTransform = xform;
290         updateInertiaTensor();
291 }
292
293
294