helper_3dmath.h

00001 // I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
00002 // 6/5/2012 by Jeff Rowberg <jeff@rowberg.net>
00003 // Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
00004 //
00005 // Changelog:
00006 //     2012-06-05 - add 3D math helper file to DMP6 example sketch
00007 
00008 /* ============================================
00009 I2Cdev device library code is placed under the MIT license
00010 Copyright (c) 2012 Jeff Rowberg
00011 
00012 Permission is hereby granted, free of charge, to any person obtaining a copy
00013 of this software and associated documentation files (the "Software"), to deal
00014 in the Software without restriction, including without limitation the rights
00015 to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
00016 copies of the Software, and to permit persons to whom the Software is
00017 furnished to do so, subject to the following conditions:
00018 
00019 The above copyright notice and this permission notice shall be included in
00020 all copies or substantial portions of the Software.
00021 
00022 THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
00023 IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
00024 FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
00025 AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
00026 LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
00027 OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
00028 THE SOFTWARE.
00029 ===============================================
00030 */
00031 
00032 #ifndef _HELPER_3DMATH_H_
00033 #define _HELPER_3DMATH_H_
00034 
00035 class Quaternion {
00036     public:
00037         float w;
00038         float x;
00039         float y;
00040         float z;
00041         
00042         Quaternion() {
00043             w = 1.0f;
00044             x = 0.0f;
00045             y = 0.0f;
00046             z = 0.0f;
00047         }
00048         
00049         Quaternion(float nw, float nx, float ny, float nz) {
00050             w = nw;
00051             x = nx;
00052             y = ny;
00053             z = nz;
00054         }
00055 
00056         Quaternion getProduct(Quaternion q) {
00057             // Quaternion multiplication is defined by:
00058             //     (Q1 * Q2).w = (w1w2 - x1x2 - y1y2 - z1z2)
00059             //     (Q1 * Q2).x = (w1x2 + x1w2 + y1z2 - z1y2)
00060             //     (Q1 * Q2).y = (w1y2 - x1z2 + y1w2 + z1x2)
00061             //     (Q1 * Q2).z = (w1z2 + x1y2 - y1x2 + z1w2
00062             return Quaternion(
00063                 w*q.w - x*q.x - y*q.y - z*q.z,  // new w
00064                 w*q.x + x*q.w + y*q.z - z*q.y,  // new x
00065                 w*q.y - x*q.z + y*q.w + z*q.x,  // new y
00066                 w*q.z + x*q.y - y*q.x + z*q.w); // new z
00067         }
00068 
00069         Quaternion getConjugate() {
00070             return Quaternion(w, -x, -y, -z);
00071         }
00072         
00073         float getMagnitude() {
00074             return sqrt(w*w + x*x + y*y + z*z);
00075         }
00076         
00077         void normalize() {
00078             float m = getMagnitude();
00079             w /= m;
00080             x /= m;
00081             y /= m;
00082             z /= m;
00083         }
00084         
00085         Quaternion getNormalized() {
00086             Quaternion r(w, x, y, z);
00087             r.normalize();
00088             return r;
00089         }
00090 };
00091 
00092 class VectorInt16 {
00093     public:
00094         int16_t x;
00095         int16_t y;
00096         int16_t z;
00097 
00098         VectorInt16() {
00099             x = 0;
00100             y = 0;
00101             z = 0;
00102         }
00103         
00104         VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
00105             x = nx;
00106             y = ny;
00107             z = nz;
00108         }
00109 
00110         float getMagnitude() {
00111             return sqrt((float)(x*x + y*y + z*z));
00112         }
00113 
00114         void normalize() {
00115             float m = getMagnitude();
00116             x /= m;
00117             y /= m;
00118             z /= m;
00119         }
00120         
00121         VectorInt16 getNormalized() {
00122             VectorInt16 r(x, y, z);
00123             r.normalize();
00124             return r;
00125         }
00126         
00127         void rotate(Quaternion *q) {
00128             // http://www.cprogramming.com/tutorial/3d/quaternions.html
00129             // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
00130             // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
00131             // ^ 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
00132         
00133             // P_out = q * P_in * conj(q)
00134             // - P_out is the output vector
00135             // - q is the orientation quaternion
00136             // - P_in is the input vector (a*aReal)
00137             // - conj(q) is the conjugate of the orientation quaternion (q=[w,x,y,z], q*=[w,-x,-y,-z])
00138             Quaternion p(0, x, y, z);
00139 
00140             // quaternion multiplication: q * p, stored back in p
00141             p = q -> getProduct(p);
00142 
00143             // quaternion multiplication: p * conj(q), stored back in p
00144             p = p.getProduct(q -> getConjugate());
00145 
00146             // p quaternion is now [0, x', y', z']
00147             x = p.x;
00148             y = p.y;
00149             z = p.z;
00150         }
00151 
00152         VectorInt16 getRotated(Quaternion *q) {
00153             VectorInt16 r(x, y, z);
00154             r.rotate(q);
00155             return r;
00156         }
00157 };
00158 
00159 class VectorFloat {
00160     public:
00161         float x;
00162         float y;
00163         float z;
00164 
00165         VectorFloat() {
00166             x = 0;
00167             y = 0;
00168             z = 0;
00169         }
00170         
00171         VectorFloat(float nx, float ny, float nz) {
00172             x = nx;
00173             y = ny;
00174             z = nz;
00175         }
00176 
00177         float getMagnitude() {
00178             return sqrt(x*x + y*y + z*z);
00179         }
00180 
00181         void normalize() {
00182             float m = getMagnitude();
00183             x /= m;
00184             y /= m;
00185             z /= m;
00186         }
00187         
00188         VectorFloat getNormalized() {
00189             VectorFloat r(x, y, z);
00190             r.normalize();
00191             return r;
00192         }
00193         
00194         void rotate(Quaternion *q) {
00195             Quaternion p(0, x, y, z);
00196 
00197             // quaternion multiplication: q * p, stored back in p
00198             p = q -> getProduct(p);
00199 
00200             // quaternion multiplication: p * conj(q), stored back in p
00201             p = p.getProduct(q -> getConjugate());
00202 
00203             // p quaternion is now [0, x', y', z']
00204             x = p.x;
00205             y = p.y;
00206             z = p.z;
00207         }
00208 
00209         VectorFloat getRotated(Quaternion *q) {
00210             VectorFloat r(x, y, z);
00211             r.rotate(q);
00212             return r;
00213         }
00214 };
00215 
00216 #endif /* _HELPER_3DMATH_H_ */