The library needs to be tested once we get the IMU
tmatrix.h@1:aac28ffd63ed, 2018-12-29 (annotated)
- Committer:
- Jamie Smith
- Date:
- Sat Dec 29 03:31:00 2018 -0800
- Revision:
- 1:aac28ffd63ed
Update from latest upstream, add missing headers
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 |
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 */ |