Planet
navi homePPSaboutscreenshotsdownloaddevelopmentforum

source: code/branches/gamestates3/src/external/bullet/BulletDynamics/Dynamics/btRigidBody.cpp @ 9450

Last change on this file since 9450 was 5781, checked in by rgrieder, 15 years ago

Reverted trunk again. We might want to find a way to delete these revisions again (x3n's changes are still available as diff in the commit mails).

  • Property svn:eol-style set to native
File size: 10.0 KB
Line 
1/*
2Bullet Continuous Collision Detection and Physics Library
3Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
4
5This software is provided 'as-is', without any express or implied warranty.
6In no event will the authors be held liable for any damages arising from the use of this software.
7Permission is granted to anyone to use this software for any purpose,
8including commercial applications, and to alter it and redistribute it freely,
9subject to the following restrictions:
10
111. 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.
122. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
133. 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
23//'temporarily' global variables
24btScalar        gDeactivationTime = btScalar(2.);
25bool    gDisableDeactivation = false;
26static int uniqueId = 0;
27
28
29btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
30{
31        setupRigidBody(constructionInfo);
32}
33
34btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
35{
36        btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
37        setupRigidBody(cinfo);
38}
39
40void    btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
41{
42
43        m_internalType=CO_RIGID_BODY;
44
45        m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
46        m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
47        m_angularFactor = btScalar(1.);
48        m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
49        m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
50        m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
51        m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
52        m_linearDamping = btScalar(0.);
53        m_angularDamping = btScalar(0.5);
54        m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
55        m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
56        m_optionalMotionState = constructionInfo.m_motionState;
57        m_contactSolverType = 0;
58        m_frictionSolverType = 0;
59        m_additionalDamping = constructionInfo.m_additionalDamping;
60        m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
61        m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
62        m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
63        m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
64
65        if (m_optionalMotionState)
66        {
67                m_optionalMotionState->getWorldTransform(m_worldTransform);
68        } else
69        {
70                m_worldTransform = constructionInfo.m_startWorldTransform;
71        }
72
73        m_interpolationWorldTransform = m_worldTransform;
74        m_interpolationLinearVelocity.setValue(0,0,0);
75        m_interpolationAngularVelocity.setValue(0,0,0);
76       
77        //moved to btCollisionObject
78        m_friction = constructionInfo.m_friction;
79        m_restitution = constructionInfo.m_restitution;
80
81        setCollisionShape( constructionInfo.m_collisionShape );
82        m_debugBodyId = uniqueId++;
83       
84        setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
85    setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
86        updateInertiaTensor();
87
88}
89
90
91void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
92{
93        btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
94}
95
96void                    btRigidBody::saveKinematicState(btScalar timeStep)
97{
98        //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
99        if (timeStep != btScalar(0.))
100        {
101                //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
102                if (getMotionState())
103                        getMotionState()->getWorldTransform(m_worldTransform);
104                btVector3 linVel,angVel;
105               
106                btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
107                m_interpolationLinearVelocity = m_linearVelocity;
108                m_interpolationAngularVelocity = m_angularVelocity;
109                m_interpolationWorldTransform = m_worldTransform;
110                //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
111        }
112}
113       
114void    btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
115{
116        getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
117}
118
119
120
121
122void btRigidBody::setGravity(const btVector3& acceleration) 
123{
124        if (m_inverseMass != btScalar(0.0))
125        {
126                m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
127        }
128        m_gravity_acceleration = acceleration;
129}
130
131
132
133
134
135
136void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
137{
138        m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
139        m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
140}
141
142
143
144
145///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping
146void                    btRigidBody::applyDamping(btScalar timeStep)
147{
148        //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
149        //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
150
151//#define USE_OLD_DAMPING_METHOD 1
152#ifdef USE_OLD_DAMPING_METHOD
153        m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
154        m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
155#else
156        m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
157        m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
158#endif
159
160        if (m_additionalDamping)
161        {
162                //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
163                //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
164                if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
165                        (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
166                {
167                        m_angularVelocity *= m_additionalDampingFactor;
168                        m_linearVelocity *= m_additionalDampingFactor;
169                }
170       
171
172                btScalar speed = m_linearVelocity.length();
173                if (speed < m_linearDamping)
174                {
175                        btScalar dampVel = btScalar(0.005);
176                        if (speed > dampVel)
177                        {
178                                btVector3 dir = m_linearVelocity.normalized();
179                                m_linearVelocity -=  dir * dampVel;
180                        } else
181                        {
182                                m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
183                        }
184                }
185
186                btScalar angSpeed = m_angularVelocity.length();
187                if (angSpeed < m_angularDamping)
188                {
189                        btScalar angDampVel = btScalar(0.005);
190                        if (angSpeed > angDampVel)
191                        {
192                                btVector3 dir = m_angularVelocity.normalized();
193                                m_angularVelocity -=  dir * angDampVel;
194                        } else
195                        {
196                                m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
197                        }
198                }
199        }
200}
201
202
203void btRigidBody::applyGravity()
204{
205        if (isStaticOrKinematicObject())
206                return;
207       
208        applyCentralForce(m_gravity);   
209
210}
211
212void btRigidBody::proceedToTransform(const btTransform& newTrans)
213{
214        setCenterOfMassTransform( newTrans );
215}
216       
217
218void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
219{
220        if (mass == btScalar(0.))
221        {
222                m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
223                m_inverseMass = btScalar(0.);
224        } else
225        {
226                m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
227                m_inverseMass = btScalar(1.0) / mass;
228        }
229       
230        m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
231                                   inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
232                                   inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
233
234}
235
236       
237
238void btRigidBody::updateInertiaTensor() 
239{
240        m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
241}
242
243
244void btRigidBody::integrateVelocities(btScalar step) 
245{
246        if (isStaticOrKinematicObject())
247                return;
248
249        m_linearVelocity += m_totalForce * (m_inverseMass * step);
250        m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
251
252#define MAX_ANGVEL SIMD_HALF_PI
253        /// clamp angular velocity. collision calculations will fail on higher angular velocities       
254        btScalar angvel = m_angularVelocity.length();
255        if (angvel*step > MAX_ANGVEL)
256        {
257                m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
258        }
259
260}
261
262btQuaternion btRigidBody::getOrientation() const
263{
264                btQuaternion orn;
265                m_worldTransform.getBasis().getRotation(orn);
266                return orn;
267}
268       
269       
270void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
271{
272
273        if (isStaticOrKinematicObject())
274        {
275                m_interpolationWorldTransform = m_worldTransform;
276        } else
277        {
278                m_interpolationWorldTransform = xform;
279        }
280        m_interpolationLinearVelocity = getLinearVelocity();
281        m_interpolationAngularVelocity = getAngularVelocity();
282        m_worldTransform = xform;
283        updateInertiaTensor();
284}
285
286
287bool btRigidBody::checkCollideWithOverride(btCollisionObject* co)
288{
289        btRigidBody* otherRb = btRigidBody::upcast(co);
290        if (!otherRb)
291                return true;
292
293        for (int i = 0; i < m_constraintRefs.size(); ++i)
294        {
295                btTypedConstraint* c = m_constraintRefs[i];
296                if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
297                        return false;
298        }
299
300        return true;
301}
302
303void btRigidBody::addConstraintRef(btTypedConstraint* c)
304{
305        int index = m_constraintRefs.findLinearSearch(c);
306        if (index == m_constraintRefs.size())
307                m_constraintRefs.push_back(c); 
308
309        m_checkCollideWith = true;
310}
311
312void btRigidBody::removeConstraintRef(btTypedConstraint* c)
313{
314        m_constraintRefs.remove(c);
315        m_checkCollideWith = m_constraintRefs.size() > 0;
316}
Note: See TracBrowser for help on using the repository browser.