Diff: helper_3dmath.h

Revision:

7:d5845b617139

Parent:

0:662207e34fba

--- a/helper_3dmath.h	Sat Jan 30 17:12:45 2016 +0000
+++ b/helper_3dmath.h	Sun Jan 31 09:12:19 2016 +0000
@@ -1,34 +1,3 @@
-// I2C device class (I2Cdev) demonstration Arduino sketch for MPU6050 class, 3D math helper
-// 6/5/2012 by Jeff Rowberg <jeff@rowberg.net>
-// Updates should (hopefully) always be available at https://github.com/jrowberg/i2cdevlib
-//
-// Changelog:
-//     2012-06-05 - add 3D math helper file to DMP6 example sketch
-
-/* ============================================
-I2Cdev device library code is placed under the MIT license
-Copyright (c) 2012 Jeff Rowberg
-
-Permission is hereby granted, free of charge, to any person obtaining a copy
-of this software and associated documentation files (the "Software"), to deal
-in the Software without restriction, including without limitation the rights
-to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
-copies of the Software, and to permit persons to whom the Software is
-furnished to do so, subject to the following conditions:
-
-The above copyright notice and this permission notice shall be included in
-all copies or substantial portions of the Software.
-
-THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
-IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
-FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
-AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
-LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
-OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN
-THE SOFTWARE.
-===============================================
-*/
-
 #ifndef _HELPER_3DMATH_H_
 #define _HELPER_3DMATH_H_
 
@@ -38,14 +7,14 @@
         float x;
         float y;
         float z;
-        
+
         Quaternion() {
             w = 1.0f;
             x = 0.0f;
             y = 0.0f;
             z = 0.0f;
         }
-        
+
         Quaternion(float nw, float nx, float ny, float nz) {
             w = nw;
             x = nx;
@@ -69,11 +38,11 @@
         Quaternion getConjugate() {
             return Quaternion(w, -x, -y, -z);
         }
-        
+
         float getMagnitude() {
-            return sqrt(w*w + x*x + y*y + z*z);
+            return sqrt((float)(w*w + x*x + y*y + z*z));
         }
-        
+
         void normalize() {
             float m = getMagnitude();
             w /= m;
@@ -81,7 +50,7 @@
             y /= m;
             z /= m;
         }
-        
+
         Quaternion getNormalized() {
             Quaternion r(w, x, y, z);
             r.normalize();
@@ -100,7 +69,7 @@
             y = 0;
             z = 0;
         }
-        
+
         VectorInt16(int16_t nx, int16_t ny, int16_t nz) {
             x = nx;
             y = ny;
@@ -117,19 +86,19 @@
             y /= m;
             z /= m;
         }
-        
+
         VectorInt16 getNormalized() {
             VectorInt16 r(x, y, z);
             r.normalize();
             return r;
         }
-        
+
         void rotate(Quaternion *q) {
             // http://www.cprogramming.com/tutorial/3d/quaternions.html
             // http://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/transforms/index.htm
             // http://content.gpwiki.org/index.php/OpenGL:Tutorials:Using_Quaternions_to_represent_rotation
             // ^ 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
-        
+
             // P_out = q * P_in * conj(q)
             // - P_out is the output vector
             // - q is the orientation quaternion
@@ -167,7 +136,7 @@
             y = 0;
             z = 0;
         }
-        
+
         VectorFloat(float nx, float ny, float nz) {
             x = nx;
             y = ny;
@@ -175,7 +144,7 @@
         }
 
         float getMagnitude() {
-            return sqrt(x*x + y*y + z*z);
+            return sqrt((float)(x*x + y*y + z*z));
         }
 
         void normalize() {
@@ -184,13 +153,13 @@
             y /= m;
             z /= m;
         }
-        
+
         VectorFloat getNormalized() {
             VectorFloat r(x, y, z);
             r.normalize();
             return r;
         }
-        
+
         void rotate(Quaternion *q) {
             Quaternion p(0, x, y, z);