Rigid Body Physics (OpenGL)

I am trying to build rigid body physics engine. I am using the glm library as my standard math library.
The problems I am facing are as follows,

[ul]
[li]When I calculate torque with respect to force (see function calculateTorque()), the body gets distorted and skewed to the extent, where it rotates and increases in size and is not visible anymore. But when I remove the force, from the torque equation that is I just randomize (see calculateForces())the torque values it works fine.[/li][li]When rotating, I think the body is skewing and increasing in size slightly, for both quaternions and matrices. (Not sure if the inertia calculation is right, the body is a cube)[/li][li]Am also having problems with the collision response, the body gradually penetrates the surface, and it does not react properly with respect to the value of epsilon.[/li][/ul]
I cant figure out what is wrong, I really want this to work. I have been breaking my head for a week now, can’t seem to pin point what the problem is exactly. If somebody figures something out, or notices something please let me know. It would really be appreciated. Always helps when more than one person is looking at it. Thank you, much appreciated.
P.S: The AABB - Plane collision is implemented with the help of the book Real Time Collision Detection by Christer Ericson, and the Rigid Body physics is implemented with the help of Braff’s implementation.

Here is my code.


RigidBody::RigidBody(void)
{
}


RigidBody::RigidBody(float height, float width, float depth, int n)
	: height(height), width(width), depth(depth), nrb(n)
{
	objPos = new glm::vec3[n];
	worldPos = new glm::vec3[n];
	initRB();
}


RigidBody::~RigidBody(void)
{
}


void RigidBody::initRB()
{
	dt = 0.01;


	// Mass
	m = 0.5;
	float m_inverse = 1/m;


	// Inertia
	IBody = glm::mat3();
	IWorld = glm::mat3();


	IBody[0][0] = 1.0 / 12.0 * (m * ((height * height) + (depth * depth)));
	IBody[1][1] = 1.0 / 12.0 * (m * ((width * width) + (depth * depth)));
	IBody[2][2] = 1.0 / 12.0 * (m * ((width * width) + (height * height)));


	IBody_inverse = glm::inverse(IBody);


	// CG
	x = glm::vec3(0.0, 0.0, 0.0);


	// Linear Momentum
	P = glm::vec3(0.0, 0.0, 0.0);


	// Linear Velocity
	v = P * m_inverse;


	// Orientation (Matrix)
	R = glm::mat3();


	// Orientation (Quaternion)
	QR = glm::fquat();


	// Use Quaternion Flag
	useQuat = true;


	// Angular Velocity
	w = glm::vec3(1.0, 0.0, 1.0);


	// Angular Momentum
	L = IBody * w;


	// Forces
	rbPhys.gravitational = 9.81;
	rbPhys.viscousdrag = 0.1;
	f = glm::vec3(0.0, -m * rbPhys.gravitational, 0.0);


	// Torque
	t = glm::vec3(0.0, 0.0, 0.0);


	// Collision Flag
	collided = false;
}


void RigidBody::setBodyPos(glm::vec3 *verts)
{
	for(int i=0; i<nrb; i++)
	{
		objPos[i] = verts[i];
	}
}


void RigidBody::calculateBodyToWorld()
{
	for(int i=0; i<nrb; i++){
		worldPos[i] = R * objPos[i] + x;
	}
}


void RigidBody::calculateAABB()
{
	aabb.FindMaxMin(worldPos, nrb);
	max = aabb.GetMaxVertices();
	min = aabb.GetMinVertices();
}


glm::mat3 RigidBody::makeSkewSymmetric(glm::vec3 v)
{
	glm::mat3 result;


	result = glm::mat3(0.0, -v.z, v.y,
					   v.z, 0.0, -v.x,
					   -v.y, v.x, 0.0);


	return result;
}


glm::mat3 RigidBody::orthonormalize(glm::mat3 r)
{
	glm::mat3 result;


	glm::vec3 v1 = glm::vec3(r[0][0], r[1][0], r[2][0]);
	glm::vec3 v2 = glm::vec3(r[0][1], r[1][1], r[2][1]);
	glm::vec3 v3 = glm::vec3(r[0][2], r[1][2], r[2][2]);


	glm::normalize(v1);
	v2 = glm::cross(v3, v1);
	glm::normalize(v2);
	v3 = glm::cross(v1, v2);
	glm::normalize(v3);


	result[0][0] = v1.x;	result[0][1] = v2.x;	result[0][2] = v3.x;
	result[1][0] = v1.y;	result[1][1] = v2.y;	result[1][2] = v3.y;
	result[2][0] = v1.z;	result[2][1] = v2.z;	result[2][2] = v3.z;


	return result;
}


void RigidBody::computeAuxillary()
{
	// Linear Velocity
	float m_inverse = 1/m;
	v = P * m_inverse;


	// Inertia Tensor (World)
	IWorld = R * IBody * glm::transpose(R);
	IWorld_inverse = R * IBody_inverse * glm::transpose(R);		
	
	// Angular Velocity
	w = IWorld_inverse * L;	
	
	if(useQuat)
	{
		// Store quaternion in Rotation Matrix
		R = glm::mat3_cast(QR);
	}
}


void RigidBody::calculateForces()
{
	// sine torque to get some spinning action


	t.x = 1.0 * sin(dt*0.9 + 0.5);
	t.y = 1.1 * sin(dt*0.5 + 0.4);
	t.z = 1.2 * sin(dt*0.7 + 0.9);


	// damping torque so we dont spin too fast


	t.x -= 0.2 * w.x;
	t.y -= 0.2 * w.y;
	t.z -= 0.2 * w.z;
}


void RigidBody::calculateTorque()
{
	// Torque about a point p acted on by a force f
	// r = cg + R * p, here p is taken for granted as the center of the cube
	glm::vec3 r = glm::vec3(0.0, 0.0, 0.0);
	glm::vec3 p = glm::vec3(0.0, 0.5 * height, 0.0);
	r = x + (R * p);
	t = glm::cross((r - x),f);
}


void RigidBody::resetForces()
{
	f = glm::vec3(0.0, -m * rbPhys.gravitational, 0.0);
	t = glm::vec3(0.0, 0.0, 0.0);
}


void RigidBody::calculateDerivatives()
{
	rbDerivative.dvdt.x = v.x;
	rbDerivative.dvdt.y = v.y;
	rbDerivative.dvdt.z = v.z;


	if(useQuat)
	{
		glm::fquat wQ;
		wQ.w = 0;
		wQ.x = w.x;
		wQ.y = w.y;
		wQ.z = w.z;
		rbDerivative.dQRdt = glm::normalize(wQ * QR);
	}
	else
	{
		glm::mat3 wR = makeSkewSymmetric(w);
		rbDerivative.dRdt = wR * R;
	}


	rbDerivative.dPdt.x = f.x;
	rbDerivative.dPdt.y = f.y;
	rbDerivative.dPdt.z = f.z;


	rbDerivative.dLdt.x = t.x;
	rbDerivative.dLdt.y = t.y;
	rbDerivative.dLdt.z = t.z;
}


void RigidBody::updateRB()
{
	calculateForces();
	calculateDerivatives();


	//x.x += rbDerivative.dvdt.x * dt;
	//x.y += rbDerivative.dvdt.y * dt;
	//x.z += rbDerivative.dvdt.z * dt;
	
	if(useQuat)
	{
		QR = QR + rbDerivative.dQRdt * dt;
	}
	else
	{
		R += rbDerivative.dRdt * dt;
		//R = orthonormalize(R); orthornormalize wrong
	}


	P.x += rbDerivative.dPdt.x * dt;
	P.y += rbDerivative.dPdt.y * dt;
	P.z += rbDerivative.dPdt.z * dt;


	L.x += rbDerivative.dLdt.x * dt;
	L.y += rbDerivative.dLdt.y * dt;
	L.z += rbDerivative.dLdt.z * dt;


	computeAuxillary();
	
	calculateAABB();
}


void RigidBody::renderRB()
{
	calculateBodyToWorld();


	glPushMatrix();
		glColor3f(0.0, 0.0, 0.0);
		glBegin(GL_LINES);
			glVertex3f(max[0], max[1], max[2]);
			glVertex3f(max[0], min[1], max[2]);


			glVertex3f(max[0], max[1], max[2]);
			glVertex3f(min[0], max[1], max[2]);	


			glVertex3f(max[0], max[1], max[2]);
			glVertex3f(max[0], max[1], min[2]);


			glVertex3f(max[0], min[1], max[2]);
			glVertex3f(max[0], min[1], min[2]);


			glVertex3f(max[0], min[1], min[2]);
			glVertex3f(max[0], max[1], min[2]);


			glVertex3f(max[0], min[1], max[2]);
			glVertex3f(min[0], min[1], max[2]);


			glVertex3f(min[0], min[1], max[2]);
			glVertex3f(min[0], max[1], max[2]);


			glVertex3f(min[0], min[1], min[2]);
			glVertex3f(min[0], max[1], min[2]);


			glVertex3f(min[0], min[1], min[2]);
			glVertex3f(min[0], min[1], max[2]);	


			glVertex3f(min[0], min[1], min[2]);
			glVertex3f(max[0], min[1], min[2]);


			glVertex3f(min[0], max[1], min[2]);
			glVertex3f(max[0], max[1], min[2]);


			glVertex3f(min[0], max[1], min[2]);
			glVertex3f(min[0], max[1], max[2]);
		glEnd();


		if(isColliding())
			glColor3f(1.0, 0.0, 0.0);
		else
			glColor3f(0.0, 0.0, 1.0);


		glBegin(GL_QUADS);
			for(int i=0; i<nrb; i++)
			{
				glVertex3f(worldPos[i].x, worldPos[i].y, worldPos[i].z);
			}
		glEnd();
	glPopMatrix();
}


void RigidBody::printDebug()
{
	std::cout<<" "<<std::endl;
	std::cout<<"RIGID BODY DEBUG:"<<std::endl;
	std::cout<<"Height :"<<height<<" "<<"Width :"<<width<<" "<<"Depth :"<<depth<<std::endl;
	std::cout<<" "<<std::endl;
	std::cout<<"Mass :"<<m<<std::endl;
	std::cout<<" "<<std::endl;
	std::cout<<"Inverse Mass :"<<1/m<<std::endl;
	std::cout<<" "<<std::endl;
	std::cout<<"CG :"<<x.x<<" "<<x.y<<" "<<x.z<<std::endl;
	std::cout<<" "<<std::endl;
	std::cout<<"Linear Velocity :"<<v.x<<" "<<v.y<<" "<<v.z<<std::endl;
	std::cout<<" "<<std::endl;
	std::cout<<"Linear Momentum :"<<P.x<<" "<<P.y<<" "<<P.z<<std::endl;
	std::cout<<" "<<std::endl;
	std::cout<<"Angular Momentum :"<<L.x<<" "<<L.y<<" "<<L.z<<std::endl;
	std::cout<<" "<<std::endl;
	std::cout<<"Angular Velocity :"<<w.x<<" "<<w.y<<" "<<w.z<<std::endl;
	std::cout<<" "<<std::endl;
	std::cout<<"Torque :"<<t.x<<" "<<t.y<<" "<<t.z<<std::endl;
	std::cout<<" "<<std::endl;
	std::cout<<"Force :"<<f.x<<" "<<f.y<<" "<<f.z<<std::endl;
	std::cout<<" "<<std::endl;
	std::cout<<"Inertia Body:"<<IBody[0][0]<<" "<<IBody[0][1]<<" "<<IBody[0][2]<<std::endl;
	std::cout<<"             "<<IBody[1][0]<<" "<<IBody[1][1]<<" "<<IBody[1][2]<<std::endl;
	std::cout<<"             "<<IBody[2][0]<<" "<<IBody[2][1]<<" "<<IBody[2][2]<<std::endl;
	std::cout<<" "<<std::endl;
	std::cout<<"Inertia Body (Inverse):"<<IBody_inverse[0][0]<<" "<<IBody_inverse[0][1]<<" "<<IBody_inverse[0][2]<<std::endl;
	std::cout<<"                       "<<IBody_inverse[1][0]<<" "<<IBody_inverse[1][1]<<" "<<IBody_inverse[1][2]<<std::endl;
	std::cout<<"                       "<<IBody_inverse[2][0]<<" "<<IBody_inverse[2][1]<<" "<<IBody_inverse[2][2]<<std::endl;
	std::cout<<" "<<std::endl;
	std::cout<<"Inertia World:"<<IWorld[0][0]<<" "<<IWorld[0][1]<<" "<<IWorld[0][2]<<std::endl;
	std::cout<<"              "<<IWorld[1][0]<<" "<<IWorld[1][1]<<" "<<IWorld[1][2]<<std::endl;
	std::cout<<"              "<<IWorld[2][0]<<" "<<IWorld[2][1]<<" "<<IWorld[2][2]<<std::endl;
	std::cout<<" "<<std::endl;
	std::cout<<"Inertia World (Inverse):"<<IWorld_inverse[0][0]<<" "<<IWorld_inverse[0][1]<<" "<<IWorld_inverse[0][2]<<std::endl;
	std::cout<<"                        "<<IWorld_inverse[1][0]<<" "<<IWorld_inverse[1][1]<<" "<<IWorld_inverse[1][2]<<std::endl;
	std::cout<<"                        "<<IWorld_inverse[2][0]<<" "<<IWorld_inverse[2][1]<<" "<<IWorld_inverse[2][2]<<std::endl;
	std::cout<<" "<<std::endl;	
	std::cout<<"Rotation (Matrix) :"<<R[0][0]<<" "<<R[0][1]<<" "<<R[0][2]<<std::endl;
	std::cout<<"                 "<<R[1][0]<<" "<<R[1][1]<<" "<<R[1][2]<<std::endl;
	std::cout<<"                 "<<R[2][0]<<" "<<R[2][1]<<" "<<R[2][2]<<std::endl;
	std::cout<<" "<<std::endl;
	if(useQuat)
	{
		std::cout<<"Rotation (Quaternion) :"<<QR.w<<" "<<QR.x<<" "<<QR.y<<" "<<QR.z<<std::endl;
	}
}	

bool AABB::IntersectsPlane(Plane p)
{
	glm::vec3 q;
	float t = 0.0;


	center = max + min;
	center /= 2.0;


	extents = max - center;


	glm::vec3 N = p.getPlaneNormal();


	float r = extents.x * abs(N.x) + extents.y * abs(N.y) + extents.z * abs(N.z);


	float d = glm::dot(N,center) - glm::dot(N,p.getVertex());
	
	if(abs(d) <= r)
	{
		t = 0.0;
		collisionPoint = center - r * N;
		return true;
	}
	else
	{
		if(glm::dot(N,p.getVertex()) >= 0.0)
		{
			return false;
		}
		else
		{
			float tempR = d > 0.0 ? r : -r;
			t = (tempR + d) - glm::dot(N,center) / glm::dot(N,p.getVertex());
			collisionPoint = center + t * p.getVertex()  - r * N;
			return true;
		}
	}
}

void CollisionManager::CheckPlaneRBCollision(RigidBody *rb, Plane *p, int n) 
{
	AABB aabb;
	glm::vec3 N;


	aabb = rb->aabb;


	for(int j=0; j<6; j++)
	{
		N = p[j].getPlaneNormal();


		if(aabb.IntersectsPlane(p[j]))
		{
			std::cout<<"Collision - Plane "<<j<<std::endl;
			rb->setCollisionFlag(true);
			rb->setCollisionPoint(aabb.GetAABBCollisionPoint());
			rb->setCollisionNormal(glm::normalize(rb->getCollisionPoint()));
			ApplyPlaneRBImpulse(rb);
		}
		else
		{
			rb->setCollisionFlag(false);
		}
	}
}

void CollisionManager::ApplyPlaneRBImpulse(RigidBody *rb)
{
	glm::vec3 j;
	glm::vec3 JN;
	
	float m_inverse = 1 / rb->getMass();
	glm::vec3 x = rb->getCG();
	glm::mat3 I_inv = rb->getInvInertiaWorldMat();
	glm::vec3 collP = rb->getCollisionPoint();
	glm::vec3 N = rb->getCollisionNormal();	


	glm::vec3 pa = getCollisionPointVelocity(rb);
	glm::vec3 ra = collP - x;
	glm::vec3 vrel = N * pa;
	glm::vec3 numerator = -(1 + epsilon) * vrel;


	glm::vec3 term1 = glm::vec3(m_inverse, m_inverse, m_inverse);
	glm::vec3 term2 = N * (glm::cross(I_inv * glm::cross(ra,N),ra));
	glm::vec3 denominator = term1 + term2;


	j = numerator / denominator;
	
	JN = j * N;


	//std::cout<<"Impulse Vector "<<JN.x<<" "<<JN.y<<" "<<JN.z<<std::endl;
	
	// Apply Impulse
	glm::vec3 P = rb->getLMomentum();
	glm::vec3 L = rb->getAMomentum();


	P += JN;
	L += glm::cross(ra,JN);


	rb->setLMomentum(P);
	rb->setAMomentum(L);
}


Besides the fact that you’re using OpenGL to visualize your scene, this doesn’t anything to do with OpenGL. You probably want to post on gamedev.net or someplace where game physics gurus hang out.

That’s wonderful, but… this question has nothing to do with OpenGL. The closest association it has to GL is the fact that it uses a math library that has “GL” in it.