3D Rigidbody Simulation

Started by
15 comments, last by jalex 4 years, 8 months ago

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

 

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.

 

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

 

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. 

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.

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.

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.

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

 

 

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.

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.

This topic is closed to new replies.

Advertisement