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:
- 5:e31eb7f8925d
- Parent:
- 4:1ced03aa8c75
- Child:
- 6:7ba72ec26bd1
--- a/Quaternion.h Fri Mar 13 09:11:22 2015 +0000 +++ b/Quaternion.h Fri Mar 13 16:10:04 2015 +0000 @@ -167,6 +167,25 @@ return lerp(q3,t); } } + + void getRotationMatrix(Vector3& row0, Vector3& row1, Vector3& row2) const { + Quaternion q = this->normalise(); + const double _w = q.w; + const double _x = q.v.x; + const double _y = q.v.y; + const double _z = q.v.z; + row0.x = 1-(2*(_y*_y))-(2*(_z*_z)); + row0.y = (2*_x*_y)-(2*_w*_z); + row0.z = (2*_x*_z)+(2*_w*_y); + + row1.x = (2*_x*_y)+(2*_w*_z); + row1.y = 1-(2*(_x*_x))-(2*(_z*_z)); + row1.z = (2*(_y*_z))-(2*(_w*_x)); + + row2.x = (2*(_x*_z))-(2*_w*_y); + row2.y = (2*_y*_z)+(2*_w*_x); + row2.z = 1-(2*(_x*_x))-(2*(_y*_y)); + } const Vector3 getEulerAngles() const { double sqw = w*w;