Blender V2.61 - r43446

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 #include "LinearMath/btSerializer.h"
00023 
00024 //'temporarily' global variables
00025 btScalar    gDeactivationTime = btScalar(2.);
00026 bool    gDisableDeactivation = false;
00027 static int uniqueId = 0;
00028 
00029 
00030 btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
00031 {
00032     setupRigidBody(constructionInfo);
00033 }
00034 
00035 btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia)
00036 {
00037     btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia);
00038     setupRigidBody(cinfo);
00039 }
00040 
00041 void    btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
00042 {
00043 
00044     m_internalType=CO_RIGID_BODY;
00045 
00046     m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
00047     m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00048     m_angularFactor.setValue(1,1,1);
00049     m_linearFactor.setValue(1,1,1);
00050     m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
00051     m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
00052     m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0));
00053     m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)),
00054     setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping);
00055 
00056     m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold;
00057     m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold;
00058     m_optionalMotionState = constructionInfo.m_motionState;
00059     m_contactSolverType = 0;
00060     m_frictionSolverType = 0;
00061     m_additionalDamping = constructionInfo.m_additionalDamping;
00062     m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor;
00063     m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr;
00064     m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr;
00065     m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor;
00066 
00067     if (m_optionalMotionState)
00068     {
00069         m_optionalMotionState->getWorldTransform(m_worldTransform);
00070     } else
00071     {
00072         m_worldTransform = constructionInfo.m_startWorldTransform;
00073     }
00074 
00075     m_interpolationWorldTransform = m_worldTransform;
00076     m_interpolationLinearVelocity.setValue(0,0,0);
00077     m_interpolationAngularVelocity.setValue(0,0,0);
00078     
00079     //moved to btCollisionObject
00080     m_friction = constructionInfo.m_friction;
00081     m_restitution = constructionInfo.m_restitution;
00082 
00083     setCollisionShape( constructionInfo.m_collisionShape );
00084     m_debugBodyId = uniqueId++;
00085     
00086     setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
00087     updateInertiaTensor();
00088 
00089     m_rigidbodyFlags = 0;
00090 
00091 
00092     m_deltaLinearVelocity.setZero();
00093     m_deltaAngularVelocity.setZero();
00094     m_invMass = m_inverseMass*m_linearFactor;
00095     m_pushVelocity.setZero();
00096     m_turnVelocity.setZero();
00097 
00098     
00099 
00100 }
00101 
00102 
00103 void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) 
00104 {
00105     btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform);
00106 }
00107 
00108 void            btRigidBody::saveKinematicState(btScalar timeStep)
00109 {
00110     //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities
00111     if (timeStep != btScalar(0.))
00112     {
00113         //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform
00114         if (getMotionState())
00115             getMotionState()->getWorldTransform(m_worldTransform);
00116         btVector3 linVel,angVel;
00117         
00118         btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity);
00119         m_interpolationLinearVelocity = m_linearVelocity;
00120         m_interpolationAngularVelocity = m_angularVelocity;
00121         m_interpolationWorldTransform = m_worldTransform;
00122         //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ());
00123     }
00124 }
00125     
00126 void    btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const
00127 {
00128     getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax);
00129 }
00130 
00131 
00132 
00133 
00134 void btRigidBody::setGravity(const btVector3& acceleration) 
00135 {
00136     if (m_inverseMass != btScalar(0.0))
00137     {
00138         m_gravity = acceleration * (btScalar(1.0) / m_inverseMass);
00139     }
00140     m_gravity_acceleration = acceleration;
00141 }
00142 
00143 
00144 
00145 
00146 
00147 
00148 void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping)
00149 {
00150     m_linearDamping = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
00151     m_angularDamping = btClamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
00152 }
00153 
00154 
00155 
00156 
00158 void            btRigidBody::applyDamping(btScalar timeStep)
00159 {
00160     //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74
00161     //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway
00162 
00163 //#define USE_OLD_DAMPING_METHOD 1
00164 #ifdef USE_OLD_DAMPING_METHOD
00165     m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
00166     m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0));
00167 #else
00168     m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep);
00169     m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep);
00170 #endif
00171 
00172     if (m_additionalDamping)
00173     {
00174         //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc.
00175         //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete
00176         if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) &&
00177             (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr))
00178         {
00179             m_angularVelocity *= m_additionalDampingFactor;
00180             m_linearVelocity *= m_additionalDampingFactor;
00181         }
00182     
00183 
00184         btScalar speed = m_linearVelocity.length();
00185         if (speed < m_linearDamping)
00186         {
00187             btScalar dampVel = btScalar(0.005);
00188             if (speed > dampVel)
00189             {
00190                 btVector3 dir = m_linearVelocity.normalized();
00191                 m_linearVelocity -=  dir * dampVel;
00192             } else
00193             {
00194                 m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00195             }
00196         }
00197 
00198         btScalar angSpeed = m_angularVelocity.length();
00199         if (angSpeed < m_angularDamping)
00200         {
00201             btScalar angDampVel = btScalar(0.005);
00202             if (angSpeed > angDampVel)
00203             {
00204                 btVector3 dir = m_angularVelocity.normalized();
00205                 m_angularVelocity -=  dir * angDampVel;
00206             } else
00207             {
00208                 m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.));
00209             }
00210         }
00211     }
00212 }
00213 
00214 
00215 void btRigidBody::applyGravity()
00216 {
00217     if (isStaticOrKinematicObject())
00218         return;
00219     
00220     applyCentralForce(m_gravity);   
00221 
00222 }
00223 
00224 void btRigidBody::proceedToTransform(const btTransform& newTrans)
00225 {
00226     setCenterOfMassTransform( newTrans );
00227 }
00228     
00229 
00230 void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia)
00231 {
00232     if (mass == btScalar(0.))
00233     {
00234         m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT;
00235         m_inverseMass = btScalar(0.);
00236     } else
00237     {
00238         m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT);
00239         m_inverseMass = btScalar(1.0) / mass;
00240     }
00241 
00242     //Fg = m * a
00243     m_gravity = mass * m_gravity_acceleration;
00244     
00245     m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0),
00246                    inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0),
00247                    inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0));
00248 
00249     m_invMass = m_linearFactor*m_inverseMass;
00250 }
00251 
00252     
00253 
00254 void btRigidBody::updateInertiaTensor() 
00255 {
00256     m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
00257 }
00258 
00259 
00260 void btRigidBody::integrateVelocities(btScalar step) 
00261 {
00262     if (isStaticOrKinematicObject())
00263         return;
00264 
00265     m_linearVelocity += m_totalForce * (m_inverseMass * step);
00266     m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
00267 
00268 #define MAX_ANGVEL SIMD_HALF_PI
00269 
00270     btScalar angvel = m_angularVelocity.length();
00271     if (angvel*step > MAX_ANGVEL)
00272     {
00273         m_angularVelocity *= (MAX_ANGVEL/step) /angvel;
00274     }
00275 
00276 }
00277 
00278 btQuaternion btRigidBody::getOrientation() const
00279 {
00280         btQuaternion orn;
00281         m_worldTransform.getBasis().getRotation(orn);
00282         return orn;
00283 }
00284     
00285     
00286 void btRigidBody::setCenterOfMassTransform(const btTransform& xform)
00287 {
00288 
00289     if (isStaticOrKinematicObject())
00290     {
00291         m_interpolationWorldTransform = m_worldTransform;
00292     } else
00293     {
00294         m_interpolationWorldTransform = xform;
00295     }
00296     m_interpolationLinearVelocity = getLinearVelocity();
00297     m_interpolationAngularVelocity = getAngularVelocity();
00298     m_worldTransform = xform;
00299     updateInertiaTensor();
00300 }
00301 
00302 
00303 bool btRigidBody::checkCollideWithOverride(btCollisionObject* co)
00304 {
00305     btRigidBody* otherRb = btRigidBody::upcast(co);
00306     if (!otherRb)
00307         return true;
00308 
00309     for (int i = 0; i < m_constraintRefs.size(); ++i)
00310     {
00311         btTypedConstraint* c = m_constraintRefs[i];
00312         if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
00313             return false;
00314     }
00315 
00316     return true;
00317 }
00318 
00319 void    btRigidBody::internalWritebackVelocity(btScalar timeStep)
00320 {
00321     (void) timeStep;
00322     if (m_inverseMass)
00323     {
00324         setLinearVelocity(getLinearVelocity()+ m_deltaLinearVelocity);
00325         setAngularVelocity(getAngularVelocity()+m_deltaAngularVelocity);
00326         
00327         //correct the position/orientation based on push/turn recovery
00328         btTransform newTransform;
00329         btTransformUtil::integrateTransform(getWorldTransform(),m_pushVelocity,m_turnVelocity,timeStep,newTransform);
00330         setWorldTransform(newTransform);
00331         //m_originalBody->setCompanionId(-1);
00332     }
00333 //  m_deltaLinearVelocity.setZero();
00334 //  m_deltaAngularVelocity .setZero();
00335 //  m_pushVelocity.setZero();
00336 //  m_turnVelocity.setZero();
00337 }
00338 
00339 
00340 
00341 void btRigidBody::addConstraintRef(btTypedConstraint* c)
00342 {
00343     int index = m_constraintRefs.findLinearSearch(c);
00344     if (index == m_constraintRefs.size())
00345         m_constraintRefs.push_back(c); 
00346 
00347     m_checkCollideWith = true;
00348 }
00349 
00350 void btRigidBody::removeConstraintRef(btTypedConstraint* c)
00351 {
00352     m_constraintRefs.remove(c);
00353     m_checkCollideWith = m_constraintRefs.size() > 0;
00354 }
00355 
00356 int btRigidBody::calculateSerializeBufferSize() const
00357 {
00358     int sz = sizeof(btRigidBodyData);
00359     return sz;
00360 }
00361 
00363 const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const
00364 {
00365     btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer;
00366 
00367     btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer);
00368 
00369     m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld);
00370     m_linearVelocity.serialize(rbd->m_linearVelocity);
00371     m_angularVelocity.serialize(rbd->m_angularVelocity);
00372     rbd->m_inverseMass = m_inverseMass;
00373     m_angularFactor.serialize(rbd->m_angularFactor);
00374     m_linearFactor.serialize(rbd->m_linearFactor);
00375     m_gravity.serialize(rbd->m_gravity);
00376     m_gravity_acceleration.serialize(rbd->m_gravity_acceleration);
00377     m_invInertiaLocal.serialize(rbd->m_invInertiaLocal);
00378     m_totalForce.serialize(rbd->m_totalForce);
00379     m_totalTorque.serialize(rbd->m_totalTorque);
00380     rbd->m_linearDamping = m_linearDamping;
00381     rbd->m_angularDamping = m_angularDamping;
00382     rbd->m_additionalDamping = m_additionalDamping;
00383     rbd->m_additionalDampingFactor = m_additionalDampingFactor;
00384     rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr;
00385     rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr;
00386     rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor;
00387     rbd->m_linearSleepingThreshold=m_linearSleepingThreshold;
00388     rbd->m_angularSleepingThreshold = m_angularSleepingThreshold;
00389 
00390     return btRigidBodyDataName;
00391 }
00392 
00393 
00394 
00395 void btRigidBody::serializeSingleObject(class btSerializer* serializer) const
00396 {
00397     btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1);
00398     const char* structType = serialize(chunk->m_oldPtr, serializer);
00399     serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this);
00400 }
00401 
00402