rbdl / src / SimpleMath / SimpleMathMixed.h

 ``` 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138``` ```/** * This is a highly inefficient math library. It was conceived by Martin * Felis while he was compiling code * that uses a highly efficient math library. * * It is intended to be used as a fast compiling substitute for the * blazingly fast Eigen3 library and tries to mimic its API to a certain * extend. * * Feel free to use it wherever you like. However, no guarantees are given * that this code does what it says it would. */ #ifndef SIMPLEMATHMIXED_H #define SIMPLEMATHMIXED_H #include #include #include #include #include "compileassert.h" /** \brief Namespace for a highly inefficient math library * */ namespace SimpleMath { // conversion Dynamic->Fixed template inline Fixed::Matrix::Matrix(const Dynamic::Matrix &dynamic_matrix) { if (dynamic_matrix.cols() != ncols || dynamic_matrix.rows() != nrows) { std::cerr << "Error: cannot assign a dynamic sized matrix of size " << dynamic_matrix.rows() << "x" << dynamic_matrix.cols() << " to a fixed size matrix of size " << nrows << "x" << ncols << "!" << std::endl; abort(); } for (unsigned int i = 0; i < nrows * ncols; i++) { mData[i] = dynamic_matrix[i]; } } template inline Fixed::Matrix& Fixed::Matrix::operator=(const Dynamic::Matrix &dynamic_matrix) { if (dynamic_matrix.cols() != ncols || dynamic_matrix.rows() != nrows) { std::cerr << "Error: cannot assign a dynamic sized matrix of size " << dynamic_matrix.rows() << "x" << dynamic_matrix.cols() << " to a fixed size matrix of size " << nrows << "x" << ncols << "!" << std::endl; abort(); } for (unsigned int i = 0; i < nrows * ncols; i++) { mData[i] = dynamic_matrix[i]; } return *this; } // multiplication template inline Dynamic::Matrix operator*( const Fixed::Matrix &matrix_a, const Dynamic::Matrix &matrix_b) { assert (matrix_a.cols() == matrix_b.rows()); Dynamic::Matrix result (nrows, matrix_b.cols()); result.setZero(); unsigned int i,j, k; for (i = 0; i < nrows; i++) { for (j = 0; j < matrix_b.cols(); j++) { for (k = 0; k < matrix_b.rows(); k++) { result(i,j) += matrix_a(i,k) * matrix_b(k,j); } } } return result; } template inline Dynamic::Matrix operator*( const Dynamic::Matrix &matrix_a, const Fixed::Matrix &matrix_b) { assert (matrix_a.cols() == matrix_b.rows()); Dynamic::Matrix result (matrix_a.rows(), ncols); result.setZero(); unsigned int i,j, k; for (i = 0; i < matrix_a.rows(); i++) { for (j = 0; j < matrix_b.cols(); j++) { for (k = 0; k < matrix_b.rows(); k++) { result(i,j) += matrix_a(i,k) * matrix_b(k,j); } } } return result; } // equality template inline bool operator==( const Fixed::Matrix &matrix_a, const Dynamic::Matrix &matrix_b) { assert (nrows == matrix_a.rows()); assert (ncols == matrix_a.cols()); unsigned int i; for (i = 0; i < matrix_a.size(); i++) { if (matrix_a[i] != matrix_b[i]) return false; } return true; } template inline bool operator==( const Dynamic::Matrix &matrix_b, const Fixed::Matrix &matrix_a) { assert (nrows == matrix_a.rows()); assert (ncols == matrix_a.cols()); unsigned int i; for (i = 0; i < matrix_a.size(); i++) { if (matrix_a[i] != matrix_b[i]) return false; } return true; } } #endif /* SIMPLEMATHMIXED_H */ ```