vmath.h

Go to the documentation of this file.
00001 /* -*- C++ -*- */
00002 /*
00003  * vmath, set of classes for computer graphics mathemtics.
00004  * Copyright (c) 2005-2007, Jan Bartipan < barzto at gmail dot com >
00005  * All rights reserved.
00006  *
00007  * Redistribution and use in source and binary forms, with or without 
00008  * modification, are permitted provided that the following conditions 
00009  * are met:
00010  *
00011  * - Redistributions of source code must retain the above copyright 
00012  *   notice, this list of conditions and the following disclaimer.
00013  * - Redistributions in binary form must reproduce the above copyright 
00014  *   notice, this list of conditions and the following disclaimer in 
00015  *   the documentation and/or other materials provided with the 
00016  *   distribution.
00017  * - Neither the names of its contributors may be used to endorse or 
00018  *   promote products derived from this software without specific 
00019  *   prior written permission.
00020  * 
00021  * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
00022  * "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
00023  * LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
00024  * FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
00025  * COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
00026  * INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
00027  * BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; 
00028  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 
00029  * CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 
00030  * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY
00031  * WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 
00032  * POSSIBILITY OF SUCH DAMAGE.
00033  */
00034 
00068 #ifndef __vmath_Header_File__
00069 #define __vmath_Header_File__
00070 
00071 #include <cmath>
00072 #include <cstring>
00073 #include <iostream>
00074 #include <cassert>
00075 
00076 
00077 
00078 #ifdef VMATH_NAMESPACE
00079 namespace VMATH_NAMESPACE
00080 {
00081 #endif
00082 
00083 #ifndef M_PI
00084 #define M_PI           3.14159265358979323846  /* pi */
00085 #endif
00086    
00087 #define DEG2RAD(x) ((x * M_PI) / 180.0)
00088    //#define EPSILON (4.37114e-07)
00089    
00090    const double epsilon = 4.37114e-05;
00091 #define EPSILON epsilon
00092    
00093    
00097    template <class T>
00098    class Vector2 
00099    {
00100       public:
00101          union 
00102          {
00106                T x;
00107                 
00112                T s; 
00113          };
00114         
00115          union 
00116          {
00120                T y; 
00121                 
00126                T t; 
00127          };
00128 
00129          //----------------[ constructors ]--------------------------
00133          Vector2() : x(0),y(0)
00134          {      }
00135         
00136         
00137 
00143          Vector2(T nx, T ny) : x(nx), y(ny)
00144          {      }
00145 
00146 
00151          Vector2(const Vector2<T>& src)
00152             : x(src.x), y(src.y) 
00153          {       }
00154 
00155 
00160          template <class FromT>
00161          Vector2(const Vector2<FromT>& src)
00162             : x(static_cast<T>(src.x)),
00163               y(static_cast<T>(src.y))
00164          {       }
00165         
00166         
00167          //----------------[ access operators ]-------------------
00172          template <class FromT>
00173          Vector2<T>& operator=(const Vector2<FromT>& rhs)
00174          {
00175             x = static_cast<T>(rhs.x);
00176             y = static_cast<T>(rhs.y);
00177             return * this;
00178          }
00179 
00184          Vector2<T>& operator=(const Vector2<T>& rhs)
00185          {
00186             x = rhs.x;
00187             y = rhs.y;
00188             return * this;
00189          }
00190 
00191 
00198          T& operator[](int n)
00199          {
00200             assert(n >= 0 && n <= 1);
00201             if (0 == n) return x;
00202             else
00203                return y;
00204          }
00205         
00206         
00207          //---------------[ vector aritmetic operator ]--------------
00212          Vector2<T> operator+(const Vector2<T>& rhs) const
00213          {
00214             return Vector2<T> (x + rhs.x, y + rhs.y);
00215          }
00216 
00221          Vector2<T> operator-(const Vector2<T>& rhs) const
00222          {
00223             return Vector2<T> (x - rhs.x, y - rhs.y);
00224          }
00225 
00230          Vector2<T> operator*(const Vector2<T>& rhs) const
00231          {
00232             return Vector2<T> (x * rhs.x, y * rhs.y);
00233          }
00234 
00239          Vector2<T> operator/(const Vector2<T>& rhs) const 
00240          {
00241             return Vector2<T> (x / rhs.x, y / rhs.y);
00242          }
00243 
00248          Vector2<T>& operator+=(const Vector2<T>& rhs)
00249          {
00250             x += rhs.x;
00251             y += rhs.y;
00252             return * this;
00253          }
00254 
00259          Vector2<T>& operator-=(const Vector2<T>& rhs)
00260          {
00261             x -= rhs.x;
00262             y -= rhs.y;
00263             return * this;
00264          }
00265         
00270          Vector2<T>& operator*=(const Vector2<T>& rhs)
00271          {
00272             x *= rhs.x;
00273             y *= rhs.y;
00274             return * this;
00275          }
00276         
00281          Vector2<T>& operator/=(const Vector2<T>& rhs)
00282          {
00283             x /= rhs.x;
00284             y /= rhs.y;
00285             return * this;
00286          }
00287 
00288          //--------------[ scalar vector operator ]--------------------
00293          Vector2<T> operator+(T rhs) const
00294          {
00295             return Vector2<T> (x + rhs, y + rhs);
00296          }
00297         
00302          Vector2<T> operator-(T rhs) const
00303          {
00304             return Vector2<T> (x - rhs, y - rhs);
00305          }
00306         
00311          Vector2<T> operator*(T rhs) const
00312          {
00313             return Vector2<T> (x * rhs, y * rhs);
00314          }
00315         
00320          Vector2<T> operator/(T rhs) const
00321          {
00322             return Vector2<T> (x / rhs, y / rhs);
00323          }
00324 
00329          Vector2<T>& operator+=(T rhs)
00330          {
00331             x += rhs;
00332             y += rhs;
00333             return * this;
00334          }
00335 
00340          Vector2<T>& operator-=(T rhs) 
00341          {
00342             x -= rhs;
00343             y -= rhs;
00344             return * this;
00345          }
00346 
00351          Vector2<T>& operator*=(T rhs)
00352          {
00353             x *= rhs;
00354             y *= rhs;
00355             return * this;
00356          }
00357 
00362          Vector2<T>& operator/=(T rhs) 
00363          {
00364             x /= rhs;
00365             y /= rhs;
00366             return * this;
00367          }
00368 
00369          //--------------[ equality operator ]------------------------
00377          bool operator==(const Vector2<T>& rhs) const 
00378          {
00379             return (std::abs(x - rhs.x) < EPSILON) && (std::abs(y - rhs.y) < EPSILON);
00380          }
00381 
00382         
00388          bool operator!=(const Vector2<T>& rhs) const { return ! (*this == rhs); }
00389 
00390          //-------------[ unary operations ]--------------------------
00395          Vector2<T> operator-() const
00396          {
00397             return Vector2<T>(-x, -y);
00398          }
00399 
00400          //-------------[ size operations ]---------------------------
00405          T length() const
00406          {
00407             return (T)std::sqrt(x * x + y * y);
00408          }
00409 
00413          void normalize()
00414          {
00415             T s = length();
00416             x /= s;
00417             y /= s;
00418          }
00419 
00427          T lengthSq() const
00428          {
00429             return x * x + y * y;
00430          }
00431         
00432          //--------------[ misc. operations ]-----------------------
00442          Vector2<T> lerp(T fact, const Vector2<T>& r) const
00443          {
00444             return (*this) + (r - (*this)) * fact;      
00445          }
00446         
00447     
00448          //-------------[ conversion ]-----------------------------
00454          operator T*(){ return (T*) this; }
00460          operator const T*() const { return (const T*) this; }
00461 
00462          //-------------[ output operator ]------------------------
00469          friend std::ostream& operator<<(std::ostream& lhs, const Vector2<T>& rhs) 
00470          {
00471             lhs << "[" << rhs.x << "," << rhs.y << "]";
00472             return lhs;
00473          }
00474 
00475 
00476    };
00477 
00478 
00479    //--------------------------------------
00480    // Typedef shortcuts for 2D vector
00481    //-------------------------------------
00483    typedef class Vector2 <float> Vector2f;
00485    typedef class Vector2 <double> Vector2d;
00486 
00487 
00491    template <class T>
00492    class Vector3 
00493    {
00494       public:
00495          //T x, y, z;
00496          union
00497          {
00501                T x;
00502                 
00507                T s;
00508                 
00513                T r; 
00514          };
00515         
00516          union
00517          {
00521                T y; 
00526                T t; 
00531                T g; 
00532          };
00533         
00534          union
00535          {
00539                T z;
00540                 
00545                T u; 
00550                T b; 
00551          };
00552         
00553          //----------------[ constructors ]--------------------------
00557          Vector3() : x(0),y(0),z(0)
00558          {      }
00559 
00566          Vector3(T nx, T ny, T nz) : x(nx),y(ny),z(nz)
00567          {      }
00568 
00573          Vector3(const Vector3<T>& src)
00574             : x(src.x), y(src.y), z(src.z)
00575          {}
00576          
00581          template <class FromT>
00582          Vector3(const Vector3<FromT>& src)
00583             : x(static_cast<T>(src.x)),
00584               y(static_cast<T>(src.y)),
00585               z(static_cast<T>(src.z))
00586          {}
00587         
00588         
00589          //----------------[ access operators ]-------------------
00594          Vector3<T> operator=(const Vector3<T>& rhs) 
00595          {
00596             x = rhs.x;
00597             y = rhs.y;
00598             z = rhs.z;
00599             return * this;
00600          }
00601 
00606          template <class FromT>
00607          Vector3<T> operator=(const Vector3<FromT>& rhs)
00608          {
00609             x = static_cast<T>(rhs.x);
00610             y = static_cast<T>(rhs.y);
00611             z = static_cast<T>(rhs.z);
00612             return * this;
00613          }
00614 
00622          T & operator[](int n)
00623          {
00624             assert(n >= 0 && n <= 2);
00625             if (0 == n) return x;
00626             else if (1 == n) return y;
00627             else
00628                return z;
00629          }
00630 
00631 
00632          //---------------[ vector aritmetic operator ]--------------
00637          Vector3<T> operator+(const Vector3<T>& rhs) const 
00638          {
00639             return Vector3<T> (x + rhs.x, y + rhs.y, z + rhs.z);
00640          }
00641         
00646          Vector3<T> operator-(const Vector3<T>& rhs) const 
00647          {
00648             return Vector3<T> (x - rhs.x, y - rhs.y, z - rhs.z);
00649          }
00650         
00655          Vector3<T> operator*(const Vector3<T>& rhs) const 
00656          {
00657             return Vector3<T> (x * rhs.x, y * rhs.y, z * rhs.z);
00658          }
00659 
00664          Vector3<T> operator/(const Vector3<T>& rhs) const 
00665          {
00666             return Vector3<T> (x / rhs.x, y / rhs.y, z / rhs.z);
00667          }
00668 
00673          Vector3<T>& operator+=(const Vector3<T>& rhs) 
00674          {
00675             x += rhs.x;
00676             y += rhs.y;
00677             z += rhs.z;
00678             return * this;
00679          }
00680 
00685          Vector3<T>& operator-=(const Vector3<T>& rhs) 
00686          {
00687             x -= rhs.x;
00688             y -= rhs.y;
00689             z -= rhs.z;
00690             return * this;
00691          }
00692 
00697          Vector3<T>& operator*=(const Vector3<T>& rhs) 
00698          {
00699             x *= rhs.x;
00700             y *= rhs.y;
00701             z *= rhs.z;
00702             return * this;
00703          }
00704 
00709          Vector3<T>& operator/=(const Vector3<T>& rhs) 
00710          {
00711             x /= rhs.x;
00712             y /= rhs.y;
00713             z /= rhs.z;
00714             return * this;
00715          }
00716 
00721          T dotProduct(const Vector3<T>& rhs) const 
00722          {
00723             return x * rhs.x + y * rhs.y + z * rhs.z;
00724          }
00725 
00730          Vector3<T> crossProduct(const Vector3<T>& rhs) const 
00731          {
00732             return Vector3<T> (y * rhs.z - rhs.y * z, z * rhs.x - rhs.z * x, x * rhs.y - rhs.x * y);
00733          }
00734         
00735 
00736          //--------------[ scalar vector operator ]--------------------
00741          Vector3<T> operator+(T rhs) const 
00742          {
00743             return Vector3<T> (x + rhs, y + rhs, z + rhs);
00744          }
00745 
00750          Vector3<T> operator-(T rhs) const 
00751          {
00752             return Vector3<T> (x - rhs, y - rhs, z - rhs);
00753          }
00754 
00759          Vector3<T> operator*(T rhs) const 
00760          {
00761             return Vector3<T> (x * rhs, y * rhs, z * rhs);
00762          }
00763         
00768          Vector3<T> operator/(T rhs) const 
00769          {
00770             return Vector3<T> (x / rhs, y / rhs, z / rhs);
00771          }
00772         
00777          Vector3<T>& operator+=(T rhs) 
00778          {
00779             x += rhs;
00780             y += rhs;
00781             z += rhs;
00782             return * this;
00783          }
00784 
00789          Vector3<T>& operator-=(T rhs) 
00790          {
00791             x -= rhs;
00792             y -= rhs;
00793             z -= rhs;
00794             return * this;
00795          }
00796 
00801          Vector3<T>& operator*=(T rhs) 
00802          {
00803             x *= rhs;
00804             y *= rhs;
00805             z *= rhs;
00806             return * this;
00807          }
00808 
00813          Vector3<T>& operator/=(T rhs) 
00814          {
00815             x /= rhs;
00816             y /= rhs;
00817             z /= rhs;
00818             return * this;
00819          }
00820 
00821          //--------------[ equiality operator ]------------------------
00829          bool operator==(const Vector3<T>& rhs) const
00830          {
00831             return std::fabs(x - rhs.x) < EPSILON 
00832                && std::fabs(y - rhs.y) < EPSILON 
00833                && std::fabs(z - rhs.z) < EPSILON;
00834          }
00835 
00841          bool operator!=(const Vector3<T>& rhs) const { return !(*this == rhs); }
00842 
00843          //-------------[ unary operations ]--------------------------
00848          Vector3<T> operator-() const
00849          {
00850             return Vector3<T>(-x, -y, -z);
00851          }
00852 
00853          //-------------[ size operations ]---------------------------
00858          T length() const 
00859          {
00860             return (T)std::sqrt(x * x + y * y + z * z);
00861          }
00862         
00870          T lengthSq() const 
00871          {
00872             return x * x + y * y + z * z;
00873          }
00874         
00878          void normalize() 
00879          {
00880             T s = length();
00881             x /= s;
00882             y /= s;
00883             z /= s;
00884          }
00885 
00886          //------------[ other operations ]---------------------------
00893          void rotate(T ax, T ay, T az) 
00894          {
00895             T a = cos(DEG2RAD(ax));
00896             T b = sin(DEG2RAD(ax));
00897             T c = cos(DEG2RAD(ay));
00898             T d = sin(DEG2RAD(ay));
00899             T e = cos(DEG2RAD(az));
00900             T f = sin(DEG2RAD(az));
00901             T nx = c * e * x - c * f * y + d * z;
00902             T ny = (a * f + b * d * e) * x + (a * e - b * d * f) * y - b * c * z;
00903             T nz = (b * f - a * d * e) * x + (a * d * f + b * e) * y + a * c * z;
00904             x = nx; y = ny; z = nz;
00905                 
00906                 
00907          }
00908         
00918          Vector3<T> lerp(T fact, const Vector3<T>& r) const
00919          {
00920             return (*this) + (r - (*this)) * fact;      
00921          }
00922         
00923     
00924 
00925 
00926 
00927          //-------------[ conversion ]-----------------------------
00928         
00934          operator T*(){ return (T*) this; }
00935 
00941          operator const T*() const { return (const T*) this; }
00942 
00943          //-------------[ output operator ]------------------------
00950          friend std::ostream& operator<<(std::ostream& lhs, const Vector3<T> rhs) 
00951          {
00952             lhs << "[" << rhs.x << "," << rhs.y << "," << rhs.z  << "]";
00953             return lhs;
00954          }
00955         
00956    };
00957 
00958 
00960    typedef Vector3 <float> Vector3f;
00962    typedef Vector3 <double> Vector3d;
00963 
00967    template <class T>
00968    class Vector4{
00969       public:
00970 
00971          union 
00972          { 
00977                T r; 
00981                T x; 
00982          };
00983         
00984          union 
00985          { 
00990                T g; 
00994                T y; 
00995          };
00996         
00997          union 
00998          {
01003                T b; 
01007                T z; 
01008          };
01009         
01010          union 
01011          {
01016                T a; 
01022                T w; 
01023          };
01024         
01025          //----------------[ constructors ]--------------------------
01029          Vector4() : x(0),y(0),z(0),w(0)
01030          { }
01031 
01032 
01040          Vector4(T nx, T ny, T nz, T nw) : x(nx), y(ny), z(nz), w(nw)
01041          { }
01042 
01047          Vector4(const Vector4<T>& src)
01048             : x(src.x), y(src.y), z(src.z), w(src.w)
01049          {}
01050 
01055          template <class FromT>
01056          Vector4(const Vector4<FromT>& src)
01057             : x(static_cast<T>(src.x)),
01058               y(static_cast<T>(src.y)),
01059               z(static_cast<T>(src.z)),
01060               w(static_cast<T>(src.w))
01061          {}
01062 
01063 
01064          //----------------[ access operators ]-------------------
01069          Vector4<T> operator=(const Vector4<T>& rhs) 
01070          {
01071             x = rhs.x;
01072             y = rhs.y;
01073             z = rhs.z;
01074             w = rhs.w;
01075             return * this;
01076          }
01077 
01082          template<class FromT>
01083          Vector4<T> operator=(const Vector4<FromT>& rhs) 
01084          {
01085             x = static_cast<T>(rhs.x);
01086             y = static_cast<T>(rhs.y);
01087             z = static_cast<T>(rhs.z);
01088             w = static_cast<T>(rhs.w);
01089             return * this;
01090          }
01091 
01092 
01100          T & operator[](int n) 
01101          {
01102             assert(n >= 0 && n <= 3);
01103             if (0 == n) return x;
01104             else if (1 == n) return y;
01105             else if (2 == n) return z;
01106             else return w;
01107          }
01108 
01109 
01110          //---------------[ vector aritmetic operator ]--------------
01115          Vector4<T> operator+(const Vector4<T>& rhs) const 
01116          {
01117             return Vector4<T> (x + rhs.x, y + rhs.y, z + rhs.z, w + rhs.w);
01118          }
01119 
01124          Vector4<T> operator-(const Vector4<T>& rhs) const 
01125          {
01126             return Vector4<T> (x - rhs.x, y - rhs.y, z - rhs.z, w - rhs.w);
01127          }
01128 
01133          Vector4<T> operator*(const Vector4<T> rhs) const 
01134          {
01135             return Vector4<T> (x * rhs.x, y * rhs.y, z * rhs.z, w * rhs.w);
01136          }
01137 
01142          Vector4<T> operator/(const Vector4<T>& rhs) const 
01143          {
01144             return Vector4<T> (x / rhs.x, y / rhs.y, z / rhs.z, w / rhs.w);
01145          }
01146 
01151          Vector4<T>& operator+=(const Vector4<T>& rhs) 
01152          {
01153             x += rhs.x;
01154             y += rhs.y;
01155             z += rhs.z;
01156             w += rhs.w;
01157             return * this;
01158          }
01159 
01164          Vector4<T>& operator-=(const Vector4<T>& rhs) 
01165          {
01166             x -= rhs.x;
01167             y -= rhs.y;
01168             z -= rhs.z;
01169             w -= rhs.w;
01170             return * this;
01171          }
01172 
01177          Vector4<T>& operator*=(const Vector4<T>& rhs) 
01178          {
01179             x *= rhs.x;
01180             y *= rhs.y;
01181             z *= rhs.z;
01182             w *= rhs.w;
01183             return * this;
01184          }
01185 
01190          Vector4<T>& operator/=(const Vector4<T>& rhs) 
01191          {
01192             x /= rhs.x;
01193             y /= rhs.y;
01194             z /= rhs.z;
01195             w /= rhs.w;
01196             return * this;
01197          }
01198 
01199          //--------------[ equiality operator ]------------------------
01207          bool operator==(const Vector4<T>& rhs) const 
01208          {
01209             return std::fabs(x - rhs.x) < EPSILON 
01210                && std::fabs(y - rhs.y) < EPSILON 
01211                && std::fabs(z - rhs.z) < EPSILON 
01212                && std::fabs(w - rhs.w) < EPSILON;
01213          }
01214 
01215         
01221          bool operator!=(const Vector4<T>& rhs) const { return ! (*this == rhs); }
01222 
01223          //-------------[ unary operations ]--------------------------
01228          Vector4<T> operator-() const
01229          {
01230             return Vector4<T>(-x, -y, -z, -w);
01231          }
01232 
01233          //--------------[ scalar vector operator ]--------------------
01234         
01239          Vector4<T> operator+(T rhs) const 
01240          {
01241             return Vector4<T> (x + rhs, y + rhs, z + rhs, w + rhs);
01242          }
01243 
01248          Vector4<T> operator-(T rhs) const 
01249          {
01250             return Vector4<T> (x - rhs, y - rhs, z - rhs, w - rhs);
01251          }
01252 
01257          Vector4<T> operator*(T rhs) const 
01258          {
01259             return Vector4<T> (x * rhs, y * rhs, z * rhs, w * rhs);
01260          }
01261 
01266          Vector4<T> operator/(T rhs) const 
01267          {
01268             return Vector4<T> (x / rhs, y / rhs, z / rhs, w / rhs);
01269          }
01270 
01275          Vector4<T>& operator+=(T rhs) 
01276          {
01277             x += rhs;
01278             y += rhs;
01279             z += rhs;
01280             w += rhs;
01281             return * this;
01282          }
01283 
01288          Vector4<T>& operator-=(T rhs) 
01289          {
01290             x -= rhs;
01291             y -= rhs;
01292             z -= rhs;
01293             w -= rhs;
01294             return * this;
01295          }
01296 
01301          Vector4<T>& operator*=(T rhs) 
01302          {
01303             x *= rhs;
01304             y *= rhs;
01305             z *= rhs;
01306             w *= rhs;
01307             return * this;
01308          }
01309 
01314          Vector4<T>& operator/=(T rhs) 
01315          {
01316             x /= rhs;
01317             y /= rhs;
01318             z /= rhs;
01319             w /= rhs;
01320             return * this;
01321          }
01322 
01323          //-------------[ size operations ]---------------------------
01328          T length() const
01329          {
01330             return (T)std::sqrt(x * x + y * y + z * z + w * w);
01331          }
01332 
01336          void normalize() 
01337          {
01338             T s = length();
01339             x /= s;
01340             y /= s;
01341             z /= s;
01342             w /= s;
01343          }
01344         
01352          T lengthSq() const 
01353          {
01354             return x * x + y * y + z * z + w * w;
01355          }
01356         
01357          //--------------[ misc. operations ]-----------------------
01367          Vector4<T> lerp(T fact, const Vector4<T>& r) const
01368          {
01369             return (*this) + (r - (*this)) * fact;      
01370          }
01371         
01372     
01373         
01374 
01375          //-------------[ conversion ]-----------------------------
01376         
01382          operator T*(){ return (T*) this; }
01383         
01384         
01390          operator const T*() const { return (const T*) this; }
01391 
01392          //-------------[ output operator ]------------------------
01399          friend std::ostream& operator<<(std::ostream& lhs, const Vector4<T>& rhs) 
01400          {
01401             lhs << "[" << rhs.x << "," << rhs.y << "," << rhs.z << "," << rhs.w << "]";
01402             return lhs;
01403          }
01404    };
01405 
01407    typedef Vector4<float> Vector4f;
01409    typedef Vector4<double> Vector4d;
01410 
01411 
01412 
01413 
01414 
01419    template <class T>
01420    class Matrix3 
01421    {
01422       public:
01424          T data[9];
01425         
01426          //--------------------------[ constructors ]-------------------------------
01430          Matrix3() 
01431          {
01432             for (int i = 0; i < 9; i++)
01433                data[i] = (i % 4) ? 0 : 1;
01434          }
01435 
01440          Matrix3(const T * dt)
01441          {
01442             std::memcpy(data, dt, sizeof(T) * 9); 
01443          }
01444 
01449          Matrix3(const Matrix3<T>& src)
01450          {
01451             std::memcpy(data, src.data, sizeof(T) * 9);
01452          }
01453 
01458          template<class FromT>
01459          Matrix3(const Matrix3<FromT>& src)
01460          {
01461             for (int i = 0; i < 9; i++)
01462             {
01463                data[i] = static_cast<T>(src.data[i]);
01464             }
01465          }
01466         
01470          void identity()
01471          {
01472             for (int i = 0; i < 9; i++)
01473                data[i] = (i % 4) ? 0 : 1;
01474          }
01475         
01476         
01477 
01484          static Matrix3<T> createRotationAroundAxis(T a, T b, T c)
01485          {
01486             Matrix3<T> ma, mb, mc;
01487             float ac = cos(a);
01488             float as = sin(a);
01489             float bc = cos(b);
01490             float bs = sin(b);
01491             float cc = cos(c);
01492             float cs = sin(c);
01493 
01494             ma.at(1,1) = ac;
01495             ma.at(2,1) = as;
01496             ma.at(1,2) = -as;
01497             ma.at(2,2) = ac;
01498 
01499             mb.at(0,0) = bc;
01500             mb.at(2,0) = -bs;
01501             mb.at(0,2) = bs;
01502             mb.at(2,2) = bc;
01503 
01504             mc.at(0,0) = cc;
01505             mc.at(1,0) = cs;
01506             mc.at(0,1) = -cs;
01507             mc.at(1,1) = cc;
01508 
01509             Matrix3<T> ret = ma * mb * mc;
01510             return ret;
01511          }
01512 
01516          template <class It>
01517          static Matrix3<T> fromOde(const It* mat)
01518          {
01519             Matrix3<T> ret;
01520             for (int i = 0; i < 3; i++)
01521             {
01522                for (int j = 0; j < 3; j++)
01523                {
01524                   ret.at(i,j) = static_cast<T>(mat[j * 4 + i]);
01525                }
01526             }
01527             return ret;
01528          }
01529 
01530 
01531          //---------------------[ equiality operators ]------------------------------
01540          bool operator==(const Matrix3<T>& rhs) const
01541          {
01542             for (int i = 0; i < 9; i++)
01543             {
01544                if (std::fabs(data[i] - rhs.data[i]) >= EPSILON)
01545                   return false;
01546             }
01547             return true;
01548          }
01549         
01550         
01556          bool operator!=(const Matrix3<T>& rhs) const
01557          {
01558             return !(*this == rhs);
01559          }
01560         
01561 
01562          //---------------------[ access operators ]---------------------------------
01568          T& at(int x, int y) 
01569          {
01570             assert(x >= 0 && x < 3);
01571             assert(y >= 0 && y < 3);
01572             return data[x * 3 + y];
01573          }
01574 
01580          const T& at(int x, int y) const 
01581          {
01582             assert(x >= 0 && x < 3);
01583             assert(y >= 0 && y < 3);
01584             return data[x * 3 + y];
01585          }
01586 
01591          Matrix3<T>& operator=(const Matrix3<T>& rhs) 
01592          {
01593             std::memcpy(data, rhs.data, sizeof(T) * 9);
01594             return * this;
01595          }
01596 
01601          template<class FromT>
01602          Matrix3<T>& operator=(const Matrix3<FromT>& rhs) 
01603          {
01604             for (int i = 0; i < 9; i++)
01605             {
01606                data[i] = static_cast<T>(rhs.data[i]);
01607             }
01608             return * this;
01609          }
01610 
01615          Matrix3<T>& operator=(const T* rhs) 
01616          {
01617             std::memcpy(data, rhs, sizeof(T) * 9);
01618             return * this;
01619          }
01620 
01621 
01622          /*Matrix3<T> & operator=(const double* m)
01623            {
01624            for (int i = 0; i < 9; i++) data[i] = (T)m[i];
01625            return * this;
01626            }*/
01627         
01628          //--------------------[ matrix with matrix operations ]---------------------
01633          Matrix3<T> operator+(const Matrix3<T>& rhs) const
01634          {
01635             Matrix3<T> ret;
01636             for (int i = 0; i < 9; i++)
01637                ret.data[i] = data[i] + rhs.data[i];
01638             return ret;
01639          }
01640         
01645          Matrix3<T> operator-(const Matrix3<T>& rhs) const
01646          {
01647             Matrix3<T> ret;
01648             for (int i = 0; i < 9; i++)
01649                ret.data[i] = data[i] - rhs.data[i];
01650             return ret;
01651          }
01652         
01653          //--------------------[ matrix with scalar operations ]---------------------
01658          Matrix3<T> operator+(T rhs) const
01659          {
01660             Matrix3<T> ret;
01661             for (int i = 0; i < 9; i++)
01662                ret.data[i] = data[i] + rhs;
01663             return ret;
01664          }
01665         
01670          Matrix3<T> operator-(T rhs) const
01671          {
01672             Matrix3<T> ret;
01673             for (int i = 0; i < 9; i++)
01674                ret.data[i] = data[i] - rhs;
01675             return ret;
01676          }
01677         
01682          Matrix3<T> operator*(T rhs) const
01683          {
01684             Matrix3<T> ret;
01685             for (int i = 0; i < 9; i++)
01686                ret.data[i] = data[i] * rhs;
01687             return ret;
01688          }
01689         
01694          Matrix3<T> operator/(T rhs) const
01695          {
01696             Matrix3<T> ret;
01697             for (int i = 0; i < 9; i++)
01698                ret.data[i] = data[i] / rhs;
01699             return ret;
01700          }
01701         
01702 
01703          //--------------------[ multiply operators ]--------------------------------
01708          Vector3<T> operator*(const Vector3<T>& rhs) const
01709          {
01710             return Vector3<T>(
01711                data[0] * rhs.x + data[3] * rhs.y + data[6] * rhs.z,
01712                data[1] * rhs.x + data[4] * rhs.y + data[7] * rhs.z,
01713                data[2] * rhs.x + data[5] * rhs.y + data[8] * rhs.z
01714                );
01715          }
01716 
01721          Matrix3<T> operator*(Matrix3<T> rhs) const 
01722          {
01723             static Matrix3<T> w;
01724             for (int i = 0; i < 3; i++) 
01725             {
01726                for (int j = 0; j < 3; j++) 
01727                {
01728                   T n = 0;
01729                   for (int k = 0; k < 3; k++) n += rhs.at(i, k) * at(k, j);
01730                   w.at(i, j) = n;
01731                }
01732             }
01733             return w;
01734                 
01735          }
01736         
01737         
01738          //---------------------------[ misc operations ]----------------------------
01742          Matrix3<T> transpose()
01743          {
01744             Matrix3<T> ret;
01745             for (int i = 0; i < 3; i++)
01746             {
01747                for (int j = 0; j < 3; j++)
01748                {
01749                   ret.at(i,j) = at(j,i);
01750                }
01751             }
01752             return ret;
01753          }
01754         
01764          Matrix3<T> lerp(T fact, const Matrix3<T>& rhs) const
01765          {
01766             Matrix3<T> ret = (*this) + (rhs - (*this)) * fact;
01767             return ret;         
01768          }
01769 
01770     
01771          //-------------[ conversion ]-----------------------------
01772         
01778          operator T*(){ return (T*) data; }
01779         
01785          operator const T*() const { return (const T*) data; }
01786 
01787          //----------[ output operator ]----------------------------
01794          friend std::ostream& operator << (std::ostream& lhs, const Matrix3<T>& rhs) 
01795          {
01796             for (int i = 0; i < 3; i++)
01797             {
01798                lhs << "|\t";
01799                for (int j = 0; j < 3; j++)
01800                {
01801                   lhs << rhs.at(j,i) << "\t";
01802                }
01803                lhs << "|" << std::endl;
01804             }
01805             return lhs;
01806          }
01807     
01808    };
01809 
01811    typedef Matrix3<float> Matrix3f;
01813    typedef Matrix3<double> Matrix3d;
01814 
01815 
01820    template <class T>
01821    class Matrix4 
01822    {
01823       public:
01825          T data[16];
01826         
01827          //--------------------------[ constructors ]-------------------------------
01831          Matrix4() 
01832          {
01833             for (int i = 0; i < 16; i++)
01834                data[i] = (i % 5) ? 0 : 1;
01835          }
01836 
01841          Matrix4(const T * dt) 
01842          {
01843             std::memcpy(data, dt, sizeof(T) * 16); 
01844          }
01845 
01850          Matrix4(const Matrix4<T>& src)
01851          {
01852             std::memcpy(data, src.data, sizeof(T) * 16);
01853          }
01854         
01859          template <class FromT>
01860          Matrix4(const Matrix4<FromT>& src)
01861          {
01862             for (int i = 0; i < 16; i++)
01863             {
01864                data[i] = static_cast<T>(src.data[i]);
01865             }
01866          }
01867 
01871          void identity()
01872          {
01873             for (int i = 0; i < 16; i++)
01874                data[i] = (i % 5) ? 0 : 1;
01875          }
01876         
01877         
01878 
01885          static Matrix4<T> createRotationAroundAxis(T a, T b, T c)
01886          {
01887             Matrix4<T> ma, mb, mc;
01888             float ac = cos(a);
01889             float as = sin(a);
01890             float bc = cos(b);
01891             float bs = sin(b);
01892             float cc = cos(c);
01893             float cs = sin(c);
01894 
01895             ma.at(1,1) = ac;
01896             ma.at(2,1) = as;
01897             ma.at(1,2) = -as;
01898             ma.at(2,2) = ac;
01899 
01900             mb.at(0,0) = bc;
01901             mb.at(2,0) = -bs;
01902             mb.at(0,2) = bs;
01903             mb.at(2,2) = bc;
01904 
01905             mc.at(0,0) = cc;
01906             mc.at(1,0) = cs;
01907             mc.at(0,1) = -cs;
01908             mc.at(1,1) = cc;
01909 
01910             /*std::cout << "RotVec = " << a << "," << b << "," << c << std::endl;
01911               std::cout << "Rx = " << std::endl << ma;
01912               std::cout << "Ry = " << std::endl << mb;
01913               std::cout << "Rz = " << std::endl << mc;*/
01914                 
01915             Matrix4<T> ret = ma * mb * mc;
01916             //std::cout << "Result = " << std::endl << ma * (mb * mc);
01917                 
01918             return ret;
01919          }
01920 
01922 
01929          static Matrix4<T> createTranslation(T x, T y, T z, T w = 1)
01930          {
01931             Matrix4 ret;
01932             ret.at(3,0) = x;
01933             ret.at(3,1) = y;
01934             ret.at(3,2) = z;
01935             ret.at(3,3) = w;
01936                 
01937             return ret;
01938          }
01939 
01940 
01941          //---------------------[ equiality operators ]------------------------------   
01950          bool operator==(const Matrix4<T>& rhs) const
01951          {
01952             for (int i = 0; i < 16; i++)
01953             {
01954                if (std::fabs(data[i] - rhs.data[i]) >= EPSILON)
01955                   return false;
01956             }
01957             return true;
01958          }
01959         
01965          bool operator!=(const Matrix4<T>& rhs) const
01966          {
01967             return !(*this == rhs);
01968          }
01969         
01970 
01971          //---------------------[ access operators ]---------------------------------
01977          T& at(int x, int y) 
01978          {
01979             assert(x >= 0 && x < 4);
01980             assert(y >= 0 && y < 4);
01981             return data[x * 4 + y];
01982          }
01983 
01989          const T& at(int x, int y) const 
01990          {
01991             assert(x >= 0 && x < 4);
01992             assert(y >= 0 && y < 4);
01993             return data[x * 4 + y];
01994          }
01995 
01996 
02002          void setTranslation(const Vector3<T>& v)
02003          {
02004             at(3,0) = v.x;
02005             at(3,1) = v.y;
02006             at(3,2) = v.z;
02007             at(3,3) = 1;
02008          }
02009 
02010          Vector3<T> getTranslation()
02011          { return Vector3<T>(at(3,0),at(3,1),at(3,2)); }
02012 
02018          void setRotation(const Matrix3<T>& m)
02019          {
02020             for (int i = 0; i < 3; i++)
02021             {
02022                for (int j = 0; j < 3; j++)
02023                {
02024                   at(i,j) = m.at(i,j);
02025                }
02026             }
02027          }
02028                                  
02029 
02034          Matrix4<T>& operator=(const Matrix4<T>& rhs) 
02035          {
02036             std::memcpy(data, rhs.data, sizeof(T) * 16);
02037             return * this;
02038          }
02039 
02044          template <class FromT>
02045          Matrix4<T>& operator=(const Matrix4<FromT>& rhs) 
02046          {
02047             for (int i = 0; i < 16; i++)
02048             {
02049                data[i] = static_cast<T>(rhs.data[i]);
02050             }
02051             return * this;
02052          }
02053 
02058          Matrix4<T>& operator=(const T* rhs) 
02059          {
02060             std::memcpy(data, rhs, sizeof(T) * 16);
02061             return * this;
02062          }
02063 
02064          /*Matrix4<T> & operator=(const double* m)
02065            {
02066            for (int i = 0; i < 16; i++) data[i] = (T)m[i];
02067            return * this;
02068            }*/
02069         
02070          //--------------------[ matrix with matrix operations ]---------------------
02075          Matrix4<T> operator+(const Matrix4<T>& rhs) const
02076          {
02077             Matrix4<T> ret;
02078             for (int i = 0; i < 16; i++)
02079                ret.data[i] = data[i] + rhs.data[i];
02080             return ret;
02081          }
02082         
02087          Matrix4<T> operator-(const Matrix4<T>& rhs) const
02088          {
02089             Matrix4<T> ret;
02090             for (int i = 0; i < 16; i++)
02091                ret.data[i] = data[i] - rhs.data[i];
02092             return ret;
02093          }
02094         
02095          //--------------------[ matrix with scalar operations ]---------------------
02100          Matrix4<T> operator+(T rhs) const
02101          {
02102             Matrix4<T> ret;
02103             for (int i = 0; i < 16; i++)
02104                ret.data[i] = data[i] + rhs;
02105             return ret;
02106          }
02107         
02112          Matrix4<T> operator-(T rhs) const
02113          {
02114             Matrix4<T> ret;
02115             for (int i = 0; i < 16; i++)
02116                ret.data[i] = data[i] - rhs;
02117             return ret;
02118          }
02119         
02124          Matrix4<T> operator*(T rhs) const
02125          {
02126             Matrix4<T> ret;
02127             for (int i = 0; i < 16; i++)
02128                ret.data[i] = data[i] * rhs;
02129             return ret;
02130          }
02131         
02136          Matrix4<T> operator/(T rhs) const
02137          {
02138             Matrix4<T> ret;
02139             for (int i = 0; i < 16; i++)
02140                ret.data[i] = data[i] / rhs;
02141             return ret;
02142          }
02143         
02144 
02145          //--------------------[ multiply operators ]--------------------------------
02150          Vector4<T> operator*(const Vector4<T>& rhs) const 
02151          {
02152             return Vector4<T>(
02153                data[0] * rhs.x + data[4] * rhs.y + data[8]  * rhs.z + data[12] * rhs.w,
02154                data[1] * rhs.x + data[5] * rhs.y + data[9]  * rhs.z + data[13] * rhs.w,
02155                data[2] * rhs.x + data[6] * rhs.y + data[10] * rhs.z + data[14] * rhs.w,
02156                data[3] * rhs.x + data[7] * rhs.y + data[11] * rhs.z + data[15] * rhs.w
02157                );
02158 
02159          }
02160         
02165          Vector3<T> operator*(const Vector3<T>& rhs) const
02166          {
02167             return Vector3<T>(
02168                data[0] * rhs.x + data[4] * rhs.y + data[8] * rhs.z,
02169                data[1] * rhs.x + data[5] * rhs.y + data[9] * rhs.z,
02170                data[2] * rhs.x + data[6] * rhs.y + data[10] * rhs.z
02171                );
02172          }
02173 
02178          Matrix4<T> operator*(Matrix4<T> rhs) const 
02179          {
02180             static Matrix4<T> w;
02181             for (int i = 0; i < 4; i++) 
02182             {
02183                for (int j = 0; j < 4; j++) 
02184                {
02185                   T n = 0;
02186                   for (int k = 0; k < 4; k++) n += rhs.at(i, k) * at(k, j);
02187                   w.at(i, j) = n;
02188                }
02189             }
02190             return w;
02191                 
02192          }
02193         
02194         
02195          //---------------------------[ misc operations ]----------------------------
02196  
02202          T det()
02203          {
02204 
02205             return
02206                + at(3,0) * at(2,1) * at(1,2) * at(0,3)
02207                - at(2,0) * at(3,1) * at(1,2) * at(0,3)
02208                - at(3,0) * at(1,1) * at(2,2) * at(0,3)
02209                + at(1,0) * at(3,1) * at(2,2) * at(0,3)
02210 
02211                + at(2,0) * at(1,1) * at(3,2) * at(0,3)
02212                - at(1,0) * at(2,1) * at(3,2) * at(0,3)
02213                - at(3,0) * at(2,1) * at(0,2) * at(1,3)
02214                + at(2,0) * at(3,1) * at(0,2) * at(1,3)
02215 
02216                + at(3,0) * at(0,1) * at(2,2) * at(1,3)
02217                - at(0,0) * at(3,1) * at(2,2) * at(1,3)
02218                - at(2,0) * at(0,1) * at(3,2) * at(1,3)
02219                + at(0,0) * at(2,1) * at(3,2) * at(1,3)
02220 
02221                + at(3,0) * at(1,1) * at(0,2) * at(2,3)
02222                - at(1,0) * at(3,1) * at(0,2) * at(2,3)
02223                - at(3,0) * at(0,1) * at(1,2) * at(2,3)
02224                + at(0,0) * at(3,1) * at(1,2) * at(2,3)
02225 
02226                + at(1,0) * at(0,1) * at(3,2) * at(2,3)
02227                - at(0,0) * at(1,1) * at(3,2) * at(2,3)
02228                - at(2,0) * at(1,1) * at(0,2) * at(3,3)
02229                + at(1,0) * at(2,1) * at(0,2) * at(3,3)
02230 
02231                + at(2,0) * at(0,1) * at(1,2) * at(3,3)
02232                - at(0,0) * at(2,1) * at(1,2) * at(3,3)
02233                - at(1,0) * at(0,1) * at(2,2) * at(3,3)
02234                + at(0,0) * at(1,1) * at(2,2) * at(3,3);
02235 
02236 
02237          }
02238 
02239 
02246          Matrix4<T> inverse()
02247          {
02248             Matrix4<T> ret;
02249 
02250             ret.at(0,0) =  
02251                + at(2,1) * at(3,2) * at(1,3) 
02252                - at(3,1) * at(2,2) * at(1,3) 
02253                + at(3,1) * at(1,2) * at(2,3) 
02254                - at(1,1) * at(3,2) * at(2,3) 
02255                - at(2,1) * at(1,2) * at(3,3) 
02256                + at(1,1) * at(2,2) * at(3,3);
02257 
02258             ret.at(1,0) =
02259                + at(3,0) * at(2,2) * at(1,3) 
02260                - at(2,0) * at(3,2) * at(1,3) 
02261                - at(3,0) * at(1,2) * at(2,3) 
02262                + at(1,0) * at(3,2) * at(2,3) 
02263                + at(2,0) * at(1,2) * at(3,3) 
02264                - at(1,0) * at(2,2) * at(3,3);
02265 
02266             ret.at(2,0) = 
02267                + at(2,0) * at(3,1) * at(1,3) 
02268                - at(3,0) * at(2,1) * at(1,3) 
02269                + at(3,0) * at(1,1) * at(2,3) 
02270                - at(1,0) * at(3,1) * at(2,3) 
02271                - at(2,0) * at(1,1) * at(3,3) 
02272                + at(1,0) * at(2,1) * at(3,3);
02273 
02274             ret.at(3,0) = 
02275                + at(3,0) * at(2,1) * at(1,2) 
02276                - at(2,0) * at(3,1) * at(1,2) 
02277                - at(3,0) * at(1,1) * at(2,2) 
02278                + at(1,0) * at(3,1) * at(2,2) 
02279                + at(2,0) * at(1,1) * at(3,2) 
02280                - at(1,0) * at(2,1) * at(3,2);
02281 
02282             ret.at(0,1) = 
02283                + at(3,1) * at(2,2) * at(0,3) 
02284                - at(2,1) * at(3,2) * at(0,3) 
02285                - at(3,1) * at(0,2) * at(2,3) 
02286                + at(0,1) * at(3,2) * at(2,3) 
02287                + at(2,1) * at(0,2) * at(3,3) 
02288                - at(0,1) * at(2,2) * at(3,3);
02289 
02290             ret.at(1,1) = 
02291                + at(2,0) * at(3,2) * at(0,3) 
02292                - at(3,0) * at(2,2) * at(0,3) 
02293                + at(3,0) * at(0,2) * at(2,3) 
02294                - at(0,0) * at(3,2) * at(2,3) 
02295                - at(2,0) * at(0,2) * at(3,3) 
02296                + at(0,0) * at(2,2) * at(3,3);
02297 
02298             ret.at(2,1) =
02299                + at(3,0) * at(2,1) * at(0,3) 
02300                - at(2,0) * at(3,1) * at(0,3) 
02301                - at(3,0) * at(0,1) * at(2,3) 
02302                + at(0,0) * at(3,1) * at(2,3) 
02303                + at(2,0) * at(0,1) * at(3,3) 
02304                - at(0,0) * at(2,1) * at(3,3);
02305 
02306             ret.at(3,1) = 
02307                + at(2,0) * at(3,1) * at(0,2) 
02308                - at(3,0) * at(2,1) * at(0,2) 
02309                + at(3,0) * at(0,1) * at(2,2) 
02310                - at(0,0) * at(3,1) * at(2,2) 
02311                - at(2,0) * at(0,1) * at(3,2) 
02312                + at(0,0) * at(2,1) * at(3,2);
02313 
02314             ret.at(0,2) =  
02315                + at(1,1) * at(3,2) * at(0,3) 
02316                - at(3,1) * at(1,2) * at(0,3) 
02317                + at(3,1) * at(0,2) * at(1,3) 
02318                - at(0,1) * at(3,2) * at(1,3) 
02319                - at(1,1) * at(0,2) * at(3,3) 
02320                + at(0,1) * at(1,2) * at(3,3);
02321 
02322             ret.at(1,2) =
02323                + at(3,0) * at(1,2) * at(0,3) 
02324                - at(1,0) * at(3,2) * at(0,3) 
02325                - at(3,0) * at(0,2) * at(1,3) 
02326                + at(0,0) * at(3,2) * at(1,3) 
02327                + at(1,0) * at(0,2) * at(3,3) 
02328                - at(0,0) * at(1,2) * at(3,3);
02329 
02330             ret.at(2,2) = 
02331                + at(1,0) * at(3,1) * at(0,3) 
02332                - at(3,0) * at(1,1) * at(0,3) 
02333                + at(3,0) * at(0,1) * at(1,3) 
02334                - at(0,0) * at(3,1) * at(1,3) 
02335                - at(1,0) * at(0,1) * at(3,3) 
02336                + at(0,0) * at(1,1) * at(3,3);
02337 
02338             ret.at(3,2) = 
02339                + at(3,0) * at(1,1) * at(0,2) 
02340                - at(1,0) * at(3,1) * at(0,2) 
02341                - at(3,0) * at(0,1) * at(1,2) 
02342                + at(0,0) * at(3,1) * at(1,2) 
02343                + at(1,0) * at(0,1) * at(3,2) 
02344                - at(0,0) * at(1,1) * at(3,2);
02345 
02346             ret.at(0,3) =
02347                + at(2,1) * at(1,2) * at(0,3) 
02348                - at(1,1) * at(2,2) * at(0,3) 
02349                - at(2,1) * at(0,2) * at(1,3) 
02350                + at(0,1) * at(2,2) * at(1,3) 
02351                + at(1,1) * at(0,2) * at(2,3) 
02352                - at(0,1) * at(1,2) * at(2,3);
02353 
02354             ret.at(1,3) = 
02355                + at(1,0) * at(2,2) * at(0,3) 
02356                - at(2,0) * at(1,2) * at(0,3) 
02357                + at(2,0) * at(0,2) * at(1,3) 
02358                - at(0,0) * at(2,2) * at(1,3) 
02359                - at(1,0) * at(0,2) * at(2,3) 
02360                + at(0,0) * at(1,2) * at(2,3);
02361 
02362             ret.at(2,3) =  
02363                + at(2,0) * at(1,1) * at(0,3) 
02364                - at(1,0) * at(2,1) * at(0,3) 
02365                - at(2,0) * at(0,1) * at(1,3) 
02366                + at(0,0) * at(2,1) * at(1,3) 
02367                + at(1,0) * at(0,1) * at(2,3) 
02368                - at(0,0) * at(1,1) * at(2,3);
02369 
02370             ret.at(3,3) = 
02371                + at(1,0) * at(2,1) * at(0,2) 
02372                - at(2,0) * at(1,1) * at(0,2) 
02373                + at(2,0) * at(0,1) * at(1,2) 
02374                - at(0,0) * at(2,1) * at(1,2) 
02375                - at(1,0) * at(0,1) * at(2,2) 
02376                + at(0,0) * at(1,1) * at(2,2);
02377   
02378             return ret * det();
02379          }
02380 
02381 
02382 
02386          Matrix4<T> transpose()
02387          {
02388             Matrix4<T> ret;
02389             for (int i = 0; i < 4; i++)
02390             {
02391                for (int j = 0; j < 4; j++)
02392                {
02393                   ret.at(i,j) = at(j,i);
02394                }
02395             }
02396             return ret;
02397          }
02398         
02408          Matrix4<T> lerp(T fact, const Matrix4<T>& rhs) const
02409          {
02410             Matrix4<T> ret = (*this) + (rhs - (*this)) * fact;
02411             return ret;         
02412          }
02413 
02414     
02415          //-------------[ conversion ]-----------------------------
02421          operator T*(){ return (T*) data; }
02422         
02428          operator const T*() const { return (const T*) data; }
02429 
02430          //----------[ output operator ]----------------------------
02437          friend std::ostream& operator << (std::ostream& lhs, const Matrix4<T>& rhs) 
02438          {
02439             for (int i = 0; i < 4; i++)
02440             {
02441                lhs << "|\t";
02442                for (int j = 0; j < 4; j++)
02443                {
02444                   lhs << rhs.at(j,i) << "\t";
02445                }
02446                lhs << "|" << std::endl;
02447             }
02448             return lhs;
02449          }
02450     
02451    };
02452 
02453 
02455    typedef Matrix4<float> Matrix4f;
02457    typedef Matrix4<double> Matrix4d;
02458 
02459 
02467    template <class T>
02468    class Quaternion
02469    {
02470       public:
02474          T w;
02478          Vector3<T> v;
02479         
02483          Quaternion(): w(0), v(0,0,0){}
02484         
02488          Quaternion(const Quaternion<T>& q): w(q.w), v(q.v){    }
02489 
02493          template <class FromT>
02494          Quaternion(const Quaternion<FromT>& q)
02495             : w(static_cast<T>(q.w)), 
02496               v(q.v){   }
02497 
02498         
02504          Quaternion(T w_, const Vector3<T>& v_): w(w_), v(v_){}
02505         
02513          Quaternion(T w_, T x, T y, T z): w(w_), v(x,y,z){}
02514         
02519          Quaternion<T>& operator= (const Quaternion<T>& rhs)
02520          {
02521             v = rhs.v;
02522             w = rhs.w;
02523             return *this;
02524          }
02525 
02530          template<class FromT>
02531          Quaternion<T>& operator= (const Quaternion<FromT>& rhs)
02532          {
02533             v = rhs.v;
02534             w = static_cast<T>(rhs.w);
02535             return *this;
02536          }
02537         
02542          Quaternion<T> operator+ (const Quaternion<T>& rhs) const
02543          {
02544             const Quaternion<T>& lhs = *this;
02545             return Quaternion<T>(lhs.w + rhs.w, lhs.v + rhs.v);  
02546          }
02547         
02552          Quaternion<T> operator* (const Quaternion<T>& rhs) const
02553          {
02554             const Quaternion<T>& lhs = *this;
02555             return Quaternion<T>(
02556                lhs.w * rhs.w    - lhs.v.x * rhs.v.x     - lhs.v.y * rhs.v.y     - lhs.v.z * rhs.v.z,
02557                lhs.w * rhs.v.x  + lhs.v.x * rhs.w       + lhs.v.y * rhs.v.z     - lhs.v.z * rhs.v.y,
02558                lhs.w * rhs.v.y  - lhs.v.x * rhs.v.z     + lhs.v.y * rhs.w       + lhs.v.z * rhs.v.x,
02559                lhs.w * rhs.v.z  + lhs.v.x * rhs.v.y     - lhs.v.y * rhs.v.x     + lhs.v.z * rhs.w
02560                );
02561          }
02562         
02567          Quaternion<T> operator* (T rhs) const 
02568          { 
02569             return Quaternion<T>(w*rhs, v*rhs);
02570          }
02571         
02576          Quaternion<T> operator- (const Quaternion<T>& rhs) const
02577          {
02578             const Quaternion<T>& lhs = *this;
02579             return Quaternion<T>(lhs.w - rhs.w, lhs.v - rhs.v);
02580          }
02581         
02586          Quaternion<T>& operator+= (const Quaternion<T>& rhs)
02587          {
02588             w += rhs.w;
02589             v += rhs.v;
02590             return *this;
02591          }
02592         
02597          Quaternion<T>& operator-= (const Quaternion<T>& rhs)
02598          {
02599             w -= rhs.w;
02600             v -= rhs.v;
02601             return *this;
02602          }
02603 
02608          Quaternion<T>& operator*= (const Quaternion<T>& rhs)
02609          {
02610             Quaternion q = (*this) * rhs;
02611             v = q.v;
02612             w = q.w;
02613             return *this;
02614          }
02615         
02620          Quaternion<T>& operator*= (T rhs)
02621          {
02622             w *= rhs;
02623             v *= rhs;
02624             return *this;
02625          }
02626 
02634          bool operator==(const Quaternion<T>& rhs) const
02635          {
02636             const Quaternion<T>& lhs = *this;
02637             return (std::fabs(lhs.w - rhs.w) < EPSILON) && lhs.v == rhs.v;
02638          }
02639         
02645          bool operator!=(const Quaternion<T>& rhs) const { return ! (*this == rhs); }
02646 
02647          //-------------[ unary operations ]--------------------------
02652          Quaternion<T> operator-() const
02653          {
02654             return Quaternion<T>(-w, -v);
02655          }
02656 
02661          Quaternion<T> operator~() const
02662          {
02663             return Quaternion<T>(w, -v);
02664          }
02665 
02670          T length() const
02671          {
02672             return (T)std::sqrt(w*w + v.lengthSq());    
02673          }
02674         
02682          T lengthSq() const
02683          {
02684             return w * w + v.lengthSq();        
02685          }
02686         
02687         
02691          void normalize()
02692          {
02693             T len = length();
02694             w /= len;
02695             v /= len;
02696          }
02697         
02705          static Quaternion<T> fromEulerAngles(T x, T y, T z)
02706          {
02707             Quaternion<T> ret = fromAxisRot(Vector3<T>(1,0,0), x)
02708                * fromAxisRot(Vector3<T>(0,1,0),y) * fromAxisRot(Vector3<T>(0,0,1),z);
02709             return ret;
02710          }
02711         
02717          static Quaternion<T> fromAxisRot(Vector3<T> axis, float angleDeg)
02718          {
02719             double angleRad = DEG2RAD(angleDeg);
02720             double sa2 = std::sin(angleRad/2);
02721             double ca2 = std::cos(angleRad/2);
02722             return Quaternion<T>( ca2, axis * sa2);
02723          }
02724         
02729          Matrix3<T> rotMatrix()
02730          {
02731             Matrix3<T> ret;
02732                 
02733             /*ret.at(0,0) = 1 - 2*v.y*v.y - 2*v.z*v.z;
02734               ret.at(1,0) = 2*v.x*v.y - 2*w*v.z;
02735               ret.at(2,0) = 2*v.x*v.z - 2*w*v.y;
02736                 
02737               ret.at(0,1) = 2*v.x*v.y + 2*w*v.z;
02738               ret.at(1,1) = 1 - 2*v.x*v.x - 2*v.z*v.z;
02739               ret.at(2,1) = 2*v.y*v.z - 2*w*v.x;
02740                 
02741               ret.at(0,2) = 2*v.x*v.z - 2*w*v.y;
02742               ret.at(1,2) = 2*v.y*v.z + 2*w*v.x;
02743               ret.at(2,2) = 1 - 2*v.x*v.x - 2*v.y*v.y;*/
02744                 
02745             T xx = v.x * v.x;
02746             T xy = v.x * v.y;
02747             T xz = v.x * v.z;
02748             T xw = v.x * w;
02749                 
02750             T yy = v.y * v.y;
02751             T yz = v.y * v.z;
02752             T yw = v.y * w;
02753                 
02754             T zz = v.z * v.z;
02755             T zw = v.z * w;
02756                 
02757             ret.at(0,0) = 1 - 2 * (yy + zz);
02758             ret.at(1,0) = 2 * (xy - zw);
02759             ret.at(2,0) = 2 * (xz + yw);
02760                 
02761             ret.at(0,1) = 2 * (xy + zw);
02762             ret.at(1,1) = 1 - 2 * (xx + zz);
02763             ret.at(2,1) = 2 * (yz - xw);
02764                 
02765             ret.at(0,2) = 2 * (xz - yw);
02766             ret.at(1,2) = 2 * (yz + xw);
02767             ret.at(2,2) = 1 - 2 * (xx + yy);
02768                 
02769                 
02770             return ret;
02771          }
02772         
02779          Matrix4<T> transform()
02780          {
02781             Matrix4<T> ret;
02782                 
02783             T xx = v.x * v.x;
02784             T xy = v.x * v.y;
02785             T xz = v.x * v.z;
02786             T xw = v.x * w;
02787                 
02788             T yy = v.y * v.y;
02789             T yz = v.y * v.z;
02790             T yw = v.y * w;
02791                 
02792             T zz = v.z * v.z;
02793             T zw = v.z * w;
02794                 
02795             ret.at(0,0) = 1 - 2 * (yy + zz);
02796             ret.at(1,0) = 2 * (xy - zw);
02797             ret.at(2,0) = 2 * (xz + yw);
02798             ret.at(3,0) = 0;
02799                 
02800             ret.at(0,1) = 2 * (xy + zw);
02801             ret.at(1,1) = 1 - 2 * (xx + zz);
02802             ret.at(2,1) = 2 * (yz - xw);
02803             ret.at(3,1) = 0;
02804                 
02805             ret.at(0,2) = 2 * (xz - yw);
02806             ret.at(1,2) = 2 * (yz + xw);
02807             ret.at(2,2) = 1 - 2 * (xx + yy);
02808             ret.at(3,2) = 0;
02809 
02810             ret.at(0,3) = 0;
02811             ret.at(1,3) = 0;
02812             ret.at(2,3) = 0;
02813             ret.at(3,3) = 1;
02814                 
02815             return ret;
02816                 
02817          }
02818 
02828          Quaternion<T> lerp(T fact, const Quaternion<T>& rhs) const
02829          {
02830             return Quaternion<T>((1-fact) * w + fact * rhs.w, v.lerp(fact, rhs.v));
02831          }
02832 
02836          friend std::ostream& operator << (std::ostream& oss, const Quaternion<T>& q)
02837          {
02838             oss << "Re: " << q.w << " Im: " << q.v;
02839             return oss;
02840          }
02841 
02848          static Quaternion<T> fromMatrix(const Matrix4<T>& m)
02849          {
02850             Quaternion<T> q;
02851                                                 
02852             T tr,s;
02853             tr = m.at(0,0) + m.at(1,1) + m.at(2,2);
02854             if (tr >= epsilon)
02855             {
02856                s = (T)sqrt(tr + 1);
02857                q.w = 0.5 * s;
02858                s = 0.5 / s;
02859                                                          
02860                q.v.x = (m.at(1,2) - m.at(2,1)) * s;
02861                q.v.y = (m.at(2,0) - m.at(0,2)) * s;
02862                q.v.z = (m.at(0,1) - m.at(1,0)) * s;
02863             }
02864             else
02865             {
02866                T d0 = m.at(0,0);
02867                T d1 = m.at(1,1);
02868                T d2 = m.at(2,2);
02869                                                          
02870                char bigIdx = (d0 > d1) ? ((d0 > d2)? 0 : 2) : ((d1 > d2) ? 1 : 2);
02871                                                          
02872                if (bigIdx == 0)
02873                {
02874                   s = (T)sqrt((d0 - (d1 + d2)) + 1);
02875                                                                         
02876                   q.v.x = 0.5 * s;
02877                   s = 0.5 / s;
02878                   q.v.y = (m.at(1,0) + m.at(0,1)) * s;
02879                   q.v.z = (m.at(2,0) + m.at(0,2)) * s;
02880                   q.w = (m.at(1,2) - m.at(2,1)) * s;
02881                }
02882                else if (bigIdx == 1)
02883                {
02884                   s = (T)sqrt(1 + d1 - (d0 + d2));
02885                   q.v.y = 0.5 * s;
02886                   s = 0.5 / s;
02887                   q.v.z = (m.at(2,1) + m.at(1,2)) / s;
02888                   q.w = (m.at(2,0) - m.at(0,2)) / s;
02889                   q.v.x = (m.at(1,0) + m.at(0,1)) / s;
02890                }
02891                else
02892                {
02893                   s = (T)sqrt(1 + d2 - (d0 + d1));
02894                   q.v.z = 0.5 * s;
02895                   s = 0.5 / s;
02896                   q.w = (m.at(0,1) - m.at(1,0)) / s;
02897                   q.v.x = (m.at(2,0) + m.at(0,2)) / s;
02898                   q.v.y = (m.at(2,1) + m.at(1,2)) / s;
02899                }
02900             }
02901                                                 
02902             return q;
02903          }
02904                                  
02911          static Quaternion<T> fromMatrix(const Matrix3<T>& m)
02912          {
02913             Quaternion<T> q;
02914                                                 
02915             T tr,s;
02916             tr = m.at(0,0) + m.at(1,1) + m.at(2,2);
02917             // if trace is greater or equal then zero
02918             if (tr >= epsilon)
02919             {
02920                s = (T)sqrt(tr + 1);
02921                q.w = 0.5 * s;
02922                s = 0.5 / s;
02923                                                          
02924                q.v.x = (m.at(1,2) - m.at(2,1)) * s;
02925                q.v.y = (m.at(2,0) - m.at(0,2)) * s;
02926                q.v.z = (m.at(0,1) - m.at(1,0)) * s;
02927             }
02928             else
02929             {
02930                T d0 = m.at(0,0);
02931                T d1 = m.at(1,1);
02932                T d2 = m.at(2,2);
02933                                                          
02934                // find greates diagonal number
02935                char bigIdx = (d0 > d1) ? ((d0 > d2)? 0 : 2) : ((d1 > d2) ? 1 : 2);
02936                                                          
02937                if (bigIdx == 0)
02938                {
02939                   s = (T)sqrt((d0 - (d1 + d2)) + 1);
02940                                                                         
02941                   q.v.x = 0.5 * s;
02942                   s = 0.5 / s;
02943                   q.v.y = (m.at(1,0) + m.at(0,1)) * s;
02944                   q.v.z = (m.at(2,0) + m.at(0,2)) * s;
02945                   q.w = (m.at(1,2) - m.at(2,1)) * s;
02946                }
02947                else if (bigIdx == 1)
02948                {
02949                   s = (T)sqrt(1 + d1 - (d0 + d2));
02950                   q.v.y = 0.5 * s;
02951                   s = 0.5 / s;
02952                   q.v.z = (m.at(2,1) + m.at(1,2)) / s;
02953                   q.w = (m.at(2,0) - m.at(0,2)) / s;
02954                   q.v.x = (m.at(1,0) + m.at(0,1)) / s;
02955                }
02956                else
02957                {
02958                   s = (T)sqrt(1 + d2 - (d0 + d1));
02959                   q.v.z = 0.5 * s;
02960                   s = 0.5 / s;
02961                   q.w = (m.at(0,1) - m.at(1,0)) / s;
02962                   q.v.x = (m.at(2,0) + m.at(0,2)) / s;
02963                   q.v.y = (m.at(2,1) + m.at(1,2)) / s;
02964                }
02965             }
02966                                                 
02967             return q;
02968          }
02969 
02978          Quaternion<T> slerp(T r, const Quaternion<T>& q2) const 
02979          {
02980             Quaternion<T> ret;
02981             T cosTheta = w * q2.w + v.x * q2.v.x + v.y *q2.v.y + v.z * q2.v.z;
02982             T theta = (T) acos(cosTheta);
02983             if (fabs(theta) < epsilon)
02984             {
02985                ret = *this;
02986             }
02987             else
02988             {
02989                T sinTheta = (T)sqrt(1.0 - cosTheta * cosTheta);
02990                if (fabs(sinTheta) < epsilon)
02991                {
02992                   ret.w = 0.5 * w + 0.5 * q2.w;
02993                   ret.v = v.lerp(0.5, q2.v);
02994                }
02995                else
02996                {
02997                   T rA = (T)sin((1.0 - r) * theta) / sinTheta;
02998                   T rB = (T)sin(r * theta) / sinTheta;
02999                                                                         
03000                   ret.w = w * rA + q2.w * rB;
03001                   ret.v.x = v.x * rA + q2.v.x * rB;
03002                   ret.v.y = v.y * rA + q2.v.y * rB;
03003                   ret.v.z = v.z * rA + q2.v.z * rB;
03004                }
03005             }
03006             return ret;
03007          }
03008                                  
03009                                                          
03010         
03011    };
03012 
03013    typedef Quaternion<float> Quatf;
03014    typedef Quaternion<double> Quatd;
03015    
03016    
03017    
03018 #ifdef VMATH_NAMESPACE
03019 }
03020 #endif
03021 
03022 #endif // __vmath_Header_File__
03023 
03024 

Generated on Mon Nov 9 21:47:50 2009 for vmath by  doxygen 1.5.8