A quick implementation of Quaternion and Vector classes for use with my MPU9150 library
Dependents: cool_step_new cool_step_1 SML2
Fork of QuaternionMath by
Diff: Quaternion.h
- Revision:
- 6:7ba72ec26bd1
- Parent:
- 5:e31eb7f8925d
- Child:
- 7:9fc4176dde36
--- a/Quaternion.h Fri Mar 13 16:10:04 2015 +0000 +++ b/Quaternion.h Wed Mar 18 15:34:12 2015 +0000 @@ -186,6 +186,20 @@ row2.y = (2*_y*_z)+(2*_w*_x); row2.z = 1-(2*(_x*_x))-(2*(_y*_y)); } + + Quaternion getAxisAngle() const { + Quaternion q1(normalise()); // get normalised version + + float const angle = 2 * acos(q1.w); + double const s = sqrt(1 - q1.w * q1.w); // assuming quaternion normalised then w is less than 1, so term always positive. + if (s < 0.001) { // test to avoid divide by zero, s is always positive due to sqrt + // if s close to zero then direction of axis not important + q1.v = Vector3(1, 0, 0); + } else { + q1.v = q1.v / s; // normalise axis + } + return q1; + } const Vector3 getEulerAngles() const { double sqw = w*w;