title: 3D Rigid Body Physics (OpenGL)
body: <p>I am trying to build rigid body physics engine. I am using the glm library as my standard math library. </p>
  
  <p>The problems I am facing are as follows,</p>
  
  <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>
  
  <p>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.</p>
  
  <p>Here is my code.</p>
  
  <pre><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&lt;nrb; i++)
      {
          objPos[i] = verts[i];
      }
  }
  
  void RigidBody::calculateBodyToWorld()
  {
      for(int i=0; i&lt;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&lt;nrb; i++)
              {
                  glVertex3f(worldPos[i].x, worldPos[i].y, worldPos[i].z);
              }
          glEnd();
      glPopMatrix();
  }
  
  void RigidBody::printDebug()
  {
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"RIGID BODY DEBUG:"&lt;&lt;std::endl;
      std::cout&lt;&lt;"Height :"&lt;&lt;height&lt;&lt;" "&lt;&lt;"Width :"&lt;&lt;width&lt;&lt;" "&lt;&lt;"Depth :"&lt;&lt;depth&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"Mass :"&lt;&lt;m&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"Inverse Mass :"&lt;&lt;1/m&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"CG :"&lt;&lt;x.x&lt;&lt;" "&lt;&lt;x.y&lt;&lt;" "&lt;&lt;x.z&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"Linear Velocity :"&lt;&lt;v.x&lt;&lt;" "&lt;&lt;v.y&lt;&lt;" "&lt;&lt;v.z&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"Linear Momentum :"&lt;&lt;P.x&lt;&lt;" "&lt;&lt;P.y&lt;&lt;" "&lt;&lt;P.z&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"Angular Momentum :"&lt;&lt;L.x&lt;&lt;" "&lt;&lt;L.y&lt;&lt;" "&lt;&lt;L.z&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"Angular Velocity :"&lt;&lt;w.x&lt;&lt;" "&lt;&lt;w.y&lt;&lt;" "&lt;&lt;w.z&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"Torque :"&lt;&lt;t.x&lt;&lt;" "&lt;&lt;t.y&lt;&lt;" "&lt;&lt;t.z&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"Force :"&lt;&lt;f.x&lt;&lt;" "&lt;&lt;f.y&lt;&lt;" "&lt;&lt;f.z&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"Inertia Body:"&lt;&lt;IBody[0][0]&lt;&lt;" "&lt;&lt;IBody[0][1]&lt;&lt;" "&lt;&lt;IBody[0][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;"             "&lt;&lt;IBody[1][0]&lt;&lt;" "&lt;&lt;IBody[1][1]&lt;&lt;" "&lt;&lt;IBody[1][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;"             "&lt;&lt;IBody[2][0]&lt;&lt;" "&lt;&lt;IBody[2][1]&lt;&lt;" "&lt;&lt;IBody[2][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"Inertia Body (Inverse):"&lt;&lt;IBody_inverse[0][0]&lt;&lt;" "&lt;&lt;IBody_inverse[0][1]&lt;&lt;" "&lt;&lt;IBody_inverse[0][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;"                       "&lt;&lt;IBody_inverse[1][0]&lt;&lt;" "&lt;&lt;IBody_inverse[1][1]&lt;&lt;" "&lt;&lt;IBody_inverse[1][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;"                       "&lt;&lt;IBody_inverse[2][0]&lt;&lt;" "&lt;&lt;IBody_inverse[2][1]&lt;&lt;" "&lt;&lt;IBody_inverse[2][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"Inertia World:"&lt;&lt;IWorld[0][0]&lt;&lt;" "&lt;&lt;IWorld[0][1]&lt;&lt;" "&lt;&lt;IWorld[0][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;"              "&lt;&lt;IWorld[1][0]&lt;&lt;" "&lt;&lt;IWorld[1][1]&lt;&lt;" "&lt;&lt;IWorld[1][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;"              "&lt;&lt;IWorld[2][0]&lt;&lt;" "&lt;&lt;IWorld[2][1]&lt;&lt;" "&lt;&lt;IWorld[2][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      std::cout&lt;&lt;"Inertia World (Inverse):"&lt;&lt;IWorld_inverse[0][0]&lt;&lt;" "&lt;&lt;IWorld_inverse[0][1]&lt;&lt;" "&lt;&lt;IWorld_inverse[0][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;"                        "&lt;&lt;IWorld_inverse[1][0]&lt;&lt;" "&lt;&lt;IWorld_inverse[1][1]&lt;&lt;" "&lt;&lt;IWorld_inverse[1][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;"                        "&lt;&lt;IWorld_inverse[2][0]&lt;&lt;" "&lt;&lt;IWorld_inverse[2][1]&lt;&lt;" "&lt;&lt;IWorld_inverse[2][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;  
      std::cout&lt;&lt;"Rotation (Matrix) :"&lt;&lt;R[0][0]&lt;&lt;" "&lt;&lt;R[0][1]&lt;&lt;" "&lt;&lt;R[0][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;"                 "&lt;&lt;R[1][0]&lt;&lt;" "&lt;&lt;R[1][1]&lt;&lt;" "&lt;&lt;R[1][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;"                 "&lt;&lt;R[2][0]&lt;&lt;" "&lt;&lt;R[2][1]&lt;&lt;" "&lt;&lt;R[2][2]&lt;&lt;std::endl;
      std::cout&lt;&lt;" "&lt;&lt;std::endl;
      if(useQuat)
      {
          std::cout&lt;&lt;"Rotation (Quaternion) :"&lt;&lt;QR.w&lt;&lt;" "&lt;&lt;QR.x&lt;&lt;" "&lt;&lt;QR.y&lt;&lt;" "&lt;&lt;QR.z&lt;&lt;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) &lt;= r)
      {
          t = 0.0;
          collisionPoint = center - r * N;
          return true;
      }
      else
      {
          if(glm::dot(N,p.getVertex()) &gt;= 0.0)
          {
              return false;
          }
          else
          {
              float tempR = d &gt; 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-&gt;aabb;
  
      for(int j=0; j&lt;6; j++)
      {
          N = p[j].getPlaneNormal();
  
          if(aabb.IntersectsPlane(p[j]))
          {
              std::cout&lt;&lt;"Collision - Plane "&lt;&lt;j&lt;&lt;std::endl;
              rb-&gt;setCollisionFlag(true);
              rb-&gt;setCollisionPoint(aabb.GetAABBCollisionPoint());
              rb-&gt;setCollisionNormal(glm::normalize(rb-&gt;getCollisionPoint()));
              ApplyPlaneRBImpulse(rb);
          }
          else
          {
              rb-&gt;setCollisionFlag(false);
          }
      }
  }
  
  void CollisionManager::ApplyPlaneRBImpulse(RigidBody *rb)
  {
      glm::vec3 j;
      glm::vec3 JN;
  
      float m_inverse = 1 / rb-&gt;getMass();
      glm::vec3 x = rb-&gt;getCG();
      glm::mat3 I_inv = rb-&gt;getInvInertiaWorldMat();
      glm::vec3 collP = rb-&gt;getCollisionPoint();
      glm::vec3 N = rb-&gt;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&lt;&lt;"Impulse Vector "&lt;&lt;JN.x&lt;&lt;" "&lt;&lt;JN.y&lt;&lt;" "&lt;&lt;JN.z&lt;&lt;std::endl;
  
      // Apply Impulse
      glm::vec3 P = rb-&gt;getLMomentum();
      glm::vec3 L = rb-&gt;getAMomentum();
  
      P += JN;
      L += glm::cross(ra,JN);
  
      rb-&gt;setLMomentum(P);
      rb-&gt;setAMomentum(L);
  }
  </code></pre>