File size: 5,161 Bytes
8efb4bd | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 141 142 143 144 145 146 147 148 149 150 151 152 153 154 155 156 157 158 159 160 161 162 163 164 165 166 167 168 169 170 171 172 173 174 175 176 177 178 179 | #include "Matrix3.h"
typedef Matrix3::real real;
Matrix3::Matrix3(const real &d)
{
vrow[0] = Vector3(d, 0, 0);
vrow[1] = Vector3(0, d, 0);
vrow[2] = Vector3(0, 0, d);
}
Matrix3::Matrix3(const real &xr, const real &yr, const real &zr)
{
real cx = cos(xr); real cy = cos(yr); real cz = cos(zr);
real sx = sin(xr); real sy = sin(yr); real sz = sin(zr);
vrow[0] = Vector3(cz*cy, -sy*sx*cz - sz*cx, -sy*cx*cz + sz*sx);
vrow[1] = Vector3(sz*cy, -sy*sx*sz + cx*cz, -sy*cx*sz - sx*cz);
vrow[2] = Vector3(sy, cy*sx, cy*cx );
}
Matrix3::Matrix3(const real &psi, const real &theta, const real &phi, const int &)
{
real cpsi = cos(psi); real ctheta = cos(theta); real cphi = cos(phi);
real spsi = sin(psi); real stheta = sin(theta); real sphi = sin(phi);
vrow[0] = Vector3( cpsi*cphi - spsi*sphi*ctheta, cpsi*sphi + spsi*cphi*ctheta, stheta*spsi);
vrow[1] = Vector3(-spsi*cphi - cpsi*sphi*ctheta, -spsi*sphi + cpsi*cphi*ctheta, stheta*cpsi);
vrow[2] = Vector3( sphi*stheta, -cphi*stheta, ctheta);
}
Matrix3::Matrix3(const real a11, const real a12, const real a13,
const real a21, const real a22, const real a23,
const real a31, const real a32, const real a33) {
vrow[0] = Vector3(a11, a12, a13);
vrow[1] = Vector3(a21, a22, a23);
vrow[2] = Vector3(a31, a32, a33);
}
Matrix3::Matrix3(const real trn[9]) {
vrow[0] = Vector3(trn);
vrow[1] = Vector3(trn+3);
vrow[2] = Vector3(trn+6);
}
Matrix3::Matrix3(const double trn[9]) {
vrow[0] = Vector3(trn);
vrow[1] = Vector3(trn+3);
vrow[2] = Vector3(trn+6);
}
Matrix3::Matrix3(const real trn[3][3]) {
vrow[0] = Vector3(trn[0]);
vrow[1] = Vector3(trn[1]);
vrow[2] = Vector3(trn[2]);
}
Matrix3::Matrix3(const double trn[3][3]) {
vrow[0] = Vector3(trn[0]);
vrow[1] = Vector3(trn[1]);
vrow[2] = Vector3(trn[2]);
}
Matrix3::Matrix3(const Vector3 &diag)
{
vrow[0] = Vector3(diag[0], 0, 0);
vrow[1] = Vector3(0, diag[1], 0);
vrow[2] = Vector3(0, 0, diag[2]);
}
Matrix3::Matrix3(const Vector3 &v1, const Vector3 &v2)
{
vrow[0] = v1[0] * v2;
vrow[1] = v1[1] * v2;
vrow[2] = v1[2] * v2;
}
real Matrix3::determinant() const
{
return
element(1,1) * (element(2,2)*element(3,3) - element(2,3)*element(3,2))
- element(1,2) * (element(2,1)*element(3,3) - element(2,3)*element(3,1))
+ element(1,3) * (element(2,1)*element(3,2) - element(2,2)*element(3,1));
}
Matrix3& Matrix3::operator*=(const real &m)
{
vrow[0]*= m;
vrow[1]*= m;
vrow[2]*= m;
return *this;
}
Matrix3& Matrix3::operator/=(const real &m)
{
vrow[0]/= m;
vrow[1]/= m;
vrow[2]/= m;
return *this;
}
Matrix3& Matrix3::operator+=(const Matrix3 &t)
{
vrow[0]+= t.vrow[0];
vrow[1]+= t.vrow[1];
vrow[2]+= t.vrow[2];
return *this;
}
Matrix3& Matrix3::operator-=(const Matrix3 &t)
{
vrow[0]-= t.vrow[0];
vrow[1]-= t.vrow[1];
vrow[2]-= t.vrow[2];
return *this;
}
Matrix3& Matrix3::operator*=(const Matrix3 &t)
{
Vector3 c1 = t.column(1);
Vector3 c2 = t.column(2);
Vector3 c3 = t.column(3);
vrow[0] = Vector3(vrow[0]*c1, vrow[0]*c2, vrow[0]*c3);
vrow[1] = Vector3(vrow[1]*c1, vrow[1]*c2, vrow[1]*c3);
vrow[2] = Vector3(vrow[2]*c1, vrow[2]*c2, vrow[2]*c3);
return *this;
}
Vector3 Matrix3::eigenvalues() const
{
const real pi23 = (const real)2.0943951023; // pi*2/3
real a0 = -determinant();
real a1 = (element(1,1)*element(2,2) - element(1,2)*element(2,1) +
element(1,1)*element(3,3) - element(1,3)*element(3,1) +
element(2,2)*element(3,3) - element(2,3)*element(3,2));
real a2 = -trace();
real b1 = (3*a1-sqr(a2))/3;
if (b1 > 0) return Vector3(); // Error! no eigenvalues.
real b0 = (2*a2*a2*a2 - 9*a2*a1 + 27*a0)/27;
real d2 = (b1*b1*b1/27 + sqr(b0)/4);
if (d2 > 0) return Vector3(); // Error! no eignevalues.
real phi = acos(-(b0/2)*pow(-3/b1, (real)1.5))/3;
real sqrtb1 = 2*sqrt(-b1/3);
a2 /= 3;
return Vector3(sqrtb1*cos(phi ) - a2,
sqrtb1*cos(phi+2*pi23) - a2,
sqrtb1*cos(phi+pi23 ) - a2);
}
Matrix3 Matrix3::adjoint() const
{
return Matrix3(Vector3(element(2,2)*element(3,3) - element(2,3)*element(3,2),
element(1,3)*element(3,2) - element(1,2)*element(3,3),
element(1,2)*element(2,3) - element(1,3)*element(2,2)
),
Vector3(element(2,3)*element(3,1) - element(2,1)*element(3,3),
element(1,1)*element(3,3) - element(1,3)*element(3,1),
element(1,3)*element(2,1) - element(1,1)*element(2,3)
),
Vector3(element(2,1)*element(3,2) - element(2,2)*element(3,1),
element(1,2)*element(3,1) - element(1,1)*element(3,2),
element(1,1)*element(2,2) - element(1,2)*element(2,1)
)
);
}
/* if B is the inverse of A then
Bij = Aji / |A| where Aji is the determinant of A without row i and col j */
Matrix3 operator!(const Matrix3 &t1)
{
real det = t1.determinant();
if (det == 0) return Matrix3();
Matrix3 inv = t1.adjoint();
inv *= ((real)1/det);
return inv;
}
|