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
Committer:
Jamie Smith
Date:
Tue Nov 24 15:06:05 2020 -0800
Revision:
9:430f5302f9e1
Parent:
6:5ba996be5312
Implement BNO080Async

Who changed what in which revision?

UserRevisionLine numberNew 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 */