| //##################################################################### |
| // Copyright 2002-2004, Ronald Fedkiw, Igor Neverov, Eftychios Sifakis, Huamin Wang, Rachel Weinstein. |
| // This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt. |
| //##################################################################### |
| // Class MATRIX_MXN |
| //##################################################################### |
| #ifndef __MATRIX_MXN__ |
| #define __MATRIX_MXN__ |
| |
| #include <assert.h> |
| #include "VECTOR_ND.h" |
| #include "MATRIX_NXN.h" |
| #include "../Math_Tools/sqr.h" |
| namespace PhysBAM |
| { |
| |
| template<class T> |
| class MATRIX_MXN |
| { |
| public: |
| int m, n; // size of the m by n matrix |
| T *x; // pointer to the one dimensional data |
| MATRIX_MXN<T> *Q, *V; |
| MATRIX_NXN<T> *R; |
| |
| MATRIX_MXN() |
| : m (0), n (0), x (0), Q (0), V (0), R (0) |
| {} |
| |
| MATRIX_MXN (const int m_input, const int n_input) |
| : m (m_input), n (n_input), Q (0), V (0), R (0) |
| { |
| x = new T[m * n]; |
| |
| for (int k = 0; k < m * n; k++) x[k] = 0; |
| } |
| |
| MATRIX_MXN (const MATRIX_MXN<T>& A) |
| : m (A.m), n (A.n), Q (0), V (0), R (0) |
| { |
| x = new T[m * n]; |
| |
| for (int k = 0; k < m * n; k++) x[k] = A.x[k]; |
| } |
| |
| ~MATRIX_MXN() |
| { |
| if (x) delete[] x; |
| |
| if (Q) delete Q; |
| |
| if (R) delete (R); |
| |
| if (V) delete (V); |
| } |
| |
| T& operator() (const int i, const int j) |
| { |
| assert (i >= 1 && i <= m); |
| assert (j >= 1 && j <= n); |
| return * (x + (j - 1) * m + (i - 1)); |
| } |
| |
| const T& operator() (const int i, const int j) const |
| { |
| assert (i >= 1 && i <= m); |
| assert (j >= 1 && j <= n); |
| return * (x + (j - 1) * m + (i - 1)); |
| } |
| |
| MATRIX_MXN<T>& operator= (const MATRIX_MXN<T>& A) |
| { |
| delete Q; |
| delete V; |
| delete R; |
| Q = V = 0; |
| R = 0; |
| |
| if (!x || m*n != A.m * A.n) |
| { |
| delete[] x; |
| x = new T[A.m * A.n]; |
| }; |
| |
| m = A.m; |
| |
| n = A.n; |
| |
| for (int k = 0; k < m * n; k++) x[k] = A.x[k]; |
| |
| return *this; |
| } |
| |
| MATRIX_MXN<T> operator- (const MATRIX_MXN<T>& A) const |
| { |
| assert (n == A.n && m == A.m); |
| int size = m * n; |
| MATRIX_MXN<T> matrix (m, n); |
| |
| for (int i = 0; i < size; i++) matrix.x[i] = x[i] - A.x[i]; |
| |
| return matrix; |
| } |
| |
| VECTOR_ND<T> operator* (const VECTOR_ND<T>& y) const |
| { |
| assert (y.n == n); |
| VECTOR_ND<T> result (m); |
| |
| for (int j = 1; j <= n; j++) for (int i = 1; i <= m; i++) result (i) += * (x + (j - 1) * m + i - 1) * y (j); |
| |
| return result; |
| } |
| |
| MATRIX_MXN<T> operator* (const MATRIX_MXN<T>& A) const |
| { |
| assert (n == A.m); |
| MATRIX_MXN<T> matrix (m, A.n); |
| |
| for (int j = 1; j <= A.n; j++) for (int i = 1; i <= m; i++) for (int k = 1; k <= n; k++) matrix (i, j) += * (x + (k - 1) * m + i - 1) * A (k, j); |
| |
| return matrix; |
| } |
| |
| void Write_To_Screen() |
| { |
| for (int i = 1; i <= m; i++) |
| { |
| for (int j = 1; j <= n; j++) std::cout << (*this) (i, j) << " "; |
| |
| std::cout << std::endl; |
| } |
| } |
| |
| MATRIX_MXN<T> Transpose() |
| { |
| MATRIX_MXN<T> matrix (n, m); |
| |
| for (int i = 1; i <= m; i++) for (int j = 1; j <= n; j++) matrix (j, i) = * (x + (j - 1) * m + (i - 1)); |
| |
| return matrix; |
| } |
| |
| VECTOR_ND<T> Transpose_Times (const VECTOR_ND<T>& y) const |
| { |
| assert (y.n == m); |
| VECTOR_ND<T> result (n); |
| |
| for (int j = 1; j <= n; j++) for (int i = 1; i <= m; i++) result (j) += * (x + (j - 1) * m + i - 1) * y (i); |
| |
| return result; |
| } |
| |
| MATRIX_MXN<T> Transpose_Times (const MATRIX_MXN<T>& A) const |
| { |
| assert (n == A.m); |
| MATRIX_MXN<T> matrix (m, A.n); |
| |
| for (int j = 1; j <= A.n; j++) for (int i = 1; i <= m; i++) for (int k = 1; k <= n; k++) matrix (i, j) += * (x + (i - 1) * m + k - 1) * A (k, j); |
| |
| return matrix; |
| } |
| |
| T Infinity_Norm() const |
| { |
| T max_sum = 0; |
| |
| for (int j = 1; j <= n; j++) |
| { |
| T sum = 0; |
| |
| for (int i = 1; i <= m; i++) sum += fabs (* (x + (j - 1) * m + i - 1)); |
| |
| max_sum = max (sum, max_sum); |
| } |
| |
| return max_sum; |
| } |
| |
| //##################################################################### |
| |
| MATRIX_NXN<T> Normal_Equations_Matrix() const |
| { |
| MATRIX_NXN<T> result (n); |
| |
| for (int j = 1; j <= n; j++) for (int i = 1; i <= n; i++) for (int k = 1; k <= m; k++) result (i, j) += * (x + (i - 1) * m + k - 1)** (x + (j - 1) * m + k - 1); |
| |
| return result; |
| } |
| |
| VECTOR_ND<T> Normal_Equations_Solve (const VECTOR_ND<T>& b) const |
| { |
| MATRIX_NXN<T> A_transpose_A (Normal_Equations_Matrix()); |
| VECTOR_ND<T> A_transpose_b (Transpose_Times (b)); |
| return A_transpose_A.Cholesky_Solve (A_transpose_b); |
| } |
| |
| void Gram_Schmidt_QR_Factorization() |
| { |
| int i, j, k; |
| |
| if (Q) delete Q; |
| |
| Q = new MATRIX_MXN<T> (*this); |
| |
| if (R) delete (R); |
| |
| R = new MATRIX_NXN<T> (n); |
| |
| for (j = 1; j <= n; j++) // for each column |
| { |
| for (i = 1; i <= m; i++) (*R) (j, j) += sqr ( (*Q) (i, j)); |
| |
| (*R) (j, j) = sqrt ( (*R) (j, j)); // compute the L2 norm |
| T one_over_Rjj = 1 / (*R) (j, j); |
| |
| for (i = 1; i <= m; i++) (*Q) (i, j) *= one_over_Rjj; // orthogonalize the column |
| |
| for (k = j + 1; k <= n; k++) // subtract this columns contributution from the rest of the columns |
| { |
| for (i = 1; i <= m; i++) (*R) (j, k) += (*Q) (i, j) * (*Q) (i, k); |
| |
| for (i = 1; i <= m; i++) (*Q) (i, k) -= (*R) (j, k) * (*Q) (i, j); |
| } |
| } |
| } |
| |
| VECTOR_ND<T> Gram_Schmidt_QR_Solve (const VECTOR_ND<T>& b) |
| { |
| Gram_Schmidt_QR_Factorization(); |
| VECTOR_ND<T> c (Q->Transpose_Times (b)); |
| return R->Upper_Triangular_Solve (c); |
| } |
| |
| void Householder_QR_Factorization() |
| { |
| int i, j, k; |
| |
| if (V) delete V; |
| |
| V = new MATRIX_MXN<T> (m, n); |
| |
| if (R) delete (R); |
| |
| R = new MATRIX_NXN<T> (n); |
| MATRIX_MXN<T> temp (*this); |
| |
| for (j = 1; j <= n; j++) // for each column |
| { |
| VECTOR_ND<T> a (m); |
| |
| for (i = 1; i <= m; i++) a (i) = temp (i, j); |
| |
| VECTOR_ND<T> v = a.Householder_Vector (j); |
| |
| for (i = 1; i <= m; i++) (*V) (i, j) = v (i); // store the v's in V |
| |
| for (k = j; k <= n; k++) // Householder transform each column |
| { |
| for (i = 1; i <= m; i++) a (i) = temp (i, k); |
| |
| VECTOR_ND<T> new_a = a.Householder_Transform (v); |
| |
| for (i = 1; i <= m; i++) temp (i, k) = new_a (i); |
| } |
| } |
| |
| for (i = 1; i <= n; i++) for (j = 1; j <= n; j++) (*R) (i, j) = temp (i, j); |
| } // store R |
| |
| static VECTOR_ND<T> Householder_Transform (const VECTOR_ND<T>& b, const MATRIX_MXN<T>& V) |
| { |
| assert (V.m == b.n); |
| VECTOR_ND<T> result (b); |
| |
| for (int j = 1; j <= V.n; j++) |
| { |
| VECTOR_ND<T> v (V.m); |
| |
| for (int i = 1; i <= V.m; i++) v (i) = V (i, j); |
| |
| result = result.Householder_Transform (v); |
| } |
| |
| return result; |
| } |
| |
| VECTOR_ND<T> Householder_QR_Solve (const VECTOR_ND<T>& b) |
| { |
| Householder_QR_Factorization(); |
| VECTOR_ND<T> c = Householder_Transform (b, *V), c_short (n); |
| |
| for (int i = 1; i <= n; i++) c_short (i) = c (i); |
| |
| return R->Upper_Triangular_Solve (c_short); |
| } |
| |
| //##################################################################### |
| }; |
| } |
| #endif |
| |
| |