fixed 6dof camera

[o2demo] / src / demo.c

diff --git a/src/demo.c b/src/demo.c
index 5af3c86..e509dfb 100644 (file)
--- a/src/demo.c
+++ b/src/demo.c
@@ -2,13 +2,22 @@
 #include <stdlib.h>
 #include <string.h>
 #include <ctype.h>
+#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 *objmat, float *cammat);
+
 static int console_active;
+static int sball_update_pending;
+
+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)
 {
@@ -28,6 +37,9 @@ int demo_init(int argc, char **argv)
        glEnable(GL_DEPTH_TEST);
        glEnable(GL_CULL_FACE);
 
+       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;
        }
@@ -52,6 +64,11 @@ void demo_cleanup(void)
 
 void demo_draw(void)
 {
+       if(sball_update_pending) {
+               recalc_sball_matrix(sball_obj_matrix, sball_cam_matrix);
+               sball_update_pending = 0;
+       }
+
        scr_update();
        scr_draw();
 }
@@ -165,3 +182,72 @@ void demo_mbutton(int bn, int pressed, int x, int y)
 void demo_mmotion(int x, int y)
 {
 }
+
+void demo_sball_motion(int x, int y, int z)
+{
+       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;
+}
+
+void demo_sball_rotate(int x, int y, int z)
+{
+       float rx = (float)x;
+       float ry = (float)y;
+       float rz = (float)z;
+       float axis_len = sqrt(rx * rx + ry * ry + rz * rz);
+       if(axis_len > 0.0) {
+               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;
+}
+
+void demo_sball_button(int bn, int pressed)
+{
+       if(!pressed) return;
+
+       switch(bn) {
+       case 0:
+               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:
+               break;
+       }
+}
+
+
+static void recalc_sball_matrix(float *objmat, float *cammat)
+{
+       float rmat[16], tmat[16];
+
+       cgm_mrotation_quat(rmat, &cam_rot);
+       cgm_mtranspose(rmat);
+
+       cgm_mtranslation(tmat, -cam_pos.x, -cam_pos.y, -cam_pos.z);
+       cgm_mcopy(cammat, tmat);
+       cgm_mmul(cammat, rmat);
+
+       cgm_mrotation_quat(rmat, &obj_rot);
+       cgm_mtranspose(rmat);
+
+       cgm_mtranslation(tmat, obj_pos.x, obj_pos.y, obj_pos.z);
+       cgm_mcopy(objmat, rmat);
+       cgm_mmul(objmat, tmat);
+}