diff options
Diffstat (limited to 'thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp')
| -rw-r--r-- | thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp | 263 |
1 files changed, 113 insertions, 150 deletions
diff --git a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp index ca0714fcfa..f4bcabada2 100644 --- a/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp +++ b/thirdparty/bullet/BulletDynamics/Dynamics/btRigidBody.cpp @@ -22,36 +22,34 @@ subject to the following restrictions: #include "LinearMath/btSerializer.h" //'temporarily' global variables -btScalar gDeactivationTime = btScalar(2.); -bool gDisableDeactivation = false; +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) +btRigidBody::btRigidBody(btScalar mass, btMotionState* motionState, btCollisionShape* collisionShape, const btVector3& localInertia) { - btRigidBodyConstructionInfo cinfo(mass,motionState,collisionShape,localInertia); + btRigidBodyConstructionInfo cinfo(mass, motionState, collisionShape, localInertia); setupRigidBody(cinfo); } -void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) +void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo) { - - m_internalType=CO_RIGID_BODY; + 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_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); + setDamping(constructionInfo.m_linearDamping, constructionInfo.m_angularDamping); m_linearSleepingThreshold = constructionInfo.m_linearSleepingThreshold; m_angularSleepingThreshold = constructionInfo.m_angularSleepingThreshold; @@ -67,48 +65,44 @@ void btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& if (m_optionalMotionState) { m_optionalMotionState->getWorldTransform(m_worldTransform); - } else + } + else { m_worldTransform = constructionInfo.m_startWorldTransform; } m_interpolationWorldTransform = m_worldTransform; - m_interpolationLinearVelocity.setValue(0,0,0); - m_interpolationAngularVelocity.setValue(0,0,0); - + 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_spinningFriction = constructionInfo.m_spinningFriction; + m_restitution = constructionInfo.m_restitution; - setCollisionShape( constructionInfo.m_collisionShape ); + 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_invMass = m_inverseMass * m_linearFactor; m_pushVelocity.setZero(); m_turnVelocity.setZero(); - - - } - -void btRigidBody::predictIntegratedTransform(btScalar timeStep,btTransform& predictedTransform) +void btRigidBody::predictIntegratedTransform(btScalar timeStep, btTransform& predictedTransform) { - btTransformUtil::integrateTransform(m_worldTransform,m_linearVelocity,m_angularVelocity,timeStep,predictedTransform); + btTransformUtil::integrateTransform(m_worldTransform, m_linearVelocity, m_angularVelocity, timeStep, predictedTransform); } -void btRigidBody::saveKinematicState(btScalar timeStep) +void btRigidBody::saveKinematicState(btScalar timeStep) { //todo: clamp to some (user definable) safe minimum timestep, to limit maximum angular/linear velocities if (timeStep != btScalar(0.)) @@ -116,25 +110,22 @@ void btRigidBody::saveKinematicState(btScalar timeStep) //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); + 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 + +void btRigidBody::getAabb(btVector3& aabbMin, btVector3& aabbMax) const { - getCollisionShape()->getAabb(m_worldTransform,aabbMin,aabbMax); + getCollisionShape()->getAabb(m_worldTransform, aabbMin, aabbMax); } - - - -void btRigidBody::setGravity(const btVector3& acceleration) +void btRigidBody::setGravity(const btVector3& acceleration) { if (m_inverseMass != btScalar(0.0)) { @@ -143,22 +134,14 @@ void btRigidBody::setGravity(const btVector3& acceleration) 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) +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 @@ -168,8 +151,8 @@ void btRigidBody::applyDamping(btScalar timeStep) 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); + m_linearVelocity *= btPow(btScalar(1) - m_linearDamping, timeStep); + m_angularVelocity *= btPow(btScalar(1) - m_angularDamping, timeStep); #endif if (m_additionalDamping) @@ -182,7 +165,6 @@ void btRigidBody::applyDamping(btScalar timeStep) m_angularVelocity *= m_additionalDampingFactor; m_linearVelocity *= m_additionalDampingFactor; } - btScalar speed = m_linearVelocity.length(); if (speed < m_linearDamping) @@ -191,10 +173,11 @@ void btRigidBody::applyDamping(btScalar timeStep) if (speed > dampVel) { btVector3 dir = m_linearVelocity.normalized(); - m_linearVelocity -= dir * dampVel; - } else + m_linearVelocity -= dir * dampVel; + } + else { - m_linearVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + m_linearVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } } @@ -205,30 +188,28 @@ void btRigidBody::applyDamping(btScalar timeStep) if (angSpeed > angDampVel) { btVector3 dir = m_angularVelocity.normalized(); - m_angularVelocity -= dir * angDampVel; - } else + m_angularVelocity -= dir * angDampVel; + } + else { - m_angularVelocity.setValue(btScalar(0.),btScalar(0.),btScalar(0.)); + m_angularVelocity.setValue(btScalar(0.), btScalar(0.), btScalar(0.)); } } } } - void btRigidBody::applyGravity() { if (isStaticOrKinematicObject()) return; - - applyCentralForce(m_gravity); + applyCentralForce(m_gravity); } void btRigidBody::proceedToTransform(const btTransform& newTrans) { - setCenterOfMassTransform( newTrans ); + setCenterOfMassTransform(newTrans); } - void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) { @@ -236,7 +217,8 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) { m_collisionFlags |= btCollisionObject::CF_STATIC_OBJECT; m_inverseMass = btScalar(0.); - } else + } + else { m_collisionFlags &= (~btCollisionObject::CF_STATIC_OBJECT); m_inverseMass = btScalar(1.0) / mass; @@ -244,50 +226,45 @@ void btRigidBody::setMassProps(btScalar mass, const btVector3& inertia) //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; + 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() +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)); + 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 btMatrix3x3& I) { - const btVector3 w2 = I*w1 + w1.cross(I*w1)*dt - (T*dt + I*w0); + 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) + const btMatrix3x3& I) { - btMatrix3x3 w1x, Iw1x; - const btVector3 Iwi = (I*w1); + 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; + const btMatrix3x3 dfw1 = I + (w1x * I - Iw1x) * dt; return dfw1; } @@ -295,58 +272,55 @@ btVector3 btRigidBody::computeGyroscopicForceExplicit(btScalar maxGyroscopicForc { btVector3 inertiaLocal = getLocalInertia(); btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose(); - btVector3 tmp = inertiaTensorWorld*getAngularVelocity(); + btVector3 tmp = inertiaTensorWorld * getAngularVelocity(); btVector3 gf = getAngularVelocity().cross(tmp); btScalar l2 = gf.length2(); - if (l2>maxGyroscopicForce*maxGyroscopicForce) + if (l2 > maxGyroscopicForce * maxGyroscopicForce) { - gf *= btScalar(1.)/btSqrt(l2)*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; + 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; + btVector3 om = Ib * omegab; btMatrix3x3 skew1; - om.getSkewSymmetricMatrix(&skew1[0],&skew1[1],&skew1[2]); - + 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; + 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); + omegab = omegab - omega_div; //Solve33(J, f); // Back to world coordinates - btVector3 omega2 = quatRotate(q,omegab); - btVector3 gf = omega2-omega1; + 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. @@ -361,7 +335,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con 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 + // f(w') = -(T*step + Iw) + Iw' + w' + w'xIw'*step = 0 // df/dw' = I + 1xIw'*step + w'xI*step btVector3 w1 = w0; @@ -383,8 +357,7 @@ btVector3 btRigidBody::computeGyroscopicImpulseImplicit_World(btScalar step) con return gf; } - -void btRigidBody::integrateVelocities(btScalar step) +void btRigidBody::integrateVelocities(btScalar step) { if (isStaticOrKinematicObject()) return; @@ -393,30 +366,28 @@ void btRigidBody::integrateVelocities(btScalar 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 + /// clamp angular velocity. collision calculations will fail on higher angular velocities btScalar angvel = m_angularVelocity.length(); - if (angvel*step > MAX_ANGVEL) + if (angvel * step > MAX_ANGVEL) { - m_angularVelocity *= (MAX_ANGVEL/step) /angvel; + m_angularVelocity *= (MAX_ANGVEL / step) / angvel; } - } btQuaternion btRigidBody::getOrientation() const { - btQuaternion orn; - m_worldTransform.getBasis().getRotation(orn); - return orn; + btQuaternion orn; + m_worldTransform.getBasis().getRotation(orn); + return orn; } - - + void btRigidBody::setCenterOfMassTransform(const btTransform& xform) { - if (isKinematicObject()) { m_interpolationWorldTransform = m_worldTransform; - } else + } + else { m_interpolationWorldTransform = xform; } @@ -426,10 +397,6 @@ void btRigidBody::setCenterOfMassTransform(const btTransform& xform) updateInertiaTensor(); } - - - - void btRigidBody::addConstraintRef(btTypedConstraint* c) { ///disable collision with the 'other' body @@ -450,39 +417,39 @@ void btRigidBody::addConstraintRef(btTypedConstraint* c) { 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); - } - } + 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 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 +///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; + btRigidBodyData* rbd = (btRigidBodyData*)dataBuffer; btCollisionObject::serialize(&rbd->m_collisionObjectData, serializer); @@ -504,7 +471,7 @@ const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali rbd->m_additionalLinearDampingThresholdSqr = m_additionalLinearDampingThresholdSqr; rbd->m_additionalAngularDampingThresholdSqr = m_additionalAngularDampingThresholdSqr; rbd->m_additionalAngularDampingFactor = m_additionalAngularDampingFactor; - rbd->m_linearSleepingThreshold=m_linearSleepingThreshold; + rbd->m_linearSleepingThreshold = m_linearSleepingThreshold; rbd->m_angularSleepingThreshold = m_angularSleepingThreshold; // Fill padding with zeros to appease msan. @@ -515,13 +482,9 @@ const char* btRigidBody::serialize(void* dataBuffer, class btSerializer* seriali return btRigidBodyDataName; } - - void btRigidBody::serializeSingleObject(class btSerializer* serializer) const { - btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(),1); + btChunk* chunk = serializer->allocate(calculateSerializeBufferSize(), 1); const char* structType = serialize(chunk->m_oldPtr, serializer); - serializer->finalizeChunk(chunk,structType,BT_RIGIDBODY_CODE,(void*)this); + serializer->finalizeChunk(chunk, structType, BT_RIGIDBODY_CODE, (void*)this); } - - |