Affordable Smart Wheelchair / BNO080_library

Contains added code for stm32-L432KC compatibility

Dependents:   BNO080_stm32_compatible

quaternion.h@9:68e4b1932497, 2019-07-30 (annotated)

Committer:
JesiMiranda
Date:
Tue Jul 30 17:23:04 2019 +0000
Revision:
9:68e4b1932497
Parent:
8:92d4109beb3c 
Contains added code for stm32-L432KC compatibility

Who changed what in which revision?

UserRevisionLine numberNew 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"
JesiMiranda 4:8e016f874c42 7 const double M_PI = 3.14159265358979323846;
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) {
JesiMiranda 8:92d4109beb3c 270 FloatType c1 = cos(euler[2] * 0.5f);
JesiMiranda 8:92d4109beb3c 271 FloatType c2 = cos(euler[1] * 0.5f);
JesiMiranda 8:92d4109beb3c 272 FloatType c3 = cos(euler[0] * 0.5f);
JesiMiranda 8:92d4109beb3c 273 FloatType s1 = sin(euler[2] * 0.5f);
JesiMiranda 8:92d4109beb3c 274 FloatType s2 = sin(euler[1] * 0.5f);
JesiMiranda 8:92d4109beb3c 275 FloatType s3 = sin(euler[0] * 0.5f);
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
JesiMiranda 8:92d4109beb3c 299 euler[1] = asin(2.0f * (mData[3]*mData[1] - mData[0]*mData[2]));
Jamie Smith 1:aac28ffd63ed 300 if (PI_OVER_2 - fabs(euler[1]) > EPSILON) {
JesiMiranda 8:92d4109beb3c 301 euler[2] = atan2(2.0f * (mData[0]*mData[1] + mData[3]*mData[2]),
Jamie Smith 1:aac28ffd63ed 302 sqx - sqy - sqz + sqw);
JesiMiranda 8:92d4109beb3c 303 euler[0] = atan2(2.0f * (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 */