Page MenuHome
Differential D1739 Diff 5899 extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp

Changeset View

Standalone View

View Options

extern/bullet2/src/BulletDynamics/Dynamics/btRigidBody.cpp

Show First 20 Lines • Show All 81 Lines • ▼ Show 20 Linesvoid btRigidBody::setupRigidBody(const btRigidBody::btRigidBodyConstructionInfo& constructionInfo)
m_restitution = constructionInfo.m_restitution; m_restitution = constructionInfo.m_restitution;
setCollisionShape( constructionInfo.m_collisionShape ); setCollisionShape( constructionInfo.m_collisionShape );
m_debugBodyId = uniqueId++; m_debugBodyId = uniqueId++;
setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia); setMassProps(constructionInfo.m_mass, constructionInfo.m_localInertia);
updateInertiaTensor(); updateInertiaTensor();
m_rigidbodyFlags = 0; m_rigidbodyFlags = BT_ENABLE_GYROSCOPIC_FORCE_IMPLICIT_BODY;
m_deltaLinearVelocity.setZero(); m_deltaLinearVelocity.setZero();
m_deltaAngularVelocity.setZero(); m_deltaAngularVelocity.setZero();
m_invMass = m_inverseMass*m_linearFactor; m_invMass = m_inverseMass*m_linearFactor;
m_pushVelocity.setZero(); m_pushVelocity.setZero();
m_turnVelocity.setZero(); m_turnVelocity.setZero();
▲ Show 20 Lines • Show All 153 Lines • ▼ Show 20 Lines
void btRigidBody::updateInertiaTensor() void btRigidBody::updateInertiaTensor()
{ {
m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose(); m_invInertiaTensorWorld = m_worldTransform.getBasis().scaled(m_invInertiaLocal) * m_worldTransform.getBasis().transpose();
} }
btVector3 btRigidBody::computeGyroscopicForce(btScalar maxGyroscopicForce) const
btVector3 btRigidBody::getLocalInertia() const
{ {
btVector3 inertiaLocal; btVector3 inertiaLocal;
inertiaLocal[0] = 1.f/getInvInertiaDiagLocal()[0]; const btVector3 inertia = m_invInertiaLocal;
inertiaLocal[1] = 1.f/getInvInertiaDiagLocal()[1]; inertiaLocal.setValue(inertia.x() != btScalar(0.0) ? btScalar(1.0) / inertia.x() : btScalar(0.0),
inertiaLocal[2] = 1.f/getInvInertiaDiagLocal()[2]; 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(); btMatrix3x3 inertiaTensorWorld = getWorldTransform().getBasis().scaled(inertiaLocal) * getWorldTransform().getBasis().transpose();
btVector3 tmp = inertiaTensorWorld*getAngularVelocity(); btVector3 tmp = inertiaTensorWorld*getAngularVelocity();
btVector3 gf = getAngularVelocity().cross(tmp); btVector3 gf = getAngularVelocity().cross(tmp);
btScalar l2 = gf.length2(); 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; 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) void btRigidBody::integrateVelocities(btScalar step)
{ {
if (isStaticOrKinematicObject()) if (isStaticOrKinematicObject())
return; return;
m_linearVelocity += m_totalForce * (m_inverseMass * step); m_linearVelocity += m_totalForce * (m_inverseMass * step);
m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step; m_angularVelocity += m_invInertiaTensorWorld * m_totalTorque * step;
Show All 27 Linesvoid btRigidBody::setCenterOfMassTransform(const btTransform& xform)
} }
m_interpolationLinearVelocity = getLinearVelocity(); m_interpolationLinearVelocity = getLinearVelocity();
m_interpolationAngularVelocity = getAngularVelocity(); m_interpolationAngularVelocity = getAngularVelocity();
m_worldTransform = xform; m_worldTransform = xform;
updateInertiaTensor(); updateInertiaTensor();
} }
bool btRigidBody::checkCollideWithOverride(const btCollisionObject* co) const
{
const btRigidBody* otherRb = btRigidBody::upcast(co);
if (!otherRb)
return true;
for (int i = 0; i < m_constraintRefs.size(); ++i)
{
const btTypedConstraint* c = m_constraintRefs[i];
if (c->isEnabled())
if (&c->getRigidBodyA() == otherRb || &c->getRigidBodyB() == otherRb)
return false;
}
return true;
}
void btRigidBody::addConstraintRef(btTypedConstraint* c) void btRigidBody::addConstraintRef(btTypedConstraint* c)
{ {
///disable collision with the 'other' body
int index = m_constraintRefs.findLinearSearch(c); int index = m_constraintRefs.findLinearSearch(c);
//don't add constraints that are already referenced
//btAssert(index == m_constraintRefs.size());
if (index == m_constraintRefs.size()) if (index == m_constraintRefs.size())
{
m_constraintRefs.push_back(c); m_constraintRefs.push_back(c);
btCollisionObject* colObjA = &c->getRigidBodyA();
m_checkCollideWith = true; btCollisionObject* colObjB = &c->getRigidBodyB();
if (colObjA == this)
{
colObjA->setIgnoreCollisionCheck(colObjB, true);
}
else
{
colObjB->setIgnoreCollisionCheck(colObjA, true);
}
}
} }
void btRigidBody::removeConstraintRef(btTypedConstraint* c) 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); m_constraintRefs.remove(c);
m_checkCollideWith = m_constraintRefs.size() > 0; 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); int sz = sizeof(btRigidBodyData);
return sz; return sz;
} }
▲ Show 20 Lines • Show All 41 Lines • Show Last 20 Lines