Jump to content
  • Advertisement
isu diss

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

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

 

Edited by isu diss

Share this post


Link to post
Share on other sites
Advertisement

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 this post


Link to post
Share on other sites

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

float CB_Mass = 0.156f;//kg
float CB_Radius = 0.36f;//0.036f;//m
float g = 9.83f; //ms-2
float I_tmp = (0.4f)*(CB_Mass*(CB_Radius*CB_Radius));
		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 this post


Link to post
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 this post


Link to post
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 this post


Link to post
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 this post


Link to post
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 this post


Link to post
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;
		m_radius = 2.0f;
		
		float32 s = 0.4f * m_mass * m_radius * m_radius;
		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();
		s.m_radius = m_radius;

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

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

	float32 m_radius;

	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 this post


Link to post
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 this post


Link to post
Share on other sites

Create an account or sign in to comment

You need to be a member in order to leave a comment

Create an account

Sign up for a new account in our community. It's easy!

Register a new account

Sign in

Already have an account? Sign in here.

Sign In Now

  • Advertisement
×

Important Information

By using GameDev.net, you agree to our community Guidelines, Terms of Use, and Privacy Policy.

GameDev.net is your game development community. Create an account for your GameDev Portfolio and participate in the largest developer community in the games industry.

Sign me up!