---

math3d.h

Go to the documentation of this file.

#ifndef H_MATH3D
#define H_MATH3D

#include <math.h>

struct Vector3D;
struct Matrix3;
struct Matrix3D;
struct Plane; 

inline float flabs(float f) { return (f>=0.0f?f:-f); }
const float epsilon=1e-8f;
inline bool IsZero(float f) { return flabs(f)<epsilon; }

Vector3D operator*(float scalar, const Vector3D& v);
float    operator*(const Vector3D& v1, const Vector3D& v2);
Vector3D operator+(const Vector3D& v1, const Vector3D& v2);
Vector3D operator-(const Vector3D& v1, const Vector3D& v2);
Vector3D CrossProduct(const Vector3D& v1, const Vector3D& v2);
Matrix3D operator*(const Matrix3D& m1, const Matrix3D& m2);
Matrix3D operator*(float scalar, const Matrix3D& m);

struct Vector3D
{
  float x,y,z;
  static const Vector3D Zero;

  Vector3D() {}
  Vector3D(float X, float Y, float Z) : x(X), y(Y), z(Z) {}
  Vector3D(const Vector3D& v) : x(v.x), y(v.y), z(v.z) {}

  Vector3D& operator+=(const Vector3D& v) { x+=v.x; y+=v.y; z+=v.z; return *this; }
  Vector3D& operator*=(float s) { x*=s; y*=s; z*=s; return *this; }
  Vector3D& operator/=(float s) { return *this *= (1.0f/s); }
  bool      operator==(const Vector3D& v) { return x==v.x && y==v.y && z==v.z; }

  Vector3D operator-       () const { return Vector3D(-x,-y,-z); }
  float    SquareMagnitude () const { return x*x+y*y+z*z; }
  float    Magnitude       () const { return (float)sqrt(SquareMagnitude()); }
  Vector3D Normalized      () const { return (1.0f/Magnitude())*(*this); }
  float    operator[] (int i) const { return ((float*)&x)[i]; }
  float&   operator[] (int i)       { return ((float*)&x)[i]; }
};

#define _11 sclr.s11
#define _12 sclr.s12
#define _13 sclr.s13
#define _14 sclr.s14
#define _21 sclr.s21
#define _22 sclr.s22
#define _23 sclr.s23
#define _24 sclr.s24
#define _31 sclr.s31
#define _32 sclr.s32
#define _33 sclr.s33
#define _34 sclr.s34
#define _41 sclr.s41
#define _42 sclr.s42
#define _43 sclr.s43
#define _44 sclr.s44

struct Matrix3
{
  union {
    struct { float s11,s12,s13,
                   s21,s22,s23,
                   s31,s32,s33; } sclr;
    float m[3][3];
  };
  static const Matrix3 Identity;

  Vector3D& baseRow(int i) { return *((Vector3D*)m[i]); }
  float  operator() (int i, int j) const { return m[i][j]; }
  float& operator() (int i, int j)       { return m[i][j]; }
};

struct Matrix3D
{
  union {
    struct { float s11,s12,s13,s14,
                   s21,s22,s23,s24,
                   s31,s32,s33,s34,
                   s41,s42,s43,s44; } sclr;
    float m[4][4];
  };
  static const Matrix3D Identity;

  Matrix3D() {}

  Matrix3D(float f11, float f12, float f13, float f14,
           float f21, float f22, float f23, float f24,
           float f31, float f32, float f33, float f34,
           float f41, float f42, float f43, float f44)
  {
    _11=f11; _12=f12; _13=f13; _14=f14;
    _21=f21; _22=f22; _23=f23; _24=f24;
    _31=f31; _32=f32; _33=f33; _34=f34;
    _41=f41; _42=f42; _43=f43; _44=f44;
  }

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

  friend Matrix3D PitchMatrix3D(const float theta);
  friend Matrix3D YawMatrix3D(const float theta);
  friend Matrix3D RollMatrix3D(const float theta);
  void rotate(const Vector3D& v);

  Matrix3D Inverse() const;
  Matrix3D Adjoint() const;
  float Determinant() const;

  float  operator() (int i, int j) const { return m[i][j]; }
  float& operator() (int i, int j)       { return m[i][j]; }
};

struct Plane
{
  Vector3D normal;
  float    d;

  Plane(const Vector3D& a, const Vector3D& b, const Vector3D& c)
  {
    normal = CrossProduct(b - a, c - a).Normalized();
    d = -normal * a;
  }

  float Classify(const Vector3D& v)
  {
    return v * normal + d;
  }
};

inline Vector3D operator* (float scalar, const Vector3D& v)
{
  return Vector3D(scalar*v.x,scalar*v.y,scalar*v.z);
}

inline Vector3D operator+ (const Vector3D& v1, const Vector3D& v2)
{
  return Vector3D(v1.x+v2.x,v1.y+v2.y,v1.z+v2.z);
}

inline Vector3D operator- (const Vector3D& v1, const Vector3D& v2)
{
  return Vector3D(v1.x-v2.x,v1.y-v2.y,v1.z-v2.z);
}

inline float operator* (const Vector3D& v1, const Vector3D& v2)
{
  return v1.x*v2.x + v1.y*v2.y + v1.z*v2.z;
}

inline Vector3D CrossProduct(const Vector3D& v1, const Vector3D& v2)
{
  return Vector3D(v1.y*v2.z-v2.y*v1.z,
                  v1.z*v2.x-v2.z*v1.x,
                  v1.x*v2.y-v2.x*v1.y);
}

inline Vector3D Transform(const Vector3D& v, const Matrix3D& m)
{
  return Vector3D(v.x*m._11 + v.y*m._21 + v.z*m._31 + m._41,
                  v.x*m._12 + v.y*m._22 + v.z*m._32 + m._42,
                  v.x*m._13 + v.y*m._23 + v.z*m._33 + m._43);
}

inline Vector3D rotateVector(const Vector3D& v, const Matrix3D& m)
{
  return Vector3D(v.x*m._11 + v.y*m._21 + v.z*m._31,
                  v.x*m._12 + v.y*m._22 + v.z*m._32,
                  v.x*m._13 + v.y*m._23 + v.z*m._33);
}

inline Matrix3D operator*(float scalar, const Matrix3D& m)
{
  return Matrix3D(scalar*m(0,0),scalar*m(0,1),scalar*m(0,2),scalar*m(0,3),
                  scalar*m(1,0),scalar*m(1,1),scalar*m(1,2),scalar*m(1,3),
                  scalar*m(2,0),scalar*m(2,1),scalar*m(2,2),scalar*m(2,3),
                  scalar*m(3,0),scalar*m(3,1),scalar*m(3,2),scalar*m(3,3));
}

inline Matrix3D operator*(const Matrix3D& m1, const Matrix3D& m2)
{
  return Matrix3D(
    m1._11*m2._11 + m1._12*m2._21 + m1._13*m2._31 + m1._14*m2._41,
    m1._11*m2._12 + m1._12*m2._22 + m1._13*m2._32 + m1._14*m2._42,
    m1._11*m2._13 + m1._12*m2._23 + m1._13*m2._33 + m1._14*m2._43,
    m1._11*m2._14 + m1._12*m2._24 + m1._13*m2._34 + m1._14*m2._44,
    m1._21*m2._11 + m1._22*m2._21 + m1._23*m2._31 + m1._24*m2._41,
    m1._21*m2._12 + m1._22*m2._22 + m1._23*m2._32 + m1._24*m2._42,
    m1._21*m2._13 + m1._22*m2._23 + m1._23*m2._33 + m1._24*m2._43,
    m1._21*m2._14 + m1._22*m2._24 + m1._23*m2._34 + m1._24*m2._44,
    m1._31*m2._11 + m1._32*m2._21 + m1._33*m2._31 + m1._34*m2._41,
    m1._31*m2._12 + m1._32*m2._22 + m1._33*m2._32 + m1._34*m2._42,
    m1._31*m2._13 + m1._32*m2._23 + m1._33*m2._33 + m1._34*m2._43,
    m1._31*m2._14 + m1._32*m2._24 + m1._33*m2._34 + m1._34*m2._44,
    m1._41*m2._11 + m1._42*m2._21 + m1._43*m2._31 + m1._44*m2._41,
    m1._41*m2._12 + m1._42*m2._22 + m1._43*m2._32 + m1._44*m2._42,
    m1._41*m2._13 + m1._42*m2._23 + m1._43*m2._33 + m1._44*m2._43,
    m1._41*m2._14 + m1._42*m2._24 + m1._43*m2._34 + m1._44*m2._44);
}

inline void
Matrix3D::rotate(const Vector3D& v)
{
   if (v.x!=0.0f) *this = PitchMatrix3D(v.x) * (*this);
   if (v.y!=0.0f) *this = YawMatrix3D  (v.y) * (*this);
   if (v.z!=0.0f) *this = RollMatrix3D (v.z) * (*this);
}

inline Matrix3D
TranslateMatrix3D(const Vector3D& v)
{
  return Matrix3D(1.0f,0.0f,0.0f,0.0f,
                  0.0f,1.0f,0.0f,0.0f,
                  0.0f,0.0f,1.0f,0.0f,
                   v.x, v.y, v.z,1.0f);
}


inline Matrix3D
ScaleMatrix3D(const Vector3D& v)
{
   return Matrix3D( v.x,0.0f,0.0f,0.0f,
                   0.0f, v.y,0.0f,0.0f,
                   0.0f,0.0f, v.z,0.0f,
                   0.0f,0.0f,0.0f,1.0f);
}


inline Matrix3D
ScaleMatrix3D(const float s)
{
   return ScaleMatrix3D(Vector3D(s,s,s));
}


inline Matrix3D
PitchMatrix3D(const float c, const float s)
{
   return Matrix3D(1.0f, 0.0f, 0.0f, 0.0f,
                   0.0f,    c,   -s, 0.0f,
                   0.0f,    s,    c, 0.0f,
                   0.0f, 0.0f, 0.0f, 1.0f);
}


inline Matrix3D
PitchMatrix3D(const float theta)
{
   return PitchMatrix3D((float) cos(theta), (float) sin(theta));
}


inline Matrix3D
YawMatrix3D(const float c, const float s)
{
   return Matrix3D(   c, 0.0f,    s, 0.0f,
                   0.0f, 1.0f, 0.0f, 0.0f,
                     -s, 0.0f,    c, 0.0f,
                   0.0f, 0.0f, 0.0f, 1.0f);
}


inline Matrix3D
YawMatrix3D(const float theta)
{
   return YawMatrix3D((float) cos(theta), (float) sin(theta));
}


inline Matrix3D
RollMatrix3D(const float c, const float s)
{
   return Matrix3D(c,   -s,    0.0f, 0.0f,
                   s,    c,    0.0f, 0.0f,
                   0.0f, 0.0f, 1.0f, 0.0f,
                   0.0f, 0.0f, 0.0f, 1.0f);
}


inline Matrix3D
RollMatrix3D(const float theta)
{
   return RollMatrix3D((float) cos(theta), (float) sin(theta));
}


template<class T>
inline T Max(T a, T b)
{
  return (a>b ? a : b);
}

template<class T>
inline T Min(T a, T b)
{
  return (a<b ? a : b);
}

#endif // H_MATH3D

---