16 template <
typename Type,
size_t M,
size_t N>
19 template <
typename Type,
size_t M>
22 template <
typename Type,
size_t P,
size_t Q,
size_t M,
size_t N>
25 template<
typename Type,
size_t M>
46 template<
size_t P,
size_t Q>
57 template <
size_t P,
size_t Q>
88 for (
size_t i = 0; i < M; i++) {
97 Vector<Type, M * (M + 1) / 2> res;
101 for (
size_t x = 0; x < M; x++) {
102 for (
size_t y = x; y < M; y++) {
103 res(idx) =
self(x, y);
116 for (
size_t i = 0; i < M; i++) {
125 template<
typename Type,
size_t M>
132 template<
typename Type,
size_t M>
135 for (
size_t i=0; i<M; i++) {
141 template<
typename Type,
size_t M>
147 size_t i_factorial = 1;
148 for (
size_t i=1; i<=order; i++) {
150 res += A_pow / Type(i_factorial);
161 template<
typename Type,
size_t M>
173 for (
size_t n = 0; n < M; n++) {
176 if (fabs(static_cast<float>(U(n, n))) <
FLT_EPSILON) {
178 for (
size_t i = n + 1; i < M; i++) {
181 if (fabs(static_cast<float>(U(i, n))) > 1e-8
f) {
201 if (fabs(static_cast<float>(U(n, n))) <
FLT_EPSILON) {
206 for (
size_t i = (n + 1); i < M; i++) {
207 L(i, n) = U(i, n) / U(n, n);
211 for (
size_t k = n; k < M; k++) {
212 U(i, k) -= L(i, n) * U(n, k);
224 for (
size_t c = 0; c < M; c++) {
226 for (
size_t i = 0; i < M; i++) {
228 for (
size_t j = 0; j < i; j++) {
232 P(i, c) -= L(i, j) *
P(j, c);
249 for (
size_t c = 0; c < M; c++) {
251 for (
size_t k = 0; k < M; k++) {
253 size_t i = M - 1 - k;
256 for (
size_t j = i + 1; j < M; j++) {
260 P(i, c) -= U(i, j) *
P(j, c);
273 for (
size_t i = 0; i < M; i++) {
274 for (
size_t j = 0; j < M; j++) {
288 template<
typename Type,
size_t M>
305 template<
typename Type,
size_t M>
309 for (
size_t j = 0; j < M; j++) {
310 for (
size_t i = j; i < M; i++) {
313 for (
size_t k = 0; k < j; k++) {
314 sum += L(j, k)*L(j, k);
316 Type res = A(j, j) - sum;
324 for (
size_t k = 0; k < j; k++) {
325 sum += L(i, k)*L(j, k);
330 L(i, j) = (A(i, j) - sum)/L(j, j);
344 template<
typename Type,
size_t M>
348 return L_inv.
T()*L_inv;
Matrix< Type, N, M > T() const
SquareMatrix< float, 3 > SquareMatrix3f
SquareMatrix(const Matrix< Type, M, M > &other)
Matrix< Type, M, N > & operator=(const Matrix< Type, M, N > &other)
SquareMatrix< float, 3 > Matrix3f
bool I(SquareMatrix< Type, M > &i) const
SquareMatrix< Type, M > eye()
SquareMatrix< Type, M > cholesky(const SquareMatrix< Type, M > &A)
cholesky decomposition
SquareMatrix(const Type data_[M][M])
void swapRows(size_t a, size_t b)
SquareMatrix(const Slice< Type, M, M, P, Q > &in_slice)
Vector< float, 6 > f(float t, const Matrix< float, 6, 1 > &, const Matrix< float, 3, 1 > &)
bool inv(const SquareMatrix< Type, M > &A, SquareMatrix< Type, M > &inv)
inverse based on LU factorization with partial pivotting
SquareMatrix< Type, M > I() const
Vector< Type, M > diag() const
SquareMatrix(const Type data_[M *M])
SquareMatrix< Type, M > & operator=(const Matrix< Type, M, M > &other)
SquareMatrix< Type, M > expm(const Matrix< Type, M, M > &A, size_t order=5)
SquareMatrix< Type, M > choleskyInv(const SquareMatrix< Type, M > &A)
cholesky inverse
Vector< Type, M *(M+1)/2 > upper_right_triangle() const
SquareMatrix< Type, M > & operator=(const Slice< Type, M, M, P, Q > &in_slice)
Dual< Scalar, N > sqrt(const Dual< Scalar, N > &a)
void swapCols(size_t a, size_t b)