# Mathlib.h

Go to the documentation of this file.
```00001 /* -*- c++ -*- */
00002 /////////////////////////////////////////////////////////////////////////////
00003 //
00004 // Mathlib.h -- Copyright (c) 2005-2006 David Henry
00005 // last modification: feb. 19, 2006
00006 //
00007 // This code is licenced under the MIT license.
00008 //
00009 // This software is provided "as is" without express or implied
00010 // warranties. You may freely copy and compile this source into
00011 // applications you distribute provided that the copyright text
00012 // below is included in the resulting source code.
00013 //
00014 // Declarations for 3D maths object and functions to use with OpenGL.
00015 //
00016 // Provide vector, matrix and quaternion operations.
00017 //
00018 /////////////////////////////////////////////////////////////////////////////
00019
00020 #ifndef __MATHLIB_H__
00021 #define __MATHLIB_H__
00022
00023 #include <cmath>
00024
00025 // Forward declarations
00026 template <typename Real> class Vector3;
00027 template <typename Real> class Matrix4x4;
00028 template <typename Real> class Quaternion;
00029
00030 // Type definitions
00031 enum Axis {
00032   kXaxis, kYaxis, kZaxis
00033 };
00034
00035 // Declare a global constant for pi and a few multiples.
00036 const float kPi = 3.14159265358979323846f;
00037 const float k2Pi = kPi * 2.0f;
00038 const float kPiOver2 = kPi / 2.0f;
00039 const float k1OverPi = 1.0f / kPi;
00040 const float k1Over2Pi = 1.0f / k2Pi;
00041 const float kPiOver180 = kPi / 180.0f;
00042 const float k180OverPi = 180.0f / kPi;
00043
00044 // "Wrap" an angle in range -pi...pi by adding the correct multiple
00045 // of 2 pi
00046 template <typename Real>
00047 Real wrapPi (Real theta);
00048
00049 // "Safe" inverse trig functions
00050 template <typename Real>
00051 Real safeAcos (Real x);
00052
00053 // Set the Euler angle triple to its "canonical" value
00054 template <typename Real>
00055 void canonizeEulerAngles (Real &roll, Real &pitch, Real &yaw);
00056
00057 // Convert between degrees and radians
00058 template <typename Real>
00059 inline Real degToRad (Real deg) { return deg * kPiOver180; }
00060
00061 template <typename Real>
00063
00064 // Convert between "field of view" and "zoom".
00065 // The FOV angle is specified in radians.
00066 template <typename Real>
00067 inline Real fovToZoom (Real fov) { return 1.0f / std::tan (fov * 0.5f); }
00068
00069 template <typename Real>
00070 inline Real zoomToFov (Real zoom) { return 2.0f * std::atan (1.0f / zoom); }
00071
00072
00073 /////////////////////////////////////////////////////////////////////////////
00074 //
00075 // class Vector3<Real> - A simple 3D vector class.
00076 //
00077 /////////////////////////////////////////////////////////////////////////////
00078
00079 template <typename Real>
00080 class Vector3
00081 {
00082 public:
00083   // Constructors
00084   Vector3 () { }
00085   Vector3 (Real x, Real y, Real z)
00086     : _x (x), _y (y), _z (z) { }
00087
00088 public:
00089   // Vector comparison
00090   bool operator== (const Vector3<Real> &v) const;
00091   bool operator!= (const Vector3<Real> &v) const;
00092
00093   // Vector negation
00094   Vector3<Real> operator- () const;
00095
00096   // Vector operations
00097   Vector3<Real> operator+ (const Vector3<Real> &v) const;
00098   Vector3<Real> operator- (const Vector3<Real> &v) const;
00099   Vector3<Real> operator* (Real s) const;
00100   Vector3<Real> operator/ (Real s) const;
00101
00102   // Combined assignment operators to conform to
00103   // C notation convention
00104   Vector3<Real> &operator+= (const Vector3<Real> &v);
00105   Vector3<Real> &operator-= (const Vector3<Real> &v);
00106   Vector3<Real> &operator*= (Real s);
00107   Vector3<Real> &operator/= (Real s);
00108
00109   // Accessor.  This allows to use the vector object
00110   // like an array of Real. For example:
00111   // Vector3<float> v (...);
00113   operator const Real *() { return _v; }
00114
00115 public:
00116   // Other vector operations
00117   bool isZero ();
00118   void normalize ();
00119
00120 public:
00121   // Member variables
00122   union
00123   {
00124     struct
00125     {
00126       Real _x, _y, _z;
00127     };
00128
00129     Real _v[3];
00130   };
00131 };
00132
00133
00134 // Predefined Vector3 types
00135 typedef Vector3<float> Vector3f;
00136 typedef Vector3<double> Vector3d;
00137
00138 // We provide a global constant zero vector
00139 static const Vector3f kZeroVectorf (0.0f, 0.0f, 0.0f);
00140 static const Vector3d kZeroVectord (0.0, 0.0, 0.0);
00141
00142
00143 //
00144 // Nonmember Vector3 functions
00145 //
00146
00147 template <typename Real>
00148 Vector3<Real> operator* (Real k, Vector3<Real> v);
00149
00150 template <typename Real>
00151 Real VectorMag (const Vector3<Real> &v);
00152
00153 template <typename Real>
00154 Real DotProduct (const Vector3<Real> &a, const Vector3<Real> &b);
00155
00156 template <typename Real>
00157 Vector3<Real> CrossProduct (const Vector3<Real> &a, const Vector3<Real> &b);
00158
00159 template <typename Real>
00160 Vector3<Real> ComputeNormal (const Vector3<Real> &p1,
00161      const Vector3<Real> &p2, const Vector3<Real> &p3);
00162
00163 template <typename Real>
00164 Real Distance (const Vector3<Real> &a, const Vector3<Real> &b);
00165
00166 template <typename Real>
00167 Real DistanceSquared (const Vector3<Real> &a, const Vector3<Real> &b);
00168
00169
00170 /////////////////////////////////////////////////////////////////////////////
00171 //
00172 // class Matrix4x4<Real> - Implement a 4x4 Matrix class that can represent
00173 // any 3D affine transformation.
00174 //
00175 /////////////////////////////////////////////////////////////////////////////
00176
00177 template <typename Real>
00178 class Matrix4x4
00179 {
00180 public:
00181   // Constructor - Initialize the last (never used) row of the matrix
00182   // so that we can do any operation on matrices on the 3x4 portion
00183   // and forget that line which will (and should) never change.
00184   Matrix4x4 ()
00185     : _h14 (0.0f), _h24 (0.0f), _h34 (0.0f), _tw (1.0f) { }
00186
00187   // Note that we don't define the copy constructor and let the compiler
00188   // doing it itself because such initialization is not necessary
00189   // since the source matrix has its last row already initialized...
00190
00191 public:
00192   // Public interface
00193   void identity ();
00194   void transpose ();
00195   void invert ();
00196   void setTranslation (const Vector3<Real> &v);
00197
00198   void transform (Vector3<Real> &v) const;
00199   void rotate (Vector3<Real> &v) const;
00200   void inverseRotate (Vector3<Real> &v) const;
00201   void inverseTranslate (Vector3<Real> &v) const;
00202
00203   void fromQuaternion (const Quaternion<Real> &q);
00204
00205   // Matrix <-> Euler conversions; XYZ rotation order; angles in radians
00206   void fromEulerAngles (Real x, Real y, Real z);
00207   void toEulerAngles (Real &x, Real &y, Real &z) const;
00208
00209   // Return a base vector from the matrix
00210   Vector3<Real> rightVector () const;
00211   Vector3<Real> upVector () const;
00212   Vector3<Real> forwardVector () const;
00213   Vector3<Real> translationVector () const;
00214
00215   // Accessor.  This allows to use the matrix object
00216   // like an array of Real. For example:
00217   // Matrix4x4<float> mat;
00219   operator const Real *() { return _m; }
00220
00221 public:
00222   // Member variables
00223
00224   // The values of the matrix.  Basically the upper 3x3 portion
00225   // contains a linear transformation, and the last column is the
00226   // translation portion. Here data is transposed, see the Mathlib.inl
00227   // for more details.
00228   union
00229   {
00230     struct
00231     {
00232       Real _m11, _m12, _m13, _h14;
00233       Real _m21, _m22, _m23, _h24;
00234       Real _m31, _m32, _m33, _h34;
00235       Real _tx,  _ty,  _tz,  _tw;
00236     };
00237
00239     // glLoadMatrixf () and glMultMatrixf ())
00240     Real _m[16];
00241   };
00242 };
00243
00244
00245 // Predefined Matrix4x4 types
00246 typedef Matrix4x4<float> Matrix4x4f;
00247 typedef Matrix4x4<double> Matrix4x4d;
00248
00249
00250 //
00251 // Nonmember Matrix4x4 functions
00252 //
00253
00254 // Matrix concatenation
00255 template <typename Real>
00256 Matrix4x4<Real> operator* (const Matrix4x4<Real> &a, const Matrix4x4<Real> &b);
00257
00258 template <typename Real>
00259 Matrix4x4<Real> &operator*= (Matrix4x4<Real> &a, const Matrix4x4<Real> &b);
00260
00261 // Vector transformation
00262 template <typename Real>
00263 Vector3<Real> operator* (const Matrix4x4<Real> &m, const Vector3<Real> &p);
00264
00265 // Transpose matrix
00266 template <typename Real>
00267 Matrix4x4<Real> Transpose (const Matrix4x4<Real> &m);
00268
00269 // Invert matrix
00270 template <typename Real>
00271 Matrix4x4<Real> Invert (const Matrix4x4<Real> &m);
00272
00273 //
00274 // Matrix-builder functions
00275 //
00276
00277 template <typename Real> Matrix4x4<Real> RotationMatrix (Axis axis, Real theta);
00278 template <typename Real> Matrix4x4<Real> RotationMatrix (const Vector3<Real> &axis, Real theta);
00279 template <typename Real> Matrix4x4<Real> TranslationMatrix (Real x, Real y, Real z);
00280 template <typename Real> Matrix4x4<Real> TranslationMatrix (const Vector3<Real> &v);
00281 template <typename Real> Matrix4x4<Real> ScaleMatrix (const Vector3<Real> &s);
00282 template <typename Real> Matrix4x4<Real> ScaleAlongAxisMatrix (const Vector3<Real> &axis, Real k);
00283 template <typename Real> Matrix4x4<Real> ShearMatrix (Axis axis, Real s, Real t);
00284 template <typename Real> Matrix4x4<Real> ProjectionMatrix (const Vector3<Real> &n);
00285 template <typename Real> Matrix4x4<Real> ReflectionMatrix (Axis axis, Real k);
00286 template <typename Real> Matrix4x4<Real> AxisReflectionMatrix (const Vector3<Real> &n);
00287
00288 template <typename Real>
00289 Matrix4x4<Real> LookAtMatrix (const Vector3<Real> &camPos,
00290         const Vector3<Real> &target, const Vector3<Real> &camUp);
00291 template <typename Real>
00292 Matrix4x4<Real> FrustumMatrix (Real l, Real r, Real b, Real t, Real n, Real f);
00293 template <typename Real>
00294 Matrix4x4<Real> PerspectiveMatrix (Real fovY, Real aspect, Real n, Real f);
00295 template <typename Real>
00296 Matrix4x4<Real> OrthoMatrix (Real l, Real r, Real b, Real t, Real n, Real f);
00297 template <typename Real>
00298 Matrix4x4<Real> OrthoNormalMatrix (const Vector3<Real> &xdir,
00299         const Vector3<Real> &ydir, const Vector3<Real> &zdir);
00300
00301
00302 /////////////////////////////////////////////////////////////////////////////
00303 //
00304 // class Quaternion<Real> - Implement a quaternion, for purposes of
00305 // representing an angular displacement (orientation) in 3D.
00306 //
00307 /////////////////////////////////////////////////////////////////////////////
00308
00309 template <typename Real>
00310 class Quaternion
00311 {
00312 public:
00313   // Constructors
00314   Quaternion () { }
00315   Quaternion (Real w, Real x, Real y, Real z)
00316     : _w (w), _x (x), _y (y), _z (z) { }
00317
00318 public:
00319   // Public interface
00320   void identity ();
00321   void normalize ();
00322   void computeW ();
00323   void rotate (Vector3<Real> &v) const;
00324
00325   void fromMatrix (const Matrix4x4<Real> &m);
00326
00327   // Quaternion <-> Euler conversions; XYZ rotation order; angles in radians
00328   void fromEulerAngles (Real x, Real y, Real z);
00329   void toEulerAngles (Real &x, Real &y, Real &z) const;
00330
00331   Real rotationAngle () const;
00332   Vector3<Real> rotationAxis () const;
00333
00334   // Quaternion operations
00335   Quaternion<Real> operator+ (const Quaternion<Real> &q) const;
00336   Quaternion<Real> &operator+= (const Quaternion<Real> &q);
00337
00338   Quaternion<Real> operator- (const Quaternion<Real> &q) const;
00339   Quaternion<Real> &operator-= (const Quaternion<Real> &q);
00340
00341   Quaternion<Real> operator* (const Quaternion<Real> &q) const;
00342   Quaternion<Real> &operator*= (const Quaternion<Real> &q);
00343
00344   Quaternion<Real> operator* (Real k) const;
00345   Quaternion<Real> &operator*= (Real k);
00346
00347   Quaternion<Real> operator* (const Vector3<Real> &v) const;
00348   Quaternion<Real> &operator*= (const Vector3<Real> &v);
00349
00350   Quaternion<Real> operator/ (Real k) const;
00351   Quaternion<Real> &operator/= (Real k);
00352
00353   Quaternion<Real> operator~ () const; // Quaternion conjugate
00354   Quaternion<Real> operator- () const; // Quaternion negation
00355
00356 public:
00357   // Member variables
00358
00359   // The 4 values of the quaternion.  Normally, it will not
00360   // be necessary to manipulate these directly.  However,
00361   // we leave them public, since prohibiting direct access
00362   // makes some operations, such as file I/O, unnecessarily
00363   // complicated.
00364
00365   union
00366   {
00367     struct
00368     {
00369       Real _w, _x, _y, _z;
00370     };
00371
00372     Real _q[4];
00373   };
00374 };
00375
00376
00377 // Predefined Quaternion types
00378 typedef Quaternion<float> Quaternionf;
00379 typedef Quaternion<double> Quaterniond;
00380
00381 // A global "identity" quaternion constant
00382 static const Quaternionf kQuaternionIdentityf (1.0f, 0.0f, 0.0f, 0.0f);
00383 static const Quaterniond kQuaternionIdentityd (1.0f, 0.0f, 0.0f, 0.0f);
00384
00385
00386 //
00387 // Nonmember Matrix4x functions
00388 //
00389
00390 template <typename Real>
00391 Quaternion<Real> operator* (Real k, const Quaternion<Real> &q);
00392
00393 template <typename Real>
00394 Real DotProduct (const Quaternion<Real> &a, const Quaternion<Real> &b);
00395
00396 template <typename Real>
00397 Quaternion<Real> Conjugate (const Quaternion<Real> &q);
00398
00399 template <typename Real>
00400 Quaternion<Real> Inverse (const Quaternion<Real> &q);
00401
00402 template <typename Real>
00403 Quaternion<Real> RotationQuaternion (Axis axis, Real theta);
00404
00405 template <typename Real>
00406 Quaternion<Real> RotationQuaternion (const Vector3<Real> &axis, Real theta);
00407
00408 template <typename Real>
00409 Quaternion<Real> Log (const Quaternion<Real> &q);
00410 template <typename Real>
00411 Quaternion<Real> Exp (const Quaternion<Real> &q);
00412 template <typename Real>
00413 Quaternion<Real> Pow (const Quaternion<Real> &q, Real exponent);
00414
00415 template <typename Real>
00416 Quaternion<Real> Slerp (const Quaternion<Real> &q0, const Quaternion<Real> &q1, Real t);
00417 template <typename Real>
00418 Quaternion<Real> Squad (const Quaternion<Real> &q0, const Quaternion<Real> &qa,
00419                         const Quaternion<Real> &qb, const Quaternion<Real> &q1, Real t);
00420 template <typename Real>
00421 inline void Intermediate (const Quaternion<Real> &qprev, const Quaternion<Real> &qcurr,
00422                           const Quaternion<Real> &qnext, Quaternion<Real> &qa,
00423                           Quaternion<Real> &qb);
00424
00425 // Include inline function definitions
00426 #include "Mathlib.inl"
00427
00428 #endif // __MATHLIB_H__
```