Fully featured I2C and SPI driver for CEVA (Hilcrest)'s BNO080 and FSM300 Inertial Measurement Units.
Dependents: BNO080-Examples BNO080-Examples
BNO080 Driver
by Jamie Smith / USC Rocket Propulsion Lab
After lots of development, we are proud to present our driver for the Hilcrest BNO080 IMU! This driver is inspired by SparkFun and Nathan Seidle's Arduino driver for this chip, but has been substantially rewritten and adapted.
It supports the main features of the chip, such as reading rotation and acceleration data, as well as some of its more esoteric functionality, such as counting steps and detecting whether the device is being hand-held.
Features
- Support for 15 different data reports from the IMU, from acceleration to rotation to tap detection
- Support for reading of sensor data, and automatic checking of update rate against allowed values in metadata
BNO_DEBUG
switch enabling verbose, detailed output about communications with the chip for ease of debugging- Ability to tare sensor rotation and set mounting orientation
- Can operate in several execution modes: polling I2C, polling SPI, and threaded SPI (which handles timing-critical functions in a dedicated thread, and automatically activates when the IMU has data available)
- Also has experimental support for using asynchronous SPI transactions, allowing other threads to execute while communication with the BNO is occurring. Note that this functionality requires a patch to Mbed OS source code due to Mbed bug #13941
- Calibration function
- Reasonable code size for what you get: the library uses about 4K of flash and one instance of the object uses about 1700 bytes of RAM.
Documentation
Full Doxygen documentation is available online here
Example Code
Here's a simple example:
BNO080 Rotation Vector and Acceleration
#include <mbed.h> #include <BNO080.h> int main() { Serial pc(USBTX, USBRX); // Create IMU, passing in output stream, pins, I2C address, and I2C frequency // These pin assignments are specific to my dev setup -- you'll need to change them BNO080I2C imu(&pc, p28, p27, p16, p30, 0x4a, 100000); pc.baud(115200); pc.printf("============================================================\n"); // Tell the IMU to report rotation every 100ms and acceleration every 200ms imu.enableReport(BNO080::ROTATION, 100); imu.enableReport(BNO080::TOTAL_ACCELERATION, 200); while (true) { wait(.001f); // poll the IMU for new data -- this returns true if any packets were received if(imu.updateData()) { // now check for the specific type of data that was received (can be multiple at once) if (imu.hasNewData(BNO080::ROTATION)) { // convert quaternion to Euler degrees and print pc.printf("IMU Rotation Euler: "); TVector3 eulerRadians = imu.rotationVector.euler(); TVector3 eulerDegrees = eulerRadians * (180.0 / M_PI); eulerDegrees.print(pc, true); pc.printf("\n"); } if (imu.hasNewData(BNO080::TOTAL_ACCELERATION)) { // print the acceleration vector using its builtin print() method pc.printf("IMU Total Acceleration: "); imu.totalAcceleration.print(pc, true); pc.printf("\n"); } } } }
If you want more, a comprehensive, ready-to-run set of examples is available on my BNO080-Examples repository.
Credits
This driver makes use of a lightweight, public-domain library for vectors and quaternions available here.
Changelog
Version 2.1 (Nov 24 2020)
- Added BNO080Async, which provides a threaded implementation of the SPI driver. This should help get the best performance and remove annoying timing requirements on the code calling the driver
- Added experimental
USE_ASYNC_SPI
option - Fixed bug in v2.0 causing calibrations to fail
Version 2.0 (Nov 18 2020)
- Added SPI support
- Refactored buffer system so that SPI could be implemented as a subclass. Unfortunately this does substantially increase the memory usage of the driver, but I believe that the benefits are worth it.
Version 1.3 (Jul 21 2020)
- Fix deprecation warnings and compile errors in Mbed 6
- Fix compile errors in Arm Compiler (why doesn't it have M_PI????)
Version 1.2 (Jan 30 2020)
- Removed accidental IRQ change
- Fixed hard iron offset reading incorrectly due to missing cast
Version 1.1 (Jun 14 2019)
- Added support for changing permanent orientation
- Add FRS writing functions
- Removed some errant printfs
Version 1.0 (Dec 29 2018)
- Initial Mbed OS release
quaternion.h@9:430f5302f9e1, 2020-11-24 (annotated)
- Committer:
- Jamie Smith
- Date:
- Tue Nov 24 15:06:05 2020 -0800
- Revision:
- 9:430f5302f9e1
- Parent:
- 5:c75be9000d75
Implement BNO080Async
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 |
5:c75be9000d75 | 8 | // Arm Compiler has no M_PI |
Jamie Smith |
5:c75be9000d75 | 9 | #ifndef M_PI |
Jamie Smith |
5:c75be9000d75 | 10 | #define M_PI 3.14159265358979323846 |
Jamie Smith |
5:c75be9000d75 | 11 | #endif |
Jamie Smith |
5:c75be9000d75 | 12 | |
Jamie Smith |
1:aac28ffd63ed | 13 | class Quaternion |
Jamie Smith |
1:aac28ffd63ed | 14 | { |
Jamie Smith |
1:aac28ffd63ed | 15 | public: |
Jamie Smith |
1:aac28ffd63ed | 16 | typedef float FloatType; |
Jamie Smith |
1:aac28ffd63ed | 17 | |
Jamie Smith |
1:aac28ffd63ed | 18 | private: |
Jamie Smith |
1:aac28ffd63ed | 19 | FloatType mData[4]; |
Jamie Smith |
1:aac28ffd63ed | 20 | |
Jamie Smith |
1:aac28ffd63ed | 21 | public: |
Jamie Smith |
1:aac28ffd63ed | 22 | |
Jamie Smith |
1:aac28ffd63ed | 23 | Quaternion() { |
Jamie Smith |
1:aac28ffd63ed | 24 | mData[0] = mData[1] = mData[2] = 0; |
Jamie Smith |
1:aac28ffd63ed | 25 | mData[3] = 1; |
Jamie Smith |
1:aac28ffd63ed | 26 | } |
Jamie Smith |
1:aac28ffd63ed | 27 | |
Jamie Smith |
1:aac28ffd63ed | 28 | Quaternion(const TVector3& v, FloatType w) { |
Jamie Smith |
1:aac28ffd63ed | 29 | mData[0] = v.element(0,0); |
Jamie Smith |
1:aac28ffd63ed | 30 | mData[1] = v.element(1,0); |
Jamie Smith |
1:aac28ffd63ed | 31 | mData[2] = v.element(2,0); |
Jamie Smith |
1:aac28ffd63ed | 32 | mData[3] = w; |
Jamie Smith |
1:aac28ffd63ed | 33 | } |
Jamie Smith |
1:aac28ffd63ed | 34 | |
Jamie Smith |
1:aac28ffd63ed | 35 | Quaternion(const TVector4& v) { |
Jamie Smith |
1:aac28ffd63ed | 36 | mData[0] = v.element(0,0); |
Jamie Smith |
1:aac28ffd63ed | 37 | mData[1] = v.element(1,0); |
Jamie Smith |
1:aac28ffd63ed | 38 | mData[2] = v.element(2,0); |
Jamie Smith |
1:aac28ffd63ed | 39 | mData[3] = v.element(3,0); |
Jamie Smith |
1:aac28ffd63ed | 40 | } |
Jamie Smith |
1:aac28ffd63ed | 41 | |
Jamie Smith |
1:aac28ffd63ed | 42 | Quaternion(const FloatType* array) { |
Jamie Smith |
2:2269b723d16a | 43 | MBED_ASSERT(array != NULL); |
Jamie Smith |
1:aac28ffd63ed | 44 | for (uint32_t i = 0; i < 4; i++) { |
Jamie Smith |
1:aac28ffd63ed | 45 | mData[i] = array[i]; |
Jamie Smith |
1:aac28ffd63ed | 46 | } |
Jamie Smith |
1:aac28ffd63ed | 47 | } |
Jamie Smith |
1:aac28ffd63ed | 48 | |
Jamie Smith |
1:aac28ffd63ed | 49 | Quaternion(FloatType x, FloatType y, FloatType z, FloatType w) { |
Jamie Smith |
1:aac28ffd63ed | 50 | mData[0] = x; |
Jamie Smith |
1:aac28ffd63ed | 51 | mData[1] = y; |
Jamie Smith |
1:aac28ffd63ed | 52 | mData[2] = z; |
Jamie Smith |
1:aac28ffd63ed | 53 | mData[3] = w; |
Jamie Smith |
1:aac28ffd63ed | 54 | } |
Jamie Smith |
1:aac28ffd63ed | 55 | |
Jamie Smith |
1:aac28ffd63ed | 56 | FloatType x() const { return mData[0]; } |
Jamie Smith |
1:aac28ffd63ed | 57 | FloatType y() const { return mData[1]; } |
Jamie Smith |
1:aac28ffd63ed | 58 | FloatType z() const { return mData[2]; } |
Jamie Smith |
1:aac28ffd63ed | 59 | FloatType w() const { return real(); } |
Jamie Smith |
1:aac28ffd63ed | 60 | |
Jamie Smith |
1:aac28ffd63ed | 61 | TVector3 complex() const { return TVector3(mData); } |
Jamie Smith |
1:aac28ffd63ed | 62 | void complex(const TVector3& c) { mData[0] = c[0]; mData[1] = c[1]; mData[2] = c[2]; } |
Jamie Smith |
1:aac28ffd63ed | 63 | |
Jamie Smith |
1:aac28ffd63ed | 64 | FloatType real() const { return mData[3]; } |
Jamie Smith |
1:aac28ffd63ed | 65 | void real(FloatType r) { mData[3] = r; } |
Jamie Smith |
1:aac28ffd63ed | 66 | |
Jamie Smith |
1:aac28ffd63ed | 67 | Quaternion conjugate(void) const { |
Jamie Smith |
1:aac28ffd63ed | 68 | return Quaternion(-complex(), real()); |
Jamie Smith |
1:aac28ffd63ed | 69 | } |
Jamie Smith |
1:aac28ffd63ed | 70 | |
Jamie Smith |
1:aac28ffd63ed | 71 | /** |
Jamie Smith |
1:aac28ffd63ed | 72 | * @brief Computes the inverse of this quaternion. |
Jamie Smith |
1:aac28ffd63ed | 73 | * |
Jamie Smith |
1:aac28ffd63ed | 74 | * @note This is a general inverse. If you know a priori |
Jamie Smith |
1:aac28ffd63ed | 75 | * that you're using a unit quaternion (i.e., norm() == 1), |
Jamie Smith |
1:aac28ffd63ed | 76 | * it will be significantly faster to use conjugate() instead. |
Jamie Smith |
1:aac28ffd63ed | 77 | * |
Jamie Smith |
1:aac28ffd63ed | 78 | * @return The quaternion q such that q * (*this) == (*this) * q |
Jamie Smith |
1:aac28ffd63ed | 79 | * == [ 0 0 0 1 ]<sup>T</sup>. |
Jamie Smith |
1:aac28ffd63ed | 80 | */ |
Jamie Smith |
1:aac28ffd63ed | 81 | Quaternion inverse(void) const { |
Jamie Smith |
1:aac28ffd63ed | 82 | return conjugate() / norm(); |
Jamie Smith |
1:aac28ffd63ed | 83 | } |
Jamie Smith |
1:aac28ffd63ed | 84 | |
Jamie Smith |
1:aac28ffd63ed | 85 | |
Jamie Smith |
1:aac28ffd63ed | 86 | /** |
Jamie Smith |
1:aac28ffd63ed | 87 | * @brief Computes the product of this quaternion with the |
Jamie Smith |
1:aac28ffd63ed | 88 | * quaternion 'rhs'. |
Jamie Smith |
1:aac28ffd63ed | 89 | * |
Jamie Smith |
1:aac28ffd63ed | 90 | * @param rhs The right-hand-side of the product operation. |
Jamie Smith |
1:aac28ffd63ed | 91 | * |
Jamie Smith |
1:aac28ffd63ed | 92 | * @return The quaternion product (*this) x @p rhs. |
Jamie Smith |
1:aac28ffd63ed | 93 | */ |
Jamie Smith |
1:aac28ffd63ed | 94 | Quaternion product(const Quaternion& rhs) const { |
Jamie Smith |
1:aac28ffd63ed | 95 | return Quaternion(y()*rhs.z() - z()*rhs.y() + x()*rhs.w() + w()*rhs.x(), |
Jamie Smith |
1:aac28ffd63ed | 96 | z()*rhs.x() - x()*rhs.z() + y()*rhs.w() + w()*rhs.y(), |
Jamie Smith |
1:aac28ffd63ed | 97 | x()*rhs.y() - y()*rhs.x() + z()*rhs.w() + w()*rhs.z(), |
Jamie Smith |
1:aac28ffd63ed | 98 | w()*rhs.w() - x()*rhs.x() - y()*rhs.y() - z()*rhs.z()); |
Jamie Smith |
1:aac28ffd63ed | 99 | } |
Jamie Smith |
1:aac28ffd63ed | 100 | |
Jamie Smith |
1:aac28ffd63ed | 101 | /** |
Jamie Smith |
1:aac28ffd63ed | 102 | * @brief Quaternion product operator. |
Jamie Smith |
1:aac28ffd63ed | 103 | * |
Jamie Smith |
1:aac28ffd63ed | 104 | * The result is a quaternion such that: |
Jamie Smith |
1:aac28ffd63ed | 105 | * |
Jamie Smith |
1:aac28ffd63ed | 106 | * result.real() = (*this).real() * rhs.real() - |
Jamie Smith |
1:aac28ffd63ed | 107 | * (*this).complex().dot(rhs.complex()); |
Jamie Smith |
1:aac28ffd63ed | 108 | * |
Jamie Smith |
1:aac28ffd63ed | 109 | * and: |
Jamie Smith |
1:aac28ffd63ed | 110 | * |
Jamie Smith |
1:aac28ffd63ed | 111 | * result.complex() = rhs.complex() * (*this).real |
Jamie Smith |
1:aac28ffd63ed | 112 | * + (*this).complex() * rhs.real() |
Jamie Smith |
1:aac28ffd63ed | 113 | * - (*this).complex().cross(rhs.complex()); |
Jamie Smith |
1:aac28ffd63ed | 114 | * |
Jamie Smith |
1:aac28ffd63ed | 115 | * @return The quaternion product (*this) x rhs. |
Jamie Smith |
1:aac28ffd63ed | 116 | */ |
Jamie Smith |
1:aac28ffd63ed | 117 | Quaternion operator*(const Quaternion& rhs) const { |
Jamie Smith |
1:aac28ffd63ed | 118 | return product(rhs); |
Jamie Smith |
1:aac28ffd63ed | 119 | } |
Jamie Smith |
1:aac28ffd63ed | 120 | |
Jamie Smith |
1:aac28ffd63ed | 121 | /** |
Jamie Smith |
1:aac28ffd63ed | 122 | * @brief Quaternion scalar product operator. |
Jamie Smith |
1:aac28ffd63ed | 123 | * @param s A scalar by which to multiply all components |
Jamie Smith |
1:aac28ffd63ed | 124 | * of this quaternion. |
Jamie Smith |
1:aac28ffd63ed | 125 | * @return The quaternion (*this) * s. |
Jamie Smith |
1:aac28ffd63ed | 126 | */ |
Jamie Smith |
1:aac28ffd63ed | 127 | Quaternion operator*(FloatType s) const { |
Jamie Smith |
1:aac28ffd63ed | 128 | return Quaternion(complex()*s, real()*s); |
Jamie Smith |
1:aac28ffd63ed | 129 | } |
Jamie Smith |
1:aac28ffd63ed | 130 | |
Jamie Smith |
1:aac28ffd63ed | 131 | /** |
Jamie Smith |
1:aac28ffd63ed | 132 | * @brief Produces the sum of this quaternion and rhs. |
Jamie Smith |
1:aac28ffd63ed | 133 | */ |
Jamie Smith |
1:aac28ffd63ed | 134 | Quaternion operator+(const Quaternion& rhs) const { |
Jamie Smith |
1:aac28ffd63ed | 135 | return Quaternion(x()+rhs.x(), y()+rhs.y(), z()+rhs.z(), w()+rhs.w()); |
Jamie Smith |
1:aac28ffd63ed | 136 | } |
Jamie Smith |
1:aac28ffd63ed | 137 | |
Jamie Smith |
1:aac28ffd63ed | 138 | /** |
Jamie Smith |
1:aac28ffd63ed | 139 | * @brief Produces the difference of this quaternion and rhs. |
Jamie Smith |
1:aac28ffd63ed | 140 | */ |
Jamie Smith |
1:aac28ffd63ed | 141 | Quaternion operator-(const Quaternion& rhs) const { |
Jamie Smith |
1:aac28ffd63ed | 142 | return Quaternion(x()-rhs.x(), y()-rhs.y(), z()-rhs.z(), w()-rhs.w()); |
Jamie Smith |
1:aac28ffd63ed | 143 | } |
Jamie Smith |
1:aac28ffd63ed | 144 | |
Jamie Smith |
1:aac28ffd63ed | 145 | /** |
Jamie Smith |
1:aac28ffd63ed | 146 | * @brief Unary negation. |
Jamie Smith |
1:aac28ffd63ed | 147 | */ |
Jamie Smith |
1:aac28ffd63ed | 148 | Quaternion operator-() const { |
Jamie Smith |
1:aac28ffd63ed | 149 | return Quaternion(-x(), -y(), -z(), -w()); |
Jamie Smith |
1:aac28ffd63ed | 150 | } |
Jamie Smith |
1:aac28ffd63ed | 151 | |
Jamie Smith |
1:aac28ffd63ed | 152 | /** |
Jamie Smith |
1:aac28ffd63ed | 153 | * @brief Quaternion scalar division operator. |
Jamie Smith |
1:aac28ffd63ed | 154 | * @param s A scalar by which to divide all components |
Jamie Smith |
1:aac28ffd63ed | 155 | * of this quaternion. |
Jamie Smith |
1:aac28ffd63ed | 156 | * @return The quaternion (*this) / s. |
Jamie Smith |
1:aac28ffd63ed | 157 | */ |
Jamie Smith |
1:aac28ffd63ed | 158 | Quaternion operator/(FloatType s) const { |
Jamie Smith |
1:aac28ffd63ed | 159 | MBED_ASSERT(s != 0); |
Jamie Smith |
1:aac28ffd63ed | 160 | return Quaternion(complex()/s, real()/s); |
Jamie Smith |
1:aac28ffd63ed | 161 | } |
Jamie Smith |
1:aac28ffd63ed | 162 | |
Jamie Smith |
1:aac28ffd63ed | 163 | /** |
Jamie Smith |
1:aac28ffd63ed | 164 | * @brief Returns a matrix representation of this |
Jamie Smith |
1:aac28ffd63ed | 165 | * quaternion. |
Jamie Smith |
1:aac28ffd63ed | 166 | * |
Jamie Smith |
1:aac28ffd63ed | 167 | * Specifically this is the matrix such that: |
Jamie Smith |
1:aac28ffd63ed | 168 | * |
Jamie Smith |
1:aac28ffd63ed | 169 | * this->matrix() * q.vector() = (*this) * q for any quaternion q. |
Jamie Smith |
1:aac28ffd63ed | 170 | * |
Jamie Smith |
1:aac28ffd63ed | 171 | * Note that this is @e NOT the rotation matrix that may be |
Jamie Smith |
1:aac28ffd63ed | 172 | * represented by a unit quaternion. |
Jamie Smith |
1:aac28ffd63ed | 173 | */ |
Jamie Smith |
1:aac28ffd63ed | 174 | TMatrix4 matrix() const { |
Jamie Smith |
1:aac28ffd63ed | 175 | FloatType m[16] = { |
Jamie Smith |
1:aac28ffd63ed | 176 | w(), -z(), y(), x(), |
Jamie Smith |
1:aac28ffd63ed | 177 | z(), w(), -x(), y(), |
Jamie Smith |
1:aac28ffd63ed | 178 | -y(), x(), w(), z(), |
Jamie Smith |
1:aac28ffd63ed | 179 | -x(), -y(), -z(), w() |
Jamie Smith |
1:aac28ffd63ed | 180 | }; |
Jamie Smith |
1:aac28ffd63ed | 181 | return TMatrix4(m); |
Jamie Smith |
1:aac28ffd63ed | 182 | } |
Jamie Smith |
1:aac28ffd63ed | 183 | |
Jamie Smith |
1:aac28ffd63ed | 184 | /** |
Jamie Smith |
1:aac28ffd63ed | 185 | * @brief Returns a matrix representation of this |
Jamie Smith |
1:aac28ffd63ed | 186 | * quaternion for right multiplication. |
Jamie Smith |
1:aac28ffd63ed | 187 | * |
Jamie Smith |
1:aac28ffd63ed | 188 | * Specifically this is the matrix such that: |
Jamie Smith |
1:aac28ffd63ed | 189 | * |
Jamie Smith |
1:aac28ffd63ed | 190 | * q.vector().transpose() * this->matrix() = (q * |
Jamie Smith |
1:aac28ffd63ed | 191 | * (*this)).vector().transpose() for any quaternion q. |
Jamie Smith |
1:aac28ffd63ed | 192 | * |
Jamie Smith |
1:aac28ffd63ed | 193 | * Note that this is @e NOT the rotation matrix that may be |
Jamie Smith |
1:aac28ffd63ed | 194 | * represented by a unit quaternion. |
Jamie Smith |
1:aac28ffd63ed | 195 | */ |
Jamie Smith |
1:aac28ffd63ed | 196 | TMatrix4 rightMatrix() const { |
Jamie Smith |
1:aac28ffd63ed | 197 | FloatType m[16] = { |
Jamie Smith |
1:aac28ffd63ed | 198 | +w(), -z(), y(), -x(), |
Jamie Smith |
1:aac28ffd63ed | 199 | +z(), w(), -x(), -y(), |
Jamie Smith |
1:aac28ffd63ed | 200 | -y(), x(), w(), -z(), |
Jamie Smith |
1:aac28ffd63ed | 201 | +x(), y(), z(), w() |
Jamie Smith |
1:aac28ffd63ed | 202 | }; |
Jamie Smith |
1:aac28ffd63ed | 203 | return TMatrix4(m); |
Jamie Smith |
1:aac28ffd63ed | 204 | } |
Jamie Smith |
1:aac28ffd63ed | 205 | |
Jamie Smith |
1:aac28ffd63ed | 206 | /** |
Jamie Smith |
1:aac28ffd63ed | 207 | * @brief Returns this quaternion as a 4-vector. |
Jamie Smith |
1:aac28ffd63ed | 208 | * |
Jamie Smith |
1:aac28ffd63ed | 209 | * This is simply the vector [x y z w]<sup>T</sup> |
Jamie Smith |
1:aac28ffd63ed | 210 | */ |
Jamie Smith |
1:aac28ffd63ed | 211 | TVector4 vector() const { return TVector4(mData); } |
Jamie Smith |
1:aac28ffd63ed | 212 | |
Jamie Smith |
1:aac28ffd63ed | 213 | /** |
Jamie Smith |
1:aac28ffd63ed | 214 | * @brief Returns the norm ("magnitude") of the quaternion. |
Jamie Smith |
1:aac28ffd63ed | 215 | * @return The 2-norm of [ w(), x(), y(), z() ]<sup>T</sup>. |
Jamie Smith |
1:aac28ffd63ed | 216 | */ |
Jamie Smith |
1:aac28ffd63ed | 217 | FloatType norm() const { return sqrt(mData[0]*mData[0]+mData[1]*mData[1]+ |
Jamie Smith |
1:aac28ffd63ed | 218 | mData[2]*mData[2]+mData[3]*mData[3]); } |
Jamie Smith |
1:aac28ffd63ed | 219 | |
Jamie Smith |
1:aac28ffd63ed | 220 | /** |
Jamie Smith |
1:aac28ffd63ed | 221 | * @brief Computes the rotation matrix represented by a unit |
Jamie Smith |
1:aac28ffd63ed | 222 | * quaternion. |
Jamie Smith |
1:aac28ffd63ed | 223 | * |
Jamie Smith |
1:aac28ffd63ed | 224 | * @note This does not check that this quaternion is normalized. |
Jamie Smith |
1:aac28ffd63ed | 225 | * It formulaically returns the matrix, which will not be a |
Jamie Smith |
1:aac28ffd63ed | 226 | * rotation if the quaternion is non-unit. |
Jamie Smith |
1:aac28ffd63ed | 227 | */ |
Jamie Smith |
1:aac28ffd63ed | 228 | TMatrix3 rotationMatrix() const { |
Jamie Smith |
1:aac28ffd63ed | 229 | FloatType m[9] = { |
Jamie Smith |
1:aac28ffd63ed | 230 | 1-2*y()*y()-2*z()*z(), 2*x()*y() - 2*z()*w(), 2*x()*z() + 2*y()*w(), |
Jamie Smith |
1:aac28ffd63ed | 231 | 2*x()*y() + 2*z()*w(), 1-2*x()*x()-2*z()*z(), 2*y()*z() - 2*x()*w(), |
Jamie Smith |
1:aac28ffd63ed | 232 | 2*x()*z() - 2*y()*w(), 2*y()*z() + 2*x()*w(), 1-2*x()*x()-2*y()*y() |
Jamie Smith |
1:aac28ffd63ed | 233 | }; |
Jamie Smith |
1:aac28ffd63ed | 234 | return TMatrix3(m); |
Jamie Smith |
1:aac28ffd63ed | 235 | } |
Jamie Smith |
1:aac28ffd63ed | 236 | /** |
Jamie Smith |
1:aac28ffd63ed | 237 | * @brief Sets quaternion to be same as rotation by scaled axis w. |
Jamie Smith |
1:aac28ffd63ed | 238 | */ |
Jamie Smith |
1:aac28ffd63ed | 239 | void scaledAxis(const TVector3& w) { |
Jamie Smith |
1:aac28ffd63ed | 240 | FloatType theta = w.norm(); |
Jamie Smith |
1:aac28ffd63ed | 241 | if (theta > 0.0001) { |
Jamie Smith |
1:aac28ffd63ed | 242 | FloatType s = sin(theta / 2.0); |
Jamie Smith |
1:aac28ffd63ed | 243 | TVector3 W(w / theta * s); |
Jamie Smith |
1:aac28ffd63ed | 244 | mData[0] = W[0]; |
Jamie Smith |
1:aac28ffd63ed | 245 | mData[1] = W[1]; |
Jamie Smith |
1:aac28ffd63ed | 246 | mData[2] = W[2]; |
Jamie Smith |
1:aac28ffd63ed | 247 | mData[3] = cos(theta / 2.0); |
Jamie Smith |
1:aac28ffd63ed | 248 | } else { |
Jamie Smith |
1:aac28ffd63ed | 249 | mData[0]=mData[1]=mData[2]=0; |
Jamie Smith |
1:aac28ffd63ed | 250 | mData[3]=1.0; |
Jamie Smith |
1:aac28ffd63ed | 251 | } |
Jamie Smith |
1:aac28ffd63ed | 252 | } |
Jamie Smith |
1:aac28ffd63ed | 253 | |
Jamie Smith |
1:aac28ffd63ed | 254 | /** |
Jamie Smith |
1:aac28ffd63ed | 255 | * @brief Returns a vector rotated by this quaternion. |
Jamie Smith |
1:aac28ffd63ed | 256 | * |
Jamie Smith |
1:aac28ffd63ed | 257 | * Functionally equivalent to: (rotationMatrix() * v) |
Jamie Smith |
1:aac28ffd63ed | 258 | * or (q * Quaternion(0, v) * q.inverse()). |
Jamie Smith |
1:aac28ffd63ed | 259 | * |
Jamie Smith |
1:aac28ffd63ed | 260 | * @warning conjugate() is used instead of inverse() for better |
Jamie Smith |
1:aac28ffd63ed | 261 | * performance, when this quaternion must be normalized. |
Jamie Smith |
1:aac28ffd63ed | 262 | */ |
Jamie Smith |
1:aac28ffd63ed | 263 | TVector3 rotatedVector(const TVector3& v) const { |
Jamie Smith |
1:aac28ffd63ed | 264 | return (((*this) * Quaternion(v, 0)) * conjugate()).complex(); |
Jamie Smith |
1:aac28ffd63ed | 265 | } |
Jamie Smith |
1:aac28ffd63ed | 266 | |
Jamie Smith |
1:aac28ffd63ed | 267 | |
Jamie Smith |
1:aac28ffd63ed | 268 | |
Jamie Smith |
1:aac28ffd63ed | 269 | /** |
Jamie Smith |
1:aac28ffd63ed | 270 | * @brief Computes the quaternion that is equivalent to a given |
Jamie Smith |
1:aac28ffd63ed | 271 | * euler angle rotation. |
Jamie Smith |
1:aac28ffd63ed | 272 | * @param euler A 3-vector in order: roll-pitch-yaw. |
Jamie Smith |
1:aac28ffd63ed | 273 | */ |
Jamie Smith |
1:aac28ffd63ed | 274 | void euler(const TVector3& euler) { |
Jamie Smith |
1:aac28ffd63ed | 275 | FloatType c1 = cos(euler[2] * 0.5); |
Jamie Smith |
1:aac28ffd63ed | 276 | FloatType c2 = cos(euler[1] * 0.5); |
Jamie Smith |
1:aac28ffd63ed | 277 | FloatType c3 = cos(euler[0] * 0.5); |
Jamie Smith |
1:aac28ffd63ed | 278 | FloatType s1 = sin(euler[2] * 0.5); |
Jamie Smith |
1:aac28ffd63ed | 279 | FloatType s2 = sin(euler[1] * 0.5); |
Jamie Smith |
1:aac28ffd63ed | 280 | FloatType s3 = sin(euler[0] * 0.5); |
Jamie Smith |
1:aac28ffd63ed | 281 | |
Jamie Smith |
1:aac28ffd63ed | 282 | mData[0] = c1*c2*s3 - s1*s2*c3; |
Jamie Smith |
1:aac28ffd63ed | 283 | mData[1] = c1*s2*c3 + s1*c2*s3; |
Jamie Smith |
1:aac28ffd63ed | 284 | mData[2] = s1*c2*c3 - c1*s2*s3; |
Jamie Smith |
1:aac28ffd63ed | 285 | mData[3] = c1*c2*c3 + s1*s2*s3; |
Jamie Smith |
1:aac28ffd63ed | 286 | } |
Jamie Smith |
1:aac28ffd63ed | 287 | |
Jamie Smith |
1:aac28ffd63ed | 288 | /** @brief Returns an equivalent euler angle representation of |
Jamie Smith |
1:aac28ffd63ed | 289 | * this quaternion. |
Jamie Smith |
1:aac28ffd63ed | 290 | * @return Euler angles in roll-pitch-yaw order. |
Jamie Smith |
1:aac28ffd63ed | 291 | */ |
Jamie Smith |
1:aac28ffd63ed | 292 | TVector3 euler(void) const { |
Jamie Smith |
1:aac28ffd63ed | 293 | TVector3 euler; |
Jamie Smith |
1:aac28ffd63ed | 294 | const static FloatType PI_OVER_2 = M_PI * 0.5; |
Jamie Smith |
1:aac28ffd63ed | 295 | const static FloatType EPSILON = 1e-10; |
Jamie Smith |
1:aac28ffd63ed | 296 | FloatType sqw, sqx, sqy, sqz; |
Jamie Smith |
1:aac28ffd63ed | 297 | |
Jamie Smith |
1:aac28ffd63ed | 298 | // quick conversion to Euler angles to give tilt to user |
Jamie Smith |
1:aac28ffd63ed | 299 | sqw = mData[3]*mData[3]; |
Jamie Smith |
1:aac28ffd63ed | 300 | sqx = mData[0]*mData[0]; |
Jamie Smith |
1:aac28ffd63ed | 301 | sqy = mData[1]*mData[1]; |
Jamie Smith |
1:aac28ffd63ed | 302 | sqz = mData[2]*mData[2]; |
Jamie Smith |
1:aac28ffd63ed | 303 | |
Jamie Smith |
1:aac28ffd63ed | 304 | euler[1] = asin(2.0 * (mData[3]*mData[1] - mData[0]*mData[2])); |
Jamie Smith |
1:aac28ffd63ed | 305 | if (PI_OVER_2 - fabs(euler[1]) > EPSILON) { |
Jamie Smith |
5:c75be9000d75 | 306 | euler[2] = atan2(static_cast<FloatType>(2.0) * (mData[0]*mData[1] + mData[3]*mData[2]), |
Jamie Smith |
1:aac28ffd63ed | 307 | sqx - sqy - sqz + sqw); |
Jamie Smith |
5:c75be9000d75 | 308 | euler[0] = atan2(static_cast<FloatType>(2.0) * (mData[3]*mData[0] + mData[1]*mData[2]), |
Jamie Smith |
1:aac28ffd63ed | 309 | sqw - sqx - sqy + sqz); |
Jamie Smith |
1:aac28ffd63ed | 310 | } else { |
Jamie Smith |
1:aac28ffd63ed | 311 | // compute heading from local 'down' vector |
Jamie Smith |
1:aac28ffd63ed | 312 | euler[2] = atan2(2*mData[1]*mData[2] - 2*mData[0]*mData[3], |
Jamie Smith |
1:aac28ffd63ed | 313 | 2*mData[0]*mData[2] + 2*mData[1]*mData[3]); |
Jamie Smith |
1:aac28ffd63ed | 314 | euler[0] = 0.0; |
Jamie Smith |
1:aac28ffd63ed | 315 | |
Jamie Smith |
1:aac28ffd63ed | 316 | // If facing down, reverse yaw |
Jamie Smith |
1:aac28ffd63ed | 317 | if (euler[1] < 0) |
Jamie Smith |
1:aac28ffd63ed | 318 | euler[2] = M_PI - euler[2]; |
Jamie Smith |
1:aac28ffd63ed | 319 | } |
Jamie Smith |
1:aac28ffd63ed | 320 | return euler; |
Jamie Smith |
1:aac28ffd63ed | 321 | } |
Jamie Smith |
1:aac28ffd63ed | 322 | |
Jamie Smith |
1:aac28ffd63ed | 323 | /** |
Jamie Smith |
1:aac28ffd63ed | 324 | * @brief Returns pointer to the internal array. |
Jamie Smith |
1:aac28ffd63ed | 325 | * |
Jamie Smith |
1:aac28ffd63ed | 326 | * Array is in order x,y,z,w. |
Jamie Smith |
1:aac28ffd63ed | 327 | */ |
Jamie Smith |
1:aac28ffd63ed | 328 | FloatType* row(uint32_t i) { return mData + i; } |
Jamie Smith |
1:aac28ffd63ed | 329 | // Const version of the above. |
Jamie Smith |
1:aac28ffd63ed | 330 | const FloatType* row(uint32_t i) const { return mData + i; } |
Jamie Smith |
1:aac28ffd63ed | 331 | }; |
Jamie Smith |
1:aac28ffd63ed | 332 | |
Jamie Smith |
1:aac28ffd63ed | 333 | /** |
Jamie Smith |
1:aac28ffd63ed | 334 | * @brief Global operator allowing left-multiply by scalar. |
Jamie Smith |
1:aac28ffd63ed | 335 | */ |
Jamie Smith |
1:aac28ffd63ed | 336 | Quaternion operator*(Quaternion::FloatType s, const Quaternion& q); |
Jamie Smith |
1:aac28ffd63ed | 337 | |
Jamie Smith |
1:aac28ffd63ed | 338 | |
Jamie Smith |
1:aac28ffd63ed | 339 | #endif /* QUATERNION_H */ |