00001
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035
00036 #ifndef _VECMATH_H_
00037 #define _VECMATH_H_
00038
00039
00040 #include <iostream>
00041 #include <cmath>
00042 #include <cstring>
00043 #include "emassert.h"
00044
00045 namespace EMAN
00046 {
00047
00048 inline bool isZero(double in_d, double in_dEps = 1e-16 )
00049 {
00050 return (in_d < in_dEps && in_d > -in_dEps)? true : false;
00051 }
00052
00053
00054
00055 class ScreenVector {
00056 public:
00057 ScreenVector() : x(0), y(0) {}
00058 ScreenVector(const ScreenVector& v) : x(v[0]), y(v[1]) {}
00059 ScreenVector(int _x, int _y) : x(_x), y(_y) {}
00060
00061 ScreenVector& operator=(const ScreenVector& a) {
00062 x = a[0]; y = a[1];
00063 return *this;
00064 }
00065
00066 const int &operator[](int n) const { return (&x)[n]; }
00067 int &operator[](int n) { return (&x)[n]; }
00068
00069 ScreenVector& operator+=(const ScreenVector& a) {
00070 x += a[0]; y += a[1];
00071 return *this;
00072 }
00073
00074 ScreenVector& operator-=(const ScreenVector& a) {
00075 x -= a[0]; y -= a[1];
00076 return *this;
00077 }
00078
00079 ScreenVector& operator*=(int s) {
00080 x *= s; y *= s;
00081 return *this;
00082 }
00083
00084 ScreenVector operator-() const {
00085 return ScreenVector(-x, -y);
00086 }
00087
00088 ScreenVector operator+() const {
00089 return *this;
00090 }
00091
00092 ScreenVector operator+( const ScreenVector &v ) const {
00093 return ScreenVector( x + v.x, y + v.y );
00094 }
00095
00096 ScreenVector operator-( const ScreenVector &v ) const {
00097 return ScreenVector( x - v.x, y - v.y );
00098 }
00099
00100 ScreenVector operator*( const double s ) const {
00101 return ScreenVector( (int)(x * s), (int)(y * s) );
00102 }
00103
00104
00105 int operator*( const ScreenVector &v ) const {
00106 return x * v.x + y * v.y;
00107 }
00108
00109 double length() const {
00110 return (double) sqrt( (double) (x * x + y * y) );
00111 }
00112
00113 int lengthSquared() const {
00114 return x * x + y * y;
00115 }
00116
00117 bool operator==( const ScreenVector &v ) const {
00118 return x == v.x && y == v.y;
00119 }
00120
00121 bool operator!=( const ScreenVector &v ) const {
00122 return x != v.x || y != v.y;
00123 }
00124
00125 void print() const {
00126 std::cout << "(" << x << ", " << y << ")";
00127 }
00128
00129 private:
00130 int x, y;
00131 };
00132
00133 inline ScreenVector operator*( const double s, const ScreenVector &v ) {
00134 return ScreenVector( (int)(v[0] * s), (int)(v[1] * s) );
00135 }
00136
00137 inline std::ostream& operator<<(std::ostream& os, const ScreenVector& v) {
00138 os << "(" << v[0] << ", " << v[1] << ")";
00139 return os;
00140 }
00141
00142
00143 class ScreenPoint {
00144 public:
00145 ScreenPoint() : x(0), y(0) {}
00146 ScreenPoint(const ScreenPoint & p) : x(p[0]), y(p[1]) {}
00147 ScreenPoint(int _x, int _y) : x(_x), y(_y) {}
00148
00149 ScreenPoint& operator=(const ScreenPoint& a) {
00150 x = a[0]; y = a[1];
00151 return *this;
00152 }
00153
00154 const int &operator[](int n) const { return (&x)[n]; }
00155 int &operator[](int n) { return (&x)[n]; }
00156
00157 ScreenPoint& operator+=(const ScreenVector& v) {
00158 x += v[0]; y += v[1];
00159 return *this;
00160 }
00161
00162 ScreenPoint& operator-=(const ScreenVector& v) {
00163 x -= v[0]; y -= v[1];
00164 return *this;
00165 }
00166
00167 ScreenPoint& operator*=(int s) {
00168 x *= s; y *= s;
00169 return *this;
00170 }
00171
00172 ScreenPoint operator+(const ScreenVector& v) const {
00173 return ScreenPoint( x + v[0], y + v[1] );
00174 }
00175
00176 ScreenVector operator-(const ScreenPoint& p) const {
00177 return ScreenVector( x - p.x, y - p.y );
00178 }
00179
00180 ScreenPoint operator-(const ScreenVector& v) const {
00181 return ScreenPoint( x - v[0], y - v[1] );
00182 }
00183
00184 bool operator==( const ScreenPoint &p ) const {
00185 return x == p.x && y == p.y;
00186 }
00187
00188 bool operator!=( const ScreenPoint &p ) const {
00189 return x != p.x || y != p.y;
00190 }
00191
00192 void print() const {
00193 std::cout << "(" << x << ", " << y << ")";
00194 }
00195
00196 private:
00197 int x, y;
00198 };
00199
00200 inline std::ostream& operator<<(std::ostream& os, const ScreenPoint& p) {
00201 os << "(" << p[0] << ", " << p[1] << ")";
00202 return os;
00203 }
00204
00205
00206 class Vector3 {
00207 public:
00208 Vector3() : x(0), y(0), z(0) {}
00209 Vector3(const Vector3& v) : x(v[0]), y(v[1]), z(v[2]) {}
00210 Vector3(double _x, double _y, double _z) : x(_x), y(_y), z(_z) {}
00211
00212 Vector3& operator=(const Vector3& a) {
00213 x = a[0]; y = a[1]; z = a[2];
00214 return *this;
00215 }
00216
00217 const double &operator[](int n) const { return (&x)[n]; }
00218 double &operator[](int n) { return (&x)[n]; }
00219
00220 Vector3& operator+=(const Vector3& a) {
00221 x += a[0]; y += a[1]; z += a[2];
00222 return *this;
00223 }
00224
00225 Vector3& operator-=(const Vector3& a) {
00226 x -= a[0]; y -= a[1]; z -= a[2];
00227 return *this;
00228 }
00229
00230 Vector3& operator*=(double s) {
00231 x *= s; y *= s; z *= s;
00232 return *this;
00233 }
00234
00235 Vector3 operator-() const {
00236 return Vector3(-x, -y, -z);
00237 }
00238
00239 Vector3 operator+() const {
00240 return *this;
00241 }
00242
00243 Vector3 operator+( const Vector3 &v ) const {
00244 return Vector3( x + v.x, y + v.y, z + v.z );
00245 }
00246
00247 Vector3 operator-( const Vector3 &v ) const {
00248 return Vector3( x - v.x, y - v.y, z - v.z );
00249 }
00250
00251 Vector3 operator/( const double s ) const {
00252 Assert( s > 0.0 );
00253 return Vector3( x / s, y / s, z / s );
00254 }
00255
00256 Vector3 operator*( const double s ) const {
00257 return Vector3( x * s, y * s, z * s );
00258 }
00259
00260
00261 double operator*( const Vector3 &v ) const {
00262 return x * v.x + y * v.y + z * v.z;
00263 }
00264
00265
00266 Vector3 operator^( const Vector3 &v ) const {
00267 return Vector3( y * v.z - z * v.y,
00268 z * v.x - x * v.z,
00269 x * v.y - y * v.x );
00270 }
00271
00272 double length() const {
00273 return (double) sqrt(x * x + y * y + z * z);
00274 }
00275
00276 double lengthSquared() const {
00277 return x * x + y * y + z * z;
00278 }
00279
00280 void normalize() {
00281 double s = 1.0 / (double) sqrt(x * x + y * y + z * z);
00282 x *= s; y *= s; z *= s;
00283 }
00284
00285 bool operator==( const Vector3 &v ) const {
00286 return x == v.x && y == v.y && z == v.z;
00287 }
00288
00289 bool operator!=( const Vector3 &v ) const {
00290 return x != v.x || y != v.y || z != v.z;
00291 }
00292
00293 bool approxEqual( const Vector3 &v, double eps = 1e-12 ) const {
00294 return isZero( x - v.x, eps ) && isZero( y - v.y, eps ) && isZero( z - v.z, eps );
00295 }
00296
00297 void print() const {
00298 std::cout << x << " " << y << " " << z << "\n";
00299 }
00300
00301 private:
00302 double x, y, z;
00303 };
00304
00305 inline Vector3 operator*( const double s, const Vector3 &v ) {
00306 return Vector3( v[0] * s, v[1] * s, v[2] * s );
00307 }
00308
00309 inline double dot( const Vector3 &w, const Vector3 &v ) {
00310 return w * v;
00311 }
00312
00313 inline Vector3 cross( const Vector3 &w, const Vector3 &v ) {
00314 return w ^ v;
00315 }
00316
00317 inline double length( const Vector3 &v ) { return v.length(); }
00318 inline Vector3 unit( const Vector3 &v ) { const double len = v.length(); return v / len; }
00319
00320 inline std::ostream& operator<<(std::ostream& os, const Vector3& v) {
00321 os << "(" << v[0] << ", " << v[1] << ", " << v[2] << ")";
00322 return os;
00323 }
00324
00325 class Point3 {
00326 public:
00327 Point3() : x(0), y(0), z(0) {}
00328 Point3(const Point3& p) : x(p[0]), y(p[1]), z(p[2]) {}
00329 Point3(double _x, double _y, double _z) : x(_x), y(_y), z(_z) {}
00330
00331 Point3& operator=(const Point3& a) {
00332 x = a[0]; y = a[1]; z = a[2];
00333 return *this;
00334 }
00335
00336 const double &operator[](int n) const { return (&x)[n]; }
00337 double &operator[](int n) { return (&x)[n]; }
00338
00339 Point3& operator+=(const Vector3& v) {
00340 x += v[0]; y += v[1]; z += v[2];
00341 return *this;
00342 }
00343
00344 Point3& operator-=(const Vector3& v) {
00345 x -= v[0]; y -= v[1]; z -= v[2];
00346 return *this;
00347 }
00348
00349 Point3& operator*=(double s) {
00350 x *= s; y *= s; z *= s;
00351 return *this;
00352 }
00353
00354 Vector3 operator-(const Point3 & p) const {
00355 return Vector3(x - p.x, y - p.y, z - p.z);
00356 }
00357
00358 Point3 operator+(const Vector3 & v) const {
00359 return Point3(x + v[0], y + v[1], z + v[2]);
00360 }
00361
00362 Point3 operator-(const Vector3 & v) const {
00363 return Point3(x - v[0], y - v[1], z - v[2]);
00364 }
00365
00366 double distanceTo(const Point3& p) const {
00367 return (double) sqrt((p[0] - x) * (p[0] - x) +
00368 (p[1] - y) * (p[1] - y) +
00369 (p[2] - z) * (p[2] - z));
00370 }
00371
00372 double distanceToSquared(const Point3& p) const {
00373 return ((p[0] - x) * (p[0] - x) +
00374 (p[1] - y) * (p[1] - y) +
00375 (p[2] - z) * (p[2] - z));
00376 }
00377
00378 double distanceFromOrigin() const {
00379 return (double) sqrt(x * x + y * y + z * z);
00380 }
00381
00382 double distanceFromOriginSquared() const {
00383 return x * x + y * y + z * z;
00384 }
00385
00386 bool operator==( const Point3 &p ) const {
00387 return x == p.x && y == p.y && z == p.z;
00388 }
00389
00390 bool operator!=( const Point3 &p ) const {
00391 return x != p.x || y != p.y || z != p.z;
00392 }
00393
00394 bool approxEqual( const Point3 &p, double eps = 1e-12 ) const {
00395 return isZero( x - p.x, eps ) && isZero( y - p.y, eps ) && isZero( z - p.z, eps );
00396 }
00397
00398 void print() const {
00399 std::cout << x << " " << y << " " << z << "\n";
00400 }
00401
00402 private:
00403 double x, y, z;
00404 };
00405
00406 inline Point3 lerp( const Point3 &p0, const Point3 &p1, double dT )
00407 {
00408 const double dTMinus = 1.0 - dT;
00409 return Point3( dTMinus * p0[0] + dT * p1[0], dTMinus * p0[1] + dT * p1[1], dTMinus * p0[2] + dT * p1[2] );
00410 }
00411
00412 inline std::ostream& operator<<(std::ostream& os, const Point3& p) {
00413 os << "(" << p[0] << ", " << p[1] << ", " << p[2] << ")";
00414 return os;
00415 }
00416
00417 class Matrix3 {
00418 public:
00419 Matrix3() {
00420 for ( int i = 0; i < 3; i++ )
00421 for ( int j = 0; j < 3; j++ )
00422 mat[ index(i,j) ] = (i == j) ? 1.0 : 0.0;
00423 }
00424
00425 Matrix3(const Vector3& row0, const Vector3& row1, const Vector3& row2) {
00426 for ( int i = 0; i < 3; i++ ) {
00427 mat[ index( 0, i ) ] = row0[i];
00428 mat[ index( 1, i ) ] = row1[i];
00429 mat[ index( 2, i ) ] = row2[i];
00430 }
00431 }
00432
00433 Matrix3(const Matrix3& m) {
00434 (*this) = m;
00435 }
00436
00437 Matrix3& operator=(const Matrix3& m) {
00438 memcpy( &mat[0], &m.mat[0], sizeof(double) * 16 );
00439 return *this;
00440 }
00441
00442
00443 int index( int row, int col ) const { Assert( row >= 0 && row < 3 ); Assert( col >= 0 && col < 3 ); return col * 3 + row; }
00444
00445 const double & operator()( int row, int col ) const { return mat[ index(row,col) ]; }
00446 double & operator()( int row, int col ) { return mat[ index(row,col) ]; }
00447
00448 Vector3 row(int r) const {
00449 return Vector3( mat[index(r,0)], mat[index(r,1)], mat[index(r,2)] );
00450 }
00451
00452 Vector3 column(int c) const {
00453 return Vector3( mat[index(0,c)], mat[index(1,c)], mat[index(2,c)] );
00454 }
00455
00456 Matrix3 transpose() const {
00457 Matrix3 matRet;
00458 for ( int i = 0; i < 3; i++ )
00459 for ( int j = 0; j < 3; j++ )
00460 matRet(i,j) = (*this)(j,i);
00461 return matRet;
00462 }
00463
00464 Matrix3 operator+( const Matrix3 &m) const {
00465 Matrix3 matRet;
00466 for ( int i = 0; i < 9; i++ )
00467 matRet.mat[i] = mat[i] + m.mat[i];
00468 return matRet;
00469 }
00470
00471 Matrix3& operator*=(double s) {
00472 for ( int i = 0; i < 9; i++ )
00473 mat[i] *= s;
00474 return *this;
00475 }
00476
00477
00478 Vector3 operator*(const Vector3& v) const {
00479 return Vector3((*this)(0,0) * v[0] + (*this)(0,1) * v[1] + (*this)(0,2) * v[2],
00480 (*this)(1,0) * v[0] + (*this)(1,1) * v[1] + (*this)(1,2) * v[2],
00481 (*this)(2,0) * v[0] + (*this)(2,1) * v[1] + (*this)(2,2) * v[2]);
00482 }
00483
00484
00485 Point3 operator*(const Point3& p) const {
00486 return Point3((*this)(0,0) * p[0] + (*this)(0,1) * p[1] + (*this)(0,2) * p[2],
00487 (*this)(1,0) * p[0] + (*this)(1,1) * p[1] + (*this)(1,2) * p[2],
00488 (*this)(2,0) * p[0] + (*this)(2,1) * p[1] + (*this)(2,2) * p[2]);
00489 }
00490
00491 Matrix3 operator*( const Matrix3 & m ) const {
00492 Matrix3 matRet;
00493 for ( int i = 0; i < 3; i++ ) {
00494 for ( int j = 0; j < 3; j++ ) {
00495 matRet(i,j) = 0.0;
00496 for ( int k = 0; k < 3; k++ )
00497 matRet(i,j) += (*this)(i,k) * m(k,j);
00498 }
00499 }
00500 return matRet;
00501 }
00502
00503 static Matrix3 identity() {
00504 return Matrix3(Vector3(1, 0, 0),
00505 Vector3(0, 1, 0),
00506 Vector3(0, 0, 1));
00507 }
00508
00509 static Matrix3 rotationXYZtoUVW(Vector3 u, Vector3 v, Vector3 w) {
00510 return Matrix3(Vector3(u[0], v[0], w[0]),
00511 Vector3(u[1], v[1], w[1]),
00512 Vector3(u[2], v[2], w[2]));
00513 }
00514
00515 static double det2x2(double a, double b, double c, double d) {
00516 return a * d - b * c;
00517 }
00518
00519 double determinant() const {
00520 return ((*this)(0,0) * (*this)(1,1) * (*this)(2,2) +
00521 (*this)(0,1) * (*this)(1,2) * (*this)(2,0) +
00522 (*this)(0,2) * (*this)(1,0) * (*this)(2,1) -
00523 (*this)(0,2) * (*this)(1,1) * (*this)(2,0) -
00524 (*this)(0,0) * (*this)(1,2) * (*this)(2,1) -
00525 (*this)(0,1) * (*this)(1,0) * (*this)(2,2));
00526 }
00527
00528 Matrix3 inverse() const {
00529 Matrix3 adjoint = Matrix3( Vector3( det2x2((*this)(1,1), (*this)(1,2), (*this)(2,1), (*this)(2,2)),
00530 -det2x2((*this)(1,0), (*this)(1,2), (*this)(2,0), (*this)(2,2)),
00531 det2x2((*this)(1,0), (*this)(1,1), (*this)(2,0), (*this)(2,1)) ),
00532 Vector3( -det2x2((*this)(0,1), (*this)(0,2), (*this)(2,1), (*this)(2,2)),
00533 det2x2((*this)(0,0), (*this)(0,2), (*this)(2,0), (*this)(2,2)),
00534 -det2x2((*this)(0,0), (*this)(0,1), (*this)(2,0), (*this)(2,1)) ),
00535 Vector3( det2x2((*this)(0,1), (*this)(0,2), (*this)(1,1), (*this)(1,2)),
00536 -det2x2((*this)(0,0), (*this)(0,2), (*this)(1,0), (*this)(1,2)),
00537 det2x2((*this)(0,0), (*this)(0,1), (*this)(1,0), (*this)(1,1)) ) );
00538 const double dDet = determinant();
00539
00540 Assert( isZero( dDet ) == false );
00541 adjoint *= 1.0 / dDet;
00542
00543 return adjoint;
00544 }
00545
00546 bool operator==( const Matrix3 &m ) const {
00547 for ( int i = 0; i < 9; i++ )
00548 if ( mat[i] != m.mat[i] )
00549 return false;
00550 return true;
00551 }
00552
00553 bool approxEqual( const Matrix3 &m, double eps = 1e-12 ) const {
00554 for ( int i = 0; i < 9; i++ )
00555 if ( isZero( mat[i] - m.mat[i], eps ) )
00556 return false;
00557 return true;
00558 }
00559
00560 void print() const {
00561 std::cout << "( " << (*this)(0,0) << ", " << (*this)(0,1) << ", " << (*this)(0,2) << "\n";
00562 std::cout << " " << (*this)(1,0) << ", " << (*this)(1,1) << ", " << (*this)(1,2) << "\n";
00563 std::cout << " " << (*this)(2,0) << ", " << (*this)(2,1) << ", " << (*this)(2,2) << ")\n";
00564 }
00565
00566 private:
00567 double mat[9];
00568 };
00569
00570
00571 inline Vector3 operator*(const Vector3& v, const Matrix3& m) {
00572 return Vector3(m(0,0) * v[0] + m(1,0) * v[1] + m(2,0) * v[2],
00573 m(0,1) * v[0] + m(1,1) * v[1] + m(2,1) * v[2],
00574 m(0,2) * v[0] + m(1,2) * v[1] + m(2,2) * v[2]);
00575 }
00576
00577
00578 inline Point3 operator*(const Point3& p, const Matrix3& m) {
00579 return Point3(m(0,0) * p[0] + m(1,0) * p[1] + m(2,0) * p[2],
00580 m(0,1) * p[0] + m(1,1) * p[1] + m(2,1) * p[2],
00581 m(0,2) * p[0] + m(1,2) * p[1] + m(2,2) * p[2]);
00582 }
00583
00584 inline std::ostream& operator<<(std::ostream& os, const Matrix3& m) {
00585 os << m.row(0) << std::endl;
00586 os << m.row(1) << std::endl;
00587 os << m.row(2) << std::endl;
00588 return os;
00589 }
00590
00591
00592 class Vector4 {
00593 public:
00594 Vector4() : x(0), y(0), z(0), w(0) {}
00595 Vector4(const Vector4& v) : x(v[0]), y(v[1]), z(v[2]), w(v[3]) {}
00596 Vector4(double _x, double _y, double _z, double _w) : x(_x), y(_y), z(_z), w(_w) {}
00597
00598 Vector4& operator=(const Vector4& a) {
00599 x = a[0]; y = a[1]; z = a[2]; w = a[3];
00600 return *this;
00601 }
00602
00603 const double &operator[](int n) const { return ((double *) this)[n]; }
00604 double &operator[](int n) { return ((double *) this)[n]; }
00605
00606 Vector4& operator+=(const Vector4& a) {
00607 x += a[0]; y += a[1]; z += a[2]; w += a[3];
00608 return *this;
00609 }
00610
00611 Vector4& operator-=(const Vector4& a) {
00612 x -= a[0]; y -= a[1]; z -= a[2]; w -= a[3];
00613 return *this;
00614 }
00615
00616 Vector4& operator*=(double s) {
00617 x *= s; y *= s; z *= s; w *= s;
00618 return *this;
00619 }
00620
00621 Vector4 operator-() {
00622 return Vector4(-x, -y, -z, -w);
00623 }
00624
00625 Vector4 operator+() {
00626 return *this;
00627 }
00628
00629 Vector4 operator+( const Vector4 &v ) const {
00630 return Vector4( x + v.x, y + v.y, z + v.z, w + v.w );
00631 }
00632
00633 Vector4 operator-( const Vector4 &v ) const {
00634 return Vector4( x - v.x, y - v.y, z - v.z, w - v.w );
00635 }
00636
00637 Vector4 operator/( const double s ) const {
00638 Assert( s > 0.0 );
00639 return Vector4( x / s, y / s, z / s, w / s );
00640 }
00641
00642 Vector4 operator*( const double s ) const {
00643 return Vector4( x * s, y * s, z * s, w * s );
00644 }
00645
00646
00647 double operator*( const Vector4 &v ) const {
00648 return x * v.x + y * v.y + z * v.z + w * v.w;
00649 }
00650
00651 double length() const {
00652 return (double) sqrt(x * x + y * y + z * z + w * w);
00653 }
00654
00655 double lengthSquared() const {
00656 return x * x + y * y + z * z + w * w;
00657 }
00658
00659 void normalize() {
00660 double s = 1.0 / length();
00661 x *= s; y *= s; z *= s; w *= s;
00662 }
00663
00664 bool operator==( const Vector4 &v ) const {
00665 return x == v.x && y == v.y && z == v.z && w == v.w;
00666 }
00667
00668 bool operator!=( const Vector4 &v ) const {
00669 return x != v.x || y != v.y || z != v.z || w != v.w;
00670 }
00671
00672 bool approxEqual( const Vector4 &v, double eps = 1e-12 ) const {
00673 return isZero( x - v.x, eps ) && isZero( y - v.y, eps ) && isZero( z - v.z, eps ) && isZero( w - v.w, eps );
00674 }
00675
00676 void print() const {
00677 std::cout << x << " " << y << " " << z << " " << w << "\n";
00678 }
00679
00680 private:
00681 double x, y, z, w;
00682 };
00683
00684 inline Vector4 operator*( const double s, const Vector4 &v ) {
00685 return Vector4( v[0] * s, v[1] * s, v[2] * s, v[3] * s );
00686 }
00687
00688 inline double length( const Vector4 &v ) { return v.length(); }
00689 inline Vector4 unit( const Vector4 &v ) { const double len = v.length(); return v / len; }
00690 inline std::ostream& operator<<(std::ostream& os, const Vector4& v) {
00691 os << "(" << v[0] << ", " << v[1] << ", " << v[2] << ", " << v[3] << ")";
00692 return os;
00693 }
00694
00695 class Matrix4 {
00696 public:
00697 Matrix4() {
00698 for ( int i = 0; i < 4; i++ )
00699 for ( int j = 0; j < 4; j++ )
00700 mat[ index(i,j) ] = (i == j) ? 1.0 : 0.0;
00701 }
00702
00703 Matrix4(const Vector4& row0, const Vector4& row1, const Vector4& row2, const Vector4& row3) {
00704 for ( int i = 0; i < 4; i++ ) {
00705 mat[ index( 0, i ) ] = row0[i];
00706 mat[ index( 1, i ) ] = row1[i];
00707 mat[ index( 2, i ) ] = row2[i];
00708 mat[ index( 3, i ) ] = row3[i];
00709 }
00710 }
00711
00712 Matrix4(const Vector3& row0, const Vector3& row1, const Vector3& row2 ) {
00713 for ( int i = 0; i < 3; i++ ) {
00714 mat[ index( 0, i ) ] = row0[i];
00715 mat[ index( 1, i ) ] = row1[i];
00716 mat[ index( 2, i ) ] = row2[i];
00717 mat[ index(i,3) ] = 0.0;
00718 mat[ index(3,i) ] = 0.0;
00719 }
00720 mat[ index(3,3) ] = 1.0;
00721 }
00722
00723 Matrix4(const Matrix4& m) {
00724 (*this) = m;
00725 }
00726
00727 Matrix4& operator=(const Matrix4& m) {
00728 memcpy( &mat[0], &m.mat[0], sizeof(double) * 16 );
00729 return *this;
00730 }
00731
00732
00733 int index( int row, int col ) const { Assert( row >= 0 && row < 4 ); Assert( col >= 0 && col < 4 ); return col * 4 + row; }
00734
00735 const double & operator()( int row, int col ) const { return mat[ index(row,col) ]; }
00736 double & operator()( int row, int col ) { return mat[ index(row,col) ]; }
00737
00738 Vector4 row(int r) const {
00739 return Vector4( mat[index(r,0)], mat[index(r,1)], mat[index(r,2)], mat[index(r,3)] );
00740 }
00741
00742 Vector4 column(int c) const {
00743 return Vector4( mat[index(0,c)], mat[index(1,c)], mat[index(2,c)], mat[index(3,c)] );
00744 }
00745
00746 Matrix4 & operator *=( const Matrix4 &m ) {
00747 const Matrix4 matRet = (*this) * m;
00748 (*this) = matRet;
00749 return *this;
00750 }
00751
00752 Matrix4 & operator +=( const Matrix4 &m ) {
00753 const Matrix4 matRet = (*this) + m;
00754 (*this) = matRet;
00755 return *this;
00756 }
00757
00758 Matrix4 & operator -=( const Matrix4 &m ) {
00759 const Matrix4 matRet = (*this) - m;
00760 (*this) = matRet;
00761 return *this;
00762 }
00763
00764 Matrix4 transpose() const {
00765 Matrix4 matRet;
00766 for ( int i = 0; i < 4; i++ )
00767 for ( int j = 0; j < 4; j++ )
00768 matRet(i,j) = (*this)(j,i);
00769 return matRet;
00770 }
00771
00772 Matrix4 operator+( const Matrix4 &m) const {
00773 Matrix4 matRet;
00774 for ( int i = 0; i < 16; i++ )
00775 matRet.mat[i] = mat[i] + m.mat[i];
00776 return matRet;
00777 }
00778
00779 Matrix4 operator-( const Matrix4 &m) const {
00780 Matrix4 matRet;
00781 for ( int i = 0; i < 16; i++ )
00782 matRet.mat[i] = mat[i] - m.mat[i];
00783 return matRet;
00784 }
00785
00786 Vector3 operator*(const Vector3& v) const {
00787 return Vector3((*this)(0,0) * v[0] + (*this)(0,1) * v[1] + (*this)(0,2) * v[2],
00788 (*this)(1,0) * v[0] + (*this)(1,1) * v[1] + (*this)(1,2) * v[2],
00789 (*this)(2,0) * v[0] + (*this)(2,1) * v[1] + (*this)(2,2) * v[2]);
00790 }
00791
00792 Point3 operator*(const Point3& p) const {
00793 const Point3 pt((*this)(0,0) * p[0] + (*this)(0,1) * p[1] + (*this)(0,2) * p[2] + (*this)(0,3),
00794 (*this)(1,0) * p[0] + (*this)(1,1) * p[1] + (*this)(1,2) * p[2] + (*this)(1,3),
00795 (*this)(2,0) * p[0] + (*this)(2,1) * p[1] + (*this)(2,2) * p[2] + (*this)(2,3));
00796 const double w = (*this)(3,0) * p[0] + (*this)(3,1) * p[1] + (*this)(3,2) * p[2] + (*this)(3,3);
00797 Assert( isZero( w ) == false );
00798 return Point3( pt[0] / w, pt[1] / w, pt[2] / w );
00799 }
00800
00801 Vector4 operator*(const Vector4& v) const {
00802 return Vector4((*this)(0,0) * v[0] + (*this)(0,1) * v[1] + (*this)(0,2) * v[2] + (*this)(0,3) * v[3],
00803 (*this)(1,0) * v[0] + (*this)(1,1) * v[1] + (*this)(1,2) * v[2] + (*this)(1,3) * v[3],
00804 (*this)(2,0) * v[0] + (*this)(2,1) * v[1] + (*this)(2,2) * v[2] + (*this)(2,3) * v[3],
00805 (*this)(3,0) * v[0] + (*this)(3,1) * v[1] + (*this)(3,2) * v[2] + (*this)(3,3) * v[3]);
00806 }
00807
00808 inline Matrix4 operator*(const Matrix4& b) const{
00809 Matrix4 matRet;
00810 for ( int i = 0; i < 4; i++ ) {
00811 for ( int j = 0; j < 4; j++ ) {
00812 matRet(i,j) = 0.0;
00813 for ( int k = 0; k < 4; k++ )
00814 matRet(i,j) += (*this)(i,k) * b(k,j);
00815 }
00816 }
00817 return matRet;
00818 }
00819
00820 Matrix4 inverse() const;
00821
00822 bool operator==( const Matrix4 &m ) const {
00823 for ( int i = 0; i < 16; i++ )
00824 if ( mat[i] != m.mat[i] )
00825 return false;
00826 return true;
00827 }
00828
00829 bool approxEqual( const Matrix4 &m, double eps = 1e-12 ) const {
00830 for ( int i = 0; i < 16; i++ )
00831 if ( isZero( mat[i] - m.mat[i], eps ) )
00832 return false;
00833 return true;
00834 }
00835
00836 void print() const {
00837 std::cout << "( " << (*this)(0,0) << ", " << (*this)(0,1) << ", " << (*this)(0,2) << ", " << (*this)(0,3) << "\n";
00838 std::cout << " " << (*this)(1,0) << ", " << (*this)(1,1) << ", " << (*this)(1,2) << ", " << (*this)(1,3) << "\n";
00839 std::cout << " " << (*this)(2,0) << ", " << (*this)(2,1) << ", " << (*this)(2,2) << ", " << (*this)(2,3) << "\n";
00840 std::cout << " " << (*this)(3,0) << ", " << (*this)(3,1) << ", " << (*this)(3,2) << ", " << (*this)(3,3) << ")\n";
00841 }
00842
00843 static Matrix4 identity() {
00844 return Matrix4(Vector4(1, 0, 0, 0),
00845 Vector4(0, 1, 0, 0),
00846 Vector4(0, 0, 1, 0),
00847 Vector4(0, 0, 0, 1));
00848 }
00849
00850 static Matrix4 translation(const Point3& p) {
00851 return Matrix4(Vector4(1, 0, 0, p[0]),
00852 Vector4(0, 1, 0, p[1]),
00853 Vector4(0, 0, 1, p[2]),
00854 Vector4(0, 0, 0, 1));
00855 }
00856
00857 static Matrix4 translation(const Vector3& v) {
00858 return Matrix4(Vector4(1, 0, 0, v[0]),
00859 Vector4(0, 1, 0, v[1]),
00860 Vector4(0, 0, 1, v[2]),
00861 Vector4(0, 0, 0, 1));
00862 }
00863
00864 static Matrix4 rotation(const Vector3& u, const Vector3& v, const Vector3& w) {
00865 return Matrix4(Vector4(u[0], u[1], u[2], 0),
00866 Vector4(v[0], v[1], v[2], 0),
00867 Vector4(w[0], w[1], w[2], 0),
00868 Vector4(0 , 0 , 0 , 1));
00869 }
00870
00871 static Matrix4 rotation(const Vector3& axis, double angle) {
00872 Vector3 a = axis;
00873 a.normalize();
00874 const double c = cos(angle);
00875 const double s = sin(angle);
00876 const double t = 1 - c;
00877
00878 return Matrix4(Vector4(t * a[0] * a[0] + c,
00879 t * a[0] * a[1] - s * a[2],
00880 t * a[0] * a[2] + s * a[1],
00881 0),
00882 Vector4(t * a[0] * a[1] + s * a[2],
00883 t * a[1] * a[1] + c,
00884 t * a[1] * a[2] - s * a[0],
00885 0),
00886 Vector4(t * a[0] * a[2] - s * a[1],
00887 t * a[1] * a[2] + s * a[0],
00888 t * a[2] * a[2] + c,
00889 0),
00890 Vector4(0, 0, 0, 1));
00891 }
00892
00893 static Matrix4 xrotation(double angle) {
00894 const double c = cos(angle);
00895 const double s = sin(angle);
00896
00897 return Matrix4(Vector4(1, 0, 0, 0),
00898 Vector4(0, c, -s, 0),
00899 Vector4(0, s, c, 0),
00900 Vector4(0, 0, 0, 1));
00901 }
00902
00903 static Matrix4 yrotation(double angle) {
00904 double c = cos(angle);
00905 double s = sin(angle);
00906
00907 return Matrix4(Vector4( c, 0, s, 0),
00908 Vector4( 0, 1, 0, 0),
00909 Vector4(-s, 0, c, 0),
00910 Vector4( 0, 0, 0, 1));
00911 }
00912
00913 static Matrix4 zrotation(double angle) {
00914 const double c = cos(angle);
00915 const double s = sin(angle);
00916
00917 return Matrix4(Vector4(c, -s, 0, 0),
00918 Vector4(s, c, 0, 0),
00919 Vector4(0, 0, 1, 0),
00920 Vector4(0, 0, 0, 1));
00921 }
00922
00923 static Matrix4 scaling(const Vector3& s) {
00924 return Matrix4(Vector4(s[0], 0 , 0 , 0),
00925 Vector4(0 , s[1], 0 , 0),
00926 Vector4(0 , 0 , s[2], 0),
00927 Vector4(0 , 0 , 0 , 1));
00928 }
00929
00930 static Matrix4 scaling( double x, double y, double z) {
00931 return Matrix4(Vector4(x, 0, 0, 0),
00932 Vector4(0, y, 0, 0),
00933 Vector4(0, 0, z, 0),
00934 Vector4(0, 0, 0, 1));
00935 }
00936
00937 static Matrix4 scaling(double scale) {
00938 return scaling(Vector3(scale, scale, scale));
00939 }
00940
00941 private:
00942 double mat[16];
00943 };
00944
00945
00946
00947
00948
00949
00950
00951
00952 inline std::ostream& operator<<(std::ostream& os, const Matrix4& m) {
00953 os << m.row(0) << std::endl;
00954 os << m.row(1) << std::endl;
00955 os << m.row(2) << std::endl;
00956 os << m.row(3) << std::endl;
00957 return os;
00958 }
00959
00960 inline Matrix4 Matrix4::inverse() const {
00961
00962
00963 Matrix4 a(*this);
00964 Matrix4 b(Matrix4::identity());
00965 int i, j;
00966 int p;
00967
00968 for (j = 0; j < 4; j++) {
00969 p = j;
00970 for (i = j + 1; i < 4; i++) {
00971 if (fabs(a(i,j)) > fabs(a(p,j)))
00972 p = i;
00973 }
00974
00975 if ( p != j ) {
00976 for ( i = 0; i < 4; i++ ) {
00977 const double ta = a(p,i);
00978 a(p,i) = a(j,i);
00979 a(j,i) = ta;
00980
00981 const double tb = b(p,i);
00982 b(p,i) = b(j,i);
00983 b(j,i) = tb;
00984 }
00985 }
00986
00987 const double s = a(j,j);
00988 Assert( isZero( s ) == false );
00989 for ( i = 0; i < 4; i++ ) {
00990 a(j,i) *= (1.0 / s);
00991 b(j,i) *= (1.0 / s);
00992 }
00993
00994 for (i = 0; i < 4; i++) {
00995 if (i != j) {
00996 for ( int k = 0; k < 4; k++ ) {
00997 b(i,k) -= a(i,j) * b(j,k);
00998 a(i,k) -= a(i,j) * a(j,k);
00999 }
01000 }
01001 }
01002 }
01003 return b;
01004 }
01005
01006 }
01007
01008 #endif