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:
- 3:c0137be74db4
- Parent:
- 1:857642c51139
- Child:
- 4:1ced03aa8c75
diff -r 26ba41c58962 -r c0137be74db4 Quaternion.h --- a/Quaternion.h Thu Mar 12 10:14:52 2015 +0000 +++ b/Quaternion.h Fri Mar 13 09:10:41 2015 +0000 @@ -18,6 +18,49 @@ w = _w; v = _v; } + Quaternion(Vector3 row0, Vector3 row1, Vector3 row2) { + // from rotation matrix + const float m[3][3] = { + { row0.x, row0.y, row0.z }, + { row1.x, row1.y, row1.z }, + { row2.x, row2.y, row2.z } + }; + + const float tr = m[0][0] + m[1][1] + m[2][2]; + + if (tr > 0) + { + const float S = sqrt(tr+1.0) * 2; + w = 0.25 * S; + v.x = (m[2][1] - m[1][2]) / S; + v.y = (m[0][2] - m[2][0]) / S; + v.z = (m[1][0] - m[0][1]) / S; + } + else if ((m[0][0] < m[1][1])&(m[0][0] < m[2][2])) + { + const float S = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2; + w = (m[2][1] - m[1][2]) / S; + v.x = 0.25 * S; + v.y = (m[0][1] + m[1][0]) / S; + v.z = (m[0][2] + m[2][0]) / S; + } + else if (m[1][1] < m[2][2]) + { + const float S = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2; + w = (m[0][2] - m[2][0]) / S; + v.x = (m[0][1] + m[1][0]) / S; + v.y = 0.25 * S; + v.z = (m[1][2] + m[2][1]) / S; + } + else + { + const float S = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2; + w = (m[1][0] - m[0][1]) / S; + v.x = (m[0][2] + m[2][0]) / S; + v.y = (m[1][2] + m[2][1]) / S; + v.z = 0.25 * S; + } + } Quaternion(float theta_x, float theta_y, float theta_z) { float cos_z_2 = cosf(0.5f*theta_z);