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