| /* |
| Bullet Continuous Collision Detection and Physics Library |
| Copyright (c) 2003-2006 Erwin Coumans http://continuousphysics.com/Bullet/ |
| |
| This software is provided 'as-is', without any express or implied warranty. |
| In no event will the authors be held liable for any damages arising from the use of this software. |
| Permission is granted to anyone to use this software for any purpose, |
| including commercial applications, and to alter it and redistribute it freely, |
| subject to the following restrictions: |
| |
| 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. |
| 2. Altered source versions must be plainly marked as such, and must not be misrepresented as being the original software. |
| 3. This notice may not be removed or altered from any source distribution. |
| */ |
| |
| #include "BulletDynamics/Dynamics/btRigidBody.h" |
| #include "BulletCollision/CollisionShapes/btConvexShape.h" |
| #include "LinearMath/btMinMax.h" |
| #include "LinearMath/btTransformUtil.h" |
| #include "LinearMath/btMotionState.h" |
| #include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" |
| |
| //'temporarily' global variables |
| btScalar gDeactivationTime = btScalar(2.); |
| bool gDisableDeactivation = false; |
| static int uniqueId = 0; |
| |
| |
| btRigidBody::btRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) |
| { |
| setupRigidBody(constructionInfo); |
| } |
| |
| btRigidBody::btRigidBody(btScalar mass, btMotionState *motionState, btCollisionShape *collisionShape, const btVector3 &localInertia) |
| { |
| btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia); |
| setupRigidBody(cinfo); |
| } |
| |
| void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) |
| { |
| |
| m_internalType=CO_RIGID_BODY; |
| |
| m_linearVelocity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); |
| m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); |
| m_angularFactor.setValue(1,1,1); |
| m_linearFactor.setValue(1,1,1); |
| m_gravity.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); |
| m_gravity_acceleration.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); |
| m_totalForce.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)); |
| m_totalTorque.setValue(btScalar(0.0), btScalar(0.0), btScalar(0.0)), |
| m_linearDamping = btScalar(0.); |
| m_angularDamping = btScalar(0.5); |
| m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold; |
| m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold; |
| m_optionalMotionState = constructionInfo.m_motionState; |
| m_contactSolverType = 0; |
| m_frictionSolverType = 0; |
| m_additionalDamping = constructionInfo.m_additionalDamping; |
| m_additionalDampingFactor = constructionInfo.m_additionalDampingFactor; |
| m_additionalLinearDampingThresholdSqr = constructionInfo.m_additionalLinearDampingThresholdSqr; |
| m_additionalAngularDampingThresholdSqr = constructionInfo.m_additionalAngularDampingThresholdSqr; |
| m_additionalAngularDampingFactor = constructionInfo.m_additionalAngularDampingFactor; |
| |
| if (m_optionalMotionState) |
| { |
| m_optionalMotionState->getWorldTransform(m_worldTransform); |
| } else |
| { |
| m_worldTransform = constructionInfo.m_startWorldTransform; |
| } |
| |
| m_interpolationWorldTransform = m_worldTransform; |
| m_interpolationLinearVelocity.setValue(0,0,0); |
| m_interpolationAngularVelocity.setValue(0,0,0); |
| |
| //moved to btCollisionObject |
| m_friction = constructionInfo.m_friction; |
| m_restitution = constructionInfo.m_restitution; |
| |
| setCollisionShape( constructionInfo.m_collisionShape ); |
| m_debugBodyId = uniqueId++; |
| |
| setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); |
| setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); |
| updateInertiaTensor(); |
| |
| } |
| |
| |
| void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) |
| { |
| btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); |
| } |
| |
| void btRigidBody::saveKinematicState(btScalar timeStep) |
| { |
| //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities |
| if (timeStep != btScalar(0.)) |
| { |
| //if we use motionstate to synchronize world transforms, get the new kinematic/animated world transform |
| if (getMotionState()) |
| getMotionState()->getWorldTransform(m_worldTransform); |
| btVector3 linVel,angVel; |
| |
| btTransformUtil::calculateVelocity(m_interpolationWorldTransform,m_worldTransform,timeStep,m_linearVelocity,m_angularVelocity); |
| m_interpolationLinearVelocity = m_linearVelocity; |
| m_interpolationAngularVelocity = m_angularVelocity; |
| m_interpolationWorldTransform = m_worldTransform; |
| //printf("angular = %f %f %f\n",m_angularVelocity.getX(),m_angularVelocity.getY(),m_angularVelocity.getZ()); |
| } |
| } |
| |
| void btRigidBody::getAabb(btVector3& aabbMin,btVector3& aabbMax) const |
| { |
| getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax); |
| } |
| |
| |
| |
| |
| void btRigidBody::setGravity(const btVector3& acceleration) |
| { |
| if (m_inverseMass != btScalar(0.0)) |
| { |
| m_gravity = acceleration * (btScalar(1.0) / m_inverseMass); |
| } |
| m_gravity_acceleration = acceleration; |
| } |
| |
| |
| |
| |
| |
| |
| void btRigidBody::setDamping(btScalar lin_damping, btScalar ang_damping) |
| { |
| m_linearDamping = GEN_clamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); |
| m_angularDamping = GEN_clamped(ang_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); |
| } |
| |
| |
| |
| |
| ///applyDamping damps the velocity, using the given m_linearDamping and m_angularDamping |
| void btRigidBody::applyDamping(btScalar timeStep) |
| { |
| //On new damping: see discussion/issue report here: http://code.google.com/p/bullet/issues/detail?id=74 |
| //todo: do some performance comparisons (but other parts of the engine are probably bottleneck anyway |
| |
| //#define USE_OLD_DAMPING_METHOD 1 |
| #ifdef USE_OLD_DAMPING_METHOD |
| m_linearVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_linearDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); |
| m_angularVelocity *= GEN_clamped((btScalar(1.) - timeStep * m_angularDamping), (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); |
| #else |
| m_linearVelocity *= btPow(btScalar(1)-m_linearDamping, timeStep); |
| m_angularVelocity *= btPow(btScalar(1)-m_angularDamping, timeStep); |
| #endif |
| |
| if (m_additionalDamping) |
| { |
| //Additional damping can help avoiding lowpass jitter motion, help stability for ragdolls etc. |
| //Such damping is undesirable, so once the overall simulation quality of the rigid body dynamics system has improved, this should become obsolete |
| if ((m_angularVelocity.length2() < m_additionalAngularDampingThresholdSqr) && |
| (m_linearVelocity.length2() < m_additionalLinearDampingThresholdSqr)) |
| { |
| m_angularVelocity *= m_additionalDampingFactor; |
| m_linearVelocity *= m_additionalDampingFactor; |
| } |
| |
| |
| btScalar speed = m_linearVelocity.length(); |
| if (speed < m_linearDamping) |
| { |
| btScalar dampVel = btScalar(0.005); |
| if (speed > dampVel) |
| { |
| btVector3 dir = m_linearVelocity.normalized(); |
| m_linearVelocity -= dir * dampVel; |
| } else |
| { |
| m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); |
| } |
| } |
| |
| btScalar angSpeed = m_angularVelocity.length(); |
| if (angSpeed < m_angularDamping) |
| { |
| btScalar angDampVel = btScalar(0.005); |
| if (angSpeed > angDampVel) |
| { |
| btVector3 dir = m_angularVelocity.normalized(); |
| m_angularVelocity -= dir * angDampVel; |
| } else |
| { |
| m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); |
| } |
| } |
| } |
| } |
| |
| |
| void btRigidBody::applyGravity() |
| { |
| if (isStaticOrKinematicObject()) |
| return; |
| |
| applyCentralForce(m_gravity); |
| |
| } |
| |
| void btRigidBody::proceedToTransform(const btTransform& newTrans) |
| { |
| setCenterOfMassTransform( newTrans ); |
| } |
| |
| |
| void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) |
| { |
| if (mass == btScalar(0.)) |
| { |
| m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT; |
| m_inverseMass = btScalar(0.); |
| } else |
| { |
| m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); |
| m_inverseMass = btScalar(1.0) / mass; |
| } |
| |
| m_invInertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x(): btScalar(0.0), |
| inertia.y() != btScalar(0.0) ? btScalar(1.0) / inertia.y(): btScalar(0.0), |
| inertia.z() != btScalar(0.0) ? btScalar(1.0) / inertia.z(): btScalar(0.0)); |
| |
| } |
| |
| |
| |
| void btRigidBody::updateInertiaTensor() |
| { |
| m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); |
| } |
| |
| |
| void btRigidBody::integrateVelocities(btScalar step) |
| { |
| if (isStaticOrKinematicObject()) |
| return; |
| |
| m_linearVelocity += m_totalForce * (m_inverseMass * step); |
| m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step; |
| |
| #define MAX_ANGVEL SIMD_HALF_PI |
| /// clamp angular velocity. collision calculations will fail on higher angular velocities |
| btScalar angvel = m_angularVelocity.length(); |
| if (angvel*step > MAX_ANGVEL) |
| { |
| m_angularVelocity *= (MAX_ANGVEL/step) /angvel; |
| } |
| |
| } |
| |
| btQuaternion btRigidBody::getOrientation() const |
| { |
| btQuaternion orn; |
| m_worldTransform.getBasis().getRotation(orn); |
| return orn; |
| } |
| |
| |
| void btRigidBody::setCenterOfMassTransform(const btTransform& xform) |
| { |
| |
| if (isStaticOrKinematicObject()) |
| { |
| m_interpolationWorldTransform = m_worldTransform; |
| } else |
| { |
| m_interpolationWorldTransform = xform; |
| } |
| m_interpolationLinearVelocity = getLinearVelocity(); |
| m_interpolationAngularVelocity = getAngularVelocity(); |
| m_worldTransform = xform; |
| updateInertiaTensor(); |
| } |
| |
| |
| bool btRigidBody::checkCollideWithOverride(btCollisionObject* co) |
| { |
| btRigidBody* otherRb = btRigidBody::upcast(co); |
| if (!otherRb) |
| return true; |
| |
| for (int i = 0; i < m_constraintRefs.size(); ++i) |
| { |
| btTypedConstraint* c = m_constraintRefs[i]; |
| if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb) |
| return false; |
| } |
| |
| return true; |
| } |
| |
| void btRigidBody::addConstraintRef(btTypedConstraint* c) |
| { |
| int index = m_constraintRefs.findLinearSearch(c); |
| if (index == m_constraintRefs.size()) |
| m_constraintRefs.push_back(c); |
| |
| m_checkCollideWith = true; |
| } |
| |
| void btRigidBody::removeConstraintRef(btTypedConstraint* c) |
| { |
| m_constraintRefs.remove(c); |
| m_checkCollideWith = m_constraintRefs.size() > 0; |
| } |