Part of the Khronos Group
OpenGL.org

The Industry's Foundation for High Performance Graphics

from games to virtual reality, mobile phones to supercomputers

Results 1 to 2 of 2

Thread: Rigid Body Physics (OpenGL)

  1. #1
    Junior Member Newbie
    Join Date
    Jan 2012
    Posts
    26

    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,

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

    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.

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

  2. #2
    Senior Member OpenGL Guru Dark Photon's Avatar
    Join Date
    Oct 2004
    Location
    Druidia
    Posts
    3,183
    Quote Originally Posted by glnoob_andre View Post
    I am trying to build rigid body physics engine. I am using the glm library as my standard math library.
    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.

Tags for this Thread

Posting Permissions

  • You may not post new threads
  • You may not post replies
  • You may not post attachments
  • You may not edit your posts
  •