• FEATURED

View more

View more

View more

Image of the Day Submit

IOTD | Top Screenshots

The latest, straight to your Inbox.

Subscribe to GameDev.net Direct to receive the latest updates and exclusive content.

Rigid Body Physics

Old topic!

Guest, the last post of this topic is over 60 days old and at this point you may not reply in this topic. If you wish to continue this conversation start a new topic.

3 replies to this topic

#1dribble04  Members

Posted 06 October 2012 - 12:52 PM

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.

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®;
IWorld_inverse = R * IBody_inverse * glm::transpose®;
// 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®; 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);
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);
}

Edited by dribble04, 06 October 2012 - 12:57 PM.

#2mv348  Members

Posted 07 October 2012 - 12:02 PM

Are you normalizing your quaternion after every iteration? If not, numerical errors can accumulate and cause weird scaling and skewing.

#3dribble04  Members

Posted 08 October 2012 - 01:24 AM

yes I am. using the built in normalize function of glm

#4dribble04  Members

Posted 08 October 2012 - 01:41 AM

Do you notice anything wrong with the collision part? Specifically the collision response?

Old topic!

Guest, the last post of this topic is over 60 days old and at this point you may not reply in this topic. If you wish to continue this conversation start a new topic.