I hope someone can help me debug my code for performing rotations about three axiis using quaternions, and so avoiding gimbal-lock. I wrote the code after trawling the internet for information, but unfortunately the code I've written still suffers from the dreaded gimbal-lock! The information was mainly extracted from "www.gamasutra.com" and "www.flipcode.com/documents/matrfaq.html". I'm using the OpenGL API, and perform the rotation by calling glMultMatrixf(). The code is as follows: /*************************************************************************************************************** FILE NAME: rotate.h AUTHOR: Justin C. Nixon CREATED/UPDATED: 24/06/2000 24/06/2000 NOTES: ***************************************************************************************************************/ #ifndef ROTATE_H #define ROTATE_H typedef float RADIANS; typedef float DEGREES; #ifndef PI #define PI 3.14159265358979323846f #endif #define RADIANS_TO_DEGREES 360.0f/(2*PI) struct axis_angle { RADIANS x, y, z; }; struct quaternion { float w, x, y, z; }; struct attitude { DEGREES roll, pitch, heading; }; void normalise_quaternion(struct quaternion *); // Normalise a quaternion. void multiply_quaternion(struct quaternion *result_p, struct quaternion *a_p, struct quaternion *b_p); // Multiply two quaternions. void axis_angles_to_quaternion(struct quaternion *, struct axis_angle *); // Convert axis angles to a quaternion. void quaternion_to_matrix(struct quaternion *, float *); void quaternion_to_attitude(struct quaternion *, struct attitude *); // Convert a quaternion to familiar roll, pitch, and heading values. #endif void normalise_quaternion(struct quaternion *quaternion_p) { float magnitude; magnitude=(quaternion_p->w*quaternion_p->w)+(quaternion_p->x*quaternion_p->x)+(quaternion_p->y*quaternion_p->y)+(quaternion_p->z*quaternion_p->z); // Find the magnitude. // Divide by the magnitude to normalise. quaternion_p->w=quaternion_p->w/magnitude; quaternion_p->x=quaternion_p->x/magnitude; quaternion_p->y=quaternion_p->y/magnitude; quaternion_p->z=quaternion_p->z/magnitude; } void multiply_quaternion(struct quaternion *result_p, struct quaternion *quaternion1_p, struct quaternion *quaternion2_p) { float a, b, c, d, e, f, g, h; a=(quaternion1_p->w+quaternion1_p->x)*(quaternion2_p->w+quaternion2_p->x); b=(quaternion1_p->z-quaternion1_p->y)*(quaternion2_p->y-quaternion2_p->z); c=(quaternion1_p->x-quaternion1_p->w)*(quaternion2_p->y+quaternion2_p->z); d=(quaternion1_p->y+quaternion1_p->z)*(quaternion2_p->x-quaternion2_p->w); e=(quaternion1_p->x+quaternion1_p->z)*(quaternion2_p->x+quaternion2_p->y); f=(quaternion1_p->x-quaternion1_p->z)*(quaternion2_p->x-quaternion2_p->y); g=(quaternion1_p->w+quaternion1_p->y)*(quaternion2_p->w-quaternion2_p->z); h=(quaternion1_p->w-quaternion1_p->y)*(quaternion2_p->w+quaternion2_p->z); result_p->w=b+((-e-f+g+h)/2); result_p->x=a-((e+f+g+h)/2); result_p->y=-c+((e-f+g-h)/2); result_p->z=-d+((e-f-g+h)/2); normalise_quaternion(result_p); } void axis_angles_to_quaternion(struct quaternion *quaternion_p, struct axis_angle *axis_angle_p) { struct quaternion quaternion_wxy, quaternion_wx, quaternion_wy, quaternion_wz; quaternion_wx.w=(float)cos(axis_angle_p->x/2.0f); quaternion_wx.x=(float)sin(axis_angle_p->x/2.0f); quaternion_wx.y=0.0f; quaternion_wx.z=0.0f; normalise_quaternion(&quaternion_wx); quaternion_wy.w=(float)cos(axis_angle_p->y/2.0f); quaternion_wy.x=0.0f; quaternion_wy.y=(float)sin(axis_angle_p->y/2.0f); quaternion_wy.z=0.0f; normalise_quaternion(&quaternion_wy); quaternion_wz.w=(float)cos(axis_angle_p->z/2.0f); quaternion_wz.x=0.0f; quaternion_wz.y=0.0f; quaternion_wz.z=(float)sin(axis_angle_p->z/2.0f); normalise_quaternion(&quaternion_wz); multiply_quaternion(&quaternion_wxy, &quaternion_wx, &quaternion_wy); multiply_quaternion(quaternion_p, &quaternion_wxy, &quaternion_wz); } void quaternion_to_matrix(struct quaternion *quaternion_p, float *matrix_p) { float xx, xy, xz, xw, yy, yz, yw, zz, zw; xx=quaternion_p->x*quaternion_p->x; xy=quaternion_p->x*quaternion_p->y; xz=quaternion_p->x*quaternion_p->z; xw=quaternion_p->x*quaternion_p->w; yy=quaternion_p->y*quaternion_p->y; yz=quaternion_p->y*quaternion_p->z; yw=quaternion_p->y*quaternion_p->w; zz=quaternion_p->z*quaternion_p->z; zw=quaternion_p->z*quaternion_p->w; *matrix_p=1.0f-2.0f*(yy+zz); // Element 0 matrix_p++; *matrix_p=2.0f*(xy-zw); // Element 1 matrix_p++; *matrix_p=2.0f*(xz+yw); // Element 2 matrix_p++; *matrix_p=0.0f; // Element 3 matrix_p++; *matrix_p=2.0f*(xy+zw); // Element 4 matrix_p++; *matrix_p=1.0f-2.0f*(xx+zz); // Element 5 matrix_p++; *matrix_p=2.0f*(yz-xw); // Element 6 matrix_p++; *matrix_p=0.0f; // Element 7 matrix_p++; *matrix_p=2.0f*(xz-yw); // Element 8 matrix_p++; *matrix_p=2.0f*(yz+xw); // Element 9 matrix_p++; *matrix_p=1.0f-2.0f*(xx+yy); // Element 10 matrix_p++; *matrix_p=0.0f; // Element 11 matrix_p++; *matrix_p=0.0f; // Element 12 matrix_p++; *matrix_p=0.0f; // Element 13 matrix_p++; *matrix_p=0.0f; // Element 14 matrix_p++; *matrix_p=1.0f; // Element 15 } void quaternion_to_attitude(struct quaternion *quaternion_p, struct attitude *attitude_p) { float sine_angle, tx, ty, tz; attitude_p->roll=(float)acos(quaternion_p->w)*2*RADIANS_TO_DEGREES; // Calculate roll. sine_angle=(float)sqrt(1.0f-(quaternion_p->w*quaternion_p->w)); // Calculate sine angle. if(fabs(sine_angle)<0.0005f) sine_angle=1.0f; // Adjust sine angle if required. tx=quaternion_p->x/sine_angle; ty=quaternion_p->y/sine_angle; tz=quaternion_p->z/sine_angle; attitude_p->pitch=(float)-asin(ty)*RADIANS_TO_DEGREES; // Calculate pitch. if((tx*tx)+(tz*tz)<0.0005f) { attitude_p->heading=0.0f; // Set heading. } else { attitude_p->heading=(float)atan2(tx,tz)*RADIANS_TO_DEGREES; // Calculate heading. } if(attitude_p->heading<0.0f) attitude_p->heading+=360.0f; // Adjust heading if required. } Edited by - Justin Nixon on 7/2/00 12:07:12 PM

Howdy, I don''t really know how to fix that (I don''t do my cameras that way), but isn''t a setup like this easier when it comes to visualization of the "camera"?

camera.h
========

#define HSPEED 100
#define VSPEED 100
#define ANGLESPEED .05
#define 2PI 6.283185

class camera
{
double eye[3], center[3], up[3], angle[3];
bool crosshair;

public:
camera();
camera(double eyeX, double eyeY, double eyeZ, double centerX, double centerY, double centerZ, double upX, double upY, double upZ, double angleX, double angleY, double angleZ);
void toggleCrosshair();
void applyCamera();
int setEye(int i, double value) { eye = value; return 0; }
int setCenter(int i, double value) { center = value; return 0; }<br> int setUp(int i, double value) { up = value; return 0; }<br> int setAngle(int i, double value) { if (value >= 2PI) value -= 2PI; else if(value <= -2PI) value += 2PI; angle = value; return 0; }<br> double getEye(int i) { return eye; }<br> double getCenter(int i) { return center; }<br> double getUp(int i) { return up; }<br> double getAngle(int i) { return angle; }<br> int slideLeft();<br> int slideRight();<br> int moveForward();<br> int moveBackward();<br> int moveUp();<br> int moveDown();<br> int lookLeft();<br> int lookRight();<br> int lookUp();<br> int lookDown();<br>};<br><br>camera.cpp /* Doesn''t have rolling yet… But that is a simple definition of angle[2], and another couple function calls. */<br>==========<br><br>#include <GL\glu.h><br>#include <math.h><br><br>#include "camera.h"<br><br>camera::camera()<br>{<br> crosshair = 1;<br> <br> eye[0] = eye[1] = eye[2] = 0.0;<br> <br> center[0] = center[1] = 0.0;<br> center[2] = 1.0;<br> <br> up[0] = up[2] = 0.0;<br> up[1] = 1.0;<br><br> angle[0] = angle[1] = angle[2] = 0.0;<br>}<br><br>camera::camera(double eyeX, double eyeY, double eyeZ, double centerX, double centerY, double centerZ, double upX, double upY, double upZ, double angleX, double angleY, double angleZ)<br>{<br> crosshair = 1;<br> <br> eye[0] = eyeX;<br> eye[1] = eyeY;<br> eye[2] = eyeZ;<br> <br> center[0] = centerX;<br> center[1] = centerY;<br> center[2] = centerZ;<br> <br> up[0] = upX;<br> up[1] = upY;<br> up[2] = upZ;<br><br> angle[0] = angleX;<br> angle[1] = angleY;<br> angle[2] = angleZ;<br>}<br><br>void camera::toggleCrosshair()<br>{<br> crosshair = !crosshair;<br>}<br><br>void camera::applyCamera()<br>{<br> gluLookAt(getEye(0), getEye(1), getEye(2), getCenter(0), getCenter(1), getCenter(2), getUp(0), getUp(1), getUp(2));<br><br> if (!crosshair) return;<br>}<br><br>int camera::slideLeft()<br>{<br> double angle = getAngle(0);<br> double Q = cos(getAngle(1));<br><br> setEye(0, getEye(0) + Q*cos(angle));<br> setEye(2, getEye(2) - Q*sin(angle));<br><br> setCenter(0, getEye(0) + Q*sin(angle));<br> setCenter(2, getEye(2) + Q*cos(angle));<br><br> return 0;<br>}<br><br>int camera::slideRight()<br>{<br> double angle = getAngle(0);<br> double Q = cos(getAngle(1));<br><br> setEye(0, getEye(0) - Q*cos(angle));<br> setEye(2, getEye(2) + Q*sin(angle));<br><br> setCenter(0, getEye(0) + Q*sin(angle));<br> setCenter(2, getEye(2) + Q*cos(angle));<br><br> return 0;<br>}<br><br>int camera::moveForward()<br>{<br> double angle = getAngle(0);<br> double angle1 = getAngle(1);<br> double Q = cos(angle1);<br><br> setEye(0, getEye(0) + Q*sin(angle));<br> setEye(1, getEye(1) + sin(angle1));<br> setEye(2, getEye(2) + Q*cos(angle));<br> <br> setCenter(0, getEye(0) + Q*sin(angle));<br> setCenter(1, getEye(1) + sin(angle1));<br> setCenter(2, getEye(2) + Q*cos(angle));<br><br> return 0;<br>}<br><br>int camera::moveBackward()<br>{<br> double angle = getAngle(0);<br> double angle1 = getAngle(1);<br> double Q = cos(angle1);<br><br> setEye(0, getEye(0) - Q*sin(angle));<br> setEye(1, getEye(1) - sin(angle1));<br> setEye(2, getEye(2) - Q*cos(angle));<br> <br> setCenter(0, getEye(0) + Q*sin(angle));<br> setCenter(1, getEye(1) + sin(angle1));<br> setCenter(2, getEye(2) + Q*cos(angle));<br><br> return 0;<br>}<br><br>int camera::moveUp()<br>{<br> setEye(1, getEye(1) + VSPEED);<br> setCenter(1, getEye(1) + sin(getAngle(1)));<br> <br> return 0;<br>}<br><br>int camera::moveDown()<br>{<br> setEye(1, getEye(1) - VSPEED);<br> setCenter(1, getEye(1) + sin(getAngle(1)));<br> <br> return 0;<br>}<br><br>int camera::lookLeft()<br>{<br> setAngle(0, getAngle(0) + ANGLESPEED);<br><br> double angle = getAngle(0);<br> double Q = cos(getAngle(1));<br> <br> setCenter(0, getEye(0) + Q*sin(angle));<br> setCenter(2, getEye(2) + Q*cos(angle));<br> <br> return 0;<br>}<br><br><br>int camera::lookRight()<br>{<br> setAngle(0, getAngle(0) - ANGLESPEED);<br><br> double angle = getAngle(0);<br> double Q = cos(getAngle(1));<br><br> setCenter(0, getEye(0) + Q*sin(angle));<br> setCenter(2, getEye(2) + Q*cos(angle));<br> <br> return 0;<br>}<br><br>int camera::lookUp()<br>{<br> if (getAngle(1) >= 1.55) return 0;<br> <br> setAngle(1, getAngle(1) + ANGLESPEED);<br><br> double Q = cos(getAngle(1));<br><br> setCenter(0, getEye(0) + Q*sin(getAngle(0)));<br> setCenter(1, getEye(1) + sin(getAngle(1)));<br> setCenter(2, getEye(2) + Q*cos(getAngle(0)));<br> <br> return 0;<br>}<br><br>int camera::lookDown()<br>{<br> if (getAngle(1) <= -1.55) return 0;<br><br> setAngle(1, getAngle(1) - ANGLESPEED);<br> <br> double Q = cos(getAngle(1));<br><br> setCenter(0, getEye(0) + Q*sin(getAngle(0)));<br> setCenter(1, getEye(1) + sin(getAngle(1)));<br> setCenter(2, getEye(2) + Q*cos(getAngle(0)));<br><br> return 0;<br>}<br><br>Is this code effective, or am I being silly trying to do this this way? </i>