I started to build a physics engine using "An Introduction to Physically Based Modeling" http://www.cs.cmu.edu/afs/cs/user/baraff/www/pbm/pbm.html by by the authors (Andrew Witkin, David Baraff and Michael Kass). This physics engine is used in a cricket simulation. So far I'm trying to simulate the cricket ball. The problem is, the ball should spin around its body space x- axis, given the angular momentum around body space x-axis but it only spins around the world space x-axis. How do I change the coordinate axis?
Rigidbody dynamics code
RigidBody::RigidBody(float M, XMMATRIX IBody, XMVECTOR x0, XMVECTOR v0, XMVECTOR L0, XMMATRIX R0)
{
Mass = M;
IBodyInverse = XMMatrixInverse(NULL, IBody);
x = x0;
v = v0;
L = L0;
I0_Inverse = XMMatrixMultiply(XMMatrixTranspose(R0), IBodyInverse);
I0_Inverse = XMMatrixMultiply(I0_Inverse, R0);
Omega = XMVector4Transform(L0, I0_Inverse);
q = MatrixToQuaternion(R0);
F_Total = XMVectorSet(0, 0, 0, 0);
Tau_Total = XMVectorSet(0, 0, 0, 0);
}
XMMATRIX RigidBody::QuaternionToMatrix(XMVECTOR q)
{
float vx = q.m128_f32[0];
float vy = q.m128_f32[1];
float vz = q.m128_f32[2];
float s = q.m128_f32[3];
return XMMatrixSet( (1 - ((2*vy*vy) + (2*vz*vz))), ((2*vx*vy) + (2*s*vz)), ((2*vx*vz) - (2*s*vy)), 0,
((2*vx*vy) - (2*s*vz)), (1 - ((2*vx*vx) + (2*vz*vz))), ((2*vy*vz) + (2*s*vx)), 0,
((2*vx*vz) + (2*s*vy)), ((2*vy*vz) - (2*s*vx)), (1 - ((2*vx*vx) + (2*vy*vy))), 0,
0, 0, 0, 1);
}
XMVECTOR RigidBody::MatrixToQuaternion(XMMATRIX m)
{
XMVECTOR q_tmp;
float tr, s;
tr = m.r[0].m128_f32[0] + m.r[1].m128_f32[1] + m.r[2].m128_f32[2];
if (tr >= 0)
{
s = sqrt(tr + 1);
q_tmp.m128_f32[3] = 0.5f*s;
s = 0.5f/s;
q_tmp.m128_f32[0] = (m.r[2].m128_f32[1] - m.r[1].m128_f32[2])*s;
q_tmp.m128_f32[1] = (m.r[0].m128_f32[2] - m.r[2].m128_f32[0])*s;
q_tmp.m128_f32[2] = (m.r[1].m128_f32[0] - m.r[0].m128_f32[1])*s;
}
else
{
int i = 0;
if (m.r[1].m128_f32[1] > m.r[0].m128_f32[0]) i = 1;
if (m.r[2].m128_f32[2] > m.r[i].m128_f32[i]) i = 2;
switch(i)
{
case 0:
{
s = sqrt((m.r[0].m128_f32[0] - (m.r[1].m128_f32[1] + m.r[2].m128_f32[2])) + 1);
q_tmp.m128_f32[0] = 0.5f*s;
s = 0.5f/s;
q_tmp.m128_f32[1] = (m.r[0].m128_f32[1] + m.r[1].m128_f32[0])*s;
q_tmp.m128_f32[2] = (m.r[2].m128_f32[0] + m.r[0].m128_f32[2])*s;
q_tmp.m128_f32[3] = (m.r[2].m128_f32[1] - m.r[1].m128_f32[2])*s;
break;
}
case 1:
{
s = sqrt((m.r[1].m128_f32[1] - (m.r[2].m128_f32[2] + m.r[0].m128_f32[0])) + 1);
q_tmp.m128_f32[1] = 0.5f*s;
s = 0.5f/s;
q_tmp.m128_f32[2] = (m.r[1].m128_f32[2] + m.r[2].m128_f32[1])*s;
q_tmp.m128_f32[0] = (m.r[0].m128_f32[1] + m.r[1].m128_f32[0])*s;
q_tmp.m128_f32[3] = (m.r[0].m128_f32[2] - m.r[2].m128_f32[0])*s;
break;
}
case 2:
{
s = sqrt((m.r[2].m128_f32[2] - (m.r[0].m128_f32[0] + m.r[1].m128_f32[1])) + 1);
q_tmp.m128_f32[2] = 0.5f*s;
s = 0.5f/s;
q_tmp.m128_f32[0] = (m.r[2].m128_f32[0] + m.r[0].m128_f32[2])*s;
q_tmp.m128_f32[1] = (m.r[1].m128_f32[2] + m.r[2].m128_f32[1])*s;
q_tmp.m128_f32[3] = (m.r[1].m128_f32[0] - m.r[0].m128_f32[1])*s;
break;
}
}
}
return q_tmp;
}
XMVECTOR RigidBody::GetPosition()
{
return x;
}
XMMATRIX RigidBody::GetOrientation()
{
return R;
}
void RigidBody::AddForce(XMVECTOR Force)
{
F_Total += Force;
}
void RigidBody::AddTorque(XMVECTOR Torque)
{
Tau_Total += Torque;
}
void RigidBody::Update(float h)
{
x += h*v;
v += h*(F_Total/Mass);
XMVECTOR Omegaq = XMQuaternionMultiply(q, XMVectorSet(Omega.m128_f32[0], Omega.m128_f32[1], Omega.m128_f32[2], 0));
q += 0.5f*h*Omegaq;
L += h*Tau_Total;
R = QuaternionToMatrix(XMQuaternionNormalize(q));
IInverse = XMMatrixMultiply(XMMatrixTranspose(R), IBodyInverse);
IInverse = XMMatrixMultiply(IInverse, R);
Omega = XMVector4Transform(L, IInverse);
}
Rendering code
mgWorld = XMMatrixMultiply(rbBall->GetOrientation(), XMMatrixTranslation(rbBall->GetPosition().m128_f32[0], rbBall->GetPosition().m128_f32[1], rbBall->GetPosition().m128_f32[2]));
cuvscb.mWorld = XMMatrixTranspose( mgWorld );
cuvscb.mView = XMMatrixTranspose( mgView );
cuvscb.mProjection = XMMatrixTranspose( mgProjection );
cuvscb.mLightView = XMMatrixTranspose(mgLightView);
cuvscb.mLightProjection = XMMatrixTranspose(mgLightProjection);
cuvscb.vCamPos = XMFLOAT3(MyCAM->GetEye().m128_f32[0], MyCAM->GetEye().m128_f32[1], MyCAM->GetEye().m128_f32[2]);
cuvscb.vSun = XMFLOAT3(cos(Theta)*cos(Phi), sin(Theta), cos(Theta)*sin(Phi));
cuvscb.MieCoeffs = MC;
cuvscb.RayleighCoeffs = RC;
pImmediateContext->UpdateSubresource( pVSCUConstBuffer, 0, NULL, &cuvscb, 0, 0 );
pImmediateContext->VSSetConstantBuffers(0, 1, &pVSCUConstBuffer);
Draw code...