helper_3dmath.h

00001 #ifndef _HELPER_3DMATH_H_
00002 #define _HELPER_3DMATH_H_
00003 
00004 class Quaternion {
00005     public:
00006         float w;
00007         float x;
00008         float y;
00009         float z;
00010 
00011         Quaternion() {
00012             w = 1.0f;
00013             x = 0.0f;
00014             y = 0.0f;
00015             z = 0.0f;
00016         }
00017 
00018         Quaternion(float nw, float nx, float ny, float nz) {
00019             w = nw;
00020             x = nx;
00021             y = ny;
00022             z = nz;
00023         }
00024 
00025         Quaternion getProduct(Quaternion q) {
00026             // Quaternion multiplication is defined by:
00027             //     (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
00028             //     (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
00029             //     (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
00030             //     (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
00031             return Quaternion(
00032                 w*q.w - x*q.x - y*q.y - z*q.z,  // new w
00033                 w*q.x + x*q.w + y*q.z - z*q.y,  // new x
00034                 w*q.y - x*q.z + y*q.w + z*q.x,  // new y
00035                 w*q.z + x*q.y - y*q.x + z*q.w); // new z
00036         }
00037 
00038         Quaternion getConjugate() {
00039             return Quaternion(w, -x, -y, -z);
00040         }
00041 
00042         float getMagnitude() {
00043             return sqrt((float)(w*w + x*x + y*y + z*z));
00044         }
00045 
00046         void normalize() {
00047             float m = getMagnitude();
00048             w /= m;
00049             x /= m;
00050             y /= m;
00051             z /= m;
00052         }
00053 
00054         Quaternion getNormalized() {
00055             Quaternion r(w, x, y, z);
00056             r.normalize();
00057             return r;
00058         }
00059 };
00060 
00061 class VectorInt16 {
00062     public:
00063         int16_t x;
00064         int16_t y;
00065         int16_t z;
00066 
00067         VectorInt16() {
00068             x = 0;
00069             y = 0;
00070             z = 0;
00071         }
00072 
00073         VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
00074             x = nx;
00075             y = ny;
00076             z = nz;
00077         }
00078 
00079         float getMagnitude() {
00080             return sqrt((float)(x*x + y*y + z*z));
00081         }
00082 
00083         void normalize() {
00084             float m = getMagnitude();
00085             x /= m;
00086             y /= m;
00087             z /= m;
00088         }
00089 
00090         VectorInt16 getNormalized() {
00091             VectorInt16 r(x, y, z);
00092             r.normalize();
00093             return r;
00094         }
00095 
00096         void rotate(Quaternion *q) {
00097             // http://www.cprogramming.com/tutorial/3d/quaternions.html
00098             // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
00099             // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
00100             // ^ or: http://webcache.googleusercontent.com/search?q=cache:xgJAp3bDNhQJ:content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation&hl=en&gl=us&strip=1
00101 
00102             // P_out = q * P_in * conj(q)
00103             // - P_out is the output vector
00104             // - q is the orientation quaternion
00105             // - P_in is the input vector (a*aReal)
00106             // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
00107             Quaternion p(0, x, y, z);
00108 
00109             // quaternion multiplication: q * p, stored back in p
00110             p = q -> getProduct(p);
00111 
00112             // quaternion multiplication: p * conj(q), stored back in p
00113             p = p.getProduct(q -> getConjugate());
00114 
00115             // p quaternion is now [0, x', y', z']
00116             x = p.x;
00117             y = p.y;
00118             z = p.z;
00119         }
00120 
00121         VectorInt16 getRotated(Quaternion *q) {
00122             VectorInt16 r(x, y, z);
00123             r.rotate(q);
00124             return r;
00125         }
00126 };
00127 
00128 class VectorFloat {
00129     public:
00130         float x;
00131         float y;
00132         float z;
00133 
00134         VectorFloat() {
00135             x = 0;
00136             y = 0;
00137             z = 0;
00138         }
00139 
00140         VectorFloat(float nx, float ny, float nz) {
00141             x = nx;
00142             y = ny;
00143             z = nz;
00144         }
00145 
00146         float getMagnitude() {
00147             return sqrt((float)(x*x + y*y + z*z));
00148         }
00149 
00150         void normalize() {
00151             float m = getMagnitude();
00152             x /= m;
00153             y /= m;
00154             z /= m;
00155         }
00156 
00157         VectorFloat getNormalized() {
00158             VectorFloat r(x, y, z);
00159             r.normalize();
00160             return r;
00161         }
00162 
00163         void rotate(Quaternion *q) {
00164             Quaternion p(0, x, y, z);
00165 
00166             // quaternion multiplication: q * p, stored back in p
00167             p = q -> getProduct(p);
00168 
00169             // quaternion multiplication: p * conj(q), stored back in p
00170             p = p.getProduct(q -> getConjugate());
00171 
00172             // p quaternion is now [0, x', y', z']
00173             x = p.x;
00174             y = p.y;
00175             z = p.z;
00176         }
00177 
00178         VectorFloat getRotated(Quaternion *q) {
00179             VectorFloat r(x, y, z);
00180             r.rotate(q);
00181             return r;
00182         }
00183 };
00184 
00185 #endif /* _HELPER_3DMATH_H_ */