/cygdrive/d/src/svn/vrut/trunk/core/src/3dmath.h

Go to the documentation of this file.

/*
 * $Id: 3dmath.h 707 2009-02-19 11:54:49Z bittner $
 *
 * Description  : One line description of file.
 * Author       : Vaclav Kyba <mail/Jabber: vaseo1@gmail.com>  <ICQ: 98576293>
 *
 * Purpose      :
 *     Long description of what the file is for.
 */


#ifndef __MATH__H__
#define __MATH__H__

#include <wx/string.h>
#include <sstream>
#include <memory.h>
#include <cmath>
#include <iostream>
#include <iomanip>
#include "common.h"

struct vector2
{
       union
       {
              struct
              {
                     float x, y;
              };
              float  _v[2];
       };

       vector2(float _x = 0, float _y = 0) : x(_x), y(_y)  {}
       vector2(const float * v) : x(v[0]), y(v[1])         {}
       vector2(const vector2 & v) : x(v.x), y(v.y)         {}
       bool operator!=(const vector2 & v) const
       {
              return (x != v.x) || ( y != v.y);
       }
       bool operator<(const vector2 & v) const
       {
              return (x < v.x) || ((x == v.x) && ( y < v.y));
       }

};


struct vector3
{
       union
       {
              struct
              {
                     float x, y, z;
              };
              float  _v[3];
       };

       vector3(float _x = 0, float _y = 0, float _z = 0) : x(_x), y(_y), z(_z)      {}
       vector3(const float * v) : x(v[0]), y(v[1]), z(v[2])    {}
       vector3(const vector3 & v) : x(v.x), y(v.y), z(v.z)            {}

       vector3 & operator=(const vector3 & v)
       {
              x = v.x;
              y = v.y;
              z = v.z;
              return *this;
       }

       float operator[](unsigned i) const
       {
              return _v[i];
       }

       vector3 operator-(const vector3 & v) const
       {
              return vector3(x-v.x, y-v.y, z-v.z);
       }

       vector3 operator-() const
       {
              return vector3(-x, -y, -z);
       }

       vector3 operator+(const vector3 & v) const
       {
              return vector3(x+v.x, y+v.y, z+v.z);
       }

       vector3 operator*(float f) const
       {
              return vector3(f*x, f*y, f*z);
       }

       friend vector3 operator*(float f, const vector3 & v)
       {
              return vector3(f*v.x, f*v.y, f*v.z);
       }

       vector3 operator/(float f) const
       {
              float div = 1.0f/f;
              return operator*(div);
       }

       friend vector3 operator/(float f, const vector3 & v)
       {
              float div = 1.0f/f;
              return v*div;
       }

       vector3 & operator+=(const vector3 & v)
       {
              x += v.x;
              y += v.y;
              z += v.z;
              return *this;
       }

       vector3 & operator-=(const vector3 & v)
       {
              x -= v.x;
              y -= v.y;
              z -= v.z;
              return *this;
       }

       vector3 & operator*=(float f)
       {
              x *= f;
              y *= f;
              z *= f;
              return *this;
       }

       vector3 & operator/=(float f)
       {
              x /= f;
              y /= f;
              z /= f;
              return *this;
       }

       bool operator==(const vector3 & v) const
       {
              return (x == v.x) && ( y == v.y) && (z == v.z);
       }

       bool operator!=(const vector3 & v) const
       {
              return (x != v.x) || ( y != v.y) || (z != v.z);
       }

       bool operator<(const vector3 & v) const
       {
              return (x < v.x) || ((x == v.x) && ( y < v.y)) || ((x == v.x) && ( y == v.y) && (z < v.z));
       }

       float Dot(const vector3 & v) const
       {
              return x * v.x + y * v.y + z * v.z;
       }

       vector3 Cross(const vector3 & v) const
       {
              return vector3(y*v.z-z*v.y,z*v.x-x*v.z,x*v.y-y*v.x);
       }

       vector3 Normalize() const
       {
              float a = 1.0f/Length();
              return operator*(a);
       }

       float LengthSq() const
       {
              return x*x + y*y + z*z;
       }

       float Length() const
       {
              return sqrtf(x*x + y*y + z*z);
       }

       wxString ToString() const
       {
              return wxString(wxT("(")) << x << wxT(", ") << y << wxT(", ") << z << wxT(")");
       }

       friend std::ostream & operator<<(std::ostream & os, const vector3 & v)
       {
              return os << "(" << v.x << ", " << v.y << ", " << v.z << ")";
       }

       vector3 * Clone() const
       {
              return new vector3(*this);
       }

  friend float Distance(const vector3 &a, const vector3 &b) {
       return (a-b).Length();
  }
  friend float SqrDistance(const vector3 &a, const vector3 &b) {
       return (a-b).LengthSq();
  }

};


struct vector4
{
       union
       {
              struct
              {
                     float x, y, z, w;
              };
              float  _v[4];
       };

       vector4(float _x = 0, float _y = 0, float _z = 0, float _w = 1) : x(_x), y(_y), z(_z), w(_w)      {}
       vector4(const float * v) : x(v[0]), y(v[1]), z(v[2]), w(v[3])  {}
       vector4(const vector4 & v) : x(v.x), y(v.y), z(v.z), w(v.w)           {}
       vector4(const vector3 & v, float _w) : x(v.x), y(v.y), z(v.z), w(_w)  {}

       vector4 & operator=(const vector4 & v)
       {
              x = v.x;
              y = v.y;
              z = v.z;
              w = v.w;
              return *this;
       }

       float operator[](unsigned i) const
       {
              return _v[i];
       }

       vector4 operator*(float f) const
       {
              return vector4(f*x, f*y, f*z, f*w);
       }

       friend vector4 operator*(float f, const vector4 & v)
       {
              return vector4(f*v.x, f*v.y, f*v.z, f*v.w);
       }

       vector4 & operator*=(float f)
       {
              x *= f;
              y *= f;
              z *= f;
              w *= f;
              return *this;
       }

       bool operator==(const vector4 & v) const
       {
              return (x == v.x) && ( y == v.y) && (z == v.z) && (w == v.w);
       }

       bool operator!=(const vector4 & v) const
       {
              return (x != v.x) || ( y != v.y) || (z != v.z) || (w != v.w);
       }

       wxString ToString() const
       {
              return wxString(wxT("(")) << x << wxT(", ") << y << wxT(", ") << z << wxT(", ") << w << wxT(")");
       }

       friend std::ostream & operator<<(std::ostream & os, const vector4 & v)
       {
              return os << "(" << v.x << ", " << v.y << ", " << v.z << ", " << v.w << ")";
       }

       vector4 * Clone() const
       {
              return new vector4(*this);
       }
};


struct plane
{
       union
       {
              struct
              {
                     float a, b, c, d;
              };
              float  _p[4];
       };

       plane(float _a = 0, float _b = 0, float _c = 0, float _d = 0) : a(_a), b(_b), c(_c), d(_d) {}

       plane(const plane & p) : a(p.a), b(p.b), c(p.c), d(p.d)        {}

       plane & operator=(const plane & p)
       {
              a = p.a;
              b = p.b;
              c = p.c;
              d = p.d;
              return *this;
       }

       plane Normalize() const
       {
              float imag = 1.0f / sqrtf(a*a + b*b + c*c);
              return plane(a * imag, b * imag, c * imag, d * imag);
       }

       static plane FromPointNorm(const vector3 & point, const vector3 & normal)
       {
              return plane(normal.x, normal.y, normal.z, -normal.Dot(point));
       }

       static plane FromPoints(const vector3 & p1, const vector3 & p2, const vector3 & p3)
       {
              vector3 v1 = p2 - p1;
              vector3 v2 = p3 - p1;
              vector3 n = v1.Cross(v2);
              return FromPointNorm(p1, n.Normalize());
       }

       float DotCoord(const vector3 & p) const
       {
              return a * p.x + b * p.y + c * p.z + d;
       }

       wxString ToString() const
       {
              return wxString(wxT("(")) << a << wxT(", ") << b << wxT(", ") << c << wxT(", ") << d << wxT(")");
       }

       friend std::ostream & operator<<(std::ostream & os, const plane & p)
       {
              return os << "(" << p.a << ", " << p.b << ", " << p.c << ", " << p.d << ")";
       }

       plane * Clone() const
       {
              return new plane(*this);
       }
};


struct matrix
{
       union
       {
              struct
              {
                     float  _m11, _m12, _m13, _m14,
                                   _m21, _m22, _m23, _m24,
                                   _m31, _m32, _m33, _m34,
                                   _m41, _m42, _m43, _m44;
              };
              float _m[16];
       };

       matrix()
       {
              memset(_m, 0, sizeof(_m));
              _m11 = _m22 = _m33 = _m44 = 1;
       }

       matrix(float _11, float _12, float _13, float _14,
              float _21, float _22, float _23, float _24,
              float _31, float _32, float _33, float _34,
              float _41, float _42, float _43, float _44)
       :_m11(_11), _m12(_12), _m13(_13), _m14(_14),
       _m21(_21), _m22(_22), _m23(_23), _m24(_24),
       _m31(_31), _m32(_32), _m33(_33), _m34(_34),
       _m41(_41), _m42(_42), _m43(_43), _m44(_44)       {}

       matrix(const float * m)
       {
              for (int i = 0; i < 16; i++)
                     _m[i] = m[i];
       }

       matrix(const matrix & m)
       {
              for (int i = 0; i < 16; i++)
                     _m[i] = m._m[i];
       }

       matrix & operator=(const matrix & mat)
       {
              for (int i = 0; i < 16; i++)
                     _m[i] = mat._m[i];
              return *this;
       }

       matrix operator+(const matrix & c) const
       {
              matrix ret;
              float * r = ret._m;
              const float * m1 = _m;
              const float * m2 = c._m;
              for (int i = 0; i < 16; i++)
                     *r++ = *m1++ + *m2++;
              return ret;
       }

       matrix & operator+=(const matrix & c)
       {
              float * m1 = _m;
              const float * m2 = c._m;
              for (int i = 0; i < 16; i++)
                     *m1++ += *m2++;
              return *this;
       }

       matrix operator-(const matrix & c) const
       {
              matrix ret;
              float * r = ret._m;
              const float * m1 = _m;
              const float * m2 = c._m;
              for (int i = 0; i < 16; i++)
                     *r++ = *m1++ - *m2++;
              return ret;
       }

       matrix & operator-=(const matrix & c)
       {
              float * m1 = _m;
              const float * m2 = c._m;
              for (int i = 0; i < 16; i++)
                     *m1++ -= *m2++;
              return *this;
       }

       matrix operator*(const matrix & c) const
       {
              matrix ret;
              ret._m11 = _m11*c._m11+_m12*c._m21+_m13*c._m31+_m14*c._m41;
              ret._m12 = _m11*c._m12+_m12*c._m22+_m13*c._m32+_m14*c._m42;
              ret._m13 = _m11*c._m13+_m12*c._m23+_m13*c._m33+_m14*c._m43;
              ret._m14 = _m11*c._m14+_m12*c._m24+_m13*c._m34+_m14*c._m44;
              ret._m21 = _m21*c._m11+_m22*c._m21+_m23*c._m31+_m24*c._m41;
              ret._m22 = _m21*c._m12+_m22*c._m22+_m23*c._m32+_m24*c._m42;
              ret._m23 = _m21*c._m13+_m22*c._m23+_m23*c._m33+_m24*c._m43;
              ret._m24 = _m21*c._m14+_m22*c._m24+_m23*c._m34+_m24*c._m44;
              ret._m31 = _m31*c._m11+_m32*c._m21+_m33*c._m31+_m34*c._m41;
              ret._m32 = _m31*c._m12+_m32*c._m22+_m33*c._m32+_m34*c._m42;
              ret._m33 = _m31*c._m13+_m32*c._m23+_m33*c._m33+_m34*c._m43;
              ret._m34 = _m31*c._m14+_m32*c._m24+_m33*c._m34+_m34*c._m44;
              ret._m41 = _m41*c._m11+_m42*c._m21+_m43*c._m31+_m44*c._m41;
              ret._m42 = _m41*c._m12+_m42*c._m22+_m43*c._m32+_m44*c._m42;
              ret._m43 = _m41*c._m13+_m42*c._m23+_m43*c._m33+_m44*c._m43;
              ret._m44 = _m41*c._m14+_m42*c._m24+_m43*c._m34+_m44*c._m44;
              return ret;
       }

       matrix operator*(float f) const
       {
              return matrix(       f*_m11, f*_m12, f*_m13, f*_m14,
                                          f*_m21, f*_m22, f*_m23, f*_m24,
                                          f*_m31, f*_m32, f*_m33, f*_m34,
                                          f*_m41, f*_m42, f*_m43, f*_m44 );
       }

       friend matrix operator*(float f, const matrix & m)
       {
              return matrix(       f*m._m11, f*m._m12, f*m._m13, f*m._m14,
                                          f*m._m21, f*m._m22, f*m._m23, f*m._m24,
                                          f*m._m31, f*m._m32, f*m._m33, f*m._m34,
                                          f*m._m41, f*m._m42, f*m._m43, f*m._m44 );
       }

       matrix & operator*=(const matrix & m)
       {
              operator=(*this * m);
              return *this;
       }

       matrix & operator*=(float f)
       {
              for (int i = 0; i < 16; i++)
                     _m[i] *= f;
              return *this;
       }

       bool operator==(const matrix & m) const
       {
              for (int i=0; i<16; i++)
                     if (_m[i]!=m._m[i])
                            return false;
              return true;
       }

       bool IsUnit() const
       {
              for (int i=0; i<4; i++)
                     for (int j=0; j<4; j++)
                            if (_m[i*4+j]!=((i==j)?1:0))
                                   return false;
              return true;
       }

       vector3 ExtractTranslation() const
       {
              return vector3(_m41, _m42, _m43);
       }

       matrix ExtractRotation() const
       {
              matrix mat(_m);
              mat._m41 = mat._m42 = mat._m43 = mat._m14 = mat._m24 = mat._m34 = 0;
              mat._m44 = 1;
              return mat;
       }

       vector3 TransformCoord(const vector3 & v) const
       {
              vector3 ret;
              ret.x = v.x * _m11 + v.y * _m21 + v.z * _m31 + _m41;
              ret.y = v.x * _m12 + v.y * _m22 + v.z * _m32 + _m42;
              ret.z = v.x * _m13 + v.y * _m23 + v.z * _m33 + _m43;
//            float w = v.x * _m14 + v.y * _m24 + v.z * _m34 + _m44;
//            if (w != 1)
//                   return ret;
              return ret;
       }

       vector3 TransformNormal(const vector3 & v) const
       {
              vector3 ret;
              ret.x = v.x * _m11 + v.y * _m21 + v.z * _m31;
              ret.y = v.x * _m12 + v.y * _m22 + v.z * _m32;
              ret.z = v.x * _m13 + v.y * _m23 + v.z * _m33;
              return ret;
       }

       matrix Inverse() const
       {
              matrix ret;

              ret._m[0] = _m[0];
              ret._m[1] = _m[4];
              ret._m[2] = _m[8];
              ret._m[4] = _m[1];
              ret._m[5] = _m[5];
              ret._m[6] = _m[9];
              ret._m[8] = _m[2];
              ret._m[9] = _m[6];
              ret._m[10] = _m[10];

              ret._m[12] = ret._m[0]*-_m[12]+ret._m[4]*-_m[13]+ret._m[8]*-_m[14];
              ret._m[13] = ret._m[1]*-_m[12]+ret._m[5]*-_m[13]+ret._m[9]*-_m[14];
              ret._m[14] = ret._m[2]*-_m[12]+ret._m[6]*-_m[13]+ret._m[10]*-_m[14];

              ret._m[3] = 0.0f;
              ret._m[7] = 0.0f;
              ret._m[11] = 0.0f;
              ret._m[15] = 1.0f;

              return ret;
       }

       float Det3x3() const
       {
              return _m11*_m22*_m33 + _m12*_m23*_m31
                     + _m13*_m21*_m32 - _m13*_m22*_m31
                     - _m11*_m23*_m32 - _m12*_m21*_m33;
       }

       bool InvertThisPrecise()
       {
              int indxc[4],indxr[4],ipiv[4];
              int i,icol,irow,j,k,l,ll,n;
              float big,dum,pivinv,temp;
              // satisfy the compiler
              icol = irow = 0;
              // the size of the matrix
              n = 4;

              for ( j = 0 ; j < n ; j++) /* zero pivots */
                     ipiv[j] = 0;
              for ( i = 0; i < n; i++)
              {
                     big = 0.0;
                     for (j = 0 ; j < n ; j++)
                            if (ipiv[j] != 1)
                                   for ( k = 0 ; k<n ; k++)
                                   {
                                          if (ipiv[k] == 0)
                                          {
                                                 if (fabs(_m[4*k+j]) >= big)
                                                 {
                                                        big = fabs(_m[4*k+j]);
                                                        irow = j;
                                                        icol = k;
                                                 }
                                          }
                                          else
                                                 if (ipiv[k] > 1)
                                                        return false; /* singular matrix */
                                   }
                                   ++(ipiv[icol]);
                                   if (irow != icol)
                                   {
                                          for ( l = 0 ; l<n ; l++)
                                          {
                                                 temp = _m[4*l+icol];
                                                 _m[4*l+icol] = _m[4*l+irow];
                                                 _m[4*l+irow] = temp;
                                          }
                                   }
                                   indxr[i] = irow;
                                   indxc[i] = icol;
                                   if (_m[4*icol+icol] == 0.0)
                                          return false; /* singular matrix */

                                   pivinv = 1.0f / _m[4*icol+icol];
                                   _m[4*icol+icol] = 1.0 ;
                                   for ( l = 0 ; l<n ; l++)
                                          _m[4*l+icol] = _m[4*l+icol] * pivinv ;
                                   for (ll = 0 ; ll < n ; ll++)
                                          if (ll != icol)
                                          {
                                                 dum = _m[4*icol+ll];
                                                 _m[4*icol+ll] = 0.0;
                                                 for ( l = 0 ; l<n ; l++)
                                                        _m[4*l+ll] = _m[4*l+ll] - _m[4*l+icol] * dum ;
                                          }
              }
              for ( l = n; l--; )
              {
                     if (indxr[l] != indxc[l])
                            for ( k = 0; k<n ; k++)
                            {
                                   temp = _m[4*indxr[l]+k];
                                   _m[4*indxr[l]+k] = _m[4*indxc[l]+k];
                                   _m[4*indxc[l]+k] = temp;
                            }
              }
              return true; // matrix is regular .. inversion has been succesfull
       }

       matrix Transpose() const
       {
              matrix mat;
              for (int i = 0; i < 4; i++)
                     for (int j = 0; j < 4; j++)
                            mat._m[4*j+i] = _m[4*i+j];
              return mat;
       }

       matrix & Translate(const vector3 & v)
       {
              *this *= Translation(v);
              return *this;
       }

       matrix & Scale(const vector3 & v)
       {
              _m11 *= v.x;
              _m22 *= v.y;
              _m33 *= v.z;
              return *this;
       }

       //TODO: check correctness in all special cases
       //TODO: update to handle arbitrary matrix 
       void ToEuler(float &x, float &y, float &z) const
       {
              x=-atan2(_m23, _m33);
              y=asin(-_m13);
              z=-atan2(_m12, _m11);
       }

       //TODO: check correctness in all special cases
       //TODO: update to handle arbitrary matrix 
       void ToAxisAngle(vector3 &axis, float &angle) const
       {
              float epsilon = 0.01f; // margin to allow for rounding errors
              float epsilon2 = 0.1f; // margin to distinguish between 0 and 180 degrees
              // optional check that input is pure rotation, 'isRotationMatrix' is defined at:
              // http://www.euclideanspace.com/maths/algebra/matrix/orthogonal/rotation/
              //assert isRotationMatrix(m) : "not valid rotation matrix" ;// for debugging
              if ((fabs(_m12-_m21)<EPSILON) && (fabs(_m13-_m31)<EPSILON) && (fabs(_m23-_m32)<EPSILON)) {
                            // singularity found
                            // first check for identity matrix which must have +1 for all terms
                            //  in leading diagonaland zero in other terms
                            if ((fabs(_m12+_m21) < epsilon2) && (fabs(_m13+_m31) < epsilon2) && (fabs(_m23+_m32) < epsilon2) && (fabs(_m11+_m22+_m33-3) < epsilon2)) {
                                          // this singularity is identity matrix so angle = 0 and arbitrary axis
                                          axis=vector3(0, 1, 0);
                                          angle=0;
                            }
                            // otherwise this singularity is angle = 180
                            angle = 3.14159f;
                            float xx = (_m11+1)/2;
                            float yy = (_m22+1)/2;
                            float zz = (_m33+1)/2;
                            float xy = (_m12+_m21)/4;
                            float xz = (_m13+_m31)/4;
                            float yz = (_m23+_m32)/4;
                            if ((xx > yy) && (xx > zz)) { // _m11 is the largest diagonal term
                                   if (xx< epsilon) {
                                          axis=vector3(0.0f, 0.7071f, 0.7071f);
                                   } else {
                                          xx=sqrt(xx);
                                          axis=vector3(xx, xy/xx, xz/xx);
                                   }
                            } else if (yy > zz) { // _m22 is the largest diagonal term
                                   if (yy< epsilon) {
                                          axis=vector3(0.7071f, 0.0f, 0.7071f);
                                   } else {
                                          yy=sqrt(yy);
                                          axis=vector3(xy/yy, yy, yz/yy);
                                   }      
                            } else { // _m33 is the largest diagonal term so base result on this
                                   if (zz< epsilon) {
                                          axis=vector3(0.7071f, 0.7071f, 0.0f);
                                   } else {
                                          zz = sqrt(zz);
                                          axis=vector3(xz/zz, yz/zz, zz);
                                   }
                            }
                            return;// return 180 deg rotation
              }
              // as we have reached here there are no singularities so we can handle normally
//            float s = sqrt((_m32 - _m23)*(_m32 - _m23) + (_m13 - _m31)*(_m13 - _m31) + (_m21 - _m12)*(_m21 - _m12)); // used to normalise
//            if (fabs(s) < 0.001) s=1; 
              // prevent divide by zero, should not happen if matrix is orthogonal and should be
              // caught by singularity test above, but I've left it in just in case
              angle = acos(( _m11 + _m22 + _m33 - 1)/2);
              axis=vector3(_m32 - _m23, _m13 - _m31, _m21 - _m12).Normalize();
       }

       static matrix ScaleMat(const vector3 & v)
       {
              matrix ret;
              ret._m11 = v.x;
              ret._m22 = v.y;
              ret._m33 = v.z;
              return ret;
       }

       static matrix Translation(const vector3 & v)
       {
              matrix mat;
              mat._m41 = v.x;
              mat._m42 = v.y;
              mat._m43 = v.z;
              return mat;
       }

       static matrix RotationX(float angle)
       {
              matrix M;
              float Cosine = cos(angle);
              float Sine = sin(angle);
              M._m22 = Cosine;
              M._m23 = -Sine;
              M._m32 = Sine;
              M._m33 = Cosine;
              return M;
       }

       static matrix RotationY(float angle)
       {
              matrix M;
              float Cosine = cos(angle);
              float Sine = sin(angle);
              M._m11 = Cosine;
              M._m13 = -Sine;
              M._m31 = Sine;
              M._m33 = Cosine;
              return M;
       }

       static matrix RotationZ(float angle)
       {
              matrix M;
              float Cosine = cos(angle);
              float Sine = sin(angle);
              M._m11 = Cosine;
              M._m12 = -Sine;
              M._m21 = Sine;
              M._m22 = Cosine;
              return M;
       }

       static matrix RotationYPR(float Yaw, float Pitch, float Roll)
       {
              matrix M;
              float ch = cos(Yaw);
              float sh = sin(Yaw);
              float cp = cos(Pitch);
              float sp = sin(Pitch);
              float cr = cos(Roll);
              float sr = sin(Roll);

              M._m11 = ch * cr + sh * sp * sr;
              M._m12 = -ch * sr + sh * sp * cr;
              M._m13 = sh * cp;
              M._m21 = sr * cp;
              M._m22 = cr * cp;
              M._m23 = -sp;
              M._m31 = -sh * cr - ch * sp * sr;
              M._m32 = sr * sh + ch * sp * cr;
              M._m33 = ch * cp;

              return M;
       }

       static matrix RotationAxis(const vector3 & axis, float angle)
       {
              matrix M;
              float cosine = cos(angle);
              float sine = sin(angle);
              float one_minus_cosine = 1 - cosine;

              M._m11 = axis.x * axis.x + (1.0f - axis.x * axis.x) * cosine;
              M._m21 = axis.x * axis.y * one_minus_cosine + axis.z * sine;
              M._m31 = axis.x * axis.z * one_minus_cosine - axis.y * sine;
              M._m41 = 0;

              M._m12 = axis.x * axis.y * one_minus_cosine - axis.z * sine;
              M._m22 = axis.y * axis.y + (1.0f - axis.y * axis.y) * cosine;
              M._m32 = axis.y * axis.z * one_minus_cosine + axis.x * sine;
              M._m42 = 0;

              M._m13 = axis.x * axis.z * one_minus_cosine + axis.y * sine;
              M._m23 = axis.y * axis.z * one_minus_cosine - axis.x * sine;
              M._m33 = axis.z * axis.z + (1.0f - axis.z * axis.z) * cosine;
              M._m43 = 0;

              M._m14 = 0;
              M._m24 = 0;
              M._m34 = 0;
              M._m44 = 1;

              return M;
       }


       static matrix RotationVectors(const vector3 & vecStart, const vector3 & vecTo)
       {
              vector3 vec = vecStart.Cross(vecTo);

              if (vec.Length() > 1e-6f)
              {
                     // vector exist, compute angle
                     float angle = acos(vecStart.Dot(vecTo));
                     // normalize for sure
                     vec = vec.Normalize();
                     return RotationAxis(vec, angle);
              }

              // opposite or colinear vectors
              matrix ret;
              if (vecStart.Dot(vecTo) < 0.0f)
                     ret *= -1.0f; // opposite vectors

              return ret;
       }

       wxString ToString() const
       {
              wxString str;
              for (int i = 0; i < 4; i++)
              { // y
                     for (int j = 0; j < 4; j++)
                     { // x
                            str.Append(wxString::Format(wxT("%.2f "), _m[4*i+j]));
                     }
                     str << wxT("\n");
              }
              return str;
       }

       friend std::ostream & operator<<(std::ostream & os, const matrix & M)
       {
              for (int i = 0; i < 4; i++)
              { // y
                     for (int j = 0; j < 4; j++)
                     { // x
                            os << std::setprecision(4) << std::setw(10) << M._m[4*i+j] << " ";
                     }
                     os << '\n';
              }
              return os;
       }

       matrix * Clone() const
       {
              return new matrix(*this);
       }
};


struct Ray
{
public:
       vector3 origin;
       vector3 direction;

       Ray(vector3 & orig, vector3 & dir)
       {
              origin = orig;
              direction = dir;
       }
       Ray(const Ray & ray) : origin(ray.origin), direction(ray.direction)
       {
       }

       float GetIntersectionDist(const plane & pln) const
       {
              return -pln.DotCoord(origin) / direction.Dot(vector3(pln._p));
       }

       vector3 GetIntersection(const plane & pln) const
       {
              return origin + direction * GetIntersectionDist(pln);
       }

       //TODO: not working in some cases
       bool IntersectsTri(const vector3 & v0, const vector3 & v1, const vector3 & v2, float * dist = (float *)NULL) const
       {
#define MOLLER_RAY_TRI_INTERSECTION 1
#if !MOLLER_RAY_TRI_INTERSECTION

         const vector3 edge1 = v1 - v0;
              const vector3 edge2 = v2 - v0;

              /* begin calculating determinant - also used to calculate U parameter */
              vector3 pvec = direction.Cross(edge2);

              /* if determinant is near zero, ray lies in plane of triangle */
              const float det = fabsf( edge1.Dot(pvec) ); // = vector3U::TripleProduct(dir,edge1,edge2);

              /* calculate distance from vert0 to ray origin */
              const vector3 tvec = origin - v0;

              /* calculate U parameter and test bounds */
              const float u = tvec.Dot(pvec); // = vector3U::TripleProduct(dir,edge2,tvec);
              if ( u < 0 || u > det )
                     return false;

              /* prepare to test V parameter */
              /* calculate V parameter and test bounds */
              const float v = direction.Dot( tvec.Cross(edge1) ); // = dir * (tvec ^ edge1)
              if ( v < 0 || u + v > det )
                     return false;

              float intDist = GetIntersectionDist(plane::FromPoints(v0, v1, v2));
              if (intDist < 0)
                     return false;
              else if (dist)
                     *dist = intDist;

              return true;
#else
              const float threshold = 1e-6f;
              const vector3 e1 = v0 - v1;
              const vector3 e2 = v2 - v1;
              const vector3 dir = direction;
              const vector3 p = dir.Cross(e2);
              float det = e1.Dot(p);
              const vector3 w0 = origin - v1;
              const float u = w0.Dot(p);
              const vector3 q = w0.Cross(e1);
              const float v = dir.Dot(q);


              if (det > threshold) {
                if (u < 0.0f || u > det)
                     return false;
                if (v < 0.0f || u + v > det)
                     return false;
              }
              else
                if (det < -threshold)
                     {
                       if (u > 0.0 || u < det)
                            return false;
                       if (v > 0.0 || u + v < det)
                            return false;
                     } else
                       return false;

              float t = e2.Dot(q) / det;

              if (t < 0.0f)
                return false;

              if (dist)
                *dist = t;
              return true;
#endif

       }

       Ray * Clone() const
       {
              return new Ray(*this);
       }
};

struct Triangle
{
       vector3 v1;
       vector3 v2;
       vector3 v3;

       Triangle() {}

       Triangle(const vector3 &_v1, const vector3 &_v2, const vector3 &_v3) : v1(_v1), v2(_v2), v3(_v3) {}

       Triangle & operator=(const Triangle & tr)
       {
              v1 = tr.v1;
              v2 = tr.v2;
              v3 = tr.v3;
              return *this;
       }

       vector3 operator[](unsigned i) const
       {
              if (i == 0)
                     return v1;
              else if (i == 1)
                     return v2;
              else if (i == 2)
                     return v3;

              return vector3();
       }

       vector3 GetCenter()
       {
              return (v1 + v2 + v3) / 3.0f;
       }
};

inline vector3 CrossProd(const vector3 & a, const vector3 & b)
{
  return a.Cross(b);
}

inline vector3 Normalize(const vector3 &a)
{
  vector3 b = a;
  return b.Normalize();
}

#endif

---