Back to index

supertuxkart  0.5+dfsg1
btRigidBody.cpp
Go to the documentation of this file.
00001 /*
00002 Bullet Continuous Collision Detection and Physics Library
00003 Copyright (c) 2003-2006 Erwin Coumans  http://continuousphysics.com/Bullet/
00004 
00005 This software is provided 'as-is', without any express or implied warranty.
00006 In no event will the authors be held liable for any damages arising from the use of this software.
00007 Permission is granted to anyone to use this software for any purpose, 
00008 including commercial applications, and to alter it and redistribute it freely, 
00009 subject to the following restrictions:
00010 
00011 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.
00012 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software.
00013 3. This notice may not be removed or altered from any source distribution.
00014 */
00015 
00016 #include "btRigidBody.h"
00017 #include "BulletCollision/CollisionShapes/btConvexShape.h"
00018 #include "LinearMath/btMinMax.h"
00019 #include "LinearMath/btTransformUtil.h"
00020 #include "LinearMath/btMotionState.h"
00021 #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h"
00022 
00023 //'temporarily' global variables
00024 btScalar      gDeactivationTime = btScalar(2.);
00025 bool   gDisableDeactivation = false;
00026 static int uniqueId = 0;
00027 
00028 
00029 btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
00030 {
00031        setupRigidBody(constructionInfo);
00032 }
00033 
00034 btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
00035 {
00036        btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
00037        setupRigidBody(cinfo);
00038 }
00039 
00040 void   btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
00041 {
00042 
00043        m_internalType=CO_RIGID_BODY;
00044 
00045        m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
00046        m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00047        m_angularFactor = btScalar(1.);
00048        m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
00049        m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
00050        m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
00051        m_linearDamping = btScalar(0.);
00052        m_angularDamping = btScalar(0.5);
00053        m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
00054        m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
00055        m_optionalMotionState = constructionInfo.m_motionState;
00056        m_contactSolverType = 0;
00057        m_frictionSolverType = 0;
00058        m_additionalDamping = constructionInfo.m_additionalDamping;
00059        m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
00060        m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
00061        m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
00062        m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
00063 
00064        if (m_optionalMotionState)
00065        {
00066               m_optionalMotionState->getWorldTransform(m_worldTransform);
00067        } else
00068        {
00069               m_worldTransform = constructionInfo.m_startWorldTransform;
00070        }
00071 
00072        m_interpolationWorldTransform = m_worldTransform;
00073        m_interpolationLinearVelocity.setValue(0,0,0);
00074        m_interpolationAngularVelocity.setValue(0,0,0);
00075        
00076        //moved to btCollisionObject
00077        m_friction = constructionInfo.m_friction;
00078        m_restitution = constructionInfo.m_restitution;
00079 
00080        m_collisionShape = constructionInfo.m_collisionShape;
00081        m_debugBodyId = uniqueId++;
00082        
00083        setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
00084     setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
00085        updateInertiaTensor();
00086 
00087 }
00088 
00089 
00090 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
00091 {
00092        btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
00093 }
00094 
00095 void                 btRigidBody::saveKinematicState(btScalar timeStep)
00096 {
00097        //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
00098        if (timeStep != btScalar(0.))
00099        {
00100               //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
00101               if (getMotionState())
00102                      getMotionState()->getWorldTransform(m_worldTransform);
00103               btVector3 linVel,angVel;
00104               
00105               btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
00106               m_interpolationLinearVelocity = m_linearVelocity;
00107               m_interpolationAngularVelocity = m_angularVelocity;
00108               m_interpolationWorldTransform = m_worldTransform;
00109               //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
00110        }
00111 }
00112        
00113 void   btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
00114 {
00115        getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
00116 }
00117 
00118 
00119 
00120 
00121 void btRigidBody::setGravity(const btVector3& acceleration) 
00122 {
00123        if (m_inverseMass != btScalar(0.0))
00124        {
00125               m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
00126        }
00127 }
00128 
00129 
00130 
00131 
00132 
00133 
00134 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
00135 {
00136        m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
00137        m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
00138 }
00139 
00140 
00141 
00142 
00144 void                 btRigidBody::applyDamping(btScalar timeStep)
00145 {
00146        m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
00147        m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
00148 
00149        if (m_additionalDamping)
00150        {
00151               //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
00152               //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
00153               if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
00154                      (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
00155               {
00156                      m_angularVelocity *= m_additionalDampingFactor;
00157                      m_linearVelocity *= m_additionalDampingFactor;
00158               }
00159        
00160 
00161               btScalar speed = m_linearVelocity.length();
00162               if (speed < m_linearDamping)
00163               {
00164                      btScalar dampVel = btScalar(0.005);
00165                      if (speed > dampVel)
00166                      {
00167                             btVector3 dir = m_linearVelocity.normalized();
00168                             m_linearVelocity -=  dir * dampVel;
00169                      } else
00170                      {
00171                             m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00172                      }
00173               }
00174 
00175               btScalar angSpeed = m_angularVelocity.length();
00176               if (angSpeed < m_angularDamping)
00177               {
00178                      btScalar angDampVel = btScalar(0.005);
00179                      if (angSpeed > angDampVel)
00180                      {
00181                             btVector3 dir = m_angularVelocity.normalized();
00182                             m_angularVelocity -=  dir * angDampVel;
00183                      } else
00184                      {
00185                             m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00186                      }
00187               }
00188        }
00189 }
00190 
00191 
00192 void btRigidBody::applyGravity()
00193 {
00194        if (isStaticOrKinematicObject())
00195               return;
00196        
00197        applyCentralForce(m_gravity);      
00198 
00199 }
00200 
00201 void btRigidBody::proceedToTransform(const btTransform& newTrans)
00202 {
00203        setCenterOfMassTransform( newTrans );
00204 }
00205        
00206 
00207 void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
00208 {
00209        if (mass == btScalar(0.))
00210        {
00211               m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
00212               m_inverseMass = btScalar(0.);
00213        } else
00214        {
00215               m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
00216               m_inverseMass = btScalar(1.0) / mass;
00217        }
00218        
00219        m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
00220                                inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
00221                                inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
00222 
00223 }
00224 
00225        
00226 
00227 void btRigidBody::updateInertiaTensor() 
00228 {
00229        m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
00230 }
00231 
00232 
00233 void btRigidBody::integrateVelocities(btScalar step) 
00234 {
00235        if (isStaticOrKinematicObject())
00236               return;
00237 
00238        m_linearVelocity += m_totalForce * (m_inverseMass * step);
00239        m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
00240 
00241 #define MAX_ANGVEL SIMD_HALF_PI
00242 
00243        btScalar angvel = m_angularVelocity.length();
00244        if (angvel*step > MAX_ANGVEL)
00245        {
00246               m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
00247        }
00248 
00249 }
00250 
00251 btQuaternion btRigidBody::getOrientation() const
00252 {
00253               btQuaternion orn;
00254               m_worldTransform.getBasis().getRotation(orn);
00255               return orn;
00256 }
00257        
00258        
00259 void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
00260 {
00261 
00262        if (isStaticOrKinematicObject())
00263        {
00264               m_interpolationWorldTransform = m_worldTransform;
00265        } else
00266        {
00267               m_interpolationWorldTransform = xform;
00268        }
00269        m_interpolationLinearVelocity = getLinearVelocity();
00270        m_interpolationAngularVelocity = getAngularVelocity();
00271        m_worldTransform = xform;
00272        updateInertiaTensor();
00273 }
00274 
00275 
00276 bool btRigidBody::checkCollideWithOverride(btCollisionObject* co)
00277 {
00278        btRigidBody* otherRb = btRigidBody::upcast(co);
00279        if (!otherRb)
00280               return true;
00281 
00282        for (int i = 0; i < m_constraintRefs.size(); ++i)
00283        {
00284               btTypedConstraint* c = m_constraintRefs[i];
00285               if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
00286                      return false;
00287        }
00288 
00289        return true;
00290 }
00291 
00292 void btRigidBody::addConstraintRef(btTypedConstraint* c)
00293 {
00294        int index = m_constraintRefs.findLinearSearch(c);
00295        if (index == m_constraintRefs.size())
00296               m_constraintRefs.push_back(c); 
00297 
00298        m_checkCollideWith = true;
00299 }
00300 
00301 void btRigidBody::removeConstraintRef(btTypedConstraint* c)
00302 {
00303        m_constraintRefs.remove(c);
00304        m_checkCollideWith = m_constraintRefs.size() > 0;
00305 }