Thanks Dirk. I have gone through the Box2D code and I couldn't find where the variable timestep was used, only for profiling. In the manual, it says that it uses a fixed timestep of 60Hz. Box2D lite is also too fast on my PC and this is not realistic when I load a terrain, perform collisions, which slows the frame rate and the physics slows down with it.
How did you go about getting a consistent physics simulation without supporting a variable timestep? I am using a fixed timestep but using that previous bit of code I pasted to keep the frame rate consistent for the physics.
Also, in Box2D lite, one thing that differs from my impulse code is the calculation in the preStep function, where in 2D, an orthonormal basis is built for the tangent vector. Here is my code that builds an orthonormal basis and preStep code, can you find anything wrong with it, it's causing instabilities when I activate friction?
Code: Select all
void AMG3DRigidBodyCollisionResponder::computeMasses(AMG3DScalar dt)
{
if(m_bSkipCollision)
return;
if(dt<=0.0f)
return;
//m_dt = dt;
//return;
const AMG3DScalar k_allowedPenetration = 0.01f;
AMG3DScalar k_biasFactor = (AMG3DPhysicsWorld::g_AMG3D_bPositionCorrection) ? 0.2f : 0.0f;
for(int i=0; i<m_cdContactData.iNumContacts; ++i) {
AMG3DVector4 normal = m_cdContactData.vContactNormal;
AMG3DVector4 rap = m_cdContactData.contacts[i].vContactPoint - m_pRigidBody1->position;
AMG3DVector4 rbp = m_cdContactData.contacts[i].vContactPoint - m_pRigidBody2->position;
AMG3DVector4 rapcrossn = rap.cross(normal);
AMG3DVector4 rbpcrossn = rbp.cross(normal);
// Compute the normal mass
AMG3DScalar Kn = m_pRigidBody1->invMass + m_pRigidBody2->invMass;
Kn += ( (m_pRigidBody1->invIWorld*rapcrossn).cross(rap) + (m_pRigidBody2->invIWorld*rbpcrossn).cross(rbp) ).dot(normal);
m_cdContactData.contacts[i].massNormal = 1.0f/Kn;
AMG3DVector4 vap = m_pRigidBody1->velocity + m_pRigidBody1->angularVelocity.cross(rap);
AMG3DVector4 vbp = m_pRigidBody2->velocity + m_pRigidBody2->angularVelocity.cross(rbp);
// Relative velocity
AMG3DVector4 vab = vap - vbp;
// Calculate the tangent vector
AMG3DVector4 tangent = vab - normal*(vab.dot(normal));
// Ensure the tangent vector is non-zero
if(tangent.magnitude()<AMG3D_PHYSICS_EPSILON) {
AMG3DVector4 tangent1, tangent2;
normal.buildOrthoBasis(&tangent1, &tangent2);
tangent = tangent1 + tangent2;
tangent.normalize();
}
AMG3DVector4 rapcrosst = rap.cross(tangent);
AMG3DVector4 rbpcrosst = rbp.cross(tangent);
// Compute the tangent mass
AMG3DScalar Kt = m_pRigidBody1->invMass + m_pRigidBody2->invMass;
Kt += ( (m_pRigidBody1->invIWorld*rapcrosst).cross(rap) + (m_pRigidBody2->invIWorld*rbpcrosst).cross(rbp) ).dot(tangent);
m_cdContactData.contacts[i].massTangent = 1.0f/Kt;
// Compute the bias (prevents sinking, useful for stacking)
m_cdContactData.contacts[i].bias = k_biasFactor / dt * Max(0.0f, m_cdContactData.contacts[i].fPenetrationDepth - k_allowedPenetration);
if(AMG3DPhysicsWorld::g_AMG3D_bAccumulateImpulses) {
AMG3DVector4 P = m_cdContactData.contacts[i].Pn*normal + m_cdContactData.contacts[i].Pt*tangent;
m_pRigidBody1->velocity += P*m_pRigidBody1->invMass;
m_pRigidBody1->angularVelocity += m_pRigidBody1->invIWorld*rap.cross(P);
m_pRigidBody2->velocity -= P*m_pRigidBody2->invMass;
m_pRigidBody2->angularVelocity -= m_pRigidBody2->invIWorld*rbp.cross(P);
}
}
}
void AMG3DVector4::buildOrthoBasis(AMG3DVector4 *pTangent1, AMG3DVector4 *pTangent2)
{
// Spherical coords
// x = r*sin(theta)*sin(phi)
// y = r*cos(phi)
// z = r*cos(theta)*sin(phi)
// r = 1 (Normalized vector)
// Where 0<=theta<=2PI and 0<=phi<=PI
// For the following original basis vectors:
// [ 1 0 0 ] theta=PI/2 and phi=PI/2
// [ 0 1 0 ] theta=0->2PI and phi=0
// [ 0 0 1 ] theta=0,2PI and phi=PI/2
//AMG3DScalar phi = acosf(y);
//AMG3DScalar div = z/sinf(phi);
//AMG3DScalar theta = (phi==0.0f || phi==AMG3D_PI) ? theta=0.0f : acosf(z/sinf(phi));
//
//phi -= AMG3D_PI/2;
//theta -= AMG3D_PI/2;
//(*pTangent1) = AMG3DVector4(sinf(theta)*sinf(phi), cosf(phi), cosf(theta)*sinf(phi));
//AMG3DVector4Cross(pTangent2, *pTangent1, *this);
AMG3DPlane XY(AMG3DVector4(0,0,1), AMG3DVector4(0,0,0), 0, 0),
YZ(AMG3DVector4(1,0,0), AMG3DVector4(0,0,0), 0, 0),
XZ(AMG3DVector4(0,1,0), AMG3DVector4(0,0,0), 0, 0);
AMG3DVector4 v = *this;
AMG3DScalar vdotz = fabsf(v.dot(XY.m_vNormal)),
vdotx = fabsf(v.dot(YZ.m_vNormal)),
vdoty = fabsf(v.dot(XZ.m_vNormal));
if(vdotz<AMG3D_MATH_DOT_PRODUCT_EPSILON) { // Vector is in the XY plane
(*pTangent1) = XY.m_vNormal;
}
else if(vdotx<AMG3D_MATH_DOT_PRODUCT_EPSILON) { // Vector is in the YZ plane
(*pTangent1) = YZ.m_vNormal;
}
else if(vdoty<AMG3D_MATH_DOT_PRODUCT_EPSILON) { // Vector is in the XZ plane
(*pTangent1) = XZ.m_vNormal;
}
else { // If vector is in neither plane, it has three non-zero components
if(v.z>0.0f)
(*pTangent1) = AMG3DVector4(-v.y, v.x, 0);
else
(*pTangent1) = AMG3DVector4(v.y, -v.x, 0);
}
AMG3DVector4Cross(pTangent2, v, *pTangent1);
(*pTangent2).normalize();
}