# 3D Rigidbody Simulation

## Recommended Posts

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;
}

{
F_Total += Force;
}

{
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...

Edited by isu diss

##### Share on other sites

It seems that your quaternion derivative is

q_dot = 0.5 * q * q_omega

where

q_omega = Quaternion(omega.x, omega.y, omega.z, 0)

This is wrong if you're using a global angular velocity.

The time derivative of an orientation given the angular velocity of the rotation frame represented by the orientation is

q_dot = 0.5 * q_omega * q

Then to integrate the current orientation q over a time step dt you use the formula

q = q + dt * q_dot

For a local angular velocity the time derivative is

q_dot = 0.5 * q * omega_q

However, you can forget local integration since is not used much in rigid body dynamics. It's used in multibody dynamics (e.g. Featherstone).

If this doesn't solve your problem I would check the transforms.

Edit:

Oops... I just checked XMQuaternionMultiply and it seems that it returns q2 * q1 instead of q1 * q2. So I think there is no problem with your quaternion integration. I would check the matrices.

Edited by Irlan Robson

##### Share on other sites

Rigidbody, Ball is initialized like this. These are the parameters.

float CB_Mass = 0.156f;//kg
float g = 9.83f; //ms-2
XMMATRIX IBody = XMMatrixSet(I_tmp,		0,		0,		 0,
0,		I_tmp,	0,		 0,
0,		0,		I_tmp,	 0,
0,		0,		0,		 1);

rbBall = new RigidBody(CB_Mass, IBody, XMVectorSet(-100.0f, 70.0f, 0, 1), XMVectorSet(0, 0, 30.0f, 0), XMVectorSet(8e-3f, 0, 0, 0), XMMatrixRotationZ(XM_PIDIV2));


##### Share on other sites

I see no problems with your initialization.

The orientation quaternion can become non-unit over time due to round-off errors. Therefore, you should normalize the quaternion just after you've integrated it and keep track of this. E.g.

q += dt * 0.5 * omega * q;
q = XMQuaternionNormalize(q);

Note the function XMQuaternionNormalize only returns the normalized quaternion of the given quaternion.

Then you can convert the quaternion to its corresponding 3-by-3 matrix using XMMatrixRotationQuaternion.

R = XMMatrixRotationQuaternion(q);

The quaternion-to-matrix conversion functions and vice-versa are already implemented in DirectXMath. The functions are XMMatrixRotationQuaternion and XMQuaternionRotationMatrix.

##### Share on other sites

Thanks @Irlan Robson I changed quat2matrix and matrixtoquat functions with the ones provided by directxmath. The result is still the same, the ball spins around the world x-axis. I don't know what to do.

##### Share on other sites

IIRC in DirectXMath quaternion multiplication order is consistent with matrix multiplication order. Maybe you can try changing the quaternion derivative to

XMVECTOR qOmega = XMQuaternionMultiply(XMVectorSet(Omega.m128_f32[0], Omega.m128_f32[1], Omega.m128_f32[2], 0), q);﻿

Also check the shaders. Possibly it's a small problem with the transforms.

##### Share on other sites

I checked the shaders, they are just fine. Can you please create a opengl or d3d project using my sourcecode? You might be able to see the problem. Because I have no idea. I hope I'm not asking too much.

##### Share on other sites

Your test with a couple of changes in the initialization works completely fine in my framework. The ball spins around the body x-axis. Here are the changes.

Increase the sphere mass and radius

Set the position to (0, 10, 0)

Set the linear velocity to (0, 0, 0)

Set the angular momentum to (10, 0, 0)

I use the standard multiplication convention (as OpenGL). You would swap around the order in DirectXMath.  Here's the source code:

class IsuDiss : public Test
{
public:
IsuDiss()
{
m_mass = 1.0f;

b3Mat33 I = b3Diagonal(s);
m_invI = b3Inverse(I);

m_f.SetZero();
m_torque.SetZero();

m_x.Set(0.0f, 10.0f, 0.0f);
m_v.SetZero();

m_q.Set(b3Vec3_z, 0.5f * B3_PI);

m_L.Set(10.0f, 0.0f, 0.0f);

m_xf.rotation = b3QuatMat33(m_q);
m_xf.position = m_x;

m_worldInvI = m_xf.rotation * m_invI * b3Transpose(m_xf.rotation);
m_omega = m_worldInvI * m_L;
}

~IsuDiss()
{

}

void Step() override
{
float32 h = g_testSettings->inv_hertz;

m_x += h * m_v;
m_v += h * (m_f / m_mass);

b3Quat w(m_omega.x, m_omega.y, m_omega.z, 0.0f);
m_q += h * 0.5f * w * m_q;
m_q.Normalize();

m_L += h * m_torque;

m_xf.position = m_x;
m_xf.rotation = b3QuatMat33(m_q);

m_worldInvI = m_xf.rotation * m_invI * b3Transpose(m_xf.rotation);
m_omega = m_worldInvI * m_L;

b3SphereShape s;
s.m_center.SetZero();

g_draw->DrawSolidSphere(&s, b3Color_pink, m_xf);
}

static Test* Create()
{
return new IsuDiss();
}

float32 m_mass;
b3Mat33 m_invI;

b3Vec3 m_x;
b3Vec3 m_v;
b3Vec3 m_f;

b3Quat m_q;
b3Vec3 m_omega;
b3Vec3 m_torque;
b3Vec3 m_L;

b3Mat33 m_worldInvI;

b3Transform m_xf;
};

##### Share on other sites

I'm sorry for repeated inquiries. I couldn't get my code to work, I did everything that you said. still the result is same. Anyway, I'm happy that my code works in your environment. Thanks for your support.

##### Share on other sites

My code finally works!!! I'm so happy. Not only I changed the radius of the rigidbody ball, I also changed the radius of the ball's 3D model. Thank you @Irlan Robson.

## Create an account

Register a new account

• ### Game Developer Survey

We are looking for qualified game developers to participate in a 10-minute online survey. Qualified participants will be offered a \$15 incentive for your time and insights. Click here to start!

• 12
• 14
• 10
• 33
• 23