I have used AABB for collision detection. Is that the problem? Back few versions ago I had rotations. After I changed the code(porting Box2D to 3D), I lost the rotations.
void RigidBody::UpdateAABB()
{
float C[3], R[3];
float Center[3], Radius[3];
if (boxupdated)
{
delete boxupdated;
boxupdated = NULL;
}
float Orientation[3][3], Translation[3];
for (int i=0; i<3; i++)
{
for (int j=0; j<3; j++)
{
Orientation[i][j] = GetOrientation().r[i].m128_f32[j];
}
}
Translation[0] = GetPosition().m128_f32[0];
Translation[1] = GetPosition().m128_f32[1];
Translation[2] = GetPosition().m128_f32[2];
Center[0] = box->GetCenter().x;
Center[1] = box->GetCenter().y;
Center[2] = box->GetCenter().z;
Radius[0] = box->GetRadius().x;
Radius[1] = box->GetRadius().y;
Radius[2] = box->GetRadius().z;
for (int i=0; i<3; i++)
{
C[i] = Translation[i];
R[i] = 0.0f;
for (int j=0; j<3; j++)
{
C[i] += Orientation[i][j] * Center[j];
R[i] += abs(Orientation[i][j]) * Radius[j];
}
}
boxupdated = new AABB(XMFLOAT3(C[0],C[1],C[2]), XMFLOAT3(R[0], R[1], R[2]));
}
But I used above code for constructing AABB when the objects rotate.
void Arbiter::PreStep(float inv_dt)
{
XMVECTOR tangent[2];
for (int i = 0; i < numContacts; ++i)
{
Contact* c = contacts + i;
XMVECTOR p = c->position;
XMVECTOR padot = body1->GetVelocityAtPoint(p);
XMVECTOR pbdot = body2->GetVelocityAtPoint(p);
XMVECTOR ra = (p - body1->GetPosition());
XMVECTOR rb = (p - body2->GetPosition());
XMVECTOR n = c->normal;
float C = min(0.0f, c->separation + 0.01f);
c->bias = -inv_dt * C *.2f;
XMVECTOR dv = -padot +pbdot;
float Cdot = XMVector3Dot(dv, n).m128_f32[0];
if (Cdot < -1.0f)
{
c->bias += -restituition * Cdot;
}
float term1 = body1->GetMass()>0.0f ? (1.0f / body1->GetMass()) : 0.0f;
float term2 = body2->GetMass()>0.0f ? (1.0f / body2->GetMass()) : 0.0f;
XMVECTOR rnA = XMVector3Cross(ra, n);
XMVECTOR rnB = XMVector3Cross(rb, n);
float K = term1 + term2 + XMVector3Dot(rnA, XMVector4Transform(rnA, body1->GetIInverse())).m128_f32[0] + XMVector3Dot(rnB, XMVector4Transform(rnB, body2->GetIInverse())).m128_f32[0];
c->massNormal = K > 0.0f ? 1.0f / K : 0.0f;
tangent[0] = XMVector3Orthogonal(n);
tangent[1] = XMVector3Cross(tangent[0], n);;
padot = body1->GetVelocityAtPoint(p);
pbdot = body2->GetVelocityAtPoint(p);
ra = (p - body1->GetPosition());
rb = (p - body2->GetPosition());
XMVECTOR rt1A = XMVector3Cross(ra, tangent[0]);
XMVECTOR rt1B = XMVector3Cross(rb, tangent[0]);
XMVECTOR rt2A = XMVector3Cross(ra, tangent[1]);
XMVECTOR rt2B = XMVector3Cross(rb, tangent[1]);
float K1 = term1 + term2 + XMVector3Dot(rt1A, XMVector4Transform(rt1A, body1->GetIInverse())).m128_f32[0] + XMVector3Dot(rt1B, XMVector4Transform(rt1B, body2->GetIInverse())).m128_f32[0];
float K2 = term1 + term2 + XMVector3Dot(rt2A, XMVector4Transform(rt2A, body1->GetIInverse())).m128_f32[0] + XMVector3Dot(rt2B, XMVector4Transform(rt2B, body2->GetIInverse())).m128_f32[0];
c->massTangent[0] = K1 > 0.0f ? 1.0f / K1 : 0.0f;
c->massTangent[1] = K2 > 0.0f ? 1.0f / K2 : 0.0f;
XMVECTOR P = c->Pn * n + c->Pt[0] * tangent[0] + c->Pt[1] * tangent[1];
body1->AddImpulse(-P);
body2->AddImpulse(P);
body1->AddImpulsiveTorque(-XMVector3Cross(ra, P));
body2->AddImpulsiveTorque(XMVector3Cross(rb, P));
}
}
float Clamp(float a, float low, float high)
{
return max(low, min(a, high));
}
void Arbiter::ApplyImpulse()
{
XMVECTOR tangent[2];
for (int i = 0; i < numContacts; ++i)
{
Contact* c = contacts + i;
XMVECTOR p = c->position;
XMVECTOR padot = body1->GetVelocityAtPoint(p);
XMVECTOR pbdot = body2->GetVelocityAtPoint(p);
XMVECTOR ra = (p - body1->GetPosition());
XMVECTOR rb = (p - body2->GetPosition());
XMVECTOR n = c->normal;
XMVECTOR dv = -padot + pbdot;
float Cdot = XMVector3Dot(dv, n).m128_f32[0];
float dPn = c->massNormal * (-Cdot + c->bias);
float Pn0 = c->Pn;
c->Pn = max(Pn0 + dPn, 0.0f);
dPn = c->Pn - Pn0;
XMVECTOR P = dPn * n;
body1->AddImpulse(-P);
body2->AddImpulse(P);
body1->AddImpulsiveTorque(-XMVector3Cross(ra, P));
body2->AddImpulsiveTorque(XMVector3Cross(rb, P));
p = c->position;
padot = body1->GetVelocityAtPoint(p);
pbdot = body2->GetVelocityAtPoint(p);
n = c->normal;
ra = (p - body1->GetPosition());
rb = (p - body2->GetPosition());
dv = -padot +pbdot;
tangent[0] = XMVector3Orthogonal(n);
tangent[1] = XMVector3Cross(tangent[0], n);
for (int z=0;z<2;z++)
{
float vt = XMVector3Dot(dv, tangent[z]).m128_f32[0];
float dPt = c->massTangent[z] * (-vt);
float maxPt = friction * c->Pn;
float oldTangentImpulse = c->Pt[z];
c->Pt[z] = Clamp(oldTangentImpulse + dPt, -maxPt, maxPt);
dPt = c->Pt[z] - oldTangentImpulse;
XMVECTOR Pt = dPt * tangent[z];
body1->AddImpulse(-Pt);
body2->AddImpulse(Pt);
body1->AddImpulsiveTorque(-XMVector3Cross(ra, Pt));
body2->AddImpulsiveTorque(XMVector3Cross(rb, Pt));
}
}
}
stacking, bounciness, friction are all there. rotations aren't.
current sim<---- check the simulation (w/o friction)