add few new functions to glframe.c

6 files changed, 399 insertions, 72 deletions

diff --git a/sdl/Makefile b/sdl/Makefile
index 74a9840..1ff2958 100644
--- a/sdl/Makefile
+++ b/sdl/Makefile
@@ -1,3 +1,9 @@
+#
+#
+# Note: object targets are not valid and get recompiled everytime simply
+#       because they do not point to a valid location
+#
+
 BIN = pyramid
 SRC = pyramid.c math3d.c gltools.c glframe.c shader.c platform.c window.c
 CC = gcc
diff --git a/sdl/glframe.c b/sdl/glframe.c
index b3dc33e..fbf8cd6 100644
--- a/sdl/glframe.c
+++ b/sdl/glframe.c
@@ -8,25 +8,26 @@
  */
 
 #include <GL/glew.h>
+#include <stdio.h>
 #include "glframe.h"
 
 void glframe_reset(GLFrame *frame)
 {
-	frame->v_location[0] = 0.0f;
-	frame->v_location[1] = 0.0f;
-	frame->v_location[2] = 0.0f;
-	
-	frame->v_forward[0] = 0.0f;
-	frame->v_forward[1] = 0.0f;
-	frame->v_forward[2] = -1.0f;
-	
-	frame->v_up[0] = 0.0f;
-	frame->v_up[1] = 1.0f;
-	frame->v_up[2] = 0.0f;
+    frame->v_location[0] = 0.0f;
+    frame->v_location[1] = 0.0f;
+    frame->v_location[2] = 0.0f;
+
+    frame->v_forward[0] = 0.0f;
+    frame->v_forward[1] = 0.0f;
+    frame->v_forward[2] = -1.0f;
+
+    frame->v_up[0] = 0.0f;
+    frame->v_up[1] = 1.0f;
+    frame->v_up[2] = 0.0f;
 }
 
 /* get a 4x4 transformation matrix that describes the camera orientation */
-void glframe_get_camera_orientation(GLFrame *frame, M3DMatrix44f m)
+inline void glframe_get_camera_orientation(GLFrame *frame, M3DMatrix44f m)
 {
     M3DVector3f x, z;
 
@@ -60,32 +61,26 @@ void glframe_get_camera_orientation(GLFrame *frame, M3DMatrix44f m)
 }
 
 /* perform viewing or modeling transformations */
-/* some of the code is unimplemented */     
-void glframe_apply_camera_transform(GLFrame *frame)    
+inline void glframe_apply_camera_transform(GLFrame *frame, const int rot_only)
 {
-	/* XXX: rotation only, should be passed in as a parameter */
-	int rot_only = 0;
-	
     M3DMatrix44f m;
     glframe_get_camera_orientation(frame, m);
-    m3dPrintMatrix44f(m);
-
     glMultMatrixf(m);
 
-    /* if rotation only, then do not do the translation */
     if (!rot_only)
-        glTranslatef(-frame->v_location[0], -frame->v_location[1], -frame->v_location[2]);
+        glTranslatef(-frame->v_location[0], -frame->v_location[1],
+                     -frame->v_location[2]);
         
 #if 0
-    gluLookAt(v_location[0], v_location[1], v_location[2],
-                v_location[0] + v_forward[0], 
-                v_location[1] + v_forward[1], 
-                v_location[2] + v_forward[2], 
-                v_up[0], v_up[1], v_up[2]);
+    gluLookAt(frame->v_location[0], frame->v_location[1], frame->v_location[2],
+              frame->v_location[0] + frame->v_forward[0],
+              frame->v_location[1] + frame->v_forward[1],
+              frame->v_location[2] + frame->v_forward[2],
+              frame->v_up[0], frame->v_up[1], frame->v_up[2]);
 #endif                
 }
 
-void glframe_move_forward(GLFrame *frame, float delta)
+inline void glframe_move_forward(GLFrame *frame, const float delta)
 {
     /* move along direction of front direction */
     frame->v_location[0] += frame->v_forward[0] * delta;
@@ -93,25 +88,63 @@ void glframe_move_forward(GLFrame *frame, float delta)
     frame->v_location[2] += frame->v_forward[2] * delta;
 }
 
-/* rotate left or right */
-void glframe_rotate_local_y(GLFrame *frame, float angle)
+/* move up or down */
+inline void glframe_move_up(GLFrame *frame, const float delta)
+{
+    frame->v_location[0] += frame->v_up[0] * delta;
+    frame->v_location[1] += frame->v_up[1] * delta;
+    frame->v_location[2] += frame->v_up[2] * delta;
+}
+
+/* move left or right */
+inline void glframe_move_right(GLFrame *frame, const float delta)
+{
+    M3DVector3f x;
+
+    m3dCrossProductf(x, frame->v_up, frame->v_forward);
+    frame->v_location[0] += x[0] * delta;
+    frame->v_location[1] += x[1] * delta;
+    frame->v_location[2] += x[2] * delta;
+}
+
+inline void glframe_translate_world(GLFrame *frame, const float x, const float y,
+                             const float z)
+{
+    frame->v_location[0] += x;
+    frame->v_location[1] += y;
+    frame->v_location[2] += z;
+}
+
+inline void glframe_translate_local(GLFrame *frame, const float x, const float y,
+                             const float z)
+{
+    glframe_move_forward(frame, z);
+    glframe_move_up(frame, y);
+    glframe_move_right(frame, x);
+}
+
+/* yaw */
+void glframe_rotate_local_y(GLFrame *frame, const float angle)
 {
     M3DMatrix44f rotmat;
     M3DVector3f newvect;
 
-    /* just rotate around the up vector */
     /* create a rotation matrix around up vector */
-    m3dRotationMatrix44f(rotmat, angle, frame->v_up[0], frame->v_up[1], frame->v_up[2]);
+    m3dRotationMatrix44f(rotmat, angle, frame->v_up[0], frame->v_up[1],
+                         frame->v_up[2]);
 
     /* rotate forward pointing vector (inlined 3x3 transform) */
-    newvect[0] = rotmat[0] * frame->v_forward[0] + rotmat[4] * frame->v_forward[1] + rotmat[8] *  frame->v_forward[2];
-    newvect[1] = rotmat[1] * frame->v_forward[0] + rotmat[5] * frame->v_forward[1] + rotmat[9] *  frame->v_forward[2];
-    newvect[2] = rotmat[2] * frame->v_forward[0] + rotmat[6] * frame->v_forward[1] + rotmat[10] * frame->v_forward[2];
+    newvect[0] = rotmat[0] * frame->v_forward[0]  + rotmat[4] *
+                 frame->v_forward[1] + rotmat[8]  * frame->v_forward[2];
+    newvect[1] = rotmat[1] * frame->v_forward[0]  + rotmat[5] *
+                 frame->v_forward[1] + rotmat[9]  * frame->v_forward[2];
+    newvect[2] = rotmat[2] * frame->v_forward[0]  + rotmat[6] *
+                 frame->v_forward[1] + rotmat[10] * frame->v_forward[2];
     m3dCopyVector3f(frame->v_forward, newvect);
 }
 
-/* rotate up or down */
-void glframe_rotate_local_x(GLFrame *frame, float angle)
+/* pitch */
+void glframe_rotate_local_x(GLFrame *frame, const float angle)
 {
     M3DMatrix44f rotmat;
     M3DVector3f newfwdvec;
@@ -130,9 +163,12 @@ void glframe_rotate_local_x(GLFrame *frame, float angle)
     m3dRotationMatrix44f(rotmat, angle, x[0], x[1], x[2]);
 
     /* rotate forward pointing vector (inlined 3x3 transform) */
-    newfwdvec[0] = rotmat[0] * frame->v_forward[0] + rotmat[4] * frame->v_forward[1] + rotmat[8] *  frame->v_forward[2];
-    newfwdvec[1] = rotmat[1] * frame->v_forward[0] + rotmat[5] * frame->v_forward[1] + rotmat[9] *  frame->v_forward[2];
-    newfwdvec[2] = rotmat[2] * frame->v_forward[0] + rotmat[6] * frame->v_forward[1] + rotmat[10] * frame->v_forward[2];
+    newfwdvec[0] = rotmat[0] * frame->v_forward[0]  + rotmat[4] *
+                   frame->v_forward[1] + rotmat[8]  * frame->v_forward[2];
+    newfwdvec[1] = rotmat[1] * frame->v_forward[0]  + rotmat[5] *
+                   frame->v_forward[1] + rotmat[9]  * frame->v_forward[2];
+    newfwdvec[2] = rotmat[2] * frame->v_forward[0]  + rotmat[6] *
+                   frame->v_forward[1] + rotmat[10] * frame->v_forward[2];
 
     /* calculate new up vector */
     m3dCrossProductf(newupvec, x, newfwdvec);
@@ -141,3 +177,191 @@ void glframe_rotate_local_x(GLFrame *frame, float angle)
     m3dCopyVector3f(frame->v_up, newupvec);
 }
 
+/* roll */
+void glframe_rotate_local_z(GLFrame *frame, const float angle)
+{
+    M3DMatrix44f rotmat;
+    M3DVector3f newupvec;
+
+    m3dRotationMatrix44f(rotmat, angle, frame->v_forward[0], frame->v_forward[1],
+                         frame->v_forward[2]);
+
+    /* rotate forward pointing vector (inlined 3x3 transform) */
+    newupvec[0] = rotmat[0]  * frame->v_up[0] + rotmat[4] * frame->v_up[1] +
+                  rotmat[8]  * frame->v_up[2];
+    newupvec[1] = rotmat[1]  * frame->v_up[0] + rotmat[5] * frame->v_up[1] +
+                  rotmat[9]  * frame->v_up[2];
+    newupvec[2] = rotmat[2]  * frame->v_up[0] + rotmat[6] * frame->v_up[1] +
+                  rotmat[10] * frame->v_up[2];
+    m3dCopyVector3f(frame->v_up, newupvec);
+}
+
+/* reset axes to make sure they are orthonormal, should be called occasionally
+ * if the matrix is long-lived and frequently transformed
+ */
+void gl_frame_normalize(GLFrame *frame)
+{
+    M3DVector3f x;
+
+    m3dCrossProductf(x, frame->v_up, frame->v_forward);
+
+    /* use result to recalculate forward vector */
+    m3dCrossProductf(frame->v_forward, x, frame->v_up);
+
+    /* also check for unit length */
+    m3dNormalizeVectorf(frame->v_up);
+    m3dNormalizeVectorf(frame->v_forward);
+}
+
+/* assemble the matrix */
+void glframe_get_matrix(GLFrame *frame, M3DMatrix44f m, const int rot_only)
+{
+    M3DVector3f x;
+
+    m3dCrossProductf(x, frame->v_up, frame->v_forward);
+
+    m3dSetMatrixColumn44f(m, x, 0);
+    m[3] = 0.0f;
+
+    m3dSetMatrixColumn44f(m, frame->v_up, 1);
+    m[7] = 0.0f;
+
+    m3dSetMatrixColumn44f(m, frame->v_forward, 2);
+    m[11] = 0.0f;
+
+    if (rot_only)
+    {
+        m[12] = 0.0f;
+        m[13] = 0.0f;
+        m[14] = 0.0f;
+    }
+    else
+        m3dSetMatrixColumn44f(m, frame->v_location, 3);
+
+    m[15] = 1.0f;
+}
+
+/* position as an object in the scene, this places and orients a coordinate
+ * frame for other objects besides the camera
+ */
+void gl_frame_apply_actor_transform(GLFrame *frame)
+{
+    M3DMatrix44f rotmat;
+    glframe_get_matrix(frame, rotmat, 0);
+
+    /* apply rotation to the current matrix */
+    glMultMatrixf(rotmat);
+}
+
+/* rotate in world coordinates */
+void glframe_rotate_world(GLFrame *frame, const float angle, const float x,
+                          const float y, const float z)
+{
+    M3DMatrix44f rotmat;
+
+    /* create the rotation matrix */
+    m3dRotationMatrix44f(rotmat, angle, x, y, z);
+
+    M3DVector3f newvect;
+
+    newvect[0] = rotmat[0] * frame->v_up[0] + rotmat[4] *
+                 frame->v_up[1] + rotmat[8] * frame->v_up[2];
+    newvect[1] = rotmat[1] * frame->v_up[0] + rotmat[5] *
+                 frame->v_up[1] + rotmat[9] * frame->v_up[2];
+    newvect[2] = rotmat[2] * frame->v_up[0] + rotmat[6] *
+                 frame->v_up[1] + rotmat[10] * frame->v_up[2];
+
+    m3dCopyVector3f(frame->v_up, newvect);
+
+    /* transform the forward axis */
+    newvect[0] = rotmat[0] * frame->v_forward[0] + rotmat[4] *
+                 frame->v_forward[1] + rotmat[8] * frame->v_forward[2];
+    newvect[1] = rotmat[1] * frame->v_forward[0] + rotmat[5] *
+                 frame->v_forward[1] + rotmat[9] * frame->v_forward[2];
+    newvect[2] = rotmat[2] * frame->v_forward[0] + rotmat[6] *
+                 frame->v_forward[1] + rotmat[10] * frame->v_forward[2];
+
+    m3dCopyVector3f(frame->v_forward, newvect);
+}
+
+/* rotate around a local axis */
+void glframe_rotate_local(GLFrame *frame, const float angle, const float x,
+                          const float y, const float z)
+{
+    M3DVector3f world_vec;
+    M3DVector3f local_vec;
+    m3dLoadVector3f(local_vec, x, y, z);
+
+    glframe_local_to_world(frame, local_vec, world_vec);
+    glframe_rotate_world(frame, angle, world_vec[0], world_vec[1], world_vec[2]);
+}
+
+/* convert coordinate systems, do the transformation represented by the rotation
+ * and position on the point
+ */
+void glframe_local_to_world(GLFrame *frame, const M3DVector3f local, M3DVector3f world)
+{
+    M3DMatrix44f rotmat;
+
+    glframe_get_matrix(frame, rotmat, 1);
+
+    world[0] = rotmat[0] * local[0] + rotmat[4] * local[1] + rotmat[8] * local[2];
+    world[1] = rotmat[1] * local[0] + rotmat[5] * local[1] + rotmat[9] * local[2];
+    world[2] = rotmat[2] * local[0] + rotmat[6] * local[1] + rotmat[10] * local[2];
+
+    /* translate the point */
+    world[0] += frame->v_location[0];
+    world[1] += frame->v_location[1];
+    world[2] += frame->v_location[2];
+}
+
+/* change world coordinates into "local" coordinates */
+void glframe_world_to_local(GLFrame *frame, const M3DVector3f world, M3DVector3f local)
+{
+    /* translate the origin */
+    M3DVector3f new_world;
+    new_world[0] = world[0] - frame->v_location[0];
+    new_world[1] = world[1] - frame->v_location[1];
+    new_world[2] = world[2] - frame->v_location[2];
+
+    /* create the rotation matrix based on the vectors */
+    M3DMatrix44f rotmat;
+    M3DMatrix44f invmat;
+    glframe_get_matrix(frame, rotmat, 1);
+
+    /* do the rotation based on inverted matrix */
+    if (m3dInvertMatrix44f(invmat, rotmat) == -1)
+        fprintf(stderr, "glframe: m3dInvertMatrix44f() failed\n");
+
+    local[0] = invmat[0] * new_world[0] + invmat[4] *
+               new_world[1] + invmat[8] * new_world[2];
+    local[1] = invmat[1] * new_world[0] + invmat[5] *
+               new_world[1] + invmat[9] * new_world[2];
+    local[2] = invmat[2] * new_world[0] + invmat[6] *
+               new_world[1] + invmat[10] * new_world[2];
+}
+
+/* transform a point by frame matrix */
+void glframe_transform_point(GLFrame *frame, const M3DVector3f src, M3DVector3f dst)
+{
+    M3DMatrix44f m;
+
+    /* rotate and translate */
+    glframe_get_matrix(frame, m, 0);
+
+    dst[0] = m[0] * src[0] + m[4] * src[1] + m[8]  * src[2] + m[12];
+    dst[1] = m[1] * src[0] + m[5] * src[1] + m[9]  * src[2] + m[13];
+    dst[2] = m[2] * src[0] + m[6] * src[1] + m[10] * src[2] + m[14];
+}
+
+/* rotate a vector by frame matrix */
+void glframe_rotate_vector(GLFrame *frame, M3DVector3f src, M3DVector3f dst)
+{
+    M3DMatrix44f m;
+    glframe_get_matrix(frame, m, 1); /* rotate only */
+
+    dst[0] = m[0] * src[0] + m[4] * src[1] + m[8]  * src[2];
+    dst[1] = m[1] * src[0] + m[5] * src[1] + m[9]  * src[2];
+    dst[2] = m[2] * src[0] + m[6] * src[1] + m[10] * src[2];
+}
+
diff --git a/sdl/glframe.h b/sdl/glframe.h
index 0fb5e58..2bc2058 100644
--- a/sdl/glframe.h
+++ b/sdl/glframe.h
@@ -3,20 +3,35 @@
 
 #include "math3d.h"
 
+/* x axis gets calculated by taking cross product of y and z */
 typedef struct
 {
     M3DVector3f v_location; /* location */
-    M3DVector3f v_forward;  /* z, where am I heading */
-    M3DVector3f v_up;       /* y, which way is up */
+    M3DVector3f v_forward;  /* z axis, where am I heading */
+    M3DVector3f v_up;       /* y axis, which way is up */
 } GLFrame;
 
 /* function prototypes */
 void glframe_reset(GLFrame *);
-void glframe_get_camera_orientation(GLFrame *, M3DMatrix44f);
-void glframe_apply_camera_transform(GLFrame *);
-void glframe_move_forward(GLFrame *, float);
-void glframe_rotate_local_y(GLFrame *, float);
-void glframe_rotate_local_x(GLFrame *, float);
+extern void glframe_get_camera_orientation(GLFrame *, M3DMatrix44f);
+extern void glframe_apply_camera_transform(GLFrame *, const int);
+extern void glframe_move_forward(GLFrame *, const float);
+extern void glframe_move_up(GLFrame *, const float);
+extern void glframe_move_right(GLFrame *, const float);
+extern void glframe_translate_world(GLFrame *, const float, const float, const float);
+extern void glframe_translate_local(GLFrame *, const float, const float, const float);
+void glframe_rotate_local_y(GLFrame *, const float);
+void glframe_rotate_local_x(GLFrame *, const float);
+void glframe_rotate_local_z(GLFrame *, const float);
+void gl_frame_normalize(GLFrame *);
+void glframe_get_matrix(GLFrame *, M3DMatrix44f, const int);
+void gl_frame_apply_actor_transform(GLFrame *);
+void glframe_rotate_world(GLFrame *, const float, const float, const float, const float);
+void glframe_rotate_local(GLFrame *, const float, const float, const float, const float);
+void glframe_local_to_world(GLFrame *, const M3DVector3f, M3DVector3f);
+void glframe_world_to_local(GLFrame *, const M3DVector3f, M3DVector3f);
+void glframe_transform_point(GLFrame *, const M3DVector3f, M3DVector3f);
+void glframe_rotate_vector(GLFrame *, M3DVector3f, M3DVector3f);
 
 #endif
 
diff --git a/sdl/math3d.c b/sdl/math3d.c
index 3277ede..ca60233 100644
--- a/sdl/math3d.c
+++ b/sdl/math3d.c
@@ -1,5 +1,3 @@
-/* revision 7 */
-
 /* @2010 Kamil Kaminski
  *
  * math3d.c
@@ -12,7 +10,7 @@
 #include "math3d.h"
 
 void m3dFindNormalf(M3DVector3f result, const M3DVector3f point1, 
-                   const M3DVector3f point2, const M3DVector3f point3)
+                    const M3DVector3f point2, const M3DVector3f point3)
 {
     M3DVector3f v1, v2;
 
@@ -59,7 +57,7 @@ void m3dLoadIdentity44f(M3DMatrix44f m) /* 4x4 float */
  * planeEq contains the a, b, c, and d of the plane equation coefficients
  */ 
 void m3dGetPlaneEquationf(M3DVector4f planeEq, const M3DVector3f p1,
-                         const M3DVector3f p2, const M3DVector3f p3)
+                          const M3DVector3f p2, const M3DVector3f p3)
 {
     /* get two vectors... do the cross product */
     M3DVector3f v1, v2;
@@ -182,7 +180,7 @@ void m3dMakePlanarShadowMatrixf(M3DMatrix44f proj, const M3DVector4f planeEq,
 
 /* hmm not sure if defines could reside outside of a function as they did */
 void m3dMatrixMultiply44f(M3DMatrix44f product, const M3DMatrix44f a, 
-                         const M3DMatrix44f b)
+                          const M3DMatrix44f b)
 {
     #define A(row,col)  a[(col<<2)+row]
     #define B(row,col)  b[(col<<2)+row]
@@ -223,3 +221,55 @@ void m3dPrintMatrix44f(const M3DMatrix44f m)
     #undef M
 }
 
+/* invert 4x4 matrix, contributed by David Moore (See Mesa bug #6748) */
+int m3dInvertMatrix44f(M3DMatrix44f dst, const M3DMatrix44f m)
+{
+    M3DMatrix44f inv;
+    double det;
+    int i;
+
+    inv[0] =   m[5]*m[10]*m[15] - m[5]*m[11]*m[14] - m[9]*m[6]*m[15]
+             + m[9]*m[7]*m[14]  + m[13]*m[6]*m[11] - m[13]*m[7]*m[10];
+    inv[4] =  -m[4]*m[10]*m[15] + m[4]*m[11]*m[14] + m[8]*m[6]*m[15]
+             - m[8]*m[7]*m[14]  - m[12]*m[6]*m[11] + m[12]*m[7]*m[10];
+    inv[8] =   m[4]*m[9]*m[15]  - m[4]*m[11]*m[13] - m[8]*m[5]*m[15]
+             + m[8]*m[7]*m[13]  + m[12]*m[5]*m[11] - m[12]*m[7]*m[9];
+    inv[12] = -m[4]*m[9]*m[14]  + m[4]*m[10]*m[13] + m[8]*m[5]*m[14]
+             - m[8]*m[6]*m[13]  - m[12]*m[5]*m[10] + m[12]*m[6]*m[9];
+    inv[1] =  -m[1]*m[10]*m[15] + m[1]*m[11]*m[14] + m[9]*m[2]*m[15]
+             - m[9]*m[3]*m[14]  - m[13]*m[2]*m[11] + m[13]*m[3]*m[10];
+    inv[5] =   m[0]*m[10]*m[15] - m[0]*m[11]*m[14] - m[8]*m[2]*m[15]
+             + m[8]*m[3]*m[14]  + m[12]*m[2]*m[11] - m[12]*m[3]*m[10];
+    inv[9] =  -m[0]*m[9]*m[15]  + m[0]*m[11]*m[13] + m[8]*m[1]*m[15]
+             - m[8]*m[3]*m[13]  - m[12]*m[1]*m[11] + m[12]*m[3]*m[9];
+    inv[13] =  m[0]*m[9]*m[14]  - m[0]*m[10]*m[13] - m[8]*m[1]*m[14]
+             + m[8]*m[2]*m[13]  + m[12]*m[1]*m[10] - m[12]*m[2]*m[9];
+    inv[2] =   m[1]*m[6]*m[15]  - m[1]*m[7]*m[14]  - m[5]*m[2]*m[15]
+             + m[5]*m[3]*m[14]  + m[13]*m[2]*m[7]  - m[13]*m[3]*m[6];
+    inv[6] =  -m[0]*m[6]*m[15]  + m[0]*m[7]*m[14]  + m[4]*m[2]*m[15]
+             - m[4]*m[3]*m[14]  - m[12]*m[2]*m[7]  + m[12]*m[3]*m[6];
+    inv[10] =  m[0]*m[5]*m[15]  - m[0]*m[7]*m[13]  - m[4]*m[1]*m[15]
+             + m[4]*m[3]*m[13]  + m[12]*m[1]*m[7]  - m[12]*m[3]*m[5];
+    inv[14] = -m[0]*m[5]*m[14]  + m[0]*m[6]*m[13]  + m[4]*m[1]*m[14]
+             - m[4]*m[2]*m[13]  - m[12]*m[1]*m[6]  + m[12]*m[2]*m[5];
+    inv[3] =  -m[1]*m[6]*m[11]  + m[1]*m[7]*m[10]  + m[5]*m[2]*m[11]
+             - m[5]*m[3]*m[10]  - m[9]*m[2]*m[7]   + m[9]*m[3]*m[6];
+    inv[7] =   m[0]*m[6]*m[11]  - m[0]*m[7]*m[10]  - m[4]*m[2]*m[11]
+             + m[4]*m[3]*m[10]  + m[8]*m[2]*m[7]   - m[8]*m[3]*m[6];
+    inv[11] = -m[0]*m[5]*m[11]  + m[0]*m[7]*m[9]   + m[4]*m[1]*m[11]
+             - m[4]*m[3]*m[9]   - m[8]*m[1]*m[7]   + m[8]*m[3]*m[5];
+    inv[15] =  m[0]*m[5]*m[10]  - m[0]*m[6]*m[9]   - m[4]*m[1]*m[10]
+             + m[4]*m[2]*m[9]   + m[8]*m[1]*m[6]   - m[8]*m[2]*m[5];
+
+    det = m[0]*inv[0] + m[1]*inv[4] + m[2]*inv[8] + m[3]*inv[12];
+    if (det == 0)
+        return -1;
+
+    det = 1.0 / det;
+
+    for (i = 0; i < 16; i++)
+        dst[i] = inv[i] * det;
+
+    return 0;
+}
+
diff --git a/sdl/math3d.h b/sdl/math3d.h
index 670b4af..875757c 100644
--- a/sdl/math3d.h
+++ b/sdl/math3d.h
@@ -18,34 +18,33 @@ typedef float M3DVector3f[3];   /* vector of 3 floats */
 typedef float M3DVector2f[2];   /* vector of 2 floats */
 
 /* math function prototypes */
-void m3dFindNormalf(M3DVector3f, const M3DVector3f, const M3DVector3f, 
-                   const M3DVector3f);
+void m3dFindNormalf(M3DVector3f, const M3DVector3f, const M3DVector3f, const M3DVector3f);
 void m3dLoadIdentity33f(M3DMatrix33f);
 void m3dLoadIdentity44f(M3DMatrix44f);
-void m3dGetPlaneEquationf(M3DVector4f, const M3DVector3f,
-                         const M3DVector3f, const M3DVector3f);
+void m3dGetPlaneEquationf(M3DVector4f, const M3DVector3f, const M3DVector3f, const M3DVector3f);
 void m3dRotationMatrix44f(M3DMatrix44f, float, float, float, float);
 void m3dMakePlanarShadowMatrixf(M3DMatrix44f, const M3DVector4f, const M3DVector3f);
 void m3dMatrixMultiply44f(M3DMatrix44f, const M3DMatrix44f, const M3DMatrix44f);
 void m3dPrintMatrix44f(const M3DMatrix44f);
+int m3dInvertMatrix44f(M3DMatrix44f, const M3DMatrix44f);
 
 static inline void m3dCrossProductf(M3DVector3f result, const M3DVector3f u, 
-                                   const M3DVector3f v)
+                                    const M3DVector3f v)
 {
-    result[0] = u[1] * v[2] - v[1] * u[2];
+    result[0] =  u[1] * v[2] - v[1] * u[2];
     result[1] = -u[0] * v[2] + v[0] * u[2];
-    result[2] = u[0] * v[1] - v[0] * u[1];
+    result[2] =  u[0] * v[1] - v[0] * u[1];
 }
 
-static inline void m3dTransformVector3f(M3DVector3f vOut, const M3DVector3f v, 
-                                       const M3DMatrix44f m)
+static inline void m3dTransformVector3f(M3DVector3f vout, const M3DVector3f v,
+                                        const M3DMatrix44f m)
 {
-    vOut[0] = m[0] * v[0] + m[4] * v[1] + m[8] *  v[2] + m[12];
-    vOut[1] = m[1] * v[0] + m[5] * v[1] + m[9] *  v[2] + m[13];
-    vOut[2] = m[2] * v[0] + m[6] * v[1] + m[10] * v[2] + m[14];
+    vout[0] = m[0] * v[0] + m[4] * v[1] + m[8] *  v[2] + m[12];
+    vout[1] = m[1] * v[0] + m[5] * v[1] + m[9] *  v[2] + m[13];
+    vout[2] = m[2] * v[0] + m[6] * v[1] + m[10] * v[2] + m[14];
 }
 
-static inline void m3dScaleVector3f(M3DVector3f v, float scale)
+static inline void m3dScaleVector3f(M3DVector3f v, const float scale)
 {
     v[0] *= scale;
     v[1] *= scale;
@@ -67,18 +66,19 @@ static inline void m3dNormalizeVectorf(M3DVector3f u)
     m3dScaleVector3f(u, 1.0f / m3dGetVectorLengthf(u));
 }
 
-static inline void m3dTranslateMatrix44f(M3DMatrix44f m, float x, float y, 
-                                        float z)
+static inline void m3dTranslateMatrix44f(M3DMatrix44f m, const float x,
+                                         const float y, const float z)
 {
     m[12] += x;
     m[13] += y;
     m[14] += z;
 }
 
-static inline void m3dScaleMatrix44f(M3DMatrix44f m, float x, float y, float z)
+static inline void m3dScaleMatrix44f(M3DMatrix44f m, const float x,
+                                     const float y, const float z)
 {
-    m[0] *= x;
-    m[5] *= y;
+    m[0]  *= x;
+    m[5]  *= y;
     m[10] *= z;
 }
 
@@ -95,8 +95,7 @@ static inline void m3dScaleMatrix44f(M3DMatrix44f m, float x, float y, float z)
     }                                        \
 }
 
-static inline void m3dTransposeMatrix44f(M3DMatrix44f dst, 
-                                        const M3DMatrix44f src)
+static inline void m3dTransposeMatrix44f(M3DMatrix44f dst, const M3DMatrix44f src)
 {
     TRANSPOSE44(dst, src);
 }
@@ -106,5 +105,35 @@ static inline void m3dCopyVector3f(M3DVector3f dst, const M3DVector3f src)
     memcpy(dst, src, sizeof(M3DVector3f));
 }
 
+static inline void m3dSetMatrixColumn44f(M3DMatrix44f dst, const M3DVector4f src,
+                                        const int col)
+{
+	memcpy(dst + (4 * col), src, sizeof(float) * 4);
+}
+
+
+
+static inline void m3dLoadVector2f(M3DVector2f v, const float x, const float y)
+{
+    v[0] = x;
+    v[1] = y;
+}
+
+static inline void m3dLoadVector3f(M3DVector3f v, const float x, const float y, const float z)
+{
+    v[0] = x;
+    v[1] = y;
+    v[2] = z;
+}
+
+static inline void m3dLoadVector4f(M3DVector4f v, const float x, const float y,
+                                  const float z, const float w)
+{
+    v[0] = x;
+    v[1] = y;
+    v[2] = z;
+    v[3] = w;
+}
+
 #endif
 
diff --git a/sdl/pyramid.c b/sdl/pyramid.c
index a3303b8..d03b4ce 100644
--- a/sdl/pyramid.c
+++ b/sdl/pyramid.c
@@ -199,7 +199,7 @@ static void render(void)
     /* save the matrix state and do the rotations */
     glPushMatrix();
         /* apply camera transform, and draw the ground */
-        glframe_apply_camera_transform(&p.camera);
+        glframe_apply_camera_transform(&p.camera, 0);
         glColor3ub(255, 0, 255);
         glCallList(p.ground_list);
 
@@ -242,6 +242,9 @@ int main(int argc, char **argv)
         render();
 
         fps_control(startclock);
+
+        /* need some sort of a timer to trigger this */
+        /* gl_frame_normalize(&p.camera); */
     }
 
     platform_destroy();