| class Matrix3 { | |
| private: | |
| double data[9]; | |
| public: | |
| Matrix3() { memset(data, 0, 9*sizeof(double)); } | |
| Matrix3(const double d) { memset(data, 0, 9*sizeof(double)); data[0] = data[4] = data[8] = d; } | |
| Matrix3(double * array) { memcpy(data, array, 9*sizeof(double)); } | |
| Matrix3(float * array) { for ( int i=0; i<9; i++ ) data[i] = array[i]; } | |
| Matrix3(const double d0, const double d1, const double d2, | |
| const double d3, const double d4, const double d5, | |
| const double d6, const double d7, const double d8) { | |
| data[0] = d0; data[1] = d1; data[2] = d2; | |
| data[3] = d3; data[4] = d4; data[5] = d5; | |
| data[6] = d6; data[7] = d7; data[8] = d8; | |
| } | |
| Matrix3(const Matrix3& m) { memcpy(data, m.data, 9*sizeof(double)); } | |
| double& operator[](int i) { if ( i<0 ) i=0; if ( i>8 ) i=8; return data[i]; } | |
| Matrix3 operator=(const Matrix3& m) { memcpy(data, m.data, 9*sizeof(double)); return *this; } | |
| Matrix3 operator+=(const Matrix3& m) { | |
| for ( int i=0; i<9; i++ ) data[i] += m.data[i]; | |
| return *this; | |
| } | |
| Matrix3 operator+(const Matrix3& m) { | |
| Matrix3 mn(*this); | |
| return mn += m; | |
| } | |
| Matrix3 operator-=(const Matrix3& m) { | |
| for ( int i=0; i<9; i++ ) data[i] -= m.data[i]; | |
| return *this; | |
| } | |
| Matrix3 operator-(const Matrix3& m) { | |
| Matrix3 mn(*this); | |
| return mn -= m; | |
| } | |
| Matrix3 operator*=(const Matrix3& m) { | |
| *this = *this * m; | |
| return *this; | |
| } | |
| Matrix3 operator*(const Matrix3& m) { | |
| int i, j, k; | |
| Matrix3 mn; | |
| for ( i=0; i<9; i+=3 ) | |
| for ( j=0; j<3; j++ ) | |
| for ( k=0; k<3; k++ ) | |
| mn.data[i+j] += data[i+k]*m.data[3*k+j]; | |
| return mn; | |
| } | |
| Matrix3 operator*=(const double d) { | |
| for ( int i=0; i<9; i++ ) data[i] *= d; | |
| return *this; | |
| } | |
| Matrix3 operator*(const double d) { | |
| Matrix3 mn; | |
| for ( int i=0; i<9; i++ ) mn.data[i] = data[i] * d; | |
| return mn; | |
| } | |
| Matrix3 operator/=(const double d) { | |
| double div = 1/d; | |
| return *this *= div; | |
| } | |
| Matrix3 operator/(const double d) { | |
| double div = 1/d; | |
| return *this * div; | |
| } | |
| bool operator==(const Matrix3& m) { | |
| int e = 0; | |
| for ( int i=0; i<9; i++ ) | |
| e += ( data[i] == m.data[i] ); | |
| return ( e == 9 ); | |
| } | |
| void check() { | |
| for ( int i=0; i<9; i++ ) { | |
| if ( fabs(data[i]) < 1e-12 ) data[i] = 0; | |
| if ( data[i] > 1 ) data[i] = 1; | |
| if ( data[i] < -1 ) data[i] = -1; | |
| } | |
| } | |
| double determinant() { | |
| return data[0]*(data[4]*data[8]-data[5]*data[7]) | |
| - data[1]*(data[3]*data[8]-data[5]*data[6]) | |
| + data[2]*(data[3]*data[7]-data[4]*data[6]); | |
| } | |
| double trace() { | |
| return data[0] + data[4] + data[8]; | |
| } | |
| Matrix3 transpose() { | |
| int i, j; | |
| Matrix3 mn; | |
| for ( i=0; i<3; i++ ) | |
| for ( j=0; j<3; j++ ) | |
| mn.data[3*i+j] = data[3*j+i]; | |
| return mn; | |
| } | |
| void normalize() { | |
| int i, j, k; | |
| double s; | |
| for ( i=0; i<3; i++ ) { | |
| for ( s = 0, j=0, k=3*i; j<3; j++, k++ ) | |
| s += data[k]*data[k]; | |
| if ( s > 0 ) { | |
| s = sqrt(s); | |
| for ( j=0, k=3*i; j<3; j++, k++ ) | |
| data[k] /= s; | |
| } | |
| } | |
| } | |
| } ; |