sglMat3.hpp Source File

00001 /*****************************************************************************
00002  * SGL: A Scene Graph Library
00003  *
00004  * Copyright (C) 1997-2001  Scott McMillan   All Rights Reserved.
00005  *
00006  * This library is free software; you can redistribute it and/or
00007  * modify it under the terms of the GNU Library General Public
00008  * License as published by the Free Software Foundation; either
00009  * version 2 of the License, or (at your option) any later version.
00010  *
00011  * This library is distributed in the hope that it will be useful,
00012  * but WITHOUT ANY WARRANTY; without even the implied warranty of
00013  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
00014  * Library General Public License for more details.
00015  *
00016  * You should have received a copy of the GNU Library General Public
00017  * License along with this library; if not, write to the Free
00018  * Software Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
00019  *****************************************************************************
00020  *     File: sglMat3.hpp
00021  *  Created: 28 February 1999
00022  *  Summary: 3x3 templated matrix class
00023  *****************************************************************************/
00024 
00025 #ifndef __SGL_MAT3_HPP
00026 #define __SGL_MAT3_HPP
00027 
00028 #include <sgl.h>
00029 #include <string.h> // memcpy
00030 #include <sglVector.hpp>
00031 #include <sglVec2.hpp>
00032 #include <sglVec3.hpp>
00033 
00034 // ---------------------------------------------------------------------------
00035 template <class T>
00036 class sglMat3
00037 {
00038 public:
00039    // constructors
00040    sglMat3() {};
00041 
00042    sglMat3(T m00, T m01, T m02, T m10, T m11, T m12, T m20, T m21, T m22)
00043       {
00044          set(m00, m01, m02, m10, m11, m12, m20, m21, m22);
00045       }
00046 
00047    template <class S>
00048    sglMat3(const sglMat3<S>& mat)
00049       {
00050          set(mat[0][0], mat[0][1], mat[0][2],
00051              mat[1][0], mat[1][1], mat[1][2],
00052              mat[2][0], mat[2][1], mat[2][2]);
00053       }
00054 
00055    template <class S>
00056    sglMat3(S mat[3][3])
00057       {
00058          set(mat[0][0], mat[0][1], mat[0][2],
00059              mat[1][0], mat[1][1], mat[1][2],
00060              mat[2][0], mat[2][1], mat[2][2]);
00061       }
00062 
00063    // destructor
00064    ~sglMat3() {};  // not virtual, do not subclass
00065 
00066    inline T *getPtr() const { return (T *)m_matrix; }
00067 
00068    // indexing operators
00069    inline sglVec3<T>& operator[](unsigned int i)
00070       {
00071          return *(sglVec3<T> *) m_matrix[i];
00072       }
00073    inline const sglVec3<T>& operator[](unsigned int i) const
00074       {
00075          return *(sglVec3<T> *) m_matrix[i];
00076       }
00077 
00078    // set/get
00079    template <class S>
00080    void set(S m00, S m01, S m02, S m10, S m11, S m12, S m20, S m21, S m22)
00081       {
00082          m_matrix[0][0] = (T)m00; m_matrix[0][1] = (T)m01;
00083          m_matrix[0][2] = (T)m02;
00084          m_matrix[1][0] = (T)m10; m_matrix[1][1] = (T)m11;
00085          m_matrix[1][2] = (T)m12;
00086          m_matrix[2][0] = (T)m20; m_matrix[2][1] = (T)m21;
00087          m_matrix[2][2] = (T)m22;
00088       }
00089 
00090    template <class S>
00091    inline void setRow(const sglVec3<S>& vec, unsigned int i)
00092       {
00093          m_matrix[i][0] = (T)vec[0]; m_matrix[i][1] = (T)vec[1];
00094          m_matrix[i][2] = (T)vec[2];
00095       }
00096 
00097    template <class S>
00098    inline void setCol(const sglVec3<S>& vec, unsigned int i)
00099       {
00100          m_matrix[0][i] = (T)vec[0]; m_matrix[1][i] = (T)vec[1];
00101          m_matrix[2][i] = (T)vec[2];
00102       }
00103 
00104    template <class S>
00105    inline void getRow(sglVec3<S>& vec, unsigned int i)
00106       {
00107          vec[0] = (S)m_matrix[i][0]; vec[1] = (S)m_matrix[i][1];
00108          vec[2] = (S)m_matrix[i][2];
00109       }
00110 
00111    template <class S>
00112    inline void getCol(sglVec3<S>& vec, unsigned int i)
00113       {
00114          vec[0] = (S)m_matrix[0][i]; vec[1] = (S)m_matrix[1][i];
00115          vec[2] = (S)m_matrix[2][i];
00116       }
00117 
00118    // assignment operators
00119    template <class S>
00120    inline sglMat3& operator=(const sglMat3<S>& mat)
00121       {
00122          set(mat[0][0], mat[0][1], mat[0][2],
00123              mat[1][0], mat[1][1], mat[1][2],
00124              mat[2][0], mat[2][1], mat[2][2]);
00125          return *this;
00126       }
00127 
00128    // boolean operators
00129    template <class S>
00130    inline bool almostEqual(const sglMat3<S>& mat, double eps) const
00131       {
00132          return ((*this)[0].almostEqual(mat[0], eps) &&
00133                  (*this)[1].almostEqual(mat[1], eps) &&
00134                  (*this)[2].almostEqual(mat[2], eps));
00135       }
00136 
00137    template <class S>
00138    inline bool operator==(const sglMat3<S>& mat) const
00139       {
00140          return ((*this)[0] == mat[0] &&
00141                  (*this)[1] == mat[1] &&
00142                  (*this)[2] == mat[2]);
00143       }
00144 
00145    template <class S>
00146    inline bool operator!=(const sglMat3<S>& mat) const
00147       {
00148          return ((*this)[0] != mat[0] ||
00149                  (*this)[1] != mat[1] ||
00150                  (*this)[2] != mat[2]);
00151       }
00152 
00153    // math operators
00154    template <class S>
00155    inline sglMat3& operator+=(const sglMat3<S>& mat)
00156       {
00157          (*this)[0] += mat[0]; (*this)[1] += mat[1]; (*this)[2] += mat[2];
00158          return *this;
00159       }
00160    template <class S>
00161    inline sglMat3& operator-=(const sglMat3<S>& mat)
00162       {
00163          (*this)[0] -= mat[0]; (*this)[1] -= mat[1]; (*this)[2] -= mat[2];
00164          return *this;
00165       }
00166    template <class S>
00167    inline sglMat3& operator*=(const sglMat3<S>& mat)
00168       {
00169          mul(*this, mat); return *this;
00170       }
00171    inline sglMat3& operator*=(const T s)
00172       {
00173          scale(*this, s, s, s); return *this;
00174       }
00175 
00176    template <class S>
00177    inline const sglMat3<T> operator+(const sglMat3<S> &rhs) const
00178       {
00179          return sglMat3(m_matrix[0][0] + rhs[0][0],
00180                         m_matrix[0][1] + rhs[0][1],
00181                         m_matrix[0][2] + rhs[0][2],
00182                         m_matrix[1][0] + rhs[1][0],
00183                         m_matrix[1][1] + rhs[1][1],
00184                         m_matrix[1][2] + rhs[1][2],
00185                         m_matrix[2][0] + rhs[2][0],
00186                         m_matrix[2][1] + rhs[2][1],
00187                         m_matrix[2][2] + rhs[2][2]);
00188       }
00189    template <class S>
00190    inline const sglMat3<T> operator-(const sglMat3<S> &rhs) const
00191       {
00192          return sglMat3(m_matrix[0][0] - rhs[0][0],
00193                         m_matrix[0][1] - rhs[0][1],
00194                         m_matrix[0][2] - rhs[0][2],
00195                         m_matrix[1][0] - rhs[1][0],
00196                         m_matrix[1][1] - rhs[1][1],
00197                         m_matrix[1][2] - rhs[1][2],
00198                         m_matrix[2][0] - rhs[2][0],
00199                         m_matrix[2][1] - rhs[2][1],
00200                         m_matrix[2][2] - rhs[2][2]);
00201       }
00202    template <class S>
00203    inline const sglMat3<T> operator*(const sglMat3<S> &rhs) const
00204       {
00205          return sglMat3(m_matrix[0][0]*rhs[0][0] +
00206                         m_matrix[0][1]*rhs[1][0] +
00207                         m_matrix[0][2]*rhs[2][0],
00208                         m_matrix[0][0]*rhs[0][1] +
00209                         m_matrix[0][1]*rhs[1][1] +
00210                         m_matrix[0][2]*rhs[2][1],
00211                         m_matrix[0][0]*rhs[0][2] +
00212                         m_matrix[0][1]*rhs[1][2] +
00213                         m_matrix[0][2]*rhs[2][2],
00214                         m_matrix[1][0]*rhs[0][0] +
00215                         m_matrix[1][1]*rhs[1][0] +
00216                         m_matrix[1][2]*rhs[2][0],
00217                         m_matrix[1][0]*rhs[0][1] +
00218                         m_matrix[1][1]*rhs[1][1] +
00219                         m_matrix[1][2]*rhs[2][1],
00220                         m_matrix[1][0]*rhs[0][2] +
00221                         m_matrix[1][1]*rhs[1][2] +
00222                         m_matrix[1][2]*rhs[2][2],
00223                         m_matrix[2][0]*rhs[0][0] +
00224                         m_matrix[2][1]*rhs[1][0] +
00225                         m_matrix[2][2]*rhs[2][0],
00226                         m_matrix[2][0]*rhs[0][1] +
00227                         m_matrix[2][1]*rhs[1][1] +
00228                         m_matrix[2][2]*rhs[2][1],
00229                         m_matrix[2][0]*rhs[0][2] +
00230                         m_matrix[2][1]*rhs[1][2] +
00231                         m_matrix[2][2]*rhs[2][2]);
00232       }
00233    inline const sglMat3<T> operator*(T scalar) const
00234       {
00235          return sglMat3(m_matrix[0][0]*scalar,
00236                         m_matrix[0][1]*scalar,
00237                         m_matrix[0][2]*scalar,
00238                         m_matrix[1][0]*scalar,
00239                         m_matrix[1][1]*scalar,
00240                         m_matrix[1][2]*scalar,
00241                         m_matrix[2][0]*scalar,
00242                         m_matrix[2][1]*scalar,
00243                         m_matrix[2][2]*scalar);
00244       }
00245 
00246    // explicit math operations (for backward compatibility)
00247    template <class S1, class S2>
00248    inline void add(const sglMat3<S1>& mat1, const sglMat3<S2>& mat2)
00249       {
00250          (*this)[0].add(mat1[0],mat2[0]);
00251          (*this)[1].add(mat1[1],mat2[1]);
00252          (*this)[2].add(mat1[2],mat2[2]);
00253       }
00254    template <class S1, class S2>
00255    inline void sub(const sglMat3<S1>& mat1, const sglMat3<S2>& mat2)
00256       {
00257          (*this)[0].sub(mat1[0],mat2[0]);
00258          (*this)[1].sub(mat1[1],mat2[1]);
00259          (*this)[2].sub(mat1[2],mat2[2]);
00260       }
00261    template <class S>
00262    inline void scale(const sglMat3<S>& mat, T sx, T sy, T sz)
00263       {
00264          (*this)[0].scale(mat[0],sx);
00265          (*this)[1].scale(mat[1],sy);
00266          (*this)[2].scale(mat[2],sz);
00267       }
00268    // matrix multiply
00269    template <class S1, class S2>
00270    void mul(const sglMat3<S1>& mat1, const sglMat3<S2>& mat2)
00271       {
00272          T t[3][3];
00273          for (unsigned int i=0; i<3; i++)
00274             for (unsigned int j=0; j<3; j++)
00275                t[i][j] = (mat1[i][0]*mat2[0][j] +
00276                           mat1[i][1]*mat2[1][j] +
00277                           mat1[i][2]*mat2[2][j]);
00278          memcpy(m_matrix, t, sizeof(m_matrix));
00279       }
00280 
00281    // methods to build a matrix
00282    inline void buildIdentity()
00283       {
00284          set(1, 0, 0,  0, 1, 0,  0, 0, 1);
00285       }
00286 
00287    inline void buildScale(T sx, T sy, T sz)
00288       {
00289          m_matrix[0][0] = sx; m_matrix[0][1] = 0;  m_matrix[0][2] = 0;
00290          m_matrix[1][0] = 0;  m_matrix[1][1] = sy; m_matrix[1][2] = 0;
00291          m_matrix[2][0] = 0;  m_matrix[2][1] = 0;  m_matrix[2][2] = sz;
00292       }
00293 
00294    void buildRotation(float x, float y, float z, double angle)  // radians
00295       {
00296          double mag = x*x + y*y + z*z;
00297 
00298          if ((angle == 0.0) || (mag == 0.0))
00299          {
00300             buildIdentity();
00301             return;
00302          }
00303 
00304          mag = sqrt(mag);
00305          x /= mag;  y /= mag;  z /= mag;
00306 
00307          double s = sin(angle);
00308          double c = cos(angle);
00309          double a = 1.0 - c;
00310 
00311          double xx = x*x, yy = y*y, zz = z*z;
00312          double xy = x*y, yz = y*z, zx = z*x;
00313          double sx = x*s, sy = y*s, sz = z*s;
00314 
00315          m_matrix[0][0] = (T)(a*xx + c);
00316          m_matrix[0][1] = (T)(a*xy + sz);
00317          m_matrix[0][2] = (T)(a*zx - sy);
00318 
00319          m_matrix[1][0] = (T)(a*xy - sz);
00320          m_matrix[1][1] = (T)(a*yy + c);
00321          m_matrix[1][2] = (T)(a*yz + sx);
00322 
00323          m_matrix[2][0] = (T)(a*zx + sy);
00324          m_matrix[2][1] = (T)(a*yz - sx);
00325          m_matrix[2][2] = (T)(a*zz + c);
00326       }
00327 
00333    void buildRotation(double h, double p, double r)  // radians
00334       {
00335          T sr = sin( p );
00336          T cr = cos( p );
00337 
00338          T sh = sin( r );
00339          T ch = cos( r );
00340 
00341          T sp = sin( h );
00342          T cp = cos( h );
00343 
00344          m_matrix[0][0] = -sp*sr*sh + cp*ch;
00345          m_matrix[0][1] =  cp*sr*sh + sp*ch;
00346          m_matrix[0][2] = -cr*sh;
00347 
00348          m_matrix[1][0] = -sp*cr;
00349          m_matrix[1][1] =  cp*cr;
00350          m_matrix[1][2] =  sr;
00351 
00352          m_matrix[2][0] =  sp*sr*ch + cp*sh;
00353          m_matrix[2][1] = -cp*sr*ch + sp*sh;
00354          m_matrix[2][2] =  cr*ch;
00355       }
00356 
00357 
00358    // other operations
00359    inline void transpose(const sglMat3& mat)
00360       {
00361          // diagonal first
00362          m_matrix[0][0] = mat[0][0];
00363          m_matrix[1][1] = mat[1][1];
00364          m_matrix[2][2] = mat[2][2];
00365          // then upper triangle
00366          T tmp = mat[0][1]; m_matrix[0][1] = mat[1][0]; m_matrix[1][0] = tmp;
00367            tmp = mat[0][2]; m_matrix[0][2] = mat[2][0]; m_matrix[2][0] = tmp;
00368            tmp = mat[1][2]; m_matrix[1][2] = mat[2][1]; m_matrix[2][1] = tmp;
00369       }
00370 
00371    // cofactors, determinants, inverse
00372    inline T cofactor(unsigned int r,unsigned int c) const
00373       {
00374          const unsigned int index[3][2] = { {1,2}, {0,2}, {0,1} };
00375          unsigned int i0 = index[r][0], i1 = index[r][1];
00376          unsigned int j0 = index[c][0], j1 = index[c][1];
00377          return (((r+c)&1) ? -1: 1)*(m_matrix[i0][j0]*m_matrix[i1][j1] -
00378                                      m_matrix[i0][j1]*m_matrix[i1][j0]);
00379       }
00380 
00381    inline T det() const
00382       {
00383          return (m_matrix[0][0] * cofactor(0,0) +
00384                  m_matrix[0][1] * cofactor(0,1) +
00385                  m_matrix[0][2] * cofactor(0,2));
00386       }
00387 
00388    template <class S>
00389    void adjoint(const sglMat3<S>& mat)
00390       {
00391          T t[3][3];
00392          t[0][0] = mat.cofactor(0,0);
00393          t[0][1] = mat.cofactor(1,0);
00394          t[0][2] = mat.cofactor(2,0);
00395          t[1][0] = mat.cofactor(0,1);
00396          t[1][1] = mat.cofactor(1,1);
00397          t[1][2] = mat.cofactor(2,1);
00398          t[2][0] = mat.cofactor(0,2);
00399          t[2][1] = mat.cofactor(1,2);
00400          t[2][2] = mat.cofactor(2,2);
00401          set(t[0][0], t[0][1], t[0][2],
00402              t[1][0], t[1][1], t[1][2],
00403              t[2][0], t[2][1], t[2][2]);  // memcpy does not work here
00404       }
00405 
00406    // try a adjoint divided by deteminant
00407    template <class S>
00408    inline bool inverse(const sglMat3<S>& mat)
00409       {
00410          T d = mat.det();
00411          if (SGL_ABS(d) < FLT_MIN) return false;
00412          adjoint(mat);
00413          *this *= (T)(1.0/d);
00414          return true;
00415       }
00416 
00417    // stream operators
00418 #if defined(TEMPLATE_OSTREAM_HACK)
00419    template <class S>
00420    friend ostream& operator<<(ostream& ostrm, const sglMat3<S>& mat);
00421 
00422    template <class S>
00423    friend istream& operator>>(istream& istrm, sglMat3<S>& mat);
00424 #else
00425    friend ostream& operator<<(ostream& ostrm, const sglMat3<T>& mat)
00426         {
00427             ostrm << '(';
00428             for (unsigned int i=0; i<3; i++)
00429                 ostrm << '(' << mat[i][0] << ',' << mat[i][1] << ','
00430                       << mat[i][2] << ')';
00431             return (ostrm << ')');
00432         }
00433 
00434    friend istream& operator>>(istream& istrm, sglMat3<T>& mat)
00435         {
00436             for (unsigned int i=0;i<3;i++)
00437                 for (unsigned int j=0;j<3;j++)
00438                     istrm >> mat[i][j];
00439             return istrm;
00440         }
00441 #endif
00442 
00443 private:
00444    //sglVec3<T> m_matrix[3];
00445    T m_matrix[3][3];
00446 };
00447 
00448 //----------------------------------------------------------------------------
00449 #if defined(TEMPLATE_OSTREAM_HACK)
00450 template <class T>
00451 inline ostream& operator<<(ostream& ostrm, const sglMat3<T>& mat)
00452 {
00453     return (ostrm << '(' << mat[0] << ", " << mat[1]
00454             << ", " << mat[2] << ')');
00455 }
00456 
00457 template <class T>
00458 inline istream& operator>>(istream& istrm, sglMat3<T>& mat)
00459 {
00460     for (unsigned int i=0;i<3;i++)
00461         for (unsigned int j=0;j<3;j++)
00462             istrm >> mat[i][j];
00463     return istrm;
00464 }
00465 #endif
00466 //----------------------------------------------------------------------------
00467 
00468 //----------------------------------------------------------------------------
00469 // vector/matrix3x3 operations
00470 //----------------------------------------------------------------------------
00471 
00472 // ---------------------------------------------------------------------------
00473 // this' = this' * m (or this = m' * this)
00474 // ---------------------------------------------------------------------------
00475 template <class S, class T>
00476 inline void mul(sglVec3<T>& vec, const sglMat3<S>& mat)
00477 {
00478    T x    = (T)(vec[0]*mat[0][0] + vec[1]*mat[1][0] + vec[2]*mat[2][0]);
00479    T y    = (T)(vec[0]*mat[0][1] + vec[1]*mat[1][1] + vec[2]*mat[2][1]);
00480    vec[2] = (T)(vec[0]*mat[0][2] + vec[1]*mat[1][2] + vec[2]*mat[2][2]);
00481    vec[0] = x; vec[1] = y;
00482 }
00483 
00484 // ---------------------------------------------------------------------------
00485 //  dest' = v' * m (or dest = m' * v)
00486 // ---------------------------------------------------------------------------
00487 template <class R, class S, class T>
00488 inline void mul(const sglVec3<R> &vec, const sglMat3<S>& mat,
00489                 sglVec3<T>& dest)
00490 {
00491    T x     = (T)(vec[0]*mat[0][0] + vec[1]*mat[1][0] + vec[2]*mat[2][0]);
00492    T y     = (T)(vec[0]*mat[0][1] + vec[1]*mat[1][1] + vec[2]*mat[2][1]);
00493    dest[2] = (T)(vec[0]*mat[0][2] + vec[1]*mat[1][2] + vec[2]*mat[2][2]);
00494    dest[0] = x; dest[1] = y;
00495 }
00496 
00497 // ---------------------------------------------------------------------------
00498 //  dest' = [v[0] v[1] z] * m (or dest = m' * v)
00499 // ---------------------------------------------------------------------------
00500 template <class R, class S, class T>
00501 inline void mul(const sglVec2<R>& vec, R z, const sglMat3<S>& mat,
00502                 sglVec3<T>& dest)
00503 {
00504    dest[0] = (T)(vec[0]*mat[0][0] + vec[1]*mat[1][0] + z*mat[2][0]);
00505    dest[1] = (T)(vec[0]*mat[0][1] + vec[1]*mat[1][1] + z*mat[2][1]);
00506    dest[2] = (T)(vec[0]*mat[0][2] + vec[1]*mat[1][2] + z*mat[2][2]);
00507 }
00508 
00509 // ---------------------------------------------------------------------------
00510 // v' = v' * m' (or v = m * v)
00511 // ---------------------------------------------------------------------------
00512 template <class S, class T>
00513 inline void mulTranspose(sglVec3<T>& vec, const sglMat3<S>& mat)
00514 {
00515    T x    = (T)(vec[0]*mat[0][0] + vec[1]*mat[0][1] + vec[2]*mat[0][2]);
00516    T y    = (T)(vec[0]*mat[1][0] + vec[1]*mat[1][1] + vec[2]*mat[1][2]);
00517    vec[2] = (T)(vec[0]*mat[2][0] + vec[1]*mat[2][1] + vec[2]*mat[2][2]);
00518    vec[0] = x; vec[1] = y;
00519 }
00520 
00521 // ---------------------------------------------------------------------------
00522 //  dest' = v' * m' (or dest = m * v)
00523 // ---------------------------------------------------------------------------
00524 template <class R, class S, class T>
00525 inline void mulTranspose(const sglVec3<R> &vec, const sglMat3<S>& mat,
00526                          sglVec3<T> &dest)
00527 {
00528    T x     = (T)(vec[0]*mat[0][0] + vec[1]*mat[0][1] + vec[2]*mat[0][2]);
00529    T y     = (T)(vec[0]*mat[1][0] + vec[1]*mat[1][1] + vec[2]*mat[1][2]);
00530    dest[2] = (T)(vec[0]*mat[2][0] + vec[1]*mat[2][1] + vec[2]*mat[2][2]);
00531    dest[0] = x; dest[1] = y;
00532 }
00533 
00534 // ---------------------------------------------------------------------------
00535 //  dest' = v' * m' (or dest = m * v)
00536 // ---------------------------------------------------------------------------
00537 template <class R, class S, class T>
00538 inline void mulTranspose(const sglVec2<R> &vec, R z, const sglMat3<S>& mat,
00539                          sglVec3<T>& dest)
00540 {
00541    dest[0] = (T)(vec[0]*mat[0][0] + vec[1]*mat[0][1] + z*mat[0][2]);
00542    dest[1] = (T)(vec[0]*mat[1][0] + vec[1]*mat[1][1] + z*mat[1][2]);
00543    dest[2] = (T)(vec[0]*mat[2][0] + vec[1]*mat[2][1] + z*mat[2][2]);
00544 }
00545 
00546 #endif