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
Quaternion.h@10:69f240dbf0df, 2015-05-27 (annotated)
- Committer:
- pvaibhav
- Date:
- Wed May 27 13:01:34 2015 +0000
- Revision:
- 10:69f240dbf0df
- Parent:
- 9:7fc3fba01e2f
Code reformatted
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
p3p | 0:3cc1a808d8c6 | 1 | #ifndef __AHRSMATHDSP_QUATERNION_ |
p3p | 0:3cc1a808d8c6 | 2 | #define __AHRSMATHDSP_QUATERNION_ |
p3p | 0:3cc1a808d8c6 | 3 | |
p3p | 0:3cc1a808d8c6 | 4 | #include "Vector3.h" |
p3p | 0:3cc1a808d8c6 | 5 | |
pvaibhav | 1:857642c51139 | 6 | const static float PI = 3.1415926; |
pvaibhav | 1:857642c51139 | 7 | |
pvaibhav | 4:1ced03aa8c75 | 8 | class Quaternion |
pvaibhav | 4:1ced03aa8c75 | 9 | { |
p3p | 0:3cc1a808d8c6 | 10 | public: |
pvaibhav | 8:08d9c0010cc0 | 11 | Quaternion() : w(0.0f), v(0.0f, 0.0f, 0.0f) {} |
pvaibhav | 10:69f240dbf0df | 12 | |
pvaibhav | 8:08d9c0010cc0 | 13 | Quaternion(float const _w, float const _x, float const _y, float const _z) |
pvaibhav | 8:08d9c0010cc0 | 14 | : w(_w), v(_x, _y, _z) {} |
pvaibhav | 10:69f240dbf0df | 15 | |
pvaibhav | 8:08d9c0010cc0 | 16 | Quaternion(float const _w, const Vector3 &_v) : w(_w), v(_v) {} |
pvaibhav | 10:69f240dbf0df | 17 | |
pvaibhav | 8:08d9c0010cc0 | 18 | Quaternion(Vector3 const &row0, Vector3 const &row1, Vector3 const &row2) { |
pvaibhav | 3:c0137be74db4 | 19 | // from rotation matrix |
pvaibhav | 8:08d9c0010cc0 | 20 | float const m[3][3] = {{row0.x, row0.y, row0.z}, |
pvaibhav | 8:08d9c0010cc0 | 21 | {row1.x, row1.y, row1.z}, |
pvaibhav | 8:08d9c0010cc0 | 22 | {row2.x, row2.y, row2.z} |
pvaibhav | 3:c0137be74db4 | 23 | }; |
pvaibhav | 4:1ced03aa8c75 | 24 | |
pvaibhav | 8:08d9c0010cc0 | 25 | float const tr = m[0][0] + m[1][1] + m[2][2]; |
pvaibhav | 4:1ced03aa8c75 | 26 | |
pvaibhav | 4:1ced03aa8c75 | 27 | if (tr > 0) { |
pvaibhav | 8:08d9c0010cc0 | 28 | float const S = sqrt(tr + 1.0) * 2; |
pvaibhav | 4:1ced03aa8c75 | 29 | w = 0.25 * S; |
pvaibhav | 4:1ced03aa8c75 | 30 | v.x = (m[2][1] - m[1][2]) / S; |
pvaibhav | 4:1ced03aa8c75 | 31 | v.y = (m[0][2] - m[2][0]) / S; |
pvaibhav | 4:1ced03aa8c75 | 32 | v.z = (m[1][0] - m[0][1]) / S; |
pvaibhav | 8:08d9c0010cc0 | 33 | } else if ((m[0][0] < m[1][1]) & (m[0][0] < m[2][2])) { |
pvaibhav | 8:08d9c0010cc0 | 34 | float const S = sqrt(1.0 + m[0][0] - m[1][1] - m[2][2]) * 2; |
pvaibhav | 4:1ced03aa8c75 | 35 | w = (m[2][1] - m[1][2]) / S; |
pvaibhav | 4:1ced03aa8c75 | 36 | v.x = 0.25 * S; |
pvaibhav | 4:1ced03aa8c75 | 37 | v.y = (m[0][1] + m[1][0]) / S; |
pvaibhav | 4:1ced03aa8c75 | 38 | v.z = (m[0][2] + m[2][0]) / S; |
pvaibhav | 4:1ced03aa8c75 | 39 | } else if (m[1][1] < m[2][2]) { |
pvaibhav | 8:08d9c0010cc0 | 40 | float const S = sqrt(1.0 + m[1][1] - m[0][0] - m[2][2]) * 2; |
pvaibhav | 4:1ced03aa8c75 | 41 | w = (m[0][2] - m[2][0]) / S; |
pvaibhav | 4:1ced03aa8c75 | 42 | v.x = (m[0][1] + m[1][0]) / S; |
pvaibhav | 4:1ced03aa8c75 | 43 | v.y = 0.25 * S; |
pvaibhav | 4:1ced03aa8c75 | 44 | v.z = (m[1][2] + m[2][1]) / S; |
pvaibhav | 4:1ced03aa8c75 | 45 | } else { |
pvaibhav | 8:08d9c0010cc0 | 46 | float const S = sqrt(1.0 + m[2][2] - m[0][0] - m[1][1]) * 2; |
pvaibhav | 4:1ced03aa8c75 | 47 | w = (m[1][0] - m[0][1]) / S; |
pvaibhav | 4:1ced03aa8c75 | 48 | v.x = (m[0][2] + m[2][0]) / S; |
pvaibhav | 4:1ced03aa8c75 | 49 | v.y = (m[1][2] + m[2][1]) / S; |
pvaibhav | 4:1ced03aa8c75 | 50 | v.z = 0.25 * S; |
pvaibhav | 3:c0137be74db4 | 51 | } |
pvaibhav | 3:c0137be74db4 | 52 | } |
pvaibhav | 8:08d9c0010cc0 | 53 | Quaternion(float const theta_x, float const theta_z, float const theta_y) { |
pvaibhav | 8:08d9c0010cc0 | 54 | float const cos_z_2 = cosf(0.5f * theta_z); |
pvaibhav | 8:08d9c0010cc0 | 55 | float const cos_y_2 = cosf(0.5f * theta_y); |
pvaibhav | 8:08d9c0010cc0 | 56 | float const cos_x_2 = cosf(0.5f * theta_x); |
p3p | 0:3cc1a808d8c6 | 57 | |
pvaibhav | 8:08d9c0010cc0 | 58 | float const sin_z_2 = sinf(0.5f * theta_z); |
pvaibhav | 8:08d9c0010cc0 | 59 | float const sin_y_2 = sinf(0.5f * theta_y); |
pvaibhav | 8:08d9c0010cc0 | 60 | float const sin_x_2 = sinf(0.5f * theta_x); |
p3p | 0:3cc1a808d8c6 | 61 | |
p3p | 0:3cc1a808d8c6 | 62 | // and now compute quaternion |
pvaibhav | 8:08d9c0010cc0 | 63 | w = cos_z_2 * cos_y_2 * cos_x_2 + sin_z_2 * sin_y_2 * sin_x_2; |
pvaibhav | 8:08d9c0010cc0 | 64 | v.x = cos_z_2 * cos_y_2 * sin_x_2 - sin_z_2 * sin_y_2 * cos_x_2; |
pvaibhav | 8:08d9c0010cc0 | 65 | v.y = cos_z_2 * sin_y_2 * cos_x_2 + sin_z_2 * cos_y_2 * sin_x_2; |
pvaibhav | 8:08d9c0010cc0 | 66 | v.z = sin_z_2 * cos_y_2 * cos_x_2 - cos_z_2 * sin_y_2 * sin_x_2; |
p3p | 0:3cc1a808d8c6 | 67 | } |
pvaibhav | 4:1ced03aa8c75 | 68 | |
pvaibhav | 8:08d9c0010cc0 | 69 | void encode(char *const buffer) const { |
p3p | 0:3cc1a808d8c6 | 70 | int value = (w * (1 << 30)); |
pvaibhav | 8:08d9c0010cc0 | 71 | char const* bytes = (char const*) & value; |
pvaibhav | 8:08d9c0010cc0 | 72 | for (int i = 0; i < 4; i++) { |
pvaibhav | 8:08d9c0010cc0 | 73 | buffer[i] = bytes[3 - i]; |
p3p | 0:3cc1a808d8c6 | 74 | } |
pvaibhav | 4:1ced03aa8c75 | 75 | |
p3p | 0:3cc1a808d8c6 | 76 | value = v.x * (1 << 30); |
pvaibhav | 8:08d9c0010cc0 | 77 | for (int i = 0; i < 4; i++) { |
pvaibhav | 8:08d9c0010cc0 | 78 | buffer[i + 4] = bytes[3 - i]; |
p3p | 0:3cc1a808d8c6 | 79 | } |
pvaibhav | 4:1ced03aa8c75 | 80 | |
p3p | 0:3cc1a808d8c6 | 81 | value = v.y * (1 << 30); |
pvaibhav | 8:08d9c0010cc0 | 82 | for (int i = 0; i < 4; i++) { |
pvaibhav | 8:08d9c0010cc0 | 83 | buffer[i + 8] = bytes[3 - i]; |
p3p | 0:3cc1a808d8c6 | 84 | } |
pvaibhav | 4:1ced03aa8c75 | 85 | |
pvaibhav | 8:08d9c0010cc0 | 86 | value = v.z * (1 << 30); |
pvaibhav | 8:08d9c0010cc0 | 87 | for (int i = 0; i < 4; i++) { |
pvaibhav | 8:08d9c0010cc0 | 88 | buffer[i + 12] = bytes[3 - i]; |
pvaibhav | 4:1ced03aa8c75 | 89 | } |
p3p | 0:3cc1a808d8c6 | 90 | } |
pvaibhav | 4:1ced03aa8c75 | 91 | |
pvaibhav | 4:1ced03aa8c75 | 92 | void decode(const char *buffer) { |
pvaibhav | 8:08d9c0010cc0 | 93 | set((float)((((int32_t)buffer[0] << 24) + ((int32_t)buffer[1] << 16) + |
pvaibhav | 8:08d9c0010cc0 | 94 | ((int32_t)buffer[2] << 8) + buffer[3])) * |
pvaibhav | 8:08d9c0010cc0 | 95 | (1.0 / (1 << 30)), |
pvaibhav | 8:08d9c0010cc0 | 96 | (float)((((int32_t)buffer[4] << 24) + ((int32_t)buffer[5] << 16) + |
pvaibhav | 8:08d9c0010cc0 | 97 | ((int32_t)buffer[6] << 8) + buffer[7])) * |
pvaibhav | 8:08d9c0010cc0 | 98 | (1.0 / (1 << 30)), |
pvaibhav | 8:08d9c0010cc0 | 99 | (float)((((int32_t)buffer[8] << 24) + ((int32_t)buffer[9] << 16) + |
pvaibhav | 8:08d9c0010cc0 | 100 | ((int32_t)buffer[10] << 8) + buffer[11])) * |
pvaibhav | 8:08d9c0010cc0 | 101 | (1.0 / (1 << 30)), |
pvaibhav | 8:08d9c0010cc0 | 102 | (float)((((int32_t)buffer[12] << 24) + ((int32_t)buffer[13] << 16) + |
pvaibhav | 8:08d9c0010cc0 | 103 | ((int32_t)buffer[14] << 8) + buffer[15])) * |
pvaibhav | 8:08d9c0010cc0 | 104 | (1.0 / (1 << 30))); |
p3p | 0:3cc1a808d8c6 | 105 | } |
pvaibhav | 4:1ced03aa8c75 | 106 | |
pvaibhav | 8:08d9c0010cc0 | 107 | void set(float const _w, float const _x, float const _y, float const _z) { |
p3p | 0:3cc1a808d8c6 | 108 | w = _w; |
pvaibhav | 4:1ced03aa8c75 | 109 | v.set(_x, _y, _z); |
p3p | 0:3cc1a808d8c6 | 110 | } |
pvaibhav | 4:1ced03aa8c75 | 111 | |
pvaibhav | 4:1ced03aa8c75 | 112 | float lengthSquared() const { |
pvaibhav | 4:1ced03aa8c75 | 113 | return w * w + (v * v); |
p3p | 0:3cc1a808d8c6 | 114 | } |
pvaibhav | 4:1ced03aa8c75 | 115 | |
pvaibhav | 4:1ced03aa8c75 | 116 | float length() const { |
p3p | 0:3cc1a808d8c6 | 117 | return sqrt(lengthSquared()); |
p3p | 0:3cc1a808d8c6 | 118 | } |
pvaibhav | 4:1ced03aa8c75 | 119 | |
pvaibhav | 8:08d9c0010cc0 | 120 | void normalise() { |
pvaibhav | 8:08d9c0010cc0 | 121 | float const magnitude = length(); |
pvaibhav | 8:08d9c0010cc0 | 122 | w /= magnitude; // pop pop |
pvaibhav | 8:08d9c0010cc0 | 123 | v.x /= magnitude; |
pvaibhav | 8:08d9c0010cc0 | 124 | v.y /= magnitude; |
pvaibhav | 8:08d9c0010cc0 | 125 | v.z /= magnitude; |
pvaibhav | 8:08d9c0010cc0 | 126 | } |
pvaibhav | 8:08d9c0010cc0 | 127 | |
pvaibhav | 8:08d9c0010cc0 | 128 | Quaternion normalised() const { |
pvaibhav | 8:08d9c0010cc0 | 129 | return (*this) / length(); |
p3p | 0:3cc1a808d8c6 | 130 | } |
pvaibhav | 4:1ced03aa8c75 | 131 | |
pvaibhav | 4:1ced03aa8c75 | 132 | Quaternion conjugate() const { |
p3p | 0:3cc1a808d8c6 | 133 | return Quaternion(w, -v); |
p3p | 0:3cc1a808d8c6 | 134 | } |
pvaibhav | 4:1ced03aa8c75 | 135 | |
p3p | 0:3cc1a808d8c6 | 136 | Quaternion inverse() const { |
p3p | 0:3cc1a808d8c6 | 137 | return conjugate() / lengthSquared(); |
p3p | 0:3cc1a808d8c6 | 138 | } |
pvaibhav | 4:1ced03aa8c75 | 139 | |
pvaibhav | 8:08d9c0010cc0 | 140 | float dot_product(Quaternion const &q) const { |
pvaibhav | 8:08d9c0010cc0 | 141 | return q.v * v + q.w * w; |
p3p | 0:3cc1a808d8c6 | 142 | } |
pvaibhav | 4:1ced03aa8c75 | 143 | |
pvaibhav | 8:08d9c0010cc0 | 144 | Vector3 rotate(Vector3 const &v) const { |
pvaibhav | 8:08d9c0010cc0 | 145 | return ((*this) * Quaternion(0, v) * conjugate()).v; |
p3p | 0:3cc1a808d8c6 | 146 | } |
pvaibhav | 4:1ced03aa8c75 | 147 | |
pvaibhav | 8:08d9c0010cc0 | 148 | Quaternion lerp(Quaternion const &q2, float t) const { |
pvaibhav | 8:08d9c0010cc0 | 149 | if (t > 1.0f) { |
pvaibhav | 8:08d9c0010cc0 | 150 | t = 1.0f; |
pvaibhav | 8:08d9c0010cc0 | 151 | } else if (t < 0.0f) { |
pvaibhav | 8:08d9c0010cc0 | 152 | t = 0.0f; |
p3p | 0:3cc1a808d8c6 | 153 | } |
pvaibhav | 8:08d9c0010cc0 | 154 | return ((*this) * (1 - t) + q2 * t).normalised(); |
p3p | 0:3cc1a808d8c6 | 155 | } |
pvaibhav | 4:1ced03aa8c75 | 156 | |
pvaibhav | 8:08d9c0010cc0 | 157 | Quaternion slerp(Quaternion const &q2, float t) const { |
pvaibhav | 8:08d9c0010cc0 | 158 | if (t > 1.0f) { |
pvaibhav | 8:08d9c0010cc0 | 159 | t = 1.0f; |
pvaibhav | 8:08d9c0010cc0 | 160 | } else if (t < 0.0f) { |
pvaibhav | 8:08d9c0010cc0 | 161 | t = 0.0f; |
p3p | 0:3cc1a808d8c6 | 162 | } |
pvaibhav | 4:1ced03aa8c75 | 163 | |
p3p | 0:3cc1a808d8c6 | 164 | Quaternion q3; |
p3p | 0:3cc1a808d8c6 | 165 | float dot = dot_product(q2); |
p3p | 0:3cc1a808d8c6 | 166 | |
pvaibhav | 4:1ced03aa8c75 | 167 | if (dot < 0) { |
p3p | 0:3cc1a808d8c6 | 168 | dot = -dot; |
p3p | 0:3cc1a808d8c6 | 169 | q3 = -q2; |
pvaibhav | 8:08d9c0010cc0 | 170 | } else |
pvaibhav | 8:08d9c0010cc0 | 171 | q3 = q2; |
pvaibhav | 4:1ced03aa8c75 | 172 | |
pvaibhav | 4:1ced03aa8c75 | 173 | if (dot < 0.95f) { |
pvaibhav | 8:08d9c0010cc0 | 174 | float const angle = acosf(dot); |
pvaibhav | 8:08d9c0010cc0 | 175 | return ((*this) * sinf(angle * (1 - t)) + q3 * sinf(angle * t)) / |
pvaibhav | 8:08d9c0010cc0 | 176 | sinf(angle); |
p3p | 0:3cc1a808d8c6 | 177 | } else { |
pvaibhav | 4:1ced03aa8c75 | 178 | // if the angle is small, use linear interpolation |
pvaibhav | 8:08d9c0010cc0 | 179 | return lerp(q3, t); |
pvaibhav | 4:1ced03aa8c75 | 180 | } |
p3p | 0:3cc1a808d8c6 | 181 | } |
pvaibhav | 8:08d9c0010cc0 | 182 | |
pvaibhav | 8:08d9c0010cc0 | 183 | void getRotationMatrix(Vector3 &row0, Vector3 &row1, Vector3 &row2) const { |
pvaibhav | 8:08d9c0010cc0 | 184 | Quaternion q(normalised()); |
pvaibhav | 5:e31eb7f8925d | 185 | const double _w = q.w; |
pvaibhav | 5:e31eb7f8925d | 186 | const double _x = q.v.x; |
pvaibhav | 5:e31eb7f8925d | 187 | const double _y = q.v.y; |
pvaibhav | 5:e31eb7f8925d | 188 | const double _z = q.v.z; |
pvaibhav | 8:08d9c0010cc0 | 189 | row0.x = 1 - (2 * (_y * _y)) - (2 * (_z * _z)); |
pvaibhav | 8:08d9c0010cc0 | 190 | row0.y = (2 * _x * _y) - (2 * _w * _z); |
pvaibhav | 8:08d9c0010cc0 | 191 | row0.z = (2 * _x * _z) + (2 * _w * _y); |
pvaibhav | 8:08d9c0010cc0 | 192 | |
pvaibhav | 8:08d9c0010cc0 | 193 | row1.x = (2 * _x * _y) + (2 * _w * _z); |
pvaibhav | 8:08d9c0010cc0 | 194 | row1.y = 1 - (2 * (_x * _x)) - (2 * (_z * _z)); |
pvaibhav | 8:08d9c0010cc0 | 195 | row1.z = (2 * (_y * _z)) - (2 * (_w * _x)); |
pvaibhav | 8:08d9c0010cc0 | 196 | |
pvaibhav | 8:08d9c0010cc0 | 197 | row2.x = (2 * (_x * _z)) - (2 * _w * _y); |
pvaibhav | 8:08d9c0010cc0 | 198 | row2.y = (2 * _y * _z) + (2 * _w * _x); |
pvaibhav | 8:08d9c0010cc0 | 199 | row2.z = 1 - (2 * (_x * _x)) - (2 * (_y * _y)); |
pvaibhav | 5:e31eb7f8925d | 200 | } |
pvaibhav | 8:08d9c0010cc0 | 201 | |
pvaibhav | 6:7ba72ec26bd1 | 202 | Quaternion getAxisAngle() const { |
pvaibhav | 8:08d9c0010cc0 | 203 | Quaternion q1(normalised()); // get normalised version |
pvaibhav | 8:08d9c0010cc0 | 204 | |
pvaibhav | 6:7ba72ec26bd1 | 205 | float const angle = 2 * acos(q1.w); |
pvaibhav | 8:08d9c0010cc0 | 206 | double const s = sqrt(1 - q1.w * q1.w); // assuming quaternion normalised |
pvaibhav | 8:08d9c0010cc0 | 207 | // then w is less than 1, so term |
pvaibhav | 8:08d9c0010cc0 | 208 | // always positive. |
pvaibhav | 8:08d9c0010cc0 | 209 | if (s < 0.001) { // test to avoid divide by zero, s is always positive due |
pvaibhav | 8:08d9c0010cc0 | 210 | // to sqrt |
pvaibhav | 6:7ba72ec26bd1 | 211 | // if s close to zero then direction of axis not important |
pvaibhav | 6:7ba72ec26bd1 | 212 | q1.v = Vector3(1, 0, 0); |
pvaibhav | 6:7ba72ec26bd1 | 213 | } else { |
pvaibhav | 6:7ba72ec26bd1 | 214 | q1.v = q1.v / s; // normalise axis |
pvaibhav | 6:7ba72ec26bd1 | 215 | } |
pvaibhav | 6:7ba72ec26bd1 | 216 | return q1; |
pvaibhav | 6:7ba72ec26bd1 | 217 | } |
pvaibhav | 4:1ced03aa8c75 | 218 | |
pvaibhav | 1:857642c51139 | 219 | const Vector3 getEulerAngles() const { |
pvaibhav | 8:08d9c0010cc0 | 220 | float const q0 = w; |
pvaibhav | 8:08d9c0010cc0 | 221 | float const q1 = v.x; |
pvaibhav | 8:08d9c0010cc0 | 222 | float const q2 = v.y; |
pvaibhav | 8:08d9c0010cc0 | 223 | float const q3 = v.z; |
pvaibhav | 4:1ced03aa8c75 | 224 | |
pvaibhav | 9:7fc3fba01e2f | 225 | float const roll = asin(2.0 * (q0 * q2 - q3 * q1)); |
pvaibhav | 8:08d9c0010cc0 | 226 | float const pitch = |
pvaibhav | 9:7fc3fba01e2f | 227 | atan2(2.0 * (q0 * q1 + q2 * q3), 1.0 - 2.0 * (q1 * q1 + q2 * q2)); |
pvaibhav | 8:08d9c0010cc0 | 228 | float const yaw = |
pvaibhav | 9:7fc3fba01e2f | 229 | atan2(2.0 * (q0 * q3 + q1 * q2), 1.0 - 2.0 * (q2 * q2 + q3 * q3)); |
pvaibhav | 4:1ced03aa8c75 | 230 | |
pvaibhav | 8:08d9c0010cc0 | 231 | return Vector3(pitch, roll, yaw); |
p3p | 0:3cc1a808d8c6 | 232 | } |
pvaibhav | 4:1ced03aa8c75 | 233 | |
p3p | 0:3cc1a808d8c6 | 234 | Quaternion difference(const Quaternion &q2) const { |
pvaibhav | 8:08d9c0010cc0 | 235 | return Quaternion(q2 * (*this).inverse()); |
pvaibhav | 4:1ced03aa8c75 | 236 | } |
pvaibhav | 4:1ced03aa8c75 | 237 | |
pvaibhav | 8:08d9c0010cc0 | 238 | // operators |
pvaibhav | 8:08d9c0010cc0 | 239 | Quaternion &operator=(const Quaternion &q) { |
p3p | 0:3cc1a808d8c6 | 240 | w = q.w; |
p3p | 0:3cc1a808d8c6 | 241 | v = q.v; |
p3p | 0:3cc1a808d8c6 | 242 | return *this; |
p3p | 0:3cc1a808d8c6 | 243 | } |
p3p | 0:3cc1a808d8c6 | 244 | |
pvaibhav | 8:08d9c0010cc0 | 245 | const Quaternion operator+(const Quaternion &q) const { |
pvaibhav | 8:08d9c0010cc0 | 246 | return Quaternion(w + q.w, v + q.v); |
p3p | 0:3cc1a808d8c6 | 247 | } |
p3p | 0:3cc1a808d8c6 | 248 | |
pvaibhav | 8:08d9c0010cc0 | 249 | const Quaternion operator-(const Quaternion &q) const { |
p3p | 0:3cc1a808d8c6 | 250 | return Quaternion(w - q.w, v - q.v); |
p3p | 0:3cc1a808d8c6 | 251 | } |
p3p | 0:3cc1a808d8c6 | 252 | |
pvaibhav | 8:08d9c0010cc0 | 253 | const Quaternion operator*(const Quaternion &q) const { |
p3p | 0:3cc1a808d8c6 | 254 | return Quaternion(w * q.w - v * q.v, |
p3p | 0:3cc1a808d8c6 | 255 | v.y * q.v.z - v.z * q.v.y + w * q.v.x + v.x * q.w, |
p3p | 0:3cc1a808d8c6 | 256 | v.z * q.v.x - v.x * q.v.z + w * q.v.y + v.y * q.w, |
p3p | 0:3cc1a808d8c6 | 257 | v.x * q.v.y - v.y * q.v.x + w * q.v.z + v.z * q.w); |
p3p | 0:3cc1a808d8c6 | 258 | } |
pvaibhav | 4:1ced03aa8c75 | 259 | |
pvaibhav | 8:08d9c0010cc0 | 260 | const Quaternion operator/(const Quaternion &q) const { |
p3p | 0:3cc1a808d8c6 | 261 | Quaternion p = q.inverse(); |
p3p | 0:3cc1a808d8c6 | 262 | return p; |
p3p | 0:3cc1a808d8c6 | 263 | } |
pvaibhav | 4:1ced03aa8c75 | 264 | |
pvaibhav | 8:08d9c0010cc0 | 265 | const Quaternion operator-() const { |
p3p | 0:3cc1a808d8c6 | 266 | return Quaternion(-w, -v); |
p3p | 0:3cc1a808d8c6 | 267 | } |
pvaibhav | 4:1ced03aa8c75 | 268 | |
pvaibhav | 8:08d9c0010cc0 | 269 | // scaler operators |
pvaibhav | 8:08d9c0010cc0 | 270 | const Quaternion operator*(float scaler) const { |
p3p | 0:3cc1a808d8c6 | 271 | return Quaternion(w * scaler, v * scaler); |
p3p | 0:3cc1a808d8c6 | 272 | } |
p3p | 0:3cc1a808d8c6 | 273 | |
pvaibhav | 8:08d9c0010cc0 | 274 | const Quaternion operator/(float scaler) const { |
p3p | 0:3cc1a808d8c6 | 275 | return Quaternion(w / scaler, v / scaler); |
pvaibhav | 4:1ced03aa8c75 | 276 | } |
pvaibhav | 4:1ced03aa8c75 | 277 | |
p3p | 0:3cc1a808d8c6 | 278 | float w; |
pvaibhav | 4:1ced03aa8c75 | 279 | Vector3 v; |
p3p | 0:3cc1a808d8c6 | 280 | }; |
p3p | 0:3cc1a808d8c6 | 281 | |
p3p | 0:3cc1a808d8c6 | 282 | #endif |