| //##################################################################### |
| // Copyright 2003-2004, Geoffrey Irving. |
| // This file is part of PhysBAM whose distribution is governed by the license contained in the accompanying file PHYSBAM_COPYRIGHT.txt. |
| //##################################################################### |
| // Class SYMMETRIC_MATRIX_2X2 |
| //##################################################################### |
| #ifndef __SYMMETRIC_MATRIX_2X2__ |
| #define __SYMMETRIC_MATRIX_2X2__ |
| |
| #include "VECTOR_2D.h" |
| namespace PhysBAM |
| { |
| |
| template<class T> class MATRIX_2X2; |
| template<class T> class DIAGONAL_MATRIX_2X2; |
| template<class T> class UPPER_TRIANGULAR_MATRIX_2X2; |
| |
| template<class T> |
| class SYMMETRIC_MATRIX_2X2 |
| { |
| public: |
| T x11, x21, x22; |
| |
| SYMMETRIC_MATRIX_2X2() |
| : x11 (0), x21 (0), x22 (0) |
| {} |
| |
| template<class T2> |
| SYMMETRIC_MATRIX_2X2 (const SYMMETRIC_MATRIX_2X2<T2>& matrix_input) |
| : x11 ( (T) matrix_input.x11), x21 ( (T) matrix_input.x21), x22 ( (T) matrix_input.x22) |
| {} |
| |
| SYMMETRIC_MATRIX_2X2 (const T y11, const T y21, const T y22) |
| : x11 (y11), x21 (y21), x22 (y22) |
| {} |
| |
| VECTOR_2D<T> Column1() const |
| { |
| return VECTOR_2D<T> (x11, x21); |
| } |
| |
| VECTOR_2D<T> Column2() const |
| { |
| return VECTOR_2D<T> (x21, x22); |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T> operator-() const |
| { |
| return SYMMETRIC_MATRIX_2X2<T> (-x11, -x21, -x22); |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T>& operator+= (const SYMMETRIC_MATRIX_2X2<T>& A) |
| { |
| x11 += A.x11; |
| x21 += A.x21; |
| x22 += A.x22; |
| return *this; |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T>& operator+= (const T& a) |
| { |
| x11 += a; |
| x22 += a; |
| return *this; |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T>& operator-= (const SYMMETRIC_MATRIX_2X2<T>& A) |
| { |
| x11 -= A.x11; |
| x21 -= A.x21; |
| x22 -= A.x22; |
| return *this; |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T>& operator-= (const T& a) |
| { |
| x11 -= a; |
| x22 -= a; |
| return *this; |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T>& operator*= (const T a) |
| { |
| x11 *= a; |
| x21 *= a; |
| x22 *= a; |
| return *this; |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T>& operator/= (const T a) |
| { |
| assert (a != 0); |
| T s = 1 / a; |
| x11 *= s; |
| x21 *= s; |
| x22 *= s; |
| return *this; |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T> operator+ (const SYMMETRIC_MATRIX_2X2<T>& A) const |
| { |
| return SYMMETRIC_MATRIX_2X2<T> (x11 + A.x11, x21 + A.x21, x22 + A.x22); |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T> operator+ (const T a) const |
| { |
| return SYMMETRIC_MATRIX_2X2<T> (x11 + a, x21, x22 + a); |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T> operator- (const SYMMETRIC_MATRIX_2X2<T>& A) const |
| { |
| return SYMMETRIC_MATRIX_2X2<T> (x11 - A.x11, x21 - A.x21, x22 - A.x22); |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T> operator- (const T a) const |
| { |
| return SYMMETRIC_MATRIX_2X2<T> (x11 - a, x21, x22 - a); |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T> operator* (const T a) const |
| { |
| return SYMMETRIC_MATRIX_2X2<T> (a * x11, a * x21, a * x22); |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T> operator/ (const T a) const |
| { |
| assert (a != 0); |
| return *this * (1 / a); |
| } |
| |
| VECTOR_2D<T> operator* (const VECTOR_2D<T>& v) const |
| { |
| return VECTOR_2D<T> (x11 * v.x + x21 * v.y, x21 * v.x + x22 * v.y); |
| } |
| |
| T Determinant() const |
| { |
| return x11 * x22 - x21 * x21; |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T> Inverse() const |
| { |
| return SYMMETRIC_MATRIX_2X2<T> (x22, -x21, x11) / Determinant(); |
| } |
| |
| VECTOR_2D<T> Solve_Linear_System (const VECTOR_2D<T>& b) const |
| { |
| return SYMMETRIC_MATRIX_2X2<T> (x22, -x21, x11) * b / Determinant(); |
| } |
| |
| T Trace() const |
| { |
| return x11 + x22; |
| } |
| |
| static T Inner_Product (const SYMMETRIC_MATRIX_2X2<T>& A, const SYMMETRIC_MATRIX_2X2<T>& B) |
| { |
| return A.x11 * B.x11 + A.x22 * B.x22 + 2 * A.x21 * B.x21; |
| } |
| |
| T Frobenius_Norm_Squared() const |
| { |
| return x11 * x11 + x22 * x22 + 2 * x21 * x21; |
| } |
| |
| T Frobenius_Norm() const |
| { |
| return sqrt (Frobenius_Norm_Squared()); |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T> Cofactor_Matrix() |
| { |
| return SYMMETRIC_MATRIX_2X2<T> (x22, -x21, x11); |
| } |
| |
| T Max_Abs_Element() const |
| { |
| return max (fabs (x11), fabs (x21), fabs (x22)); |
| } |
| |
| static SYMMETRIC_MATRIX_2X2<T> Outer_Product (const VECTOR_2D<T>& u) |
| { |
| return SYMMETRIC_MATRIX_2X2<T> (u.x * u.x, u.x * u.y, u.y * u.y); |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T> Log() const |
| { |
| DIAGONAL_MATRIX_2X2<T> D; |
| MATRIX_2X2<T> Q; |
| Solve_Eigenproblem (D, Q); |
| return Conjugate (Q, D.Log()); |
| } |
| |
| SYMMETRIC_MATRIX_2X2<T> Exp() const |
| { |
| DIAGONAL_MATRIX_2X2<T> D; |
| MATRIX_2X2<T> Q; |
| Solve_Eigenproblem (D, Q); |
| return Conjugate (Q, D.Exp()); |
| } |
| |
| static SYMMETRIC_MATRIX_2X2<T> Identity_Matrix() |
| { |
| return SYMMETRIC_MATRIX_2X2<T> (1, 0, 1); |
| } |
| |
| template<class RW> |
| void Read (std::istream &input_stream) |
| { |
| Read_Binary<RW> (input_stream, x11, x21, x22); |
| } |
| |
| template<class RW> |
| void Write (std::ostream &output_stream) const |
| { |
| Write_Binary<RW> (output_stream, x11, x21, x22); |
| } |
| |
| //##################################################################### |
| void Solve_Eigenproblem (DIAGONAL_MATRIX_2X2<T>& eigenvalues, MATRIX_2X2<T>& eigenvectors) const; |
| MATRIX_2X2<T> operator* (const UPPER_TRIANGULAR_MATRIX_2X2<T>& A) const; |
| MATRIX_2X2<T> Multiply_With_Transpose (const UPPER_TRIANGULAR_MATRIX_2X2<T>& A) const; |
| static SYMMETRIC_MATRIX_2X2<T> Conjugate (const MATRIX_2X2<T>& A, const DIAGONAL_MATRIX_2X2<T>& B); |
| static SYMMETRIC_MATRIX_2X2<T> Conjugate (const MATRIX_2X2<T>& A, const SYMMETRIC_MATRIX_2X2<T>& B); |
| static SYMMETRIC_MATRIX_2X2<T> Conjugate (const UPPER_TRIANGULAR_MATRIX_2X2<T>& A, const SYMMETRIC_MATRIX_2X2<T>& B); |
| //##################################################################### |
| }; |
| // global functions |
| template<class T> |
| inline SYMMETRIC_MATRIX_2X2<T> operator* (const T a, const SYMMETRIC_MATRIX_2X2<T>& A) |
| { |
| return A * a; |
| } |
| |
| template<class T> |
| inline SYMMETRIC_MATRIX_2X2<T> operator- (const T a, const SYMMETRIC_MATRIX_2X2<T>& A) |
| { |
| return -A + a; |
| } |
| |
| template<class T> |
| inline std::ostream& operator<< (std::ostream& output_stream, const SYMMETRIC_MATRIX_2X2<T>& A) |
| { |
| output_stream << A.x11 << "\n" << A.x21 << " " << A.x22 << "\n"; |
| return output_stream; |
| } |
| //##################################################################### |
| } |
| #include "MATRIX_2X2.h" |
| #include "UPPER_TRIANGULAR_MATRIX_2X2.h" |
| namespace PhysBAM |
| { |
| //##################################################################### |
| // Function Solve_Eigenproblem |
| //##################################################################### |
| template<class T> void SYMMETRIC_MATRIX_2X2<T>:: |
| Solve_Eigenproblem (DIAGONAL_MATRIX_2X2<T>& eigenvalues, MATRIX_2X2<T>& eigenvectors) const |
| { |
| T cosine, sine, da; |
| |
| if (x21 == 0) |
| { |
| cosine = 1; |
| sine = 0; |
| da = 0; |
| } |
| else |
| { |
| T theta = (T).5 * (x22 - x11) / x21, t = 1 / (fabs (theta) + sqrt (1 + sqr (theta))); |
| |
| if (theta < 0) t = -t; |
| |
| cosine = 1 / sqrt (1 + t * t); |
| sine = t * cosine; |
| da = t * x21; |
| } |
| |
| eigenvalues = DIAGONAL_MATRIX_2X2<T> (x11 - da, x22 + da); |
| |
| if (eigenvalues.x11 >= eigenvalues.x22) eigenvectors = MATRIX_2X2<T> (cosine, -sine, sine, cosine); |
| else |
| { |
| exchange (eigenvalues.x11, eigenvalues.x22); |
| eigenvectors = MATRIX_2X2<T> (sine, cosine, -cosine, sine); |
| } |
| } |
| //##################################################################### |
| // Function Conjugate |
| //##################################################################### |
| template<class T> SYMMETRIC_MATRIX_2X2<T> SYMMETRIC_MATRIX_2X2<T>:: |
| Conjugate (const MATRIX_2X2<T>& A, const DIAGONAL_MATRIX_2X2<T>& B) |
| { |
| MATRIX_2X2<T> BA = B * A.Transposed(); |
| return SYMMETRIC_MATRIX_2X2<T> (A.x[0] * BA.x[0] + A.x[2] * BA.x[1], A.x[1] * BA.x[0] + A.x[3] * BA.x[1], A.x[1] * BA.x[2] + A.x[3] * BA.x[3]); |
| } |
| //##################################################################### |
| // Function Conjugate |
| //##################################################################### |
| template<class T> SYMMETRIC_MATRIX_2X2<T> SYMMETRIC_MATRIX_2X2<T>:: |
| Conjugate (const MATRIX_2X2<T>& A, const SYMMETRIC_MATRIX_2X2<T>& B) |
| { |
| MATRIX_2X2<T> BA = (A * B).Transposed(); |
| return SYMMETRIC_MATRIX_2X2<T> (A.x[0] * BA.x[0] + A.x[2] * BA.x[1], A.x[1] * BA.x[0] + A.x[3] * BA.x[1], A.x[1] * BA.x[2] + A.x[3] * BA.x[3]); |
| } |
| //##################################################################### |
| // Function Conjugate |
| //##################################################################### |
| template<class T> SYMMETRIC_MATRIX_2X2<T> SYMMETRIC_MATRIX_2X2<T>:: |
| Conjugate (const UPPER_TRIANGULAR_MATRIX_2X2<T>& A, const SYMMETRIC_MATRIX_2X2<T>& B) |
| { |
| MATRIX_2X2<T> BA = B.Multiply_With_Transpose (A); |
| return SYMMETRIC_MATRIX_2X2<T> (A.x11 * BA.x[0] + A.x12 * BA.x[1], A.x22 * BA.x[1], A.x22 * BA.x[3]); |
| } |
| //##################################################################### |
| // Function Multiply_With_Transpose |
| //##################################################################### |
| template<class T> MATRIX_2X2<T> SYMMETRIC_MATRIX_2X2<T>:: |
| Multiply_With_Transpose (const UPPER_TRIANGULAR_MATRIX_2X2<T>& A) const |
| { |
| return (A**this).Transposed(); |
| } |
| //##################################################################### |
| // Function operator* |
| //##################################################################### |
| template<class T> MATRIX_2X2<T> SYMMETRIC_MATRIX_2X2<T>:: |
| operator* (const UPPER_TRIANGULAR_MATRIX_2X2<T>& A) const |
| { |
| return MATRIX_2X2<T> (x11 * A.x11, x21 * A.x11, x11 * A.x12 + x21 * A.x22, x21 * A.x12 + x22 * A.x22); |
| } |
| //##################################################################### |
| // Function operator* |
| //##################################################################### |
| template<class T> MATRIX_2X2<T> operator* (const DIAGONAL_MATRIX_2X2<T>& D, const SYMMETRIC_MATRIX_2X2<T>& A) |
| { |
| return MATRIX_2X2<T> (D.x11 * A.x11, D.x22 * A.x21, D.x11 * A.x21, D.x22 * A.x22); |
| } |
| //##################################################################### |
| // Function operator* |
| //##################################################################### |
| template<class T> |
| inline MATRIX_2X2<T> operator* (const UPPER_TRIANGULAR_MATRIX_2X2<T>& A, const SYMMETRIC_MATRIX_2X2<T>& B) |
| { |
| return MATRIX_2X2<T> (A.x11 * B.x11 + A.x12 * B.x21, A.x22 * B.x21, A.x11 * B.x21 + A.x12 * B.x22, A.x22 * B.x22); |
| } |
| //##################################################################### |
| // Function operator* |
| //##################################################################### |
| template<class T> |
| inline MATRIX_2X2<T> operator* (const SYMMETRIC_MATRIX_2X2<T>& A, const MATRIX_2X2<T>& B) |
| { |
| return MATRIX_2X2<T> (A.x11 * B.x[0] + A.x21 * B.x[1], A.x21 * B.x[0] + A.x22 * B.x[1], A.x11 * B.x[2] + A.x21 * B.x[3], A.x21 * B.x[2] + A.x22 * B.x[3]); |
| } |
| //##################################################################### |
| } |
| #endif |
| |