Jump to content
  • Advertisement
isu diss

Add a constraint to the physics system

Recommended Posts

Advertisement

I'm not using any sdk or engine. I'm creating one for my game. It's very basic.

//RIGIDBODY.CPP

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);
	IInverse = XMMatrixIdentity();
	R = XMMatrixIdentity();
	Omega = XMVector4Transform(L0, I0_Inverse);
	q = XMQuaternionRotationMatrix(R0);
	F_Total = XMVectorSet(0, 0, 0, 0);
	Tau_Total = XMVectorSet(0, 0, 0, 0);
}

float RigidBody::GetMass()
{
	return Mass;
}

XMMATRIX RigidBody::GetIInverse()
{
	return IInverse;
}

void RigidBody::SetPosition(XMVECTOR Pos)
{
	x = Pos;
}

XMVECTOR RigidBody::GetPosition()
{
	return x;
}

XMMATRIX RigidBody::GetOrientation()
{
	return R;
}

XMVECTOR RigidBody::GetVelocityAtPoint(XMVECTOR p)
{
	return (v + XMVector3Cross(Omega, (p - x)));
}

void RigidBody::AddForce(XMVECTOR Force)
{
	F_Total += Force;
}

void RigidBody::AddImpulse(XMVECTOR Impulse)
{
	v += Impulse*(1/Mass);
}

void RigidBody::AddTorque(XMVECTOR Torque)
{
	Tau_Total += Torque;
}

void RigidBody::AddImpulsiveTorque(XMVECTOR ImpulsiveTorque)
{
	Omega += XMVector4Transform(ImpulsiveTorque, IInverse);
}

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;
	q = XMQuaternionNormalize(q);
	R = XMMatrixRotationQuaternion(q);
	IInverse = XMMatrixMultiply(XMMatrixTranspose(R), IBodyInverse);
	IInverse = XMMatrixMultiply(IInverse, R);
	Omega = XMVector4Transform(L, IInverse);
	F_Total = XMVectorSet(0, 0, 0, 0);
	Tau_Total = XMVectorSet(0, 0, 0, 0);
}

// INITIALIZATION

float I_tmp1 = ((CS_Mass*CS_Height*CS_Height)/12)+((CS_Mass*CS_Radius*CS_Radius)/4);
float I_tmp2 = (CS_Mass*CS_Radius*CS_Radius)/2;
					XMMATRIX IBody = XMMatrixSet(I_tmp1,	0,		0,		 0,
													 0,		I_tmp1,	0,		 0,
													 0,		0,		I_tmp2,	 0,
													 0,		0,		0,		 1);
rbStump1 = new RigidBody(CS_Mass, IBody, XMVectorSet(5, 29, 0, 1), XMVectorSet(0, 0, 0, 0), XMVectorSet(0, 0, 0, 0), XMMatrixRotationX(XM_PIDIV2));
hr = rbStump1 ? S_OK: S_FALSE;
if (FAILED(hr))
	return hr;

cdStump1 = new AABB(XMFLOAT3(0, 0, 0), XMFLOAT3(3, 3, 41.25f));
hr = cdStump1 ? S_OK: S_FALSE;
if (FAILED(hr))
	return hr;

I_tmp = (0.4f)*(CB_Mass*(CB_Radius*CB_Radius));
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(5, 70.0f, -375, 1), XMVectorSet(0, -5, 45.0f, 0), XMVectorSet(-8e-4f, 0, 0, 0), XMMatrixRotationZ(XM_PIDIV2));
hr = rbBall ? S_OK: S_FALSE;
if (FAILED(hr))
	return hr;

// COLLISION RESOLUTION
void CollisionResponse(Contact *c, float epsilon)
{
	XMVECTOR padot = c->a->GetVelocityAtPoint(c->p);
	XMVECTOR pbdot = c->b->GetVelocityAtPoint(c->p);
	XMVECTOR n = c->n;
	XMVECTOR ra = (c->p - c->a->GetPosition());
	XMVECTOR rb = (c->p - c->b->GetPosition());
	XMVECTOR vrel = XMVector3Dot(c->n, (padot - pbdot));
 
	float numerator = (-(1.0f + epsilon)*vrel.m128_f32[0]);
	float term1 = (1.0f / c->a->GetMass());
	float term2 = (1.0f / c->b->GetMass());
	XMVECTOR term3 = XMVector3Dot(c->n, XMVector3Cross(XMVector4Transform(XMVector3Cross(ra, n), c->a->GetIInverse()), ra));
	XMVECTOR term4 = XMVector3Dot(c->n, XMVector3Cross(XMVector4Transform(XMVector3Cross(rb, n), c->b->GetIInverse()), rb));
	float j = (numerator / (term1+ term2 + term3.m128_f32[0] + term4.m128_f32[0]));
	XMVECTOR f = (j*n);
	c->a->AddImpulse(f);
	c->b->AddImpulse(-f);
	c->a->AddImpulsiveTorque(XMVector3Cross(ra, f));
	c->b->AddImpulsiveTorque(-XMVector3Cross(rb, f));
}

//RENDERING CODE
mWorld = XMMatrixMultiply(rbBall->GetOrientation(), XMMatrixTranslation(rbBall->GetPosition().m128_f32[0], rbBall->GetPosition().m128_f32[1], rbBall->GetPosition().m128_f32[2]));

 

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!