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
tmatrix.h@6:5ba996be5312, 2020-07-21 (annotated)
- Committer:
- Jamie Smith
- Date:
- Tue Jul 21 21:37:10 2020 -0700
- Revision:
- 6:5ba996be5312
- Parent:
- 1:aac28ffd63ed
Update for MBed OS 6
Who changed what in which revision?
User | Revision | Line number | New contents of line |
---|---|---|---|
Jamie Smith |
1:aac28ffd63ed | 1 | #ifndef TMATRIX_H |
Jamie Smith |
1:aac28ffd63ed | 2 | #define TMATRIX_H |
Jamie Smith |
1:aac28ffd63ed | 3 | |
Jamie Smith |
1:aac28ffd63ed | 4 | /** |
Jamie Smith |
1:aac28ffd63ed | 5 | * @file tmatrix.h |
Jamie Smith |
1:aac28ffd63ed | 6 | * |
Jamie Smith |
1:aac28ffd63ed | 7 | * @brief A dimension-templatized class for matrices of values. |
Jamie Smith |
1:aac28ffd63ed | 8 | */ |
Jamie Smith |
1:aac28ffd63ed | 9 | #include <cmath> |
Jamie Smith |
1:aac28ffd63ed | 10 | #include <mbed.h> |
Jamie Smith |
6:5ba996be5312 | 11 | #include <Stream.h> |
Jamie Smith |
1:aac28ffd63ed | 12 | |
Jamie Smith |
1:aac28ffd63ed | 13 | // Structures for static assert. http://www.boost.org |
Jamie Smith |
1:aac28ffd63ed | 14 | template <bool x> struct STATIC_ASSERTION_FAILURE; |
Jamie Smith |
1:aac28ffd63ed | 15 | template <> struct STATIC_ASSERTION_FAILURE<true> { enum { value = 1 }; }; |
Jamie Smith |
1:aac28ffd63ed | 16 | template<int x> struct static_assert_test{}; |
Jamie Smith |
1:aac28ffd63ed | 17 | #define STATIC_ASSERT( B ) \ |
Jamie Smith |
1:aac28ffd63ed | 18 | typedef __attribute__((unused)) static_assert_test<sizeof(STATIC_ASSERTION_FAILURE<(bool)(B)>) > \ |
Jamie Smith |
1:aac28ffd63ed | 19 | static_assert_typedef##__LINE__ |
Jamie Smith |
1:aac28ffd63ed | 20 | |
Jamie Smith |
1:aac28ffd63ed | 21 | #if DEBUG |
Jamie Smith |
1:aac28ffd63ed | 22 | #define ERROR_CHECK(X) (X) |
Jamie Smith |
1:aac28ffd63ed | 23 | #else |
Jamie Smith |
1:aac28ffd63ed | 24 | #define ERROR_CHECK(X) |
Jamie Smith |
1:aac28ffd63ed | 25 | #endif |
Jamie Smith |
1:aac28ffd63ed | 26 | |
Jamie Smith |
1:aac28ffd63ed | 27 | // Forward Decl. |
Jamie Smith |
1:aac28ffd63ed | 28 | template <uint16_t, uint16_t, typename> class BasicMatrix; |
Jamie Smith |
1:aac28ffd63ed | 29 | template <uint16_t, uint16_t, typename> class TMatrix; |
Jamie Smith |
1:aac28ffd63ed | 30 | class TMatrixDummy { }; |
Jamie Smith |
1:aac28ffd63ed | 31 | |
Jamie Smith |
1:aac28ffd63ed | 32 | /** |
Jamie Smith |
1:aac28ffd63ed | 33 | * @brief Class that layers on operator[] functionality for |
Jamie Smith |
1:aac28ffd63ed | 34 | * typical matrices. |
Jamie Smith |
1:aac28ffd63ed | 35 | */ |
Jamie Smith |
1:aac28ffd63ed | 36 | template <uint16_t Rows, uint16_t Cols, typename value_type> |
Jamie Smith |
1:aac28ffd63ed | 37 | class BasicIndexMatrix : public BasicMatrix<Rows,Cols,value_type> { |
Jamie Smith |
1:aac28ffd63ed | 38 | typedef BasicMatrix<Rows,Cols,value_type> BaseType; |
Jamie Smith |
1:aac28ffd63ed | 39 | protected: |
Jamie Smith |
1:aac28ffd63ed | 40 | BasicIndexMatrix(TMatrixDummy d) : BaseType(d) { } |
Jamie Smith |
1:aac28ffd63ed | 41 | public: |
Jamie Smith |
1:aac28ffd63ed | 42 | BasicIndexMatrix() { } |
Jamie Smith |
1:aac28ffd63ed | 43 | BasicIndexMatrix(const value_type* data) : BaseType(data) {} |
Jamie Smith |
1:aac28ffd63ed | 44 | |
Jamie Smith |
1:aac28ffd63ed | 45 | const value_type* operator[](uint16_t r) const { |
Jamie Smith |
1:aac28ffd63ed | 46 | ERROR_CHECK(if (r >= Rows) { |
Jamie Smith |
1:aac28ffd63ed | 47 | std::clog << "Invalid row index " << r << std::endl; |
Jamie Smith |
1:aac28ffd63ed | 48 | return &BaseType::mData[0]; |
Jamie Smith |
1:aac28ffd63ed | 49 | } |
Jamie Smith |
1:aac28ffd63ed | 50 | ) |
Jamie Smith |
1:aac28ffd63ed | 51 | return &BaseType::mData[r*Cols]; |
Jamie Smith |
1:aac28ffd63ed | 52 | } |
Jamie Smith |
1:aac28ffd63ed | 53 | |
Jamie Smith |
1:aac28ffd63ed | 54 | value_type* operator[](uint16_t r) { |
Jamie Smith |
1:aac28ffd63ed | 55 | ERROR_CHECK(if (r >= Rows) { |
Jamie Smith |
1:aac28ffd63ed | 56 | std::clog << "Invalid row index " << r << std::endl; |
Jamie Smith |
1:aac28ffd63ed | 57 | return &BaseType::mData[0]; |
Jamie Smith |
1:aac28ffd63ed | 58 | } |
Jamie Smith |
1:aac28ffd63ed | 59 | ) |
Jamie Smith |
1:aac28ffd63ed | 60 | return &BaseType::mData[r*Cols]; |
Jamie Smith |
1:aac28ffd63ed | 61 | } |
Jamie Smith |
1:aac28ffd63ed | 62 | }; |
Jamie Smith |
1:aac28ffd63ed | 63 | |
Jamie Smith |
1:aac28ffd63ed | 64 | /** |
Jamie Smith |
1:aac28ffd63ed | 65 | * @brief Specialization of BasicIndexMatrix that provides |
Jamie Smith |
1:aac28ffd63ed | 66 | * single-indexing operator for column vectors. |
Jamie Smith |
1:aac28ffd63ed | 67 | */ |
Jamie Smith |
1:aac28ffd63ed | 68 | template <uint16_t Rows, typename value_type> |
Jamie Smith |
1:aac28ffd63ed | 69 | class BasicIndexMatrix<Rows,1,value_type> : |
Jamie Smith |
1:aac28ffd63ed | 70 | public BasicMatrix<Rows,1,value_type> { |
Jamie Smith |
1:aac28ffd63ed | 71 | typedef BasicMatrix<Rows,1,value_type> BaseType; |
Jamie Smith |
1:aac28ffd63ed | 72 | protected: |
Jamie Smith |
1:aac28ffd63ed | 73 | BasicIndexMatrix(TMatrixDummy dummy) : BaseType(dummy) {} |
Jamie Smith |
1:aac28ffd63ed | 74 | public: |
Jamie Smith |
1:aac28ffd63ed | 75 | BasicIndexMatrix() { } |
Jamie Smith |
1:aac28ffd63ed | 76 | BasicIndexMatrix(const value_type* data) : BaseType(data) {} |
Jamie Smith |
1:aac28ffd63ed | 77 | |
Jamie Smith |
1:aac28ffd63ed | 78 | value_type operator[](uint16_t r) const { |
Jamie Smith |
1:aac28ffd63ed | 79 | ERROR_CHECK(if (r >= Rows) { |
Jamie Smith |
1:aac28ffd63ed | 80 | std::clog << "Invalid vector index " << r << std::endl; |
Jamie Smith |
1:aac28ffd63ed | 81 | return BaseType::mData[0]; |
Jamie Smith |
1:aac28ffd63ed | 82 | }) |
Jamie Smith |
1:aac28ffd63ed | 83 | return BaseType::mData[r]; |
Jamie Smith |
1:aac28ffd63ed | 84 | } |
Jamie Smith |
1:aac28ffd63ed | 85 | value_type& operator[](uint16_t r) { |
Jamie Smith |
1:aac28ffd63ed | 86 | ERROR_CHECK(if (r >= Rows) { |
Jamie Smith |
1:aac28ffd63ed | 87 | std::clog << "Invalid vector index " << r << std::endl; |
Jamie Smith |
1:aac28ffd63ed | 88 | return BaseType::mData[0]; |
Jamie Smith |
1:aac28ffd63ed | 89 | }) |
Jamie Smith |
1:aac28ffd63ed | 90 | return BaseType::mData[r]; |
Jamie Smith |
1:aac28ffd63ed | 91 | } |
Jamie Smith |
1:aac28ffd63ed | 92 | |
Jamie Smith |
1:aac28ffd63ed | 93 | value_type norm() const { |
Jamie Smith |
1:aac28ffd63ed | 94 | return sqrt(norm2()); |
Jamie Smith |
1:aac28ffd63ed | 95 | } |
Jamie Smith |
1:aac28ffd63ed | 96 | |
Jamie Smith |
1:aac28ffd63ed | 97 | value_type norm2() const { |
Jamie Smith |
1:aac28ffd63ed | 98 | double normSum = 0; |
Jamie Smith |
1:aac28ffd63ed | 99 | for (uint32_t i = 0; i < Rows; i++) { |
Jamie Smith |
1:aac28ffd63ed | 100 | normSum += BaseType::mData[i]*BaseType::mData[i]; |
Jamie Smith |
1:aac28ffd63ed | 101 | } |
Jamie Smith |
1:aac28ffd63ed | 102 | return normSum; |
Jamie Smith |
1:aac28ffd63ed | 103 | } |
Jamie Smith |
1:aac28ffd63ed | 104 | |
Jamie Smith |
1:aac28ffd63ed | 105 | /** @brief Returns matrix with vector elements on diagonal. */ |
Jamie Smith |
1:aac28ffd63ed | 106 | TMatrix<Rows, Rows, value_type> diag(void) const { |
Jamie Smith |
1:aac28ffd63ed | 107 | TMatrix<Rows, Rows, value_type> d; |
Jamie Smith |
1:aac28ffd63ed | 108 | for (uint32_t i = 0; i < Rows; i++) d.element(i,i, BaseType::mData[i]); |
Jamie Smith |
1:aac28ffd63ed | 109 | return d; |
Jamie Smith |
1:aac28ffd63ed | 110 | } |
Jamie Smith |
1:aac28ffd63ed | 111 | |
Jamie Smith |
1:aac28ffd63ed | 112 | }; |
Jamie Smith |
1:aac28ffd63ed | 113 | |
Jamie Smith |
1:aac28ffd63ed | 114 | /** |
Jamie Smith |
1:aac28ffd63ed | 115 | * @brief A dimension-templatized class for matrices of values. |
Jamie Smith |
1:aac28ffd63ed | 116 | * |
Jamie Smith |
1:aac28ffd63ed | 117 | * This template class generically supports any constant-sized |
Jamie Smith |
1:aac28ffd63ed | 118 | * matrix of values. The @p Rows and @p Cols template parameters |
Jamie Smith |
1:aac28ffd63ed | 119 | * define the size of the matrix at @e compile-time. Hence, the |
Jamie Smith |
1:aac28ffd63ed | 120 | * size of the matrix cannot be chosen at runtime. However, the |
Jamie Smith |
1:aac28ffd63ed | 121 | * dimensions are appropriately type-checked at compile-time where |
Jamie Smith |
1:aac28ffd63ed | 122 | * possible. |
Jamie Smith |
1:aac28ffd63ed | 123 | * |
Jamie Smith |
1:aac28ffd63ed | 124 | * By default, the matrix contains values of type @p double. The @p |
Jamie Smith |
1:aac28ffd63ed | 125 | * value_type template parameter may be selected to allow matrices |
Jamie Smith |
1:aac28ffd63ed | 126 | * of integers or floats. |
Jamie Smith |
1:aac28ffd63ed | 127 | * |
Jamie Smith |
1:aac28ffd63ed | 128 | * @note At present, type cohersion between matrices with different |
Jamie Smith |
1:aac28ffd63ed | 129 | * @p value_type parameters is not implemented. It is recommended |
Jamie Smith |
1:aac28ffd63ed | 130 | * that matrices with value type @p double be used for all numerical |
Jamie Smith |
1:aac28ffd63ed | 131 | * computation. |
Jamie Smith |
1:aac28ffd63ed | 132 | * |
Jamie Smith |
1:aac28ffd63ed | 133 | * Note that the special cases of row and column vectors are |
Jamie Smith |
1:aac28ffd63ed | 134 | * subsumed by this class. |
Jamie Smith |
1:aac28ffd63ed | 135 | */ |
Jamie Smith |
1:aac28ffd63ed | 136 | template <uint16_t Rows, uint16_t Cols, typename value_type = double> |
Jamie Smith |
1:aac28ffd63ed | 137 | class TMatrix : public BasicIndexMatrix<Rows, Cols, value_type> { |
Jamie Smith |
1:aac28ffd63ed | 138 | typedef BasicIndexMatrix<Rows, Cols, value_type> BaseType; |
Jamie Smith |
1:aac28ffd63ed | 139 | template <uint16_t R, uint16_t C, typename vt> friend class BasicMatrix; |
Jamie Smith |
1:aac28ffd63ed | 140 | TMatrix(TMatrixDummy d) : BaseType(d) {} |
Jamie Smith |
1:aac28ffd63ed | 141 | |
Jamie Smith |
1:aac28ffd63ed | 142 | public: |
Jamie Smith |
1:aac28ffd63ed | 143 | TMatrix() : BaseType() { } |
Jamie Smith |
1:aac28ffd63ed | 144 | TMatrix(const value_type* data) : BaseType(data) {} |
Jamie Smith |
1:aac28ffd63ed | 145 | }; |
Jamie Smith |
1:aac28ffd63ed | 146 | |
Jamie Smith |
1:aac28ffd63ed | 147 | /** |
Jamie Smith |
1:aac28ffd63ed | 148 | * @brief Template specialization of TMatrix for Vector4. |
Jamie Smith |
1:aac28ffd63ed | 149 | */ |
Jamie Smith |
1:aac28ffd63ed | 150 | template <typename value_type> |
Jamie Smith |
1:aac28ffd63ed | 151 | class TMatrix<4,1, value_type> : public BasicIndexMatrix<4,1, value_type> { |
Jamie Smith |
1:aac28ffd63ed | 152 | typedef BasicIndexMatrix<4, 1, value_type> BaseType; |
Jamie Smith |
1:aac28ffd63ed | 153 | template <uint16_t R, uint16_t C, typename vt> friend class BasicMatrix; |
Jamie Smith |
1:aac28ffd63ed | 154 | TMatrix(TMatrixDummy d) : BaseType(d) {} |
Jamie Smith |
1:aac28ffd63ed | 155 | |
Jamie Smith |
1:aac28ffd63ed | 156 | public: |
Jamie Smith |
1:aac28ffd63ed | 157 | TMatrix() { } |
Jamie Smith |
1:aac28ffd63ed | 158 | TMatrix(const value_type* data) : BaseType(data) {} |
Jamie Smith |
1:aac28ffd63ed | 159 | TMatrix(value_type a0, value_type a1, value_type a2, value_type a3) : BaseType(TMatrixDummy()) { |
Jamie Smith |
1:aac28ffd63ed | 160 | BaseType::mData[0] = a0; |
Jamie Smith |
1:aac28ffd63ed | 161 | BaseType::mData[1] = a1; |
Jamie Smith |
1:aac28ffd63ed | 162 | BaseType::mData[2] = a2; |
Jamie Smith |
1:aac28ffd63ed | 163 | BaseType::mData[3] = a3; |
Jamie Smith |
1:aac28ffd63ed | 164 | } |
Jamie Smith |
1:aac28ffd63ed | 165 | }; |
Jamie Smith |
1:aac28ffd63ed | 166 | |
Jamie Smith |
1:aac28ffd63ed | 167 | /** |
Jamie Smith |
1:aac28ffd63ed | 168 | * @brief Template specialization of TMatrix for Vector3. |
Jamie Smith |
1:aac28ffd63ed | 169 | */ |
Jamie Smith |
1:aac28ffd63ed | 170 | template <typename value_type> |
Jamie Smith |
1:aac28ffd63ed | 171 | class TMatrix<3,1, value_type> : public BasicIndexMatrix<3,1, value_type> { |
Jamie Smith |
1:aac28ffd63ed | 172 | typedef BasicIndexMatrix<3, 1, value_type> BaseType; |
Jamie Smith |
1:aac28ffd63ed | 173 | template <uint16_t R, uint16_t C, typename vt> friend class BasicMatrix; |
Jamie Smith |
1:aac28ffd63ed | 174 | TMatrix(TMatrixDummy d) : BaseType(d) {} |
Jamie Smith |
1:aac28ffd63ed | 175 | |
Jamie Smith |
1:aac28ffd63ed | 176 | public: |
Jamie Smith |
1:aac28ffd63ed | 177 | TMatrix() { } |
Jamie Smith |
1:aac28ffd63ed | 178 | TMatrix(const value_type* data) : BaseType(data) {} |
Jamie Smith |
1:aac28ffd63ed | 179 | TMatrix(value_type a0, value_type a1, value_type a2) : BaseType(TMatrixDummy()) { |
Jamie Smith |
1:aac28ffd63ed | 180 | BaseType::mData[0] = a0; |
Jamie Smith |
1:aac28ffd63ed | 181 | BaseType::mData[1] = a1; |
Jamie Smith |
1:aac28ffd63ed | 182 | BaseType::mData[2] = a2; |
Jamie Smith |
1:aac28ffd63ed | 183 | } |
Jamie Smith |
1:aac28ffd63ed | 184 | |
Jamie Smith |
1:aac28ffd63ed | 185 | TMatrix<3,1,value_type> cross(const TMatrix<3,1, value_type>& v) const { |
Jamie Smith |
1:aac28ffd63ed | 186 | const TMatrix<3,1,value_type>& u = *this; |
Jamie Smith |
1:aac28ffd63ed | 187 | return TMatrix<3,1,value_type>(u[1]*v[2]-u[2]*v[1], |
Jamie Smith |
1:aac28ffd63ed | 188 | u[2]*v[0]-u[0]*v[2], |
Jamie Smith |
1:aac28ffd63ed | 189 | u[0]*v[1]-u[1]*v[0]); |
Jamie Smith |
1:aac28ffd63ed | 190 | } |
Jamie Smith |
1:aac28ffd63ed | 191 | }; |
Jamie Smith |
1:aac28ffd63ed | 192 | |
Jamie Smith |
1:aac28ffd63ed | 193 | /** |
Jamie Smith |
1:aac28ffd63ed | 194 | * @brief Template specialization of TMatrix for Vector2. |
Jamie Smith |
1:aac28ffd63ed | 195 | */ |
Jamie Smith |
1:aac28ffd63ed | 196 | template <typename value_type> |
Jamie Smith |
1:aac28ffd63ed | 197 | class TMatrix<2,1, value_type> : public BasicIndexMatrix<2,1, value_type> { |
Jamie Smith |
1:aac28ffd63ed | 198 | typedef BasicIndexMatrix<2, 1, value_type> BaseType; |
Jamie Smith |
1:aac28ffd63ed | 199 | template <uint16_t R, uint16_t C, typename vt> friend class BasicMatrix; |
Jamie Smith |
1:aac28ffd63ed | 200 | TMatrix(TMatrixDummy d) : BaseType(d) {} |
Jamie Smith |
1:aac28ffd63ed | 201 | |
Jamie Smith |
1:aac28ffd63ed | 202 | public: |
Jamie Smith |
1:aac28ffd63ed | 203 | TMatrix() { } |
Jamie Smith |
1:aac28ffd63ed | 204 | TMatrix(const value_type* data) : BaseType(data) {} |
Jamie Smith |
1:aac28ffd63ed | 205 | TMatrix(value_type a0, value_type a1) : BaseType(TMatrixDummy()) { |
Jamie Smith |
1:aac28ffd63ed | 206 | BaseType::mData[0] = a0; |
Jamie Smith |
1:aac28ffd63ed | 207 | BaseType::mData[1] = a1; |
Jamie Smith |
1:aac28ffd63ed | 208 | } |
Jamie Smith |
1:aac28ffd63ed | 209 | }; |
Jamie Smith |
1:aac28ffd63ed | 210 | |
Jamie Smith |
1:aac28ffd63ed | 211 | /** |
Jamie Smith |
1:aac28ffd63ed | 212 | * @brief Template specialization of TMatrix for a 1x1 vector. |
Jamie Smith |
1:aac28ffd63ed | 213 | */ |
Jamie Smith |
1:aac28ffd63ed | 214 | template <typename value_type> |
Jamie Smith |
1:aac28ffd63ed | 215 | class TMatrix<1,1, value_type> : public BasicIndexMatrix<1,1, value_type> { |
Jamie Smith |
1:aac28ffd63ed | 216 | typedef BasicIndexMatrix<1, 1, value_type> BaseType; |
Jamie Smith |
1:aac28ffd63ed | 217 | template <uint16_t R, uint16_t C, typename vt> friend class BasicMatrix; |
Jamie Smith |
1:aac28ffd63ed | 218 | TMatrix(TMatrixDummy dummy) : BaseType(dummy) {} |
Jamie Smith |
1:aac28ffd63ed | 219 | |
Jamie Smith |
1:aac28ffd63ed | 220 | public: |
Jamie Smith |
1:aac28ffd63ed | 221 | TMatrix() { } |
Jamie Smith |
1:aac28ffd63ed | 222 | TMatrix(const value_type* data) : BaseType(data) {} |
Jamie Smith |
1:aac28ffd63ed | 223 | |
Jamie Smith |
1:aac28ffd63ed | 224 | // explicit conversion from value_type |
Jamie Smith |
1:aac28ffd63ed | 225 | explicit TMatrix(value_type a0) : BaseType(TMatrixDummy()) { // don't initialize |
Jamie Smith |
1:aac28ffd63ed | 226 | BaseType::mData[0] = a0; |
Jamie Smith |
1:aac28ffd63ed | 227 | } |
Jamie Smith |
1:aac28ffd63ed | 228 | |
Jamie Smith |
1:aac28ffd63ed | 229 | // implicit conversion to value_type |
Jamie Smith |
1:aac28ffd63ed | 230 | operator value_type() const { |
Jamie Smith |
1:aac28ffd63ed | 231 | return BaseType::mData[0]; |
Jamie Smith |
1:aac28ffd63ed | 232 | } |
Jamie Smith |
1:aac28ffd63ed | 233 | |
Jamie Smith |
1:aac28ffd63ed | 234 | const TMatrix<1,1, value_type>& |
Jamie Smith |
1:aac28ffd63ed | 235 | operator=(const TMatrix<1,1, value_type>& m) { |
Jamie Smith |
1:aac28ffd63ed | 236 | BaseType::operator=(m); |
Jamie Smith |
1:aac28ffd63ed | 237 | return *this; |
Jamie Smith |
1:aac28ffd63ed | 238 | } |
Jamie Smith |
1:aac28ffd63ed | 239 | |
Jamie Smith |
1:aac28ffd63ed | 240 | double operator=(double a0) { |
Jamie Smith |
1:aac28ffd63ed | 241 | BaseType::mData[0] = a0; |
Jamie Smith |
1:aac28ffd63ed | 242 | return BaseType::mData[0]; |
Jamie Smith |
1:aac28ffd63ed | 243 | } |
Jamie Smith |
1:aac28ffd63ed | 244 | }; |
Jamie Smith |
1:aac28ffd63ed | 245 | |
Jamie Smith |
1:aac28ffd63ed | 246 | |
Jamie Smith |
1:aac28ffd63ed | 247 | /** |
Jamie Smith |
1:aac28ffd63ed | 248 | * @brief Base class implementing standard matrix functionality. |
Jamie Smith |
1:aac28ffd63ed | 249 | */ |
Jamie Smith |
1:aac28ffd63ed | 250 | template <uint16_t Rows, uint16_t Cols, typename value_type = double> |
Jamie Smith |
1:aac28ffd63ed | 251 | class BasicMatrix { |
Jamie Smith |
1:aac28ffd63ed | 252 | protected: |
Jamie Smith |
1:aac28ffd63ed | 253 | value_type mData[Rows*Cols]; |
Jamie Smith |
1:aac28ffd63ed | 254 | |
Jamie Smith |
1:aac28ffd63ed | 255 | // Constructs uninitialized matrix. |
Jamie Smith |
1:aac28ffd63ed | 256 | BasicMatrix(TMatrixDummy dummy) {} |
Jamie Smith |
1:aac28ffd63ed | 257 | public: |
Jamie Smith |
1:aac28ffd63ed | 258 | BasicMatrix() { // constructs zero matrix |
Jamie Smith |
1:aac28ffd63ed | 259 | for (uint16_t i = 0; i < Rows*Cols; ++i) { |
Jamie Smith |
1:aac28ffd63ed | 260 | mData[i] = 0; |
Jamie Smith |
1:aac28ffd63ed | 261 | } |
Jamie Smith |
1:aac28ffd63ed | 262 | } |
Jamie Smith |
1:aac28ffd63ed | 263 | BasicMatrix(const value_type* data) { // constructs from array |
Jamie Smith |
1:aac28ffd63ed | 264 | MBED_ASSERT(data); |
Jamie Smith |
1:aac28ffd63ed | 265 | |
Jamie Smith |
1:aac28ffd63ed | 266 | for (uint16_t i = 0; i < Rows*Cols; ++i) { |
Jamie Smith |
1:aac28ffd63ed | 267 | mData[i] = data[i]; |
Jamie Smith |
1:aac28ffd63ed | 268 | } |
Jamie Smith |
1:aac28ffd63ed | 269 | } |
Jamie Smith |
1:aac28ffd63ed | 270 | |
Jamie Smith |
1:aac28ffd63ed | 271 | uint32_t rows() const { return Rows; } |
Jamie Smith |
1:aac28ffd63ed | 272 | uint32_t columns() const { return Cols; } |
Jamie Smith |
1:aac28ffd63ed | 273 | uint32_t elementCount() const { return Cols*Rows; } |
Jamie Smith |
1:aac28ffd63ed | 274 | |
Jamie Smith |
1:aac28ffd63ed | 275 | value_type element(uint16_t row, uint16_t col) const { |
Jamie Smith |
1:aac28ffd63ed | 276 | ERROR_CHECK(if (row >= rows() || col >= columns()) { |
Jamie Smith |
1:aac28ffd63ed | 277 | std::cerr << "Illegal read access: " << row << ", " << col |
Jamie Smith |
1:aac28ffd63ed | 278 | << " in " << Rows << "x" << Cols << " matrix." << std::endl; |
Jamie Smith |
1:aac28ffd63ed | 279 | return mData[0]; |
Jamie Smith |
1:aac28ffd63ed | 280 | }) |
Jamie Smith |
1:aac28ffd63ed | 281 | return mData[row*Cols+col]; |
Jamie Smith |
1:aac28ffd63ed | 282 | } |
Jamie Smith |
1:aac28ffd63ed | 283 | |
Jamie Smith |
1:aac28ffd63ed | 284 | value_type& element(uint16_t row, uint16_t col) { |
Jamie Smith |
1:aac28ffd63ed | 285 | ERROR_CHECK(if (row >= rows() || col >= columns()) { |
Jamie Smith |
1:aac28ffd63ed | 286 | std::cerr << "Illegal read access: " << row << ", " << col |
Jamie Smith |
1:aac28ffd63ed | 287 | << " in " << Rows << "x" << Cols << " matrix." << std::endl; |
Jamie Smith |
1:aac28ffd63ed | 288 | return mData[0]; |
Jamie Smith |
1:aac28ffd63ed | 289 | }) |
Jamie Smith |
1:aac28ffd63ed | 290 | return mData[row*Cols+col]; |
Jamie Smith |
1:aac28ffd63ed | 291 | } |
Jamie Smith |
1:aac28ffd63ed | 292 | |
Jamie Smith |
1:aac28ffd63ed | 293 | void element(uint16_t row, uint16_t col, value_type value) { |
Jamie Smith |
1:aac28ffd63ed | 294 | ERROR_CHECK(if (row >= rows() || col >= columns()) { |
Jamie Smith |
1:aac28ffd63ed | 295 | std::cerr << "Illegal write access: " << row << ", " << col |
Jamie Smith |
1:aac28ffd63ed | 296 | << " in " << Rows << "x" << Cols << " matrix." << std::endl; |
Jamie Smith |
1:aac28ffd63ed | 297 | return ; |
Jamie Smith |
1:aac28ffd63ed | 298 | }) |
Jamie Smith |
1:aac28ffd63ed | 299 | mData[row*Cols+col] = value; |
Jamie Smith |
1:aac28ffd63ed | 300 | } |
Jamie Smith |
1:aac28ffd63ed | 301 | |
Jamie Smith |
1:aac28ffd63ed | 302 | TMatrix<Rows*Cols,1, value_type> vec() const { |
Jamie Smith |
1:aac28ffd63ed | 303 | return TMatrix<Rows*Cols,1, value_type>(mData); |
Jamie Smith |
1:aac28ffd63ed | 304 | } |
Jamie Smith |
1:aac28ffd63ed | 305 | |
Jamie Smith |
1:aac28ffd63ed | 306 | void vec(const TMatrix<Rows*Cols, 1, value_type>& vector) { |
Jamie Smith |
1:aac28ffd63ed | 307 | for (uint32_t i = 0; i < Rows*Cols; i++) { |
Jamie Smith |
1:aac28ffd63ed | 308 | mData[i] = vector.mData[i]; |
Jamie Smith |
1:aac28ffd63ed | 309 | } |
Jamie Smith |
1:aac28ffd63ed | 310 | } |
Jamie Smith |
1:aac28ffd63ed | 311 | |
Jamie Smith |
1:aac28ffd63ed | 312 | template <uint16_t R, uint16_t C, uint16_t RowRangeSize, uint16_t ColRangeSize> |
Jamie Smith |
1:aac28ffd63ed | 313 | TMatrix<RowRangeSize, ColRangeSize, value_type> subMatrix(void) const { |
Jamie Smith |
1:aac28ffd63ed | 314 | STATIC_ASSERT((R+RowRangeSize <= Rows) && |
Jamie Smith |
1:aac28ffd63ed | 315 | (C+ColRangeSize <= Cols)); |
Jamie Smith |
1:aac28ffd63ed | 316 | TMatrix<RowRangeSize, ColRangeSize, value_type> result; |
Jamie Smith |
1:aac28ffd63ed | 317 | for (uint32_t i = 0; i < RowRangeSize; i++) { |
Jamie Smith |
1:aac28ffd63ed | 318 | for (uint32_t j = 0; j < ColRangeSize; j++) { |
Jamie Smith |
1:aac28ffd63ed | 319 | result.element(i,j, element(i+R, j+C)); |
Jamie Smith |
1:aac28ffd63ed | 320 | } |
Jamie Smith |
1:aac28ffd63ed | 321 | } |
Jamie Smith |
1:aac28ffd63ed | 322 | return result; |
Jamie Smith |
1:aac28ffd63ed | 323 | } |
Jamie Smith |
1:aac28ffd63ed | 324 | |
Jamie Smith |
1:aac28ffd63ed | 325 | |
Jamie Smith |
1:aac28ffd63ed | 326 | template <uint16_t R, uint16_t C, uint16_t RowRangeSize, uint16_t ColRangeSize> |
Jamie Smith |
1:aac28ffd63ed | 327 | void subMatrix(const TMatrix<RowRangeSize, ColRangeSize, value_type>& m) { |
Jamie Smith |
1:aac28ffd63ed | 328 | STATIC_ASSERT((R+RowRangeSize <= Rows) && |
Jamie Smith |
1:aac28ffd63ed | 329 | (C+ColRangeSize <= Cols)); |
Jamie Smith |
1:aac28ffd63ed | 330 | for (uint32_t i = 0; i < RowRangeSize; i++) { |
Jamie Smith |
1:aac28ffd63ed | 331 | for (uint32_t j = 0; j < ColRangeSize; j++) { |
Jamie Smith |
1:aac28ffd63ed | 332 | element(i+R,j+C, m.element(i, j)); |
Jamie Smith |
1:aac28ffd63ed | 333 | } |
Jamie Smith |
1:aac28ffd63ed | 334 | } |
Jamie Smith |
1:aac28ffd63ed | 335 | } |
Jamie Smith |
1:aac28ffd63ed | 336 | |
Jamie Smith |
1:aac28ffd63ed | 337 | /** |
Jamie Smith |
1:aac28ffd63ed | 338 | * @brief Matrix multiplication operator. |
Jamie Smith |
1:aac28ffd63ed | 339 | * @return A matrix where result is the matrix product |
Jamie Smith |
1:aac28ffd63ed | 340 | * (*this) * rhs. |
Jamie Smith |
1:aac28ffd63ed | 341 | */ |
Jamie Smith |
1:aac28ffd63ed | 342 | template <uint16_t RhsCols> |
Jamie Smith |
1:aac28ffd63ed | 343 | TMatrix<Rows, RhsCols, value_type> operator*(const TMatrix<Cols, RhsCols, value_type>& rhs) const { |
Jamie Smith |
1:aac28ffd63ed | 344 | |
Jamie Smith |
1:aac28ffd63ed | 345 | TMatrix<Rows, RhsCols, value_type> result; |
Jamie Smith |
1:aac28ffd63ed | 346 | const value_type* rPtr = rhs.row(0); |
Jamie Smith |
1:aac28ffd63ed | 347 | for (uint32_t i = 0; i < Rows; i++) |
Jamie Smith |
1:aac28ffd63ed | 348 | { |
Jamie Smith |
1:aac28ffd63ed | 349 | const value_type* rL = row(i); |
Jamie Smith |
1:aac28ffd63ed | 350 | const value_type* cR = rPtr; |
Jamie Smith |
1:aac28ffd63ed | 351 | value_type* resultRow = result.row(i); |
Jamie Smith |
1:aac28ffd63ed | 352 | for (uint32_t j = 0; j < RhsCols; j++) |
Jamie Smith |
1:aac28ffd63ed | 353 | { |
Jamie Smith |
1:aac28ffd63ed | 354 | const value_type* rR = cR; // start at first element of right col |
Jamie Smith |
1:aac28ffd63ed | 355 | const value_type* cL = rL; // start at first element of left row |
Jamie Smith |
1:aac28ffd63ed | 356 | double r = 0; |
Jamie Smith |
1:aac28ffd63ed | 357 | for (uint32_t k = 0; k < Cols; k++) |
Jamie Smith |
1:aac28ffd63ed | 358 | { |
Jamie Smith |
1:aac28ffd63ed | 359 | r += (*cL)*(*rR); |
Jamie Smith |
1:aac28ffd63ed | 360 | cL++; // step to next col of left matrix |
Jamie Smith |
1:aac28ffd63ed | 361 | rR += Cols; // step to next row of right matrix |
Jamie Smith |
1:aac28ffd63ed | 362 | } |
Jamie Smith |
1:aac28ffd63ed | 363 | resultRow[j] = r; |
Jamie Smith |
1:aac28ffd63ed | 364 | cR++; // step to next column of right matrix |
Jamie Smith |
1:aac28ffd63ed | 365 | } |
Jamie Smith |
1:aac28ffd63ed | 366 | } |
Jamie Smith |
1:aac28ffd63ed | 367 | return result; |
Jamie Smith |
1:aac28ffd63ed | 368 | } |
Jamie Smith |
1:aac28ffd63ed | 369 | |
Jamie Smith |
1:aac28ffd63ed | 370 | /** |
Jamie Smith |
1:aac28ffd63ed | 371 | * @brief Element-wise addition operator. |
Jamie Smith |
1:aac28ffd63ed | 372 | * @return A matrix where result(i,j) = (*this)(i,j) + rhs(i,j). |
Jamie Smith |
1:aac28ffd63ed | 373 | */ |
Jamie Smith |
1:aac28ffd63ed | 374 | TMatrix<Rows, Cols, value_type> operator+(const TMatrix<Rows, Cols, value_type>& rhs) const { |
Jamie Smith |
1:aac28ffd63ed | 375 | TMatrixDummy dummy; |
Jamie Smith |
1:aac28ffd63ed | 376 | TMatrix<Rows, Cols, value_type> result(dummy); |
Jamie Smith |
1:aac28ffd63ed | 377 | for (uint32_t i = 0; i < Rows*Cols; i++) { |
Jamie Smith |
1:aac28ffd63ed | 378 | result.mData[i] = mData[i] + rhs.mData[i]; |
Jamie Smith |
1:aac28ffd63ed | 379 | } |
Jamie Smith |
1:aac28ffd63ed | 380 | return result; |
Jamie Smith |
1:aac28ffd63ed | 381 | } |
Jamie Smith |
1:aac28ffd63ed | 382 | |
Jamie Smith |
1:aac28ffd63ed | 383 | /** |
Jamie Smith |
1:aac28ffd63ed | 384 | * @brief Element-wise subtraction operator. |
Jamie Smith |
1:aac28ffd63ed | 385 | * @return A matrix where result(i,j) = (*this)(i,j) - rhs(i,j). |
Jamie Smith |
1:aac28ffd63ed | 386 | */ |
Jamie Smith |
1:aac28ffd63ed | 387 | TMatrix<Rows, Cols, value_type> operator-(const TMatrix<Rows, Cols, value_type>& rhs) const { |
Jamie Smith |
1:aac28ffd63ed | 388 | TMatrixDummy dummy; |
Jamie Smith |
1:aac28ffd63ed | 389 | TMatrix<Rows, Cols, value_type> result(dummy); |
Jamie Smith |
1:aac28ffd63ed | 390 | for (uint32_t i = 0; i < Rows*Cols; i++) { |
Jamie Smith |
1:aac28ffd63ed | 391 | result.mData[i] = mData[i] - rhs.mData[i]; |
Jamie Smith |
1:aac28ffd63ed | 392 | } |
Jamie Smith |
1:aac28ffd63ed | 393 | return result; |
Jamie Smith |
1:aac28ffd63ed | 394 | } |
Jamie Smith |
1:aac28ffd63ed | 395 | |
Jamie Smith |
1:aac28ffd63ed | 396 | /** |
Jamie Smith |
1:aac28ffd63ed | 397 | * @brief Scalar multiplication operator. |
Jamie Smith |
1:aac28ffd63ed | 398 | * @return A matrix where result(i,j) = (*this)(i,j) * s. |
Jamie Smith |
1:aac28ffd63ed | 399 | */ |
Jamie Smith |
1:aac28ffd63ed | 400 | TMatrix<Rows, Cols, value_type> operator*(value_type s) const { |
Jamie Smith |
1:aac28ffd63ed | 401 | TMatrixDummy dummy; |
Jamie Smith |
1:aac28ffd63ed | 402 | TMatrix<Rows, Cols, value_type> result(dummy); |
Jamie Smith |
1:aac28ffd63ed | 403 | for (uint32_t i = 0; i < Rows*Cols; i++) { |
Jamie Smith |
1:aac28ffd63ed | 404 | result.mData[i] = mData[i] * s; |
Jamie Smith |
1:aac28ffd63ed | 405 | } |
Jamie Smith |
1:aac28ffd63ed | 406 | return result; |
Jamie Smith |
1:aac28ffd63ed | 407 | } |
Jamie Smith |
1:aac28ffd63ed | 408 | |
Jamie Smith |
1:aac28ffd63ed | 409 | /** |
Jamie Smith |
1:aac28ffd63ed | 410 | * @brief Scalar division operator. |
Jamie Smith |
1:aac28ffd63ed | 411 | * @return A matrix where result(i,j) = (*this)(i,j) / s. |
Jamie Smith |
1:aac28ffd63ed | 412 | */ |
Jamie Smith |
1:aac28ffd63ed | 413 | TMatrix<Rows, Cols, value_type> operator/(value_type s) const { |
Jamie Smith |
1:aac28ffd63ed | 414 | TMatrixDummy dummy; |
Jamie Smith |
1:aac28ffd63ed | 415 | TMatrix<Rows, Cols, value_type> result(dummy); |
Jamie Smith |
1:aac28ffd63ed | 416 | for (uint32_t i = 0; i < Rows*Cols; i++) { |
Jamie Smith |
1:aac28ffd63ed | 417 | result.mData[i] = mData[i] / s; |
Jamie Smith |
1:aac28ffd63ed | 418 | } |
Jamie Smith |
1:aac28ffd63ed | 419 | return result; |
Jamie Smith |
1:aac28ffd63ed | 420 | } |
Jamie Smith |
1:aac28ffd63ed | 421 | |
Jamie Smith |
1:aac28ffd63ed | 422 | /** |
Jamie Smith |
1:aac28ffd63ed | 423 | * @brief Unary negation operator. |
Jamie Smith |
1:aac28ffd63ed | 424 | * @return A matrix where result(i,j) = -(*this)(i,j). |
Jamie Smith |
1:aac28ffd63ed | 425 | */ |
Jamie Smith |
1:aac28ffd63ed | 426 | TMatrix<Rows, Cols, value_type> operator-(void) const { |
Jamie Smith |
1:aac28ffd63ed | 427 | TMatrixDummy dummy; |
Jamie Smith |
1:aac28ffd63ed | 428 | TMatrix<Rows, Cols, value_type> result(dummy); |
Jamie Smith |
1:aac28ffd63ed | 429 | for (uint32_t i = 0; i < Rows*Cols; i++) { |
Jamie Smith |
1:aac28ffd63ed | 430 | result.mData[i] = -mData[i]; |
Jamie Smith |
1:aac28ffd63ed | 431 | } |
Jamie Smith |
1:aac28ffd63ed | 432 | return result; |
Jamie Smith |
1:aac28ffd63ed | 433 | } |
Jamie Smith |
1:aac28ffd63ed | 434 | |
Jamie Smith |
1:aac28ffd63ed | 435 | /** |
Jamie Smith |
1:aac28ffd63ed | 436 | * @brief Returns the matrix transpose of this matrix. |
Jamie Smith |
1:aac28ffd63ed | 437 | * |
Jamie Smith |
1:aac28ffd63ed | 438 | * @return A TMatrix of dimension @p Cols by @p Rows where |
Jamie Smith |
1:aac28ffd63ed | 439 | * result(i,j) = (*this)(j,i) for each element. |
Jamie Smith |
1:aac28ffd63ed | 440 | */ |
Jamie Smith |
1:aac28ffd63ed | 441 | TMatrix<Cols, Rows, value_type> transpose(void) const { |
Jamie Smith |
1:aac28ffd63ed | 442 | TMatrixDummy dummy; |
Jamie Smith |
1:aac28ffd63ed | 443 | TMatrix<Cols, Rows, value_type> result(dummy); |
Jamie Smith |
1:aac28ffd63ed | 444 | for (uint16_t i = 0; i < Rows; i++) { |
Jamie Smith |
1:aac28ffd63ed | 445 | for (uint16_t j = 0; j < Cols; j++) { |
Jamie Smith |
1:aac28ffd63ed | 446 | result.element(j,i, element(i,j)); |
Jamie Smith |
1:aac28ffd63ed | 447 | } |
Jamie Smith |
1:aac28ffd63ed | 448 | } |
Jamie Smith |
1:aac28ffd63ed | 449 | return result; |
Jamie Smith |
1:aac28ffd63ed | 450 | } |
Jamie Smith |
1:aac28ffd63ed | 451 | |
Jamie Smith |
1:aac28ffd63ed | 452 | /** |
Jamie Smith |
1:aac28ffd63ed | 453 | * @brief Returns the diagonal elements of the matrix. |
Jamie Smith |
1:aac28ffd63ed | 454 | * |
Jamie Smith |
1:aac28ffd63ed | 455 | * @return A column vector @p v with dimension MIN(Rows,Cols) where |
Jamie Smith |
1:aac28ffd63ed | 456 | * @p v[i] = (*this)[i][i]. |
Jamie Smith |
1:aac28ffd63ed | 457 | */ |
Jamie Smith |
1:aac28ffd63ed | 458 | TMatrix<(Rows>Cols)?Cols:Rows, 1, value_type> diag() const { |
Jamie Smith |
1:aac28ffd63ed | 459 | TMatrixDummy dummy; |
Jamie Smith |
1:aac28ffd63ed | 460 | TMatrix<(Rows>Cols)?Cols:Rows, 1> d(dummy); |
Jamie Smith |
1:aac28ffd63ed | 461 | for (uint32_t i = 0; i < d.rows(); i++) { |
Jamie Smith |
1:aac28ffd63ed | 462 | d[i] = mData[i*(Cols + 1)]; |
Jamie Smith |
1:aac28ffd63ed | 463 | } |
Jamie Smith |
1:aac28ffd63ed | 464 | return d; |
Jamie Smith |
1:aac28ffd63ed | 465 | } |
Jamie Smith |
1:aac28ffd63ed | 466 | |
Jamie Smith |
1:aac28ffd63ed | 467 | /** @brief Returns the sum of the matrix entries. */ |
Jamie Smith |
1:aac28ffd63ed | 468 | value_type sum(void) const { |
Jamie Smith |
1:aac28ffd63ed | 469 | value_type s = 0; |
Jamie Smith |
1:aac28ffd63ed | 470 | for (uint32_t i = 0; i < Rows*Cols; i++) { s += mData[i]; } |
Jamie Smith |
1:aac28ffd63ed | 471 | return s; |
Jamie Smith |
1:aac28ffd63ed | 472 | } |
Jamie Smith |
1:aac28ffd63ed | 473 | /** @brief Returns the sum of the log of the matrix entries. |
Jamie Smith |
1:aac28ffd63ed | 474 | */ |
Jamie Smith |
1:aac28ffd63ed | 475 | value_type sumLog(void) const { |
Jamie Smith |
1:aac28ffd63ed | 476 | value_type s = 0; |
Jamie Smith |
1:aac28ffd63ed | 477 | for (uint32_t i = 0; i < Rows*Cols; i++) { s += log(mData[i]); } |
Jamie Smith |
1:aac28ffd63ed | 478 | return s; |
Jamie Smith |
1:aac28ffd63ed | 479 | } |
Jamie Smith |
1:aac28ffd63ed | 480 | |
Jamie Smith |
1:aac28ffd63ed | 481 | /** @brief Returns this vector with its elements replaced by their reciprocals. */ |
Jamie Smith |
1:aac28ffd63ed | 482 | TMatrix<Rows,Cols, value_type> recip(void) const { |
Jamie Smith |
1:aac28ffd63ed | 483 | TMatrixDummy dummy; |
Jamie Smith |
1:aac28ffd63ed | 484 | TMatrix<Rows,Cols, value_type> result(dummy); |
Jamie Smith |
1:aac28ffd63ed | 485 | for (uint32_t i = 0; i < Rows*Cols; i++) { |
Jamie Smith |
1:aac28ffd63ed | 486 | result.mData[i] = 1.0/mData[i]; |
Jamie Smith |
1:aac28ffd63ed | 487 | } |
Jamie Smith |
1:aac28ffd63ed | 488 | return result; |
Jamie Smith |
1:aac28ffd63ed | 489 | } |
Jamie Smith |
1:aac28ffd63ed | 490 | |
Jamie Smith |
1:aac28ffd63ed | 491 | /** @brief Returns this vector with its elements replaced by their reciprocals, |
Jamie Smith |
1:aac28ffd63ed | 492 | * unless a value is less than epsilon, in which case it is left as zero. |
Jamie Smith |
1:aac28ffd63ed | 493 | * |
Jamie Smith |
1:aac28ffd63ed | 494 | * This is used mostly for pseudo-inverse computations. |
Jamie Smith |
1:aac28ffd63ed | 495 | */ |
Jamie Smith |
1:aac28ffd63ed | 496 | TMatrix<Rows,Cols, value_type> pseudoRecip(double epsilon = 1e-50) const { |
Jamie Smith |
1:aac28ffd63ed | 497 | TMatrixDummy dummy; |
Jamie Smith |
1:aac28ffd63ed | 498 | TMatrix<Rows,Cols, value_type> result(dummy); |
Jamie Smith |
1:aac28ffd63ed | 499 | for (uint32_t i = 0; i < Rows*Cols; i++) { |
Jamie Smith |
1:aac28ffd63ed | 500 | if (fabs(mData[i]) >= epsilon) { |
Jamie Smith |
1:aac28ffd63ed | 501 | result.mData[i] = 1.0/mData[i]; |
Jamie Smith |
1:aac28ffd63ed | 502 | } else { |
Jamie Smith |
1:aac28ffd63ed | 503 | result.mData[i] = 0; |
Jamie Smith |
1:aac28ffd63ed | 504 | } |
Jamie Smith |
1:aac28ffd63ed | 505 | } |
Jamie Smith |
1:aac28ffd63ed | 506 | return result; |
Jamie Smith |
1:aac28ffd63ed | 507 | } |
Jamie Smith |
1:aac28ffd63ed | 508 | |
Jamie Smith |
1:aac28ffd63ed | 509 | /** |
Jamie Smith |
1:aac28ffd63ed | 510 | * @brief Returns an "identity" matrix with dimensions given by the |
Jamie Smith |
1:aac28ffd63ed | 511 | * class's template parameters. |
Jamie Smith |
1:aac28ffd63ed | 512 | * |
Jamie Smith |
1:aac28ffd63ed | 513 | * In the case that @p Rows != @p Cols, this matrix is simply the |
Jamie Smith |
1:aac28ffd63ed | 514 | * one where the Aii elements for i < min(Rows, Cols) are 1, and all |
Jamie Smith |
1:aac28ffd63ed | 515 | * other elements are 0. |
Jamie Smith |
1:aac28ffd63ed | 516 | * |
Jamie Smith |
1:aac28ffd63ed | 517 | * @return A TMatrix<Rows, Cols, value_type> with off-diagonal |
Jamie Smith |
1:aac28ffd63ed | 518 | * elements set to 0, and diagonal elements set to 1. |
Jamie Smith |
1:aac28ffd63ed | 519 | */ |
Jamie Smith |
1:aac28ffd63ed | 520 | static TMatrix<Rows, Cols, value_type> identity() { |
Jamie Smith |
1:aac28ffd63ed | 521 | TMatrix<Rows, Cols, value_type> id; |
Jamie Smith |
1:aac28ffd63ed | 522 | for (uint16_t i = 0; i < Rows && i < Cols; i++) { |
Jamie Smith |
1:aac28ffd63ed | 523 | id.element(i,i) = 1; |
Jamie Smith |
1:aac28ffd63ed | 524 | } |
Jamie Smith |
1:aac28ffd63ed | 525 | return id; |
Jamie Smith |
1:aac28ffd63ed | 526 | } |
Jamie Smith |
1:aac28ffd63ed | 527 | |
Jamie Smith |
1:aac28ffd63ed | 528 | /** |
Jamie Smith |
1:aac28ffd63ed | 529 | * @brief Returns a ones matrix with dimensions given by the |
Jamie Smith |
1:aac28ffd63ed | 530 | * class's template parameters. |
Jamie Smith |
1:aac28ffd63ed | 531 | * |
Jamie Smith |
1:aac28ffd63ed | 532 | * @return A TMatrix<Rows, Cols, value_type> with all elements set |
Jamie Smith |
1:aac28ffd63ed | 533 | * to 1. |
Jamie Smith |
1:aac28ffd63ed | 534 | */ |
Jamie Smith |
1:aac28ffd63ed | 535 | static TMatrix<Rows, Cols, value_type> one() { |
Jamie Smith |
1:aac28ffd63ed | 536 | TMatrix<Rows, Cols, value_type> ones; |
Jamie Smith |
1:aac28ffd63ed | 537 | for (uint16_t i = 0; i < Rows; i++) { |
Jamie Smith |
1:aac28ffd63ed | 538 | for (uint16_t j = 0; j < Cols; j++) { |
Jamie Smith |
1:aac28ffd63ed | 539 | ones.element(i,j, 1); |
Jamie Smith |
1:aac28ffd63ed | 540 | } |
Jamie Smith |
1:aac28ffd63ed | 541 | } |
Jamie Smith |
1:aac28ffd63ed | 542 | return ones; |
Jamie Smith |
1:aac28ffd63ed | 543 | } |
Jamie Smith |
1:aac28ffd63ed | 544 | |
Jamie Smith |
1:aac28ffd63ed | 545 | /** |
Jamie Smith |
1:aac28ffd63ed | 546 | * @brief Returns a zero matrix with dimensions given by the |
Jamie Smith |
1:aac28ffd63ed | 547 | * class's template parameters. |
Jamie Smith |
1:aac28ffd63ed | 548 | * |
Jamie Smith |
1:aac28ffd63ed | 549 | * @return A TMatrix<Rows, Cols, value_type> containing all 0. |
Jamie Smith |
1:aac28ffd63ed | 550 | */ |
Jamie Smith |
1:aac28ffd63ed | 551 | static TMatrix<Rows, Cols, value_type> zero() { |
Jamie Smith |
1:aac28ffd63ed | 552 | return TMatrix<Rows, Cols, value_type>(); |
Jamie Smith |
1:aac28ffd63ed | 553 | } |
Jamie Smith |
1:aac28ffd63ed | 554 | |
Jamie Smith |
1:aac28ffd63ed | 555 | value_type* row(uint32_t i) { return &mData[i*Cols]; } |
Jamie Smith |
1:aac28ffd63ed | 556 | const value_type* row(uint32_t i) const { return &mData[i*Cols]; } |
Jamie Smith |
1:aac28ffd63ed | 557 | |
Jamie Smith |
1:aac28ffd63ed | 558 | /** |
Jamie Smith |
1:aac28ffd63ed | 559 | * @brief Checks to see if any of this matrix's elements are NaN. |
Jamie Smith |
1:aac28ffd63ed | 560 | */ |
Jamie Smith |
1:aac28ffd63ed | 561 | bool hasNaN(void) const { |
Jamie Smith |
1:aac28ffd63ed | 562 | for (uint32_t i = 0; i < Rows*Cols; i++) { |
Jamie Smith |
1:aac28ffd63ed | 563 | if (isnan(mData[i])) { |
Jamie Smith |
1:aac28ffd63ed | 564 | return true; |
Jamie Smith |
1:aac28ffd63ed | 565 | } |
Jamie Smith |
1:aac28ffd63ed | 566 | } |
Jamie Smith |
1:aac28ffd63ed | 567 | return false; |
Jamie Smith |
1:aac28ffd63ed | 568 | } |
Jamie Smith |
1:aac28ffd63ed | 569 | |
Jamie Smith |
1:aac28ffd63ed | 570 | void print(Stream & os, bool oneLine = false) const { |
Jamie Smith |
1:aac28ffd63ed | 571 | for (uint16_t i = 0; i < Rows; i++) { |
Jamie Smith |
1:aac28ffd63ed | 572 | for (uint16_t j = 0; j < Cols; j++) { |
Jamie Smith |
1:aac28ffd63ed | 573 | os.printf("%.06f ", element(i, j)); |
Jamie Smith |
1:aac28ffd63ed | 574 | } |
Jamie Smith |
1:aac28ffd63ed | 575 | |
Jamie Smith |
1:aac28ffd63ed | 576 | if(!oneLine) |
Jamie Smith |
1:aac28ffd63ed | 577 | { |
Jamie Smith |
1:aac28ffd63ed | 578 | os.printf("\n"); |
Jamie Smith |
1:aac28ffd63ed | 579 | } |
Jamie Smith |
1:aac28ffd63ed | 580 | } |
Jamie Smith |
1:aac28ffd63ed | 581 | } |
Jamie Smith |
1:aac28ffd63ed | 582 | |
Jamie Smith |
1:aac28ffd63ed | 583 | private: |
Jamie Smith |
1:aac28ffd63ed | 584 | |
Jamie Smith |
1:aac28ffd63ed | 585 | template <uint16_t Rows2, uint16_t Cols2, typename value_type2> |
Jamie Smith |
1:aac28ffd63ed | 586 | friend TMatrix<Rows2,Cols2,value_type2> operator*(double s, const TMatrix<Rows2, Cols2, value_type2>& m); |
Jamie Smith |
1:aac28ffd63ed | 587 | }; |
Jamie Smith |
1:aac28ffd63ed | 588 | |
Jamie Smith |
1:aac28ffd63ed | 589 | typedef TMatrix<2,2, float> TMatrix2; |
Jamie Smith |
1:aac28ffd63ed | 590 | typedef TMatrix<3,3, float> TMatrix3; |
Jamie Smith |
1:aac28ffd63ed | 591 | typedef TMatrix<4,4, float> TMatrix4; |
Jamie Smith |
1:aac28ffd63ed | 592 | typedef TMatrix<2,1, float> TVector2; |
Jamie Smith |
1:aac28ffd63ed | 593 | typedef TMatrix<3,1, float> TVector3; |
Jamie Smith |
1:aac28ffd63ed | 594 | typedef TMatrix<4,1, float> TVector4; |
Jamie Smith |
1:aac28ffd63ed | 595 | |
Jamie Smith |
1:aac28ffd63ed | 596 | // left-side scalar multiply |
Jamie Smith |
1:aac28ffd63ed | 597 | template <uint16_t Rows, uint16_t Cols, typename value_type> |
Jamie Smith |
1:aac28ffd63ed | 598 | TMatrix<Rows,Cols,value_type> operator*(double s, const TMatrix<Rows, Cols, value_type>& m) { |
Jamie Smith |
1:aac28ffd63ed | 599 | return m * s; |
Jamie Smith |
1:aac28ffd63ed | 600 | } |
Jamie Smith |
1:aac28ffd63ed | 601 | |
Jamie Smith |
1:aac28ffd63ed | 602 | |
Jamie Smith |
1:aac28ffd63ed | 603 | #endif /* TMATRIX_H */ |