fixed 6dof camera

diff --git a/src/demo.c b/src/demo.c
index 2e444cf..e509dfb 100644 (file)
--- a/src/demo.c
+++ b/src/demo.c
@@ -5,17 +5,19 @@
 #include <math.h>
 #include <GL/gl.h>
 #include <GL/glu.h>
+#include <cgmath/cgmath.h>
 #include "demo.h"
 #include "screen.h"
 #include "cfgopt.h"
 
-static void recalc_sball_matrix(float *xform, float *inv_xform);
+static void recalc_sball_matrix(float *objmat, float *cammat);
 
 static int console_active;
 static int sball_update_pending;
 
-static float pos[3];
-static float rot[4] = {0, 0, 0, 1};
+static cgm_vec3 obj_pos, cam_pos;
+static cgm_quat obj_rot = {0, 0, 0, 1};
+static cgm_quat cam_rot = {0, 0, 0, 1};
 
 int demo_init(int argc, char **argv)
 {
@@ -35,8 +37,8 @@ int demo_init(int argc, char **argv)
        glEnable(GL_DEPTH_TEST);
        glEnable(GL_CULL_FACE);
 
-       sball_matrix[0] = sball_matrix[5] = sball_matrix[10] = sball_matrix[15] = 1.0f;
-       sball_inv_matrix[0] = sball_inv_matrix[5] = sball_inv_matrix[10] = sball_inv_matrix[15] = 1.0f;
+       sball_obj_matrix[0] = sball_obj_matrix[5] = sball_obj_matrix[10] = sball_obj_matrix[15] = 1.0f;
+       sball_cam_matrix[0] = sball_cam_matrix[5] = sball_cam_matrix[10] = sball_cam_matrix[15] = 1.0f;
 
        if(scr_init() == -1) {
                return -1;
@@ -63,7 +65,7 @@ void demo_cleanup(void)
 void demo_draw(void)
 {
        if(sball_update_pending) {
-               recalc_sball_matrix(sball_matrix, sball_inv_matrix);
+               recalc_sball_matrix(sball_obj_matrix, sball_cam_matrix);
                sball_update_pending = 0;
        }
 
@@ -181,16 +183,17 @@ void demo_mmotion(int x, int y)
 {
 }
 
-
-static void quat_rotate(float *qres, const float *q, float angle, float x, float y, float z);
-static void quat_mul(float *qres, const float *q1, const float *q2);
-static void quat_to_mat(float *res, const float *q);
-
 void demo_sball_motion(int x, int y, int z)
 {
-       pos[0] += (float)x * 0.01;
-       pos[1] += (float)y * 0.01;
-       pos[2] -= (float)z * 0.01;
+       cgm_vec3 dir;
+       dir.x = (float)x * 0.001;
+       dir.y = (float)y * 0.001;
+       dir.z = (float)-z * 0.001;
+       cgm_vadd(&obj_pos, &dir);
+
+       cgm_vrotate_quat(&dir, &cam_rot);
+       cgm_vadd(&cam_pos, &dir);
+
        sball_update_pending = 1;
 }
 
@@ -201,8 +204,12 @@ void demo_sball_rotate(int x, int y, int z)
        float rz = (float)z;
        float axis_len = sqrt(rx * rx + ry * ry + rz * rz);
        if(axis_len > 0.0) {
-               quat_rotate(rot, rot, axis_len * 0.001, -rx / axis_len,
-                               -ry / axis_len, rz / axis_len);
+               cgm_quat q;
+               cgm_qrotation(&q, -axis_len * 0.001, rx / axis_len, ry / axis_len, -rz / axis_len);
+               cgm_qmul(&obj_rot, &q);
+
+               cgm_qrotation(&q, axis_len * 0.001, rx / axis_len, ry / axis_len, -rz / axis_len);
+               cgm_qmul(&cam_rot, &q);
        }
        sball_update_pending = 1;
 }
@@ -213,9 +220,11 @@ void demo_sball_button(int bn, int pressed)
 
        switch(bn) {
        case 0:
-               pos[0] = pos[1] = pos[2] = 0;
-               rot[0] = rot[1] = rot[2] = 0;
-               rot[3] = 1;
+               cgm_vcons(&obj_pos, 0, 0, 0);
+               cgm_vcons(&cam_pos, 0, 0, 0);
+               cgm_qcons(&obj_rot, 0, 0, 0, 1);
+               cgm_qcons(&cam_rot, 0, 0, 0, 1);
+               sball_update_pending = 1;
                break;
 
        default:
@@ -223,78 +232,22 @@ void demo_sball_button(int bn, int pressed)
        }
 }
 
-static void recalc_sball_matrix(float *xform, float *inv_xform)
-{
-       float tx, ty, tz;
-
-       quat_to_mat(xform, rot);
-       xform[12] = pos[0];
-       xform[13] = pos[1];
-       xform[14] = pos[2];
-
-       inv_xform[0] = xform[0];
-       inv_xform[5] = xform[5];
-       inv_xform[10] = xform[10];
-       inv_xform[15] = 1.0f;
-
-       inv_xform[1] = xform[4];
-       inv_xform[2] = xform[8];
-       inv_xform[6] = xform[9];
-       inv_xform[4] = xform[1];
-       inv_xform[8] = xform[2];
-       inv_xform[9] = xform[6];
-
-       inv_xform[3] = inv_xform[7] = inv_xform[11] = 0.0f;
-       tx = -pos[0];
-       ty = -pos[1];
-       tz = -pos[2];
-
-       inv_xform[12] = tx * inv_xform[0] + ty * inv_xform[4] + tz * inv_xform[8];
-       inv_xform[13] = tx * inv_xform[1] + ty * inv_xform[5] + tz * inv_xform[9];
-       inv_xform[14] = tx * inv_xform[2] + ty * inv_xform[6] + tz * inv_xform[10];
-}
 
-static void quat_rotate(float *qres, const float *q, float angle, float x, float y, float z)
+static void recalc_sball_matrix(float *objmat, float *cammat)
 {
-       float rq[4];
-       float half_angle = angle * 0.5f;
-       float sin_half = sin(half_angle);
+       float rmat[16], tmat[16];
 
-       rq[3] = cos(half_angle);
-       rq[0] = x * sin_half;
-       rq[1] = y * sin_half;
-       rq[2] = z * sin_half;
+       cgm_mrotation_quat(rmat, &cam_rot);
+       cgm_mtranspose(rmat);
 
-       quat_mul(qres, q, rq);
-}
+       cgm_mtranslation(tmat, -cam_pos.x, -cam_pos.y, -cam_pos.z);
+       cgm_mcopy(cammat, tmat);
+       cgm_mmul(cammat, rmat);
 
-static void quat_mul(float *qres, const float *q1, const float *q2)
-{
-       float w = q1[3] * q2[3] - (q1[0] * q2[0] + q1[1] * q2[1] + q1[2] * q2[2]);
-       float x = q2[0] * q1[3] + q1[0] * q2[3] + (q1[1] * q2[2] - q1[2] * q2[1]);
-       float y = q2[1] * q1[3] + q1[1] * q2[3] + (q1[2] * q2[0] - q1[0] * q2[2]);
-       float z = q2[2] * q1[3] + q1[2] * q2[3] + (q1[0] * q2[1] - q1[1] * q2[0]);
-       qres[0] = x;
-       qres[1] = y;
-       qres[2] = z;
-       qres[3] = w;
-}
+       cgm_mrotation_quat(rmat, &obj_rot);
+       cgm_mtranspose(rmat);
 
-static void quat_to_mat(float *res, const float *q)
-{
-       res[0] = 1.0f - 2.0f * q[1]*q[1] - 2.0f * q[2]*q[2];
-       res[1] = 2.0f * q[0] * q[1] - 2.0f * q[3] * q[2];
-       res[2] = 2.0f * q[2] * q[0] + 2.0f * q[3] * q[1];
-       res[3] = 0.0f;
-       res[4] = 2.0f * q[0] * q[1] + 2.0f * q[3] * q[2];
-       res[5] = 1.0f - 2.0f * q[0]*q[0] - 2.0f * q[2]*q[2];
-       res[6] = 2.0f * q[1] * q[2] - 2.0f * q[3] * q[0];
-       res[7] = 0.0f;
-       res[8] = 2.0f * q[2] * q[0] - 2.0f * q[3] * q[1];
-       res[9] = 2.0f * q[1] * q[2] + 2.0f * q[3] * q[0];
-       res[10] = 1.0f - 2.0f * q[0]*q[0] - 2.0f * q[1]*q[1];
-       res[11] = 0.0f;
-       res[12] = res[13] = res[14] = 0.0f;
-       res[15] = 1.0f;
+       cgm_mtranslation(tmat, obj_pos.x, obj_pos.y, obj_pos.z);
+       cgm_mcopy(objmat, rmat);
+       cgm_mmul(objmat, tmat);
 }
-

diff --git a/src/demo.h b/src/demo.h
index 3bb190a..3e88036 100644 (file)
--- a/src/demo.h
+++ b/src/demo.h
@@ -5,7 +5,7 @@ int win_width, win_height;
 float win_aspect;
 unsigned int time_msec;
 
-float sball_matrix[16], sball_inv_matrix[16];
+float sball_obj_matrix[16], sball_cam_matrix[16];
 
 int demo_init(int argc, char **argv);
 void demo_cleanup(void);

diff --git a/src/parts/example.c b/src/parts/example.c
index d52f20f..8b9f58f 100644 (file)
--- a/src/parts/example.c
+++ b/src/parts/example.c
@@ -33,7 +33,7 @@ struct screen *example_screen(void)
 
 static int init(void)
 {
-       if(gen_torus_mesh(&torus, 1.0f, 0.35, 24, 12) == -1) {
+       if(gen_torus_mesh(&torus, 1.0f, 0.15, 24, 12) == -1) {
                return -1;
        }
        return 0;
@@ -64,10 +64,11 @@ static void draw(void)
 
        glMatrixMode(GL_MODELVIEW);
        glLoadIdentity();
-       glMultMatrixf(sball_inv_matrix);
-       glTranslatef(0, 0, -cam_dist);
+       /*glTranslatef(0, 0, -cam_dist);
        glRotatef(cam_phi, 1, 0, 0);
-       glRotatef(cam_theta, 0, 1, 0);
+       glRotatef(cam_theta, 0, 1, 0);*/
+
+       glMultMatrixf(sball_cam_matrix);
 
        /* draw floor */
        glPushMatrix();
@@ -80,10 +81,10 @@ static void draw(void)
 
        glBegin(GL_QUADS);
        glNormal3f(0, 1, 0);
-       glVertex3f(-2, 0, 2);
-       glVertex3f(2, 0, 2);
-       glVertex3f(2, 0, -2);
-       glVertex3f(-2, 0, -2);
+       glVertex3f(-4, 0, 4);
+       glVertex3f(4, 0, 4);
+       glVertex3f(4, 0, -4);
+       glVertex3f(-4, 0, -4);
        glEnd();
 
        glPopMatrix();
@@ -100,6 +101,10 @@ static void draw(void)
        glMaterialfv(GL_FRONT_AND_BACK, GL_AMBIENT_AND_DIFFUSE, color);
 
        draw_mesh(&torus);
+       glRotatef(90, 1, 0, 0);
+       draw_mesh(&torus);
+       glRotatef(90, 0, 0, 1);
+       draw_mesh(&torus);
 
        glPopMatrix();
 }