Added rudimentary support for rigid body manipulation

Author: chiplet

Camera.cpp

@@ -230,3 +230,21 @@ int Camera::getMouseButton()
 {
     return mb;
 }
+
+glm::mat4 Camera::getModelview()
+{
+    if (mode == CAM_ORBIT)
+    {
+        return glm::lookAt(vec3(loc), vec3(focus), vec3(up));
+    }
+    else if (mode == CAM_FPS)
+    {
+        return glm::lookAt(vec3(loc), vec3(focus+dir), vec3(up));
+    }
+}
+
+glm::mat4 Camera::getProjection()
+{
+    float rad = 60.0*M_PI/180;
+    gluPerspective(rad, (float)screenWidth/(float)screenHeight, near, far);
+}

Camera.hpp

@@ -14,6 +14,7 @@
 #include "helpers.hpp"
 #include "Transform.hpp"
 #include <glm/glm.hpp>
+#include <glm/gtc/matrix_transform.hpp>
 
 class Camera
 {
@@ -50,5 +51,7 @@ class Camera
         void mov_sideways(float amount);
         void changeSpeed(float amount);
         int getMouseButton();
+        glm::mat4 getModelview();
+        glm::mat4 getProjection();
 };
 #endif

Makefile

@@ -1,7 +1,7 @@
 UNAME := $(shell uname)
 
-headers = helpers.hpp Camera.hpp callbacks.hpp ODE.hpp display.hpp globals.hpp Transform.hpp
-sources = main.cpp helpers.cpp Camera.cpp callbacks.cpp ODE.cpp display.cpp globals.cpp Transform.cpp
+headers = helpers.hpp Camera.hpp callbacks.hpp ODE.hpp display.hpp globals.hpp Transform.hpp RigidBody.hpp
+sources = main.cpp helpers.cpp Camera.cpp callbacks.cpp ODE.cpp display.cpp globals.cpp Transform.cpp RigidBody.cpp
 
 CPPFLAGS = -std=c++11 -Wno-deprecated -lm
 

ODE.cpp

@@ -1,6 +1,8 @@
 #include "ODE.hpp"
 
-vector<float> EulerMethod(vector<float> y0, float t0, float t_end,
+/*
+// solve a vector of differential equations
+//vector<float> EulerMethod(vector<float> y0, float t0, float t_end,
         float dt, dydt_func dydt)
 {
     vector<float> y_end;
@@ -10,7 +12,6 @@ vector<float> EulerMethod(vector<float> y0, float t0, float t_end,
     {
         throw "Bad arguments for t0 and t_end. Make sure that t0 < t_end.";
     }
-
     if (dt <=0)
     {
         throw "Bad argument for dt: Make sure that dt > 0.";
@@ -36,3 +37,85 @@ vector<float> EulerMethod(vector<float> y0, float t0, float t_end,
 
     return y_end;
 }
+*/
+
+/*
+// solve scalar differential equation
+float EulerMethod(float y0, float t0, float t_end,
+        float dt, dydt_func dydt)
+{
+    // verify that t0 < t_end
+    if (t0 >= t_end)
+    {
+        throw "Bad arguments for t0 and t_end. Make sure that t0 < t_end.";
+    }
+
+    if (dt <=0)
+    {
+        throw "Bad argument for dt: Make sure that dt > 0.";
+    }
+
+    // solve ODE using Euler's method
+    float t = t0;
+    float y = y0;
+
+    while (t < t_end)
+    {
+        // step
+        y += dydt(t, y)*dt;
+        t += dt;
+    }
+
+    return y;
+}
+*/
+
+// solve 3x3 matrix differential equation
+mat3 EulerMethod(mat3 M0, float t0, float t_end, float dt,
+        mat3 dMdt)
+{
+    // verify that t0 < t_end
+    if (t0 >= t_end)
+    {
+        throw "Bad arguments for t0 and t_end. Make sure that t0 < t_end.";
+    }
+    if (dt <=0)
+    {
+        throw "Bad argument for dt: Make sure that dt > 0.";
+    }
+
+    float t = t0;
+    mat3 M = M0;
+
+    while (t < t_end)
+    {
+        M += dMdt*dt;
+        t += dt;
+    }
+    return M;
+}
+
+// solve vec3 differential equation
+vec3 EulerMethod(vec3 V0, float t0, float t_end, float dt,
+        vec3 dV3dt)
+{
+    // verify that t0 < t_end
+    if (t0 >= t_end)
+    {
+        throw "Bad arguments for t0 and t_end. Make sure that t0 < t_end.";
+    }
+    if (dt <=0)
+    {
+        throw "Bad argument for dt: Make sure that dt > 0.";
+    }
+
+    float t = t0;
+    vec3 V = V0;
+
+    while (t < t_end)
+    {
+        V += dV3dt*dt;
+        t += dt;
+    }
+    return V;
+}

ODE.hpp

@@ -1,8 +1,25 @@
 #include <vector>
+#include <glm/glm.hpp>
 
 using namespace std;
+using namespace glm;
 
-typedef float (*dydt_func)(float t, float y);
+// solve ODE for a vector of equations
+//typedef float (*dydt_func)(float t, float y);
+//vector<float> EulerMethod(vector<float> y0, float t0, float t_end,
+//      float dt, dydt_func dydt);
 
-vector<float> EulerMethod(vector<float> y0, float t0, float t_end,
-        float dt, dydt_func dydt);
+// solve ODE for a single equation
+//float EulerMethod(float y0, float t0, float t_end,
+//      float dt, dydt_func dydt);
+
+// solve ODE for a 3x3 matrix value equation
+// These functions do not use callback function for evaluating
+// the time derivative because non-static member functions cannot
+// be used as callback => RigidBody's member functions could not be
+// passed to this and therefore have to be evaluated and the 
+// result passed to these functions.
+mat3 EulerMethod(mat3 M0, float t0, float t_end,
+        float dt, mat3 dMdt);
+vec3 EulerMethod(vec3 V0, float t0, float t_end,
+        float dt, vec3 dV3dt);

RigidBody.cpp

@@ -0,0 +1,77 @@
+#include "RigidBody.hpp"
+
+RigidBody::RigidBody()
+{
+    mass = 1.0;
+    Ibody = (mass/12.0f)*mat3(1.0f);
+    Ibodyinv = inverse(Ibody);
+
+    x = vec3(0.0f, 0.0f, 2.0f);
+    R = mat3(1.0f);
+    P = vec3(0.0f);
+    L = vec3(0.0f, 0.0f, 0.1f);
+
+    Iinv = R*Ibodyinv*transpose(R);
+    v = P/mass;
+    omega = Iinv * L;
+
+    force = vec3(0.0f, 0.0f, 0.0f);
+    torque = vec3(0.0f);
+
+    size = 0.5f;
+}
+
+// simulate one step forward
+// the time step in the argument in in simulation time
+void RigidBody::sim_step(float dt)
+{
+    v = P/mass;
+    omega = Iinv * L;
+
+    // the time step used by the solver is different (preferably smaller)
+    // than the time step of the simulation 
+    // forces are constant only (for now) so time is assumed 0 to dt for every update
+    // vec3 EulerMethod(vec3 V0, float t0, float t_end, float dt, dV3dt_func dV3dt)
+    x = EulerMethod(x, 0, dt, 0.01f, dxdt(0, x));
+    R = EulerMethod(R, 0, dt, 0.01f, dRdt(0, R));
+
+    // normalise vectors in R and make sure that they are orthogonal
+    vec3 w = normalize(R[2]);
+    vec3 u = normalize(cross(R[1], w));
+    vec3 v = cross(w, u);
+    R = mat3(u, v, w);
+    
+
+    P = EulerMethod(P, 0, dt, 0.01f, force);
+    L = EulerMethod(L, 0, dt, 0.01f, torque);
+
+}
+
+mat3 RigidBody::dRdt(float t, mat3 R)
+{
+    // dual defined in helpers.hpp
+    return dual(omega)*R;
+}
+
+vec3 RigidBody::dxdt(float t, vec3 V)
+{
+    return v;
+};
+
+State RigidBody::getState()
+{
+    State s;
+    s.x = x;
+    s.R = R;
+    s.P = P;
+    s.L = L;
+    return s;
+}
+
+mat4 RigidBody::getTransformation()
+{
+    mat4 S = Transform::scale(size, size, size/2.0f);
+    mat4 T = Transform::translate(x);
+    mat4 Rot = Transform::rotate(R);
+    return T*Rot*S;
+}

RigidBody.hpp

@@ -0,0 +1,53 @@
+#ifndef RIGIDBODY
+#define RIGIDBODY
+
+#include "Transform.hpp"
+#include <glm/glm.hpp>
+#include "ODE.hpp"
+#include "helpers.hpp"
+
+using namespace glm;
+
+typedef struct
+{
+    vec3 x;
+    mat3 R;
+    vec3 P;
+    vec3 L;
+} State;
+
+class RigidBody
+{
+    private:
+        // constants
+        float mass;
+        mat3 Ibody;
+        mat3 Ibodyinv;
+
+        // state variables
+        vec3 x; // displacement of CM from origin
+        mat3 R; // Orientation
+        vec3 P; // Linear momentum
+        vec3 L; // Angular momentum
+
+        // derived quantities
+        mat3 Iinv;  
+        vec3 v;     // linear velocity
+        vec3 omega; // angular velocity
+
+        vec3 force;
+        vec3 torque;
+
+        // for drawing (assuming rigid body is a cube)
+        float size;
+
+    public:
+        RigidBody();
+        void sim_step(float dt);    // simulate one step forward
+        mat3 dRdt(float t, mat3 R);
+        vec3 dxdt(float t, vec3 M);
+        State getState();
+        mat4 getTransformation();   // returns the transformation matrix from origin
+};
+
+#endif

Transform.cpp

@@ -1,17 +1,17 @@
 #include "Transform.hpp"
 
 // Rotate vector about axis by an angle rad
-glm::mat4 Transform::rotate(glm::vec3 axis, float rad)
+mat4 Transform::rotate(vec3 axis, float rad)
 {
     // normalise axis
-    axis = glm::normalize(axis);
+    axis = normalize(axis);
 
     // declare relevant matrices
-    glm::mat3 I = glm::mat3(1.0);
-    glm::mat3 A_dual = glm::mat3(0, axis.z, -axis.y, -axis.z, 0, axis.x, axis.y, -axis.x, 0);
+    mat3 I = mat3(1.0);
+    mat3 A_dual = mat3(0, axis.z, -axis.y, -axis.z, 0, axis.x, axis.y, -axis.x, 0);
 
-    glm::mat3 R3 = I*cosf(rad) + glm::outerProduct(axis, axis)*(1-cosf(rad)) + A_dual*sinf(rad);
-    glm::mat4 R4 = glm::mat4(
+    mat3 R3 = I*cosf(rad) + outerProduct(axis, axis)*(1-cosf(rad)) + A_dual*sinf(rad);
+    mat4 R4 = mat4(
             R3[0].x, R3[0].y, R3[0].z, 0.0,
             R3[1].x, R3[1].y, R3[1].z, 0.0,
             R3[2].x, R3[2].y, R3[2].z, 0.0,
@@ -20,6 +20,15 @@ glm::mat4 Transform::rotate(glm::vec3 axis, float rad)
     return R4;
 }
 
+mat4 Transform::rotate(mat3 R)
+{
+    return mat4(
+            R[0].x, R[0].y, R[0].z, 0.0f,
+            R[1].x, R[1].y, R[1].z, 0.0f,
+            R[2].x, R[2].y, R[2].z, 0.0f,
+            0.0f,     0.0f,   0.0f, 1.0f);
+}
+
 // Treat the xyz component of vec3 amount as scale factors
 mat4 Transform::scale(vec3 amount)
 {

Transform.hpp

@@ -8,6 +8,7 @@ using namespace glm;
 namespace Transform
 {
     mat4 rotate (vec3 axis, float rad);
+    mat4 rotate (mat3 R);
     mat4 scale (vec3 amount);
     mat4 scale (float sx, float sy, float sz);
     mat4 translate(vec3 s);