EMAN2
vecmath.h
Go to the documentation of this file.
00001 
00005 /*
00006  * Author: Tao Ju, 5/16/2007 <taoju@cs.wustl.edu>, code ported by Grant Tang
00007  * Copyright (c) 2000-2006 Baylor College of Medicine
00008  * 
00009  * This software is issued under a joint BSD/GNU license. You may use the
00010  * source code in this file under either license. However, note that the
00011  * complete EMAN2 and SPARX software packages have some GPL dependencies,
00012  * so you are responsible for compliance with the licenses of these packages
00013  * if you opt to use BSD licensing. The warranty disclaimer below holds
00014  * in either instance.
00015  * 
00016  * This complete copyright notice must be included in any revised version of the
00017  * source code. Additional authorship citations may be added, but existing
00018  * author citations must be preserved.
00019  * 
00020  * This program is free software; you can redistribute it and/or modify
00021  * it under the terms of the GNU General Public License as published by
00022  * the Free Software Foundation; either version 2 of the License, or
00023  * (at your option) any later version.
00024  * 
00025  * This program is distributed in the hope that it will be useful,
00026  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00027  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the
00028  * GNU General Public License for more details.
00029  * 
00030  * You should have received a copy of the GNU General Public License
00031  * along with this program; if not, write to the Free Software
00032  * Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA 02111-1307 USA
00033  * 
00034  * */
00035 
00036 #ifndef _VECMATH_H_
00037 #define _VECMATH_H_
00038 
00039 //#include "cse452.h"
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         // Dot product, subtraction, ofstream, etc., are declared after class
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             // Dot
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             // Dot
00261             double operator*( const Vector3 &v ) const {
00262                 return x * v.x + y * v.y + z * v.z;
00263             }
00264           
00265             // Cross product
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             // The indexing is this way to match OpenGL
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             // pre-multiply column vector by a 3x3 matrix
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             // pre-multiply column point by a 3x3 matrix
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         // post-multiply row vector by a 3x3 matrix
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         // post-multiply row point by a 3x3 matrix
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             // Dot
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             // The indexing is this way to match OpenGL
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         // **** Matrix4 operators ****
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             // Compute inverse using Gauss-Jordan elimination; caller is responsible
00962             // for ensuring that the matrix isn't singular.
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                 // Swap rows p and j
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);  // if s == 0, the matrix is singular
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                 // Eliminate off-diagonal elements
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 /* _VECMATH_H_ */