22 template <
size_t dimcount,
typename T>
37 std::size_t
num_rows(std::vector<T>
const& in) {
41 std::size_t
num_rows(std::vector<std::vector<T>>
const& in) {
46 std::size_t
max_cols(std::vector<std::vector<T>>
const& in) {
49 for (std::size_t i = 0; i < in.size(); i++) {
63 for (std::size_t i = 0; i < in.size(); i++) {
64 if (cols != in[i].size()) {
73 std::size_t
num_cols(std::vector<T>
const& in) {
77 std::size_t
num_cols(std::vector<std::vector<T>>
const& in) {
97 std::vector<T>
eigen_to_vec1D(
const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& coefficients,
int axis = 0) {
98 std::vector<T> result;
99 size_t r = coefficients.rows(), c = coefficients.cols();
101 if (c != 1)
throw ValueError(
format(
"Your matrix has the wrong dimensions: %d,%d", r, c));
103 for (
size_t i = 0; i < r; ++i) {
104 result[i] = coefficients(i, 0);
106 }
else if (axis == 1) {
107 if (r != 1)
throw ValueError(
format(
"Your matrix has the wrong dimensions: %d,%d", r, c));
109 for (
size_t i = 0; i < c; ++i) {
110 result[i] = coefficients(0, i);
113 throw ValueError(
format(
"You have to provide axis information: %d is not valid. ", axis));
119 std::vector<std::vector<T>>
eigen_to_vec(
const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& coefficients) {
122 std::vector<std::vector<T>> result;
123 size_t r = coefficients.rows(), c = coefficients.cols();
124 result.resize(r, std::vector<T>(c, 0));
125 for (
size_t i = 0; i < r; ++i) {
126 result[i].resize(c, 0);
127 for (
size_t j = 0; j < c; ++j) {
128 result[i][j] = coefficients(i, j);
136 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
vec_to_eigen(
const std::vector<std::vector<T>>& coefficients) {
138 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> result(nRows, nCols);
139 for (
size_t i = 0; i < nCols; ++i) {
140 for (
size_t j = 0; j < nRows; ++j) {
141 result(j, i) = coefficients[j][i];
151 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
vec_to_eigen(
const std::vector<T>& coefficients,
int axis = 0) {
152 size_t nRows =
num_rows(coefficients);
153 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> result;
155 result.resize(nRows, 1);
157 result.resize(1, nRows);
159 throw ValueError(
format(
"You have to provide axis information: %d is not valid. ", axis));
160 for (
size_t i = 0; i < nRows; ++i) {
161 if (axis == 0) result(i, 0) = coefficients[i];
162 if (axis == 1) result(0, i) = coefficients[i];
168 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>
vec_to_eigen(
const T& coefficient) {
169 Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic> result = Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>(1, 1);
170 result(0, 0) = coefficient;
181 Eigen::Matrix<T, Eigen::Dynamic, 1>
makeColVector(
const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& matrix) {
182 std::size_t r = matrix.rows();
183 std::size_t c = matrix.cols();
184 Eigen::Matrix<T, Eigen::Dynamic, 1> vector;
185 if (r == 1 && c >= 1) {
186 vector = matrix.transpose().block(0, 0, c, r);
187 }
else if (r >= 1 && c == 1) {
188 vector = matrix.block(0, 0, r, c);
190 throw ValueError(
format(
"Your matrix (%d,%d) cannot be converted into a vector (x,1).", r, c));
195 Eigen::Matrix<T, Eigen::Dynamic, 1>
makeVector(
const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& matrix) {
199 Eigen::Matrix<T, 1, Eigen::Dynamic>
makeRowVector(
const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& matrix) {
200 std::size_t r = matrix.rows();
201 std::size_t c = matrix.cols();
202 Eigen::Matrix<T, 1, Eigen::Dynamic> vector;
203 if (r == 1 && c >= 1) {
204 vector = matrix.block(0, 0, r, c);
205 }
else if (r >= 1 && c == 1) {
206 vector = matrix.transpose().block(0, 0, c, r);
208 throw ValueError(
format(
"Your matrix (%d,%d) cannot be converted into a vector (1,x).", r, c));
218 void removeRow(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& matrix, std::size_t rowToRemove) {
222 std::size_t numRows = matrix.rows() - 1;
223 std::size_t numCols = matrix.cols();
224 if (rowToRemove < numRows) {
225 matrix.block(rowToRemove, 0, numRows - rowToRemove, numCols) = matrix.block(rowToRemove + 1, 0, numRows - rowToRemove, numCols);
227 if (rowToRemove > numRows) {
228 throw ValueError(
format(
"Your matrix does not have enough rows, %d is not greater or equal to %d.", numRows, rowToRemove));
232 matrix.conservativeResize(numRows, numCols);
236 void removeColumn(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& matrix, std::size_t colToRemove) {
240 std::size_t numRows = matrix.rows();
241 std::size_t numCols = matrix.cols() - 1;
242 if (colToRemove < numCols) {
243 matrix.block(0, colToRemove, numRows, numCols - colToRemove) = matrix.block(0, colToRemove + 1, numRows, numCols - colToRemove);
245 if (colToRemove > numCols) {
246 throw ValueError(
format(
"Your matrix does not have enough columns, %d is not greater or equal to %d.", numCols, colToRemove));
250 matrix.conservativeResize(numRows, numCols);
315 static const char* stdFmt =
"%8.3f";
320 if (a.size() < 1)
return std::string(
"");
321 std::stringstream out;
322 out <<
"[ " <<
format(fmt, a[0]);
323 for (
size_t j = 1; j < a.size(); j++) {
324 out <<
", " <<
format(fmt, a[j]);
331 return vec_to_string(std::vector<double>(a.begin(), a.end()), stdFmt);
335 if (a.size() < 1)
return std::string(
"");
336 std::stringstream out;
337 out <<
"[ " <<
format(
"%s", a[0].c_str());
338 for (
size_t j = 1; j < a.size(); j++) {
339 out <<
", " <<
format(
"%s", a[j].c_str());
359 std::string
vec_to_string(
const std::vector<std::vector<T>>& A,
const char* fmt) {
360 if (A.size() < 1)
return std::string(
"");
361 std::stringstream out;
363 for (
size_t j = 1; j < A.size(); j++) {
376 std::string
mat_to_string(
const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A,
const char* fmt) {
378 std::size_t r = A.rows();
379 std::size_t c = A.cols();
380 if ((r < 1) || (c < 1))
return std::string(
"");
381 std::stringstream out;
384 out <<
format(fmt, A(0, 0));
385 for (
size_t j = 1; j < c; j++) {
386 out <<
", " <<
format(fmt, A(0, j));
389 out << mat_to_string(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>(A.row(0)), fmt);
390 for (
size_t i = 1; i < r; i++) {
391 out <<
", " << std::endl <<
" " <<
mat_to_string(Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>(A.row(i)), fmt);
398 std::string
mat_to_string(
const Eigen::Matrix<T, Eigen::Dynamic, Eigen::Dynamic>& A) {
558 template <
typename T>
559 void swap_rows(std::vector<std::vector<T>>* A,
size_t row1,
size_t row2) {
560 for (
size_t col = 0; col < (*A)[0].size(); col++) {
561 std::swap((*A)[row1][col], (*A)[row2][col]);
564 template <
typename T>
566 for (
size_t col = 0; col < (*A)[0].size(); col++) {
567 (*A)[row][col] -= multiple * (*A)[pivot_row][col];
570 template <
typename T>
572 for (
size_t col = 0; col < (*A)[0].size(); col++) {
573 (*A)[row][col] /= value;
577 template <
typename T>
579 std::size_t index = col;
582 for (
size_t row = col; row < (*A).size(); row++) {
583 val = (*A)[row][col];
584 if (std::abs(val) > max) {
592 template <
typename T>
593 std::vector<std::vector<T>>
linsolve_Gauss_Jordan(std::vector<std::vector<T>>
const& A, std::vector<std::vector<T>>
const& B) {
594 std::vector<std::vector<T>> AB;
595 std::vector<std::vector<T>>
X;
604 if (NrowA != NrowB)
throw ValueError(
format(
"You have to provide matrices with the same number of rows: %d is not %d. ", NrowA, NrowB));
606 AB.resize(NrowA, std::vector<T>(NcolA + NcolB, 0));
607 X.resize(NrowA, std::vector<T>(NcolB, 0));
610 for (
size_t row = 0; row < NrowA; row++) {
611 for (
size_t col = 0; col < NcolA; col++) {
612 AB[row][col] = A[row][col];
614 for (
size_t col = NcolA; col < NcolA + NcolB; col++) {
615 AB[row][col] = B[row][col - NcolA];
619 for (
size_t col = 0; col < NcolA; col++) {
623 if (std::abs(AB[pivot_row][col]) < 10 *
DBL_EPSILON) {
624 throw ValueError(
format(
"Zero occurred in row %d, the matrix is singular. ", pivot_row));
627 if (pivot_row >= col) {
632 pivot_element = AB[col][col];
636 if (col < NrowA - 1) {
638 for (
size_t row = col + 1; row < NrowA; row++) {
643 for (std::size_t col = NcolA - 1; col > 0; col--) {
644 for (
int row =
static_cast<int>(col) - 1; row >= 0; row--) {
649 for (
size_t row = 0; row < NrowA; row++) {
650 for (
size_t col = 0; col < NcolB; col++) {
651 X[row][col] = AB[row][NcolA + col];
719 std::vector<std::vector<T>>
linsolve(std::vector<std::vector<T>>
const& A, std::vector<std::vector<T>>
const& B) {
724 std::vector<T>
linsolve(std::vector<std::vector<T>>
const& A, std::vector<T>
const& b) {
725 std::vector<std::vector<T>> B;
726 for (
size_t i = 0; i < b.size(); i++) {
727 B.push_back(std::vector<T>(1, b[i]));
730 B[0].resize(B.size(), 0.0);
731 for (
size_t i = 1; i < B.size(); i++) {
738 std::vector<T>
get_row(std::vector<std::vector<T>>
const& in,
size_t row) {
742 std::vector<T>
get_col(std::vector<std::vector<T>>
const& in,
size_t col) {
743 std::size_t sizeX = in.size();
744 if (sizeX < 1)
throw ValueError(
format(
"You have to provide values, a vector length of %d is not valid. ", sizeX));
745 size_t sizeY = in[0].size();
746 if (sizeY < 1)
throw ValueError(
format(
"You have to provide values, a vector length of %d is not valid. ", sizeY));
748 for (std::size_t i = 0; i < sizeX; i++) {
749 sizeY = in[i].size();
751 throw ValueError(
format(
"Your matrix does not have enough entries in row %d, last index %d is less than %d. ", i, sizeY - 1, col));
752 out.push_back(in[i][col]);
758 std::vector<std::vector<T>>
make_squared(std::vector<std::vector<T>>
const& in) {
761 std::size_t maxVal = 0;
762 std::vector<std::vector<T>> out;
771 for (std::size_t i = 0; i < in.size(); i++) {
773 for (std::size_t j = 0; j < in[i].size(); j++) {
774 tmp.push_back(in[i][j]);
776 while (maxVal > tmp.size()) {
783 tmp.resize(maxVal, 0.0);
784 while (maxVal > out.size()) {
791 T
multiply(std::vector<T>
const& a, std::vector<T>
const& b) {
795 std::vector<T>
multiply(std::vector<std::vector<T>>
const& A, std::vector<T>
const& b) {
796 std::vector<std::vector<T>> B;
797 for (
size_t i = 0; i < b.size(); i++) {
798 B.push_back(std::vector<T>(1, b[i]));
801 B[0].resize(B.size(), 0.0);
802 for (
size_t i = 1; i < B.size(); i++) {
809 std::vector<std::vector<T>>
multiply(std::vector<std::vector<T>>
const& A, std::vector<std::vector<T>>
const& B) {
816 std::vector<std::vector<T>> outVec;
817 std::vector<T> tmpVec;
819 for (
size_t i = 0; i < rows; i++) {
821 for (
size_t j = 0; j < cols; j++) {
823 for (
size_t k = 0; k <
num_cols(A); k++) {
824 tmp += A[i][k] * B[k][j];
826 tmpVec.push_back(tmp);
828 outVec.push_back(tmpVec);
835 if (a.size() == b.size()) {
836 return std::inner_product(a.begin(), a.end(), b.begin(), 0.0);
838 throw ValueError(
format(
"You have to provide vectors with the same length: %d is not equal to %d. ", a.size(), b.size()));
842 std::vector<T>
cross_product(std::vector<T>
const& a, std::vector<T>
const& b) {
847 std::vector<std::vector<T>>
transpose(std::vector<std::vector<T>>
const& in) {
848 size_t sizeX = in.size();
849 if (sizeX < 1)
throw ValueError(
format(
"You have to provide values, a vector length of %d is not a valid. ", sizeX));
850 size_t sizeY = in[0].size();
851 size_t sizeYOld = sizeY;
852 if (sizeY < 1)
throw ValueError(
format(
"You have to provide values, a vector length of %d is not a valid. ", sizeY));
853 std::vector<std::vector<T>> out(sizeY, std::vector<T>(sizeX));
854 for (
size_t i = 0; i < sizeX; ++i) {
855 sizeY = in[i].size();
856 if (sizeY != sizeYOld)
throw ValueError(
format(
"You have to provide a rectangular matrix: %d is not equal to %d. ", sizeY, sizeYOld));
857 for (
size_t j = 0; j < sizeY; ++j) {
858 out[j][i] = in[i][j];
865 std::vector<std::vector<T>>
invert(std::vector<std::vector<T>>
const& in) {
867 std::vector<std::vector<T>> identity;
870 identity.resize(dim, std::vector<T>(dim, 0));
871 for (
size_t row = 0; row < dim; row++) {
872 identity[row][row] = 1.0;
877 inline void removeRow(Eigen::MatrixXd& matrix,
unsigned int rowToRemove) {
878 unsigned int numRows =
static_cast<unsigned int>(matrix.rows()) - 1;
879 unsigned int numCols =
static_cast<unsigned int>(matrix.cols());
881 if (rowToRemove <= numRows)
882 matrix.block(rowToRemove, 0, numRows - rowToRemove, numCols) = matrix.block(rowToRemove + 1, 0, numRows - rowToRemove, numCols);
884 throw ValueError(
format(
"Trying to remove row index [%d] greater than max index [%d] ", rowToRemove, numRows));
886 matrix.conservativeResize(numRows, numCols);
889 inline void removeColumn(Eigen::MatrixXd& matrix,
unsigned int colToRemove) {
890 unsigned int numRows =
static_cast<unsigned int>(matrix.rows());
891 unsigned int numCols =
static_cast<unsigned int>(matrix.cols()) - 1;
893 if (colToRemove <= numCols)
894 matrix.block(0, colToRemove, numRows, numCols - colToRemove) = matrix.block(0, colToRemove + 1, numRows, numCols - colToRemove);
896 throw ValueError(
format(
"Trying to remove column index [%d] greater than max index [%d] ", colToRemove, numCols));
898 matrix.conservativeResize(numRows, numCols);
900 template <
typename Derived>
901 inline Eigen::MatrixXd
minor_matrix(
const Eigen::MatrixBase<Derived>& A, std::size_t i, std::size_t j) {
902 Eigen::MatrixXd Am = A;
903 removeRow(Am,
static_cast<unsigned int>(i));
908 template <
typename Derived>
909 static Eigen::MatrixXd adjugate(
const Eigen::MatrixBase<Derived>& A) {
910 std::size_t N = A.rows();
912 Eigen::MatrixXd Aadj(1, 1);
916 Eigen::MatrixXd Aadj(N, N);
917 for (std::size_t i = 0; i < N; ++i) {
918 for (std::size_t j = 0; j < N; ++j) {
919 int negative_1_to_the_i_plus_j = ((i + j) % 2 == 0) ? 1 : -1;
920 Aadj(i, j) = negative_1_to_the_i_plus_j *
minor_matrix(A, j, i).determinant();
926 template <
typename Derived>
927 static Eigen::MatrixXd adjugate_derivative(
const Eigen::MatrixBase<Derived>& A,
const Eigen::MatrixBase<Derived>& dAdt) {
928 std::size_t N = A.rows();
929 Eigen::MatrixXd Aadj(N, N);
930 for (std::size_t i = 0; i < N; ++i) {
931 for (std::size_t j = 0; j < N; ++j) {
932 int negative_1_to_the_i_plus_j = ((i + j) % 2 == 0) ? 1 : -1;