diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp | 527 |
1 files changed, 527 insertions, 0 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp new file mode 100644 index 0000000000..ca0714fcfa --- /dev/null +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp @@ -0,0 +1,527 @@ +/* +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 "btRigidBody.h" +#include "BulletCollision/CollisionShapes/btConvexShape.h" +#include "LinearMath/btMinMax.h" +#include "LinearMath/btTransformUtil.h" +#include "LinearMath/btMotionState.h" +#include "BulletDynamics/ConstraintSolver/btTypedConstraint.h" +#include "LinearMath/btSerializer.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)), + setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); + + 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_rollingFriction = constructionInfo.m_rollingFriction; + m_spinningFriction = constructionInfo.m_spinningFriction; + + m_restitution = constructionInfo.m_restitution; + + setCollisionShape( constructionInfo.m_collisionShape ); + m_debugBodyId = uniqueId++; + + setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); + updateInertiaTensor(); + + m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY; + + + m_deltaLinearVelocity.setZero(); + m_deltaAngularVelocity.setZero(); + m_invMass = m_inverseMass*m_linearFactor; + m_pushVelocity.setZero(); + m_turnVelocity.setZero(); + + + +} + + +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 = btClamped(lin_damping, (btScalar)btScalar(0.0), (btScalar)btScalar(1.0)); + m_angularDamping = btClamped(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; + } + + //Fg = m * a + m_gravity = mass * m_gravity_acceleration; + + 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)); + + m_invMass = m_linearFactor*m_inverseMass; +} + + +void btRigidBody::updateInertiaTensor() +{ + m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); +} + + + +btVector3 btRigidBody::getLocalInertia() const +{ + + btVector3 inertiaLocal; + const btVector3 inertia = m_invInertiaLocal; + inertiaLocal.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)); + return inertiaLocal; +} + +inline btVector3 evalEulerEqn(const btVector3& w1, const btVector3& w0, const btVector3& T, const btScalar dt, + const btMatrix3x3 &I) +{ + const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0); + return w2; +} + +inline btMatrix3x3 evalEulerEqnDeriv(const btVector3& w1, const btVector3& w0, const btScalar dt, + const btMatrix3x3 &I) +{ + + btMatrix3x3 w1x, Iw1x; + const btVector3 Iwi = (I*w1); + w1.getSkewSymmetricMatrix(&w1x[0], &w1x[1], &w1x[2]); + Iwi.getSkewSymmetricMatrix(&Iw1x[0], &Iw1x[1], &Iw1x[2]); + + const btMatrix3x3 dfw1 = I + (w1x*I - Iw1x)*dt; + return dfw1; +} + +btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForce) const +{ + btVector3 inertiaLocal = getLocalInertia(); + btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); + btVector3 tmp = inertiaTensorWorld*getAngularVelocity(); + btVector3 gf = getAngularVelocity().cross(tmp); + btScalar l2 = gf.length2(); + if (l2>maxGyroscopicForce*maxGyroscopicForce) + { + gf *= btScalar(1.)/btSqrt(l2)*maxGyroscopicForce; + } + return gf; +} + + +btVector3 btRigidBody::computeGyroscopicImpulseImplicit_Body(btScalar step) const +{ + btVector3 idl = getLocalInertia(); + btVector3 omega1 = getAngularVelocity(); + btQuaternion q = getWorldTransform().getRotation(); + + // Convert to body coordinates + btVector3 omegab = quatRotate(q.inverse(), omega1); + btMatrix3x3 Ib; + Ib.setValue(idl.x(),0,0, + 0,idl.y(),0, + 0,0,idl.z()); + + btVector3 ibo = Ib*omegab; + + // Residual vector + btVector3 f = step * omegab.cross(ibo); + + btMatrix3x3 skew0; + omegab.getSkewSymmetricMatrix(&skew0[0], &skew0[1], &skew0[2]); + btVector3 om = Ib*omegab; + btMatrix3x3 skew1; + om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]); + + // Jacobian + btMatrix3x3 J = Ib + (skew0*Ib - skew1)*step; + +// btMatrix3x3 Jinv = J.inverse(); +// btVector3 omega_div = Jinv*f; + btVector3 omega_div = J.solve33(f); + + // Single Newton-Raphson update + omegab = omegab - omega_div;//Solve33(J, f); + // Back to world coordinates + btVector3 omega2 = quatRotate(q,omegab); + btVector3 gf = omega2-omega1; + return gf; +} + + + +btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) const +{ + // use full newton-euler equations. common practice to drop the wxIw term. want it for better tumbling behavior. + // calculate using implicit euler step so it's stable. + + const btVector3 inertiaLocal = getLocalInertia(); + const btVector3 w0 = getAngularVelocity(); + + btMatrix3x3 I; + + I = m_worldTransform.getBasis().scaled(inertiaLocal) * + m_worldTransform.getBasis().transpose(); + + // use newtons method to find implicit solution for new angular velocity (w') + // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 + // df/dw' = I + 1xIw'*step + w'xI*step + + btVector3 w1 = w0; + + // one step of newton's method + { + const btVector3 fw = evalEulerEqn(w1, w0, btVector3(0, 0, 0), step, I); + const btMatrix3x3 dfw = evalEulerEqnDeriv(w1, w0, step, I); + + btVector3 dw; + dw = dfw.solve33(fw); + //const btMatrix3x3 dfw_inv = dfw.inverse(); + //dw = dfw_inv*fw; + + w1 -= dw; + } + + btVector3 gf = (w1 - w0); + return gf; +} + + +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 (isKinematicObject()) + { + m_interpolationWorldTransform = m_worldTransform; + } else + { + m_interpolationWorldTransform = xform; + } + m_interpolationLinearVelocity = getLinearVelocity(); + m_interpolationAngularVelocity = getAngularVelocity(); + m_worldTransform = xform; + updateInertiaTensor(); +} + + + + + +void btRigidBody::addConstraintRef(btTypedConstraint* c) +{ + ///disable collision with the 'other' body + + int index = m_constraintRefs.findLinearSearch(c); + //don't add constraints that are already referenced + //btAssert(index == m_constraintRefs.size()); + if (index == m_constraintRefs.size()) + { + m_constraintRefs.push_back(c); + btCollisionObject* colObjA = &c->getRigidBodyA(); + btCollisionObject* colObjB = &c->getRigidBodyB(); + if (colObjA == this) + { + colObjA->setIgnoreCollisionCheck(colObjB, true); + } + else + { + colObjB->setIgnoreCollisionCheck(colObjA, true); + } + } +} + +void btRigidBody::removeConstraintRef(btTypedConstraint* c) +{ + int index = m_constraintRefs.findLinearSearch(c); + //don't remove constraints that are not referenced + if(index < m_constraintRefs.size()) + { + m_constraintRefs.remove(c); + btCollisionObject* colObjA = &c->getRigidBodyA(); + btCollisionObject* colObjB = &c->getRigidBodyB(); + if (colObjA == this) + { + colObjA->setIgnoreCollisionCheck(colObjB, false); + } + else + { + colObjB->setIgnoreCollisionCheck(colObjA, false); + } + } +} + +int btRigidBody::calculateSerializeBufferSize() const +{ + int sz = sizeof(btRigidBodyData); + return sz; +} + + ///fills the dataBuffer and returns the struct name (and 0 on failure) +const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* serializer) const +{ + btRigidBodyData* rbd = (btRigidBodyData*) dataBuffer; + + btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer); + + m_invInertiaTensorWorld.serialize(rbd->m_invInertiaTensorWorld); + m_linearVelocity.serialize(rbd->m_linearVelocity); + m_angularVelocity.serialize(rbd->m_angularVelocity); + rbd->m_inverseMass = m_inverseMass; + m_angularFactor.serialize(rbd->m_angularFactor); + m_linearFactor.serialize(rbd->m_linearFactor); + m_gravity.serialize(rbd->m_gravity); + m_gravity_acceleration.serialize(rbd->m_gravity_acceleration); + m_invInertiaLocal.serialize(rbd->m_invInertiaLocal); + m_totalForce.serialize(rbd->m_totalForce); + m_totalTorque.serialize(rbd->m_totalTorque); + rbd->m_linearDamping = m_linearDamping; + rbd->m_angularDamping = m_angularDamping; + rbd->m_additionalDamping = m_additionalDamping; + rbd->m_additionalDampingFactor = m_additionalDampingFactor; + rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr; + rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr; + rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor; + rbd->m_linearSleepingThreshold=m_linearSleepingThreshold; + rbd->m_angularSleepingThreshold = m_angularSleepingThreshold; + + // Fill padding with zeros to appease msan. +#ifdef BT_USE_DOUBLE_PRECISION + memset(rbd->m_padding, 0, sizeof(rbd->m_padding)); +#endif + + return btRigidBodyDataName; +} + + + +void btRigidBody::serializeSingleObject(class btSerializer* serializer) const +{ + btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1); + const char* structType = serialize(chunk->m_oldPtr, serializer); + serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this); +} + + |