The library needs to be tested once we get the IMU
quaternion.h@3:197ad972fb7c, 2019-06-14 (annotated)
- Committer:
- Jamie Smith
- Date:
- Fri Jun 14 20:31:37 2019 -0700
- Revision:
- 3:197ad972fb7c
- Parent:
- 2:2269b723d16a
Add permanent orientation support, fix a few bugs
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jamie Smith |
1:aac28ffd63ed | 1 | #ifndef QUATERNION_H |
Jamie Smith |
1:aac28ffd63ed | 2 | #define QUATERNION_H |
Jamie Smith |
1:aac28ffd63ed | 3 | |
Jamie Smith |
1:aac28ffd63ed | 4 | #include <cmath> |
Jamie Smith |
1:aac28ffd63ed | 5 | #include <mbed.h> |
Jamie Smith |
1:aac28ffd63ed | 6 | #include "tmatrix.h" |
Jamie Smith |
1:aac28ffd63ed | 7 | |
Jamie Smith |
1:aac28ffd63ed | 8 | class Quaternion |
Jamie Smith |
1:aac28ffd63ed | 9 | { |
Jamie Smith |
1:aac28ffd63ed | 10 | public: |
Jamie Smith |
1:aac28ffd63ed | 11 | typedef float FloatType; |
Jamie Smith |
1:aac28ffd63ed | 12 | |
Jamie Smith |
1:aac28ffd63ed | 13 | private: |
Jamie Smith |
1:aac28ffd63ed | 14 | FloatType mData[4]; |
Jamie Smith |
1:aac28ffd63ed | 15 | |
Jamie Smith |
1:aac28ffd63ed | 16 | public: |
Jamie Smith |
1:aac28ffd63ed | 17 | |
Jamie Smith |
1:aac28ffd63ed | 18 | Quaternion() { |
Jamie Smith |
1:aac28ffd63ed | 19 | mData[0] = mData[1] = mData[2] = 0; |
Jamie Smith |
1:aac28ffd63ed | 20 | mData[3] = 1; |
Jamie Smith |
1:aac28ffd63ed | 21 | } |
Jamie Smith |
1:aac28ffd63ed | 22 | |
Jamie Smith |
1:aac28ffd63ed | 23 | Quaternion(const TVector3& v, FloatType w) { |
Jamie Smith |
1:aac28ffd63ed | 24 | mData[0] = v.element(0,0); |
Jamie Smith |
1:aac28ffd63ed | 25 | mData[1] = v.element(1,0); |
Jamie Smith |
1:aac28ffd63ed | 26 | mData[2] = v.element(2,0); |
Jamie Smith |
1:aac28ffd63ed | 27 | mData[3] = w; |
Jamie Smith |
1:aac28ffd63ed | 28 | } |
Jamie Smith |
1:aac28ffd63ed | 29 | |
Jamie Smith |
1:aac28ffd63ed | 30 | Quaternion(const TVector4& v) { |
Jamie Smith |
1:aac28ffd63ed | 31 | mData[0] = v.element(0,0); |
Jamie Smith |
1:aac28ffd63ed | 32 | mData[1] = v.element(1,0); |
Jamie Smith |
1:aac28ffd63ed | 33 | mData[2] = v.element(2,0); |
Jamie Smith |
1:aac28ffd63ed | 34 | mData[3] = v.element(3,0); |
Jamie Smith |
1:aac28ffd63ed | 35 | } |
Jamie Smith |
1:aac28ffd63ed | 36 | |
Jamie Smith |
1:aac28ffd63ed | 37 | Quaternion(const FloatType* array) { |
Jamie Smith |
2:2269b723d16a | 38 | MBED_ASSERT(array != NULL); |
Jamie Smith |
1:aac28ffd63ed | 39 | for (uint32_t i = 0; i < 4; i++) { |
Jamie Smith |
1:aac28ffd63ed | 40 | mData[i] = array[i]; |
Jamie Smith |
1:aac28ffd63ed | 41 | } |
Jamie Smith |
1:aac28ffd63ed | 42 | } |
Jamie Smith |
1:aac28ffd63ed | 43 | |
Jamie Smith |
1:aac28ffd63ed | 44 | Quaternion(FloatType x, FloatType y, FloatType z, FloatType w) { |
Jamie Smith |
1:aac28ffd63ed | 45 | mData[0] = x; |
Jamie Smith |
1:aac28ffd63ed | 46 | mData[1] = y; |
Jamie Smith |
1:aac28ffd63ed | 47 | mData[2] = z; |
Jamie Smith |
1:aac28ffd63ed | 48 | mData[3] = w; |
Jamie Smith |
1:aac28ffd63ed | 49 | } |
Jamie Smith |
1:aac28ffd63ed | 50 | |
Jamie Smith |
1:aac28ffd63ed | 51 | FloatType x() const { return mData[0]; } |
Jamie Smith |
1:aac28ffd63ed | 52 | FloatType y() const { return mData[1]; } |
Jamie Smith |
1:aac28ffd63ed | 53 | FloatType z() const { return mData[2]; } |
Jamie Smith |
1:aac28ffd63ed | 54 | FloatType w() const { return real(); } |
Jamie Smith |
1:aac28ffd63ed | 55 | |
Jamie Smith |
1:aac28ffd63ed | 56 | TVector3 complex() const { return TVector3(mData); } |
Jamie Smith |
1:aac28ffd63ed | 57 | void complex(const TVector3& c) { mData[0] = c[0]; mData[1] = c[1]; mData[2] = c[2]; } |
Jamie Smith |
1:aac28ffd63ed | 58 | |
Jamie Smith |
1:aac28ffd63ed | 59 | FloatType real() const { return mData[3]; } |
Jamie Smith |
1:aac28ffd63ed | 60 | void real(FloatType r) { mData[3] = r; } |
Jamie Smith |
1:aac28ffd63ed | 61 | |
Jamie Smith |
1:aac28ffd63ed | 62 | Quaternion conjugate(void) const { |
Jamie Smith |
1:aac28ffd63ed | 63 | return Quaternion(-complex(), real()); |
Jamie Smith |
1:aac28ffd63ed | 64 | } |
Jamie Smith |
1:aac28ffd63ed | 65 | |
Jamie Smith |
1:aac28ffd63ed | 66 | /** |
Jamie Smith |
1:aac28ffd63ed | 67 | * @brief Computes the inverse of this quaternion. |
Jamie Smith |
1:aac28ffd63ed | 68 | * |
Jamie Smith |
1:aac28ffd63ed | 69 | * @note This is a general inverse. If you know a priori |
Jamie Smith |
1:aac28ffd63ed | 70 | * that you're using a unit quaternion (i.e., norm() == 1), |
Jamie Smith |
1:aac28ffd63ed | 71 | * it will be significantly faster to use conjugate() instead. |
Jamie Smith |
1:aac28ffd63ed | 72 | * |
Jamie Smith |
1:aac28ffd63ed | 73 | * @return The quaternion q such that q * (*this) == (*this) * q |
Jamie Smith |
1:aac28ffd63ed | 74 | * == [ 0 0 0 1 ]<sup>T</sup>. |
Jamie Smith |
1:aac28ffd63ed | 75 | */ |
Jamie Smith |
1:aac28ffd63ed | 76 | Quaternion inverse(void) const { |
Jamie Smith |
1:aac28ffd63ed | 77 | return conjugate() / norm(); |
Jamie Smith |
1:aac28ffd63ed | 78 | } |
Jamie Smith |
1:aac28ffd63ed | 79 | |
Jamie Smith |
1:aac28ffd63ed | 80 | |
Jamie Smith |
1:aac28ffd63ed | 81 | /** |
Jamie Smith |
1:aac28ffd63ed | 82 | * @brief Computes the product of this quaternion with the |
Jamie Smith |
1:aac28ffd63ed | 83 | * quaternion 'rhs'. |
Jamie Smith |
1:aac28ffd63ed | 84 | * |
Jamie Smith |
1:aac28ffd63ed | 85 | * @param rhs The right-hand-side of the product operation. |
Jamie Smith |
1:aac28ffd63ed | 86 | * |
Jamie Smith |
1:aac28ffd63ed | 87 | * @return The quaternion product (*this) x @p rhs. |
Jamie Smith |
1:aac28ffd63ed | 88 | */ |
Jamie Smith |
1:aac28ffd63ed | 89 | Quaternion product(const Quaternion& rhs) const { |
Jamie Smith |
1:aac28ffd63ed | 90 | return Quaternion(y()*rhs.z() - z()*rhs.y() + x()*rhs.w() + w()*rhs.x(), |
Jamie Smith |
1:aac28ffd63ed | 91 | z()*rhs.x() - x()*rhs.z() + y()*rhs.w() + w()*rhs.y(), |
Jamie Smith |
1:aac28ffd63ed | 92 | x()*rhs.y() - y()*rhs.x() + z()*rhs.w() + w()*rhs.z(), |
Jamie Smith |
1:aac28ffd63ed | 93 | w()*rhs.w() - x()*rhs.x() - y()*rhs.y() - z()*rhs.z()); |
Jamie Smith |
1:aac28ffd63ed | 94 | } |
Jamie Smith |
1:aac28ffd63ed | 95 | |
Jamie Smith |
1:aac28ffd63ed | 96 | /** |
Jamie Smith |
1:aac28ffd63ed | 97 | * @brief Quaternion product operator. |
Jamie Smith |
1:aac28ffd63ed | 98 | * |
Jamie Smith |
1:aac28ffd63ed | 99 | * The result is a quaternion such that: |
Jamie Smith |
1:aac28ffd63ed | 100 | * |
Jamie Smith |
1:aac28ffd63ed | 101 | * result.real() = (*this).real() * rhs.real() - |
Jamie Smith |
1:aac28ffd63ed | 102 | * (*this).complex().dot(rhs.complex()); |
Jamie Smith |
1:aac28ffd63ed | 103 | * |
Jamie Smith |
1:aac28ffd63ed | 104 | * and: |
Jamie Smith |
1:aac28ffd63ed | 105 | * |
Jamie Smith |
1:aac28ffd63ed | 106 | * result.complex() = rhs.complex() * (*this).real |
Jamie Smith |
1:aac28ffd63ed | 107 | * + (*this).complex() * rhs.real() |
Jamie Smith |
1:aac28ffd63ed | 108 | * - (*this).complex().cross(rhs.complex()); |
Jamie Smith |
1:aac28ffd63ed | 109 | * |
Jamie Smith |
1:aac28ffd63ed | 110 | * @return The quaternion product (*this) x rhs. |
Jamie Smith |
1:aac28ffd63ed | 111 | */ |
Jamie Smith |
1:aac28ffd63ed | 112 | Quaternion operator*(const Quaternion& rhs) const { |
Jamie Smith |
1:aac28ffd63ed | 113 | return product(rhs); |
Jamie Smith |
1:aac28ffd63ed | 114 | } |
Jamie Smith |
1:aac28ffd63ed | 115 | |
Jamie Smith |
1:aac28ffd63ed | 116 | /** |
Jamie Smith |
1:aac28ffd63ed | 117 | * @brief Quaternion scalar product operator. |
Jamie Smith |
1:aac28ffd63ed | 118 | * @param s A scalar by which to multiply all components |
Jamie Smith |
1:aac28ffd63ed | 119 | * of this quaternion. |
Jamie Smith |
1:aac28ffd63ed | 120 | * @return The quaternion (*this) * s. |
Jamie Smith |
1:aac28ffd63ed | 121 | */ |
Jamie Smith |
1:aac28ffd63ed | 122 | Quaternion operator*(FloatType s) const { |
Jamie Smith |
1:aac28ffd63ed | 123 | return Quaternion(complex()*s, real()*s); |
Jamie Smith |
1:aac28ffd63ed | 124 | } |
Jamie Smith |
1:aac28ffd63ed | 125 | |
Jamie Smith |
1:aac28ffd63ed | 126 | /** |
Jamie Smith |
1:aac28ffd63ed | 127 | * @brief Produces the sum of this quaternion and rhs. |
Jamie Smith |
1:aac28ffd63ed | 128 | */ |
Jamie Smith |
1:aac28ffd63ed | 129 | Quaternion operator+(const Quaternion& rhs) const { |
Jamie Smith |
1:aac28ffd63ed | 130 | return Quaternion(x()+rhs.x(), y()+rhs.y(), z()+rhs.z(), w()+rhs.w()); |
Jamie Smith |
1:aac28ffd63ed | 131 | } |
Jamie Smith |
1:aac28ffd63ed | 132 | |
Jamie Smith |
1:aac28ffd63ed | 133 | /** |
Jamie Smith |
1:aac28ffd63ed | 134 | * @brief Produces the difference of this quaternion and rhs. |
Jamie Smith |
1:aac28ffd63ed | 135 | */ |
Jamie Smith |
1:aac28ffd63ed | 136 | Quaternion operator-(const Quaternion& rhs) const { |
Jamie Smith |
1:aac28ffd63ed | 137 | return Quaternion(x()-rhs.x(), y()-rhs.y(), z()-rhs.z(), w()-rhs.w()); |
Jamie Smith |
1:aac28ffd63ed | 138 | } |
Jamie Smith |
1:aac28ffd63ed | 139 | |
Jamie Smith |
1:aac28ffd63ed | 140 | /** |
Jamie Smith |
1:aac28ffd63ed | 141 | * @brief Unary negation. |
Jamie Smith |
1:aac28ffd63ed | 142 | */ |
Jamie Smith |
1:aac28ffd63ed | 143 | Quaternion operator-() const { |
Jamie Smith |
1:aac28ffd63ed | 144 | return Quaternion(-x(), -y(), -z(), -w()); |
Jamie Smith |
1:aac28ffd63ed | 145 | } |
Jamie Smith |
1:aac28ffd63ed | 146 | |
Jamie Smith |
1:aac28ffd63ed | 147 | /** |
Jamie Smith |
1:aac28ffd63ed | 148 | * @brief Quaternion scalar division operator. |
Jamie Smith |
1:aac28ffd63ed | 149 | * @param s A scalar by which to divide all components |
Jamie Smith |
1:aac28ffd63ed | 150 | * of this quaternion. |
Jamie Smith |
1:aac28ffd63ed | 151 | * @return The quaternion (*this) / s. |
Jamie Smith |
1:aac28ffd63ed | 152 | */ |
Jamie Smith |
1:aac28ffd63ed | 153 | Quaternion operator/(FloatType s) const { |
Jamie Smith |
1:aac28ffd63ed | 154 | MBED_ASSERT(s != 0); |
Jamie Smith |
1:aac28ffd63ed | 155 | return Quaternion(complex()/s, real()/s); |
Jamie Smith |
1:aac28ffd63ed | 156 | } |
Jamie Smith |
1:aac28ffd63ed | 157 | |
Jamie Smith |
1:aac28ffd63ed | 158 | /** |
Jamie Smith |
1:aac28ffd63ed | 159 | * @brief Returns a matrix representation of this |
Jamie Smith |
1:aac28ffd63ed | 160 | * quaternion. |
Jamie Smith |
1:aac28ffd63ed | 161 | * |
Jamie Smith |
1:aac28ffd63ed | 162 | * Specifically this is the matrix such that: |
Jamie Smith |
1:aac28ffd63ed | 163 | * |
Jamie Smith |
1:aac28ffd63ed | 164 | * this->matrix() * q.vector() = (*this) * q for any quaternion q. |
Jamie Smith |
1:aac28ffd63ed | 165 | * |
Jamie Smith |
1:aac28ffd63ed | 166 | * Note that this is @e NOT the rotation matrix that may be |
Jamie Smith |
1:aac28ffd63ed | 167 | * represented by a unit quaternion. |
Jamie Smith |
1:aac28ffd63ed | 168 | */ |
Jamie Smith |
1:aac28ffd63ed | 169 | TMatrix4 matrix() const { |
Jamie Smith |
1:aac28ffd63ed | 170 | FloatType m[16] = { |
Jamie Smith |
1:aac28ffd63ed | 171 | w(), -z(), y(), x(), |
Jamie Smith |
1:aac28ffd63ed | 172 | z(), w(), -x(), y(), |
Jamie Smith |
1:aac28ffd63ed | 173 | -y(), x(), w(), z(), |
Jamie Smith |
1:aac28ffd63ed | 174 | -x(), -y(), -z(), w() |
Jamie Smith |
1:aac28ffd63ed | 175 | }; |
Jamie Smith |
1:aac28ffd63ed | 176 | return TMatrix4(m); |
Jamie Smith |
1:aac28ffd63ed | 177 | } |
Jamie Smith |
1:aac28ffd63ed | 178 | |
Jamie Smith |
1:aac28ffd63ed | 179 | /** |
Jamie Smith |
1:aac28ffd63ed | 180 | * @brief Returns a matrix representation of this |
Jamie Smith |
1:aac28ffd63ed | 181 | * quaternion for right multiplication. |
Jamie Smith |
1:aac28ffd63ed | 182 | * |
Jamie Smith |
1:aac28ffd63ed | 183 | * Specifically this is the matrix such that: |
Jamie Smith |
1:aac28ffd63ed | 184 | * |
Jamie Smith |
1:aac28ffd63ed | 185 | * q.vector().transpose() * this->matrix() = (q * |
Jamie Smith |
1:aac28ffd63ed | 186 | * (*this)).vector().transpose() for any quaternion q. |
Jamie Smith |
1:aac28ffd63ed | 187 | * |
Jamie Smith |
1:aac28ffd63ed | 188 | * Note that this is @e NOT the rotation matrix that may be |
Jamie Smith |
1:aac28ffd63ed | 189 | * represented by a unit quaternion. |
Jamie Smith |
1:aac28ffd63ed | 190 | */ |
Jamie Smith |
1:aac28ffd63ed | 191 | TMatrix4 rightMatrix() const { |
Jamie Smith |
1:aac28ffd63ed | 192 | FloatType m[16] = { |
Jamie Smith |
1:aac28ffd63ed | 193 | +w(), -z(), y(), -x(), |
Jamie Smith |
1:aac28ffd63ed | 194 | +z(), w(), -x(), -y(), |
Jamie Smith |
1:aac28ffd63ed | 195 | -y(), x(), w(), -z(), |
Jamie Smith |
1:aac28ffd63ed | 196 | +x(), y(), z(), w() |
Jamie Smith |
1:aac28ffd63ed | 197 | }; |
Jamie Smith |
1:aac28ffd63ed | 198 | return TMatrix4(m); |
Jamie Smith |
1:aac28ffd63ed | 199 | } |
Jamie Smith |
1:aac28ffd63ed | 200 | |
Jamie Smith |
1:aac28ffd63ed | 201 | /** |
Jamie Smith |
1:aac28ffd63ed | 202 | * @brief Returns this quaternion as a 4-vector. |
Jamie Smith |
1:aac28ffd63ed | 203 | * |
Jamie Smith |
1:aac28ffd63ed | 204 | * This is simply the vector [x y z w]<sup>T</sup> |
Jamie Smith |
1:aac28ffd63ed | 205 | */ |
Jamie Smith |
1:aac28ffd63ed | 206 | TVector4 vector() const { return TVector4(mData); } |
Jamie Smith |
1:aac28ffd63ed | 207 | |
Jamie Smith |
1:aac28ffd63ed | 208 | /** |
Jamie Smith |
1:aac28ffd63ed | 209 | * @brief Returns the norm ("magnitude") of the quaternion. |
Jamie Smith |
1:aac28ffd63ed | 210 | * @return The 2-norm of [ w(), x(), y(), z() ]<sup>T</sup>. |
Jamie Smith |
1:aac28ffd63ed | 211 | */ |
Jamie Smith |
1:aac28ffd63ed | 212 | FloatType norm() const { return sqrt(mData[0]*mData[0]+mData[1]*mData[1]+ |
Jamie Smith |
1:aac28ffd63ed | 213 | mData[2]*mData[2]+mData[3]*mData[3]); } |
Jamie Smith |
1:aac28ffd63ed | 214 | |
Jamie Smith |
1:aac28ffd63ed | 215 | /** |
Jamie Smith |
1:aac28ffd63ed | 216 | * @brief Computes the rotation matrix represented by a unit |
Jamie Smith |
1:aac28ffd63ed | 217 | * quaternion. |
Jamie Smith |
1:aac28ffd63ed | 218 | * |
Jamie Smith |
1:aac28ffd63ed | 219 | * @note This does not check that this quaternion is normalized. |
Jamie Smith |
1:aac28ffd63ed | 220 | * It formulaically returns the matrix, which will not be a |
Jamie Smith |
1:aac28ffd63ed | 221 | * rotation if the quaternion is non-unit. |
Jamie Smith |
1:aac28ffd63ed | 222 | */ |
Jamie Smith |
1:aac28ffd63ed | 223 | TMatrix3 rotationMatrix() const { |
Jamie Smith |
1:aac28ffd63ed | 224 | FloatType m[9] = { |
Jamie Smith |
1:aac28ffd63ed | 225 | 1-2*y()*y()-2*z()*z(), 2*x()*y() - 2*z()*w(), 2*x()*z() + 2*y()*w(), |
Jamie Smith |
1:aac28ffd63ed | 226 | 2*x()*y() + 2*z()*w(), 1-2*x()*x()-2*z()*z(), 2*y()*z() - 2*x()*w(), |
Jamie Smith |
1:aac28ffd63ed | 227 | 2*x()*z() - 2*y()*w(), 2*y()*z() + 2*x()*w(), 1-2*x()*x()-2*y()*y() |
Jamie Smith |
1:aac28ffd63ed | 228 | }; |
Jamie Smith |
1:aac28ffd63ed | 229 | return TMatrix3(m); |
Jamie Smith |
1:aac28ffd63ed | 230 | } |
Jamie Smith |
1:aac28ffd63ed | 231 | /** |
Jamie Smith |
1:aac28ffd63ed | 232 | * @brief Sets quaternion to be same as rotation by scaled axis w. |
Jamie Smith |
1:aac28ffd63ed | 233 | */ |
Jamie Smith |
1:aac28ffd63ed | 234 | void scaledAxis(const TVector3& w) { |
Jamie Smith |
1:aac28ffd63ed | 235 | FloatType theta = w.norm(); |
Jamie Smith |
1:aac28ffd63ed | 236 | if (theta > 0.0001) { |
Jamie Smith |
1:aac28ffd63ed | 237 | FloatType s = sin(theta / 2.0); |
Jamie Smith |
1:aac28ffd63ed | 238 | TVector3 W(w / theta * s); |
Jamie Smith |
1:aac28ffd63ed | 239 | mData[0] = W[0]; |
Jamie Smith |
1:aac28ffd63ed | 240 | mData[1] = W[1]; |
Jamie Smith |
1:aac28ffd63ed | 241 | mData[2] = W[2]; |
Jamie Smith |
1:aac28ffd63ed | 242 | mData[3] = cos(theta / 2.0); |
Jamie Smith |
1:aac28ffd63ed | 243 | } else { |
Jamie Smith |
1:aac28ffd63ed | 244 | mData[0]=mData[1]=mData[2]=0; |
Jamie Smith |
1:aac28ffd63ed | 245 | mData[3]=1.0; |
Jamie Smith |
1:aac28ffd63ed | 246 | } |
Jamie Smith |
1:aac28ffd63ed | 247 | } |
Jamie Smith |
1:aac28ffd63ed | 248 | |
Jamie Smith |
1:aac28ffd63ed | 249 | /** |
Jamie Smith |
1:aac28ffd63ed | 250 | * @brief Returns a vector rotated by this quaternion. |
Jamie Smith |
1:aac28ffd63ed | 251 | * |
Jamie Smith |
1:aac28ffd63ed | 252 | * Functionally equivalent to: (rotationMatrix() * v) |
Jamie Smith |
1:aac28ffd63ed | 253 | * or (q * Quaternion(0, v) * q.inverse()). |
Jamie Smith |
1:aac28ffd63ed | 254 | * |
Jamie Smith |
1:aac28ffd63ed | 255 | * @warning conjugate() is used instead of inverse() for better |
Jamie Smith |
1:aac28ffd63ed | 256 | * performance, when this quaternion must be normalized. |
Jamie Smith |
1:aac28ffd63ed | 257 | */ |
Jamie Smith |
1:aac28ffd63ed | 258 | TVector3 rotatedVector(const TVector3& v) const { |
Jamie Smith |
1:aac28ffd63ed | 259 | return (((*this) * Quaternion(v, 0)) * conjugate()).complex(); |
Jamie Smith |
1:aac28ffd63ed | 260 | } |
Jamie Smith |
1:aac28ffd63ed | 261 | |
Jamie Smith |
1:aac28ffd63ed | 262 | |
Jamie Smith |
1:aac28ffd63ed | 263 | |
Jamie Smith |
1:aac28ffd63ed | 264 | /** |
Jamie Smith |
1:aac28ffd63ed | 265 | * @brief Computes the quaternion that is equivalent to a given |
Jamie Smith |
1:aac28ffd63ed | 266 | * euler angle rotation. |
Jamie Smith |
1:aac28ffd63ed | 267 | * @param euler A 3-vector in order: roll-pitch-yaw. |
Jamie Smith |
1:aac28ffd63ed | 268 | */ |
Jamie Smith |
1:aac28ffd63ed | 269 | void euler(const TVector3& euler) { |
Jamie Smith |
1:aac28ffd63ed | 270 | FloatType c1 = cos(euler[2] * 0.5); |
Jamie Smith |
1:aac28ffd63ed | 271 | FloatType c2 = cos(euler[1] * 0.5); |
Jamie Smith |
1:aac28ffd63ed | 272 | FloatType c3 = cos(euler[0] * 0.5); |
Jamie Smith |
1:aac28ffd63ed | 273 | FloatType s1 = sin(euler[2] * 0.5); |
Jamie Smith |
1:aac28ffd63ed | 274 | FloatType s2 = sin(euler[1] * 0.5); |
Jamie Smith |
1:aac28ffd63ed | 275 | FloatType s3 = sin(euler[0] * 0.5); |
Jamie Smith |
1:aac28ffd63ed | 276 | |
Jamie Smith |
1:aac28ffd63ed | 277 | mData[0] = c1*c2*s3 - s1*s2*c3; |
Jamie Smith |
1:aac28ffd63ed | 278 | mData[1] = c1*s2*c3 + s1*c2*s3; |
Jamie Smith |
1:aac28ffd63ed | 279 | mData[2] = s1*c2*c3 - c1*s2*s3; |
Jamie Smith |
1:aac28ffd63ed | 280 | mData[3] = c1*c2*c3 + s1*s2*s3; |
Jamie Smith |
1:aac28ffd63ed | 281 | } |
Jamie Smith |
1:aac28ffd63ed | 282 | |
Jamie Smith |
1:aac28ffd63ed | 283 | /** @brief Returns an equivalent euler angle representation of |
Jamie Smith |
1:aac28ffd63ed | 284 | * this quaternion. |
Jamie Smith |
1:aac28ffd63ed | 285 | * @return Euler angles in roll-pitch-yaw order. |
Jamie Smith |
1:aac28ffd63ed | 286 | */ |
Jamie Smith |
1:aac28ffd63ed | 287 | TVector3 euler(void) const { |
Jamie Smith |
1:aac28ffd63ed | 288 | TVector3 euler; |
Jamie Smith |
1:aac28ffd63ed | 289 | const static FloatType PI_OVER_2 = M_PI * 0.5; |
Jamie Smith |
1:aac28ffd63ed | 290 | const static FloatType EPSILON = 1e-10; |
Jamie Smith |
1:aac28ffd63ed | 291 | FloatType sqw, sqx, sqy, sqz; |
Jamie Smith |
1:aac28ffd63ed | 292 | |
Jamie Smith |
1:aac28ffd63ed | 293 | // quick conversion to Euler angles to give tilt to user |
Jamie Smith |
1:aac28ffd63ed | 294 | sqw = mData[3]*mData[3]; |
Jamie Smith |
1:aac28ffd63ed | 295 | sqx = mData[0]*mData[0]; |
Jamie Smith |
1:aac28ffd63ed | 296 | sqy = mData[1]*mData[1]; |
Jamie Smith |
1:aac28ffd63ed | 297 | sqz = mData[2]*mData[2]; |
Jamie Smith |
1:aac28ffd63ed | 298 | |
Jamie Smith |
1:aac28ffd63ed | 299 | euler[1] = asin(2.0 * (mData[3]*mData[1] - mData[0]*mData[2])); |
Jamie Smith |
1:aac28ffd63ed | 300 | if (PI_OVER_2 - fabs(euler[1]) > EPSILON) { |
Jamie Smith |
1:aac28ffd63ed | 301 | euler[2] = atan2(2.0 * (mData[0]*mData[1] + mData[3]*mData[2]), |
Jamie Smith |
1:aac28ffd63ed | 302 | sqx - sqy - sqz + sqw); |
Jamie Smith |
1:aac28ffd63ed | 303 | euler[0] = atan2(2.0 * (mData[3]*mData[0] + mData[1]*mData[2]), |
Jamie Smith |
1:aac28ffd63ed | 304 | sqw - sqx - sqy + sqz); |
Jamie Smith |
1:aac28ffd63ed | 305 | } else { |
Jamie Smith |
1:aac28ffd63ed | 306 | // compute heading from local 'down' vector |
Jamie Smith |
1:aac28ffd63ed | 307 | euler[2] = atan2(2*mData[1]*mData[2] - 2*mData[0]*mData[3], |
Jamie Smith |
1:aac28ffd63ed | 308 | 2*mData[0]*mData[2] + 2*mData[1]*mData[3]); |
Jamie Smith |
1:aac28ffd63ed | 309 | euler[0] = 0.0; |
Jamie Smith |
1:aac28ffd63ed | 310 | |
Jamie Smith |
1:aac28ffd63ed | 311 | // If facing down, reverse yaw |
Jamie Smith |
1:aac28ffd63ed | 312 | if (euler[1] < 0) |
Jamie Smith |
1:aac28ffd63ed | 313 | euler[2] = M_PI - euler[2]; |
Jamie Smith |
1:aac28ffd63ed | 314 | } |
Jamie Smith |
1:aac28ffd63ed | 315 | return euler; |
Jamie Smith |
1:aac28ffd63ed | 316 | } |
Jamie Smith |
1:aac28ffd63ed | 317 | |
Jamie Smith |
1:aac28ffd63ed | 318 | /** |
Jamie Smith |
1:aac28ffd63ed | 319 | * @brief Returns pointer to the internal array. |
Jamie Smith |
1:aac28ffd63ed | 320 | * |
Jamie Smith |
1:aac28ffd63ed | 321 | * Array is in order x,y,z,w. |
Jamie Smith |
1:aac28ffd63ed | 322 | */ |
Jamie Smith |
1:aac28ffd63ed | 323 | FloatType* row(uint32_t i) { return mData + i; } |
Jamie Smith |
1:aac28ffd63ed | 324 | // Const version of the above. |
Jamie Smith |
1:aac28ffd63ed | 325 | const FloatType* row(uint32_t i) const { return mData + i; } |
Jamie Smith |
1:aac28ffd63ed | 326 | }; |
Jamie Smith |
1:aac28ffd63ed | 327 | |
Jamie Smith |
1:aac28ffd63ed | 328 | /** |
Jamie Smith |
1:aac28ffd63ed | 329 | * @brief Global operator allowing left-multiply by scalar. |
Jamie Smith |
1:aac28ffd63ed | 330 | */ |
Jamie Smith |
1:aac28ffd63ed | 331 | Quaternion operator*(Quaternion::FloatType s, const Quaternion& q); |
Jamie Smith |
1:aac28ffd63ed | 332 | |
Jamie Smith |
1:aac28ffd63ed | 333 | |
Jamie Smith |
1:aac28ffd63ed | 334 | #endif /* QUATERNION_H */ |