1
Dependencies: ArduinoSerial I2Cdev
Fork of MPU6050 by
Diff: helper_3dmath.h
- Revision:
- 7:d5845b617139
- Parent:
- 0:662207e34fba
diff -r f38dfe62d74c -r d5845b617139 helper_3dmath.h --- 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);