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:
Thu Jan 30 02:16:15 2020 -0800
Revision:
4:70d05578f041
Parent:
1:aac28ffd63ed
Child:
6:5ba996be5312
Fix a couple more minor issues

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