#ifndef __JMATRIX4S__ #define __JMATRIX4S__ #include #include #include "JMath/JMatrix4D.hh" #include "JLang/JException.hh" /** * \author mdejong */ namespace JMATH {} namespace JPP { using namespace JMATH; } namespace JMATH { using JLANG::JDivisionByZero; /** * 4 x 4 symmetric matrix */ class JMatrix4S : public JMatrix4D { public: /** * Default constructor. */ JMatrix4S() : JMatrix4D() {} /** * Contructor. * * \param A matrix */ JMatrix4S(const JMatrix4D& A) : JMatrix4D(A) {} /** * Contructor.\n * The upper triangle is internally set. * * \param __a00 (0,0) * \param __a10 (1,0) * \param __a11 (1,1) * \param __a20 (2,0) * \param __a21 (2,1) * \param __a22 (2,2) * \param __a30 (3,0) * \param __a31 (3,1) * \param __a32 (3,2) * \param __a33 (3,3) */ JMatrix4S(const double __a00, const double __a10, const double __a11, const double __a20, const double __a21, const double __a22, const double __a30, const double __a31, const double __a32, const double __a33) : JMatrix4D(__a00, __a10, __a20, __a30, __a10, __a11, __a21, __a31, __a20, __a21, __a22, __a32, __a30, __a31, __a32, __a33) {} /** * Invert matrix. */ void invert() { using std::swap; // LDU decomposition int p0 = 0; // permute row 0 int p1 = 0; // permute row 1 int p2 = 0; // permute row 2 double val; val = a00; if (fabs(a10) > fabs(val)) { p0 = 1; val = a10; } if (fabs(a20) > fabs(val)) { p0 = 2; val = a20; } if (fabs(a30) > fabs(val)) { p0 = 3; val = a30; } switch (p0) { case 1: swap(a00,a10); swap(a01,a11); swap(a02,a12); swap(a03,a13); break; case 2: swap(a00,a20); swap(a01,a21); swap(a02,a22); swap(a03,a23); break; case 3: swap(a00,a30); swap(a01,a31); swap(a02,a32); swap(a03,a33); break; } if (val == 0) { throw JDivisionByZero("LDU decomposition zero pivot"); } a10 /= val; a11 -= a10 * a01; a12 -= a10 * a02; a13 -= a10 * a03; a20 /= val; a21 -= a20 * a01; a22 -= a20 * a02; a23 -= a20 * a03; a30 /= val; a31 -= a30 * a01; a32 -= a30 * a02; a33 -= a30 * a03; val = a11; if (fabs(a21) > fabs(val)) { p1 = 2; val = a21; } if (fabs(a31) > fabs(val)) { p1 = 3; val = a31; } switch (p1) { case 2: swap(a10,a20); swap(a11,a21); swap(a12,a22); swap(a13,a23); break; case 3: swap(a10,a30); swap(a11,a31); swap(a12,a32); swap(a13,a33); break; } if (val == 0) { throw JDivisionByZero("LDU decomposition zero pivot"); } a21 /= val; a22 -= a21 * a12; a23 -= a21 * a13; a31 /= val; a32 -= a31 * a12; a33 -= a31 * a13; val = a22; if (fabs(a32) > fabs(val)) { p2 = 3; val = a32; } switch (p2) { case 3: swap(a20,a30); swap(a21,a31); swap(a22,a32); swap(a23,a33); break; } if (val == 0) { throw JDivisionByZero("LDU decomposition zero pivot"); } a32 /= val; a33 -= a32 * a23; // invert D if (a33 == 0) { throw JDivisionByZero("D matrix not invertable"); } a00 = 1.0 / a00; a11 = 1.0 / a11; a22 = 1.0 / a22; a33 = 1.0 / a33; // invert U a01 *= -a00; a01 *= a11; a02 *= -a00; a02 -= a01 * a12; a02 *= a22; a03 *= -a00; a03 -= a01 * a13; a03 -= a02 * a23; a03 *= a33; a12 *= -a11; a12 *= a22; a13 *= -a11; a13 -= a12 * a23; a13 *= a33; a23 *= -a22; a23 *= a33; // invert L a32 = -a32; a31 = -a31; a31 -= a32 * a21; a30 = -a30; a30 -= a31 * a10; a30 -= a32 * a20; a21 = -a21; a20 = -a20; a20 -= a21 * a10; a10 = -a10; // U^-1 x L^-1 a00 += a01 * a10 + a02 * a20 + a03 * a30; a01 += a02 * a21 + a03 * a31; a02 += a03 * a32; a10 *= a11; a10 += a12 * a20 + a13 * a30; a11 += a12 * a21 + a13 * a31; a12 += a13 * a32; a20 *= a22; a20 += a23 * a30; a21 *= a22; a21 += a23 * a31; a22 += a23 * a32; a30 *= a33; a31 *= a33; a32 *= a33; switch (p2) { case 3: swap(a02,a03); swap(a12,a13); swap(a22,a23); swap(a32,a33); break; } switch (p1) { case 2: swap(a01,a02); swap(a11,a12); swap(a21,a22); swap(a31,a32); break; case 3: swap(a01,a03); swap(a11,a13); swap(a21,a23); swap(a31,a33); break; } switch (p0) { case 1: swap(a00,a01); swap(a10,a11); swap(a20,a21); swap(a30,a31); break; case 2: swap(a00,a02); swap(a10,a12); swap(a20,a22); swap(a30,a32); break; case 3: swap(a00,a03); swap(a10,a13); swap(a20,a23); swap(a30,a33); break; } } }; } #endif