Translate

Sunday, March 13, 2011

Looking for a portable 3D vector class for your game?

Here's what I have pieced together over the years.  Still needs a little clean up. But solid and works great.

// Usage Adding 2 vectors:
Vector3Df v0(1,2,3);
Vector3Df v1(4,5,6);
Vector3D v2;

v2 = v0 + v1;

// Find angle between vectors v0 and v1
float angle = v0.angleInDegree(v1);

// Normalize a vector (to unit length 1)
Vector3Df v2n = v2.normalize();

// Vector3D is a template so feel free to use other types
Vector3D<int> screen_pixels(10,10); // etc


Also, it interfaces with a Matrix class. I'll post that soon so you can do some fun things there too! :)


File Starts below......

//*********************************************************************

/* vim: set filetype=cpp.doxygen : */ 
#ifndef VECTOR3D_H_
#define VECTOR3D_H_


#include <vector>
#include <cstdlib>
#include <cstdio>
#include <iostream>
#include <sstream>
#include <cmath>
//#include "mesh.h"

const double M_180_OVER_PI = (180/M_PI);
const double M_PI_OVER_180 = (M_PI/180);

// Forward Declaration
template <typename T>
class Matrix;

#define NULL_VECTOR  Vector3D<T>(0,0,0)


//**********************************************************************
/**
 *   Represent a 3D vector and many common vector maths associated
 */
template<typename T>
class Vector3D
{

  public:

    typedef enum {
      X_CORD=0,Y_CORD=1,Z_CORD=2}cords;

      /// XCord of Vector
      T x;
      /// YCord of Vector
      T y;
      /// ZCord of Vector
      T z;

      Vector3D();
      Vector3D(T x, T y, T z);
      Vector3D(T* v);

      //Vector3D(Vec3f vec3f);
      //virtual ~Vector3D();

      //---------------------------------------------
      // Operators
      //---------------------------------------------
      Vector3D<T> operator+(const Vector3D<T>);
      Vector3D<T>  operator-(void);
      Vector3D<T> operator*(const Vector3D<T>);

      Vector3D<T> operator-(const Vector3D<T> d);
      T distanceBetween(Vector3D<T> v);
      T distanceBetweenSquared(Vector3D<T> v);


      ///  Dot product
      T  operator^ (const Vector3D v2) const;
      T dot(const Vector3D<T> v2) const;

      //  cross product of vectors
      Vector3D<T>  operator%(const Vector3D<T> v2) const;
      Vector3D<T>  cross(const Vector3D<T> v1) const;

      Vector3D<T>  operator/(const T n);
      Vector3D<T>  operator/=(const T n);

      template<typename R, typename M>
        friend Vector3D<R> operator*(Vector3D<R> v, const M s);
      template<typename R, typename M>
        friend Vector3D<R> operator*=(Vector3D<R>& v, const M s);
      template<typename R, typename M>
        friend Vector3D<R> operator*(const M s, Vector3D<R> v);

      bool operator==(const Vector3D<T> v);
      bool operator!=(const Vector3D<T> v)
      {
        return !equal(v);

      }
      Vector3D<T>& operator+=(const Vector3D<T> v2);
      Vector3D<T>& operator-=(const Vector3D<T> v2);
      Vector3D<T> operator*=(const Vector3D<T>);

      // Array like access to attributes (for convenience)
      T& operator[](int i);
      T& getCord(unsigned int i);

      // Matrix multiplications
      Vector3D<T>& operator*(Matrix<T>& m);

      //---------------------------------------------
      // Static utility functions
      //---------------------------------------------
      static int exists(std::vector<int> index, int a);
      static int existsApprox(std::vector<Vector3D <T> > index,
          Vector3D<T> a);
      //---------------------------------------------
      //   Member Functions
      //---------------------------------------------
      T angleInDegree(const Vector3D<T> v1);
      void toFloatArray(T* a);
      void printVector();

      template<typename R>
        friend std::ostream& operator<<(
            std::ostream& o, const Vector3D<R> v);

      T magnitude();

      T length();

      Vector3D<T> normalize();
      void normalizeToFloat(T* n);
      void TArrayTo3DVectorList(T* a, int size,
          std::vector<Vector3D> v);
      bool equal(const Vector3D<T> v);
      bool equalApprox(const Vector3D<T> v);

      Vector3D<T> closestCubePoint(Vector3D<T> min,
          Vector3D<T> max);

      std::string toString();


};

//**********************************************************
/**
 * Default contructor
 */
template<typename T>
Vector3D<T>::Vector3D():
    x(0),
    y(0),
    z(0)
{

}

//**********************************************************
/**
 *   @param x cord
 *   @param y cord
 *   @param z cord
 */
template<typename T>
Vector3D<T>::Vector3D(T x_, T y_, T z_):
    x(x_),
    y(y_),
    z(z_)
{

}

//**********************************************************
/**
 *  @param v vector (array of 3 typename Ts)
 */
template<typename T>
Vector3D<T>::Vector3D(T* v)
{

  this->x = v[0];
  this->y = v[1];
  this->z = v[2];  

}

////**********************************************************
//template<typename T>
//Vector3D<T>::~Vector3D()
//{
//}

//**********************************************************
/**
 * Same as magnitude
 *
 * @return the length of this vector
 */

  template<typename T>
T Vector3D<T>::length()
{

  return magnitude();
}


//**********************************************************************
/**
 *   Account for Matrix multiplications of form Vector * Matrix
 * (More of the DirectX way of doing things as it would be
 *   vector * transform_matrix).
 *
 *   @param m matrix to multiply with
 * @code
 *
 *            4 X 4      
 *   [v0,v1,v2,v3] *   [a0, a4, a8,  a12 ] = [v0a0  + v1a1  + v2a2  + v3a3,
 *                     [a1, a5, a9,  a13 ]    v0a4  + v1a5  + v2a6  + v3a7,
 *                     [a2, a6, a10, a14 ]    v0a8  + v1a9  + v2a10 + v3a11,
 *                     [a3, a7, a11, a15 ]    v0a12 + v1a13 + v2a14 + v3a15]
 *
 *
 *      Note: Most vectors will have v3 = 1
 *
 * @endcode
 *
 */
  template <typename T>
Vector3D<T>& Vector3D<T>::operator*(Matrix<T>& m)
{

  Vector3D<T>& v = *this;

  return Vector3D<T>(
      // X cord
      v[0]*m(0,0) + v[1]*m(1,0) + v[2]*m(2,0) + m(3,0),
      // y cord
      v[0]*m(0,1) + v[1]*m(1,1) + v[2]*m(2,1) + m(3,1),
      // Z cord
      v[0]*m(0,2) + v[1]*m(1,1) + v[2]*m(2,2) + m(3,2)
      );

}


//**********************************************************************
/**
 * Array like access to attributes (for convenience)
 *
 * @param i index into Vector (x=0,y=1,z=2)
 */
  template<typename T>
inline T& Vector3D<T>::operator[](int i)
{

  //    if(!(i >= 0 && i <  3))
  //    {
  //
  //        printf("Vector4D[%d] Your program tried to access "
  //                "out of bounds!",i);
  //        throw  i;
  //    }
  //
  //    return &(x)[i];

  return getCord(i);

}

//**********************************************************
/**
 * Get the cordinate of this vector in array like access
 *
 * @param i index into Vector (x=0,y=1,z=2)
 */
  template<typename T>
inline T& Vector3D<T>::getCord(unsigned int i)
{

  if(i == X_CORD)
  {

    return x;
  }

  else if(i == Y_CORD)
  {

    return y;
  }

  else if(i == Z_CORD)
  {

    return z;
  }


  else
  {

    printf("Vector3D[%d] Tried to access vector index that is"
        "out of bounds!", i);
    throw  i;
  }


}

//************************************************************
/**
 *  Create an stl-vector of Vector3Ds from a array of verticies
 */
  template<typename T>
void Vector3D<T>::TArrayTo3DVectorList(T* a, int size, std::vector<Vector3D> v)
{
  for(int i=0; i<size+3; i+=3)
  {
    Vector3D<T> v3d = Vector3D(a[i],a[i+1],a[i+2]);
    v.push_back(v3d);      
  }
}

//**********************************************************
/**
 *   Change the vector to a 3 element T array
 *    @param a array to save Ts to
 */
  template<typename T>
void Vector3D<T>::toFloatArray(T* a)
{
  if(a == NULL)
  {
    std::cout << "Tried to convert Vector3D<T> to T "\
      "array using NULL reference!"<< std::endl;
  }
  a[0] = x;
  a[1] = y;
  a[2] = z;
}

//**********************************************************
  template<typename T>
bool Vector3D<T>::equal(const Vector3D<T> v)
{
  return (x == v.x && y == v.y && z == v.z);
}

//**********************************************************
  template<typename T>
bool Vector3D<T>::operator==(const Vector3D<T> v)
{
  return equal(v);
}

//**********************************************************
/**
 *   See if two vertexes are approximately equal
 */
  template<typename T>
bool Vector3D<T>::equalApprox(const Vector3D<T> v)
{
  // Is really 0.05
  T margin = 5;

  int x1 = (int)(x*100.0f);
  int y1 = (int)(y*100.0f);
  int z1 = (int)(z*100.0f);

  int x2 = (int)(v.x*100.0f);
  int y2 = (int)(v.y*100.0f);
  int z2 = (int)(v.z*100.0f);

  /*
     printf("{%d %d %d}\n",x1,y1,z1);
     printf("{%d %d %d}\n",x2,y2,z2);
   */
  return (abs(x1 - x2) <= margin &&
      abs(y1 - y2) <= margin &&
      abs(z1 - z2) <= margin);
}

//**********************************************************
/**
 *   Check to see if an Vector3D<T> exists in a std::vector<Vector3D>
 *  (e.g.  to see if two verticies match)
 *
 *  @param index vector to check
 *  @param a integer to look for
 *  @return  -1 if not found, else place where found
 */
  template<typename T>
int Vector3D<T>::existsApprox(std::vector<Vector3D> index, Vector3D<T> a)
{

  for(unsigned int i = 0; i < index.size();i++)
  {

    if(index[i].equalApprox(a))
    {

      return i;
    }
  }
  return -1;
}

//**********************************************************
/**
 *     adding of vectors
 *      @param v1 first to add
 *      @param v2 second
 *    @return new vector
 */
  template<typename T>
Vector3D<T>  Vector3D<T>::operator+ (const Vector3D<T> v2)
{

  const Vector3D<T>& v1 = *(this);
  return Vector3D(v1.x+v2.x, v1.y+v2.y, v1.z+v2.z);
}

//**********************************************************
/**
 *     adding of vectors
 *      @param v2 second
 *    @return new vector
 */
  template<typename T>
Vector3D<T>&  Vector3D<T>::operator+= (const Vector3D<T> v2)
{

  *this = *this + v2;
  return *this;
}

//**********************************************************
/**
 *     subtraction of vectors
 *      @param v2 second
 *    @return new vector
 */
  template<typename T>
Vector3D<T>&  Vector3D<T>::operator-= (const Vector3D<T> v2)
{

  *this = *this - v2;
  return *this;
}


//**********************************************************
/**
 *     subtraction of vectors
 *      @param v1 first to add
 *      @param v2 second
 *    @return new vector
 */
  template<typename T>
Vector3D<T>  Vector3D<T>::operator- (const Vector3D<T> v2)
{

  const Vector3D<T>& v1 = *(this);
  return Vector3D(v1.x-v2.x, v1.y-v2.y, v1.z-v2.z);
}

//**********************************************************
/**
 *     negation of vector
 *    @return new vector
 */
  template<typename T>
Vector3D<T>  Vector3D<T>::operator- ()
{
  const Vector3D<T>& v1 = *(this);
  return Vector3D(-v1.x, -v1.y, -v1.z);
}

//**********************************************************
/**
 *     mult of vector with scalar
 *      @param v1 first to add
 *      @param v2 second
 *    @return new vector scaled by s
 */
  template<typename T, typename M>
inline Vector3D<T> operator* (Vector3D<T> v, const M s)
{
  return Vector3D<T>(v.x*s,v.y*s,v.z*s);
}

//**********************************************************
/**
 *     mult of vector with scalar
 *      @param v1 first to add
 *      @param v2 second
 *    @return new vector scaled by s
 */
  template<typename T, typename M>
inline Vector3D<T> operator*= (Vector3D<T>& v, const M s)
{
  v.x *= s;
  v.y *= s;
  v.z *= s;
  return v;
}


//**********************************************************
/**
 *     mult of vector with scalar
 *      @param v1 first to add
 *      @param v2 second
 *    @return new vector scaled by s
 */
  template<typename T, typename M>
inline Vector3D<T> operator*(const M s, Vector3D<T> v)
{

  return Vector3D<T>(v.x*s,v.y*s,v.z*s);

}

//**********************************************************
/**
 *     mult of vector with vector (used for color only!)
 *      @param v1 first to mult
 *      @param v2 second
 *    @return new vector scaled by s
 */
  template<typename T>
inline Vector3D<T> Vector3D<T>::operator* (Vector3D<T> v)
{

  return Vector3D<T>(
      v.x * x,
      v.y * y,
      v.z * z);

}

//**********************************************************
/**
 *     mult of vector with vector (used for color only!)
 *      @param v1 first to mult
 *      @param v2 second
 *    @return new vector scaled by s
 */
  template<typename T>
inline Vector3D<T> Vector3D<T>::operator*= (Vector3D<T> b)
{

  x *= b.x;
  y *= b.y;
  z *= b.z;
  return *this;

}

//**********************************************************
/**
 *     dot product of vectors
 *      @param v1 first to add
 *      @param v2 second
 *    @return new T based on dot product of both vectors
 */
template<typename T>
T  Vector3D<T>::operator^ (const Vector3D<T> v2) const
{
  return dot(v2);
  //  const Vector3D<T> v1 = *(this);
  //  return v1.x*v2.x+ v1.y*v2.y+ v1.z*v2.z;
}

//**********************************************************
/**
 *     dot product of vectors
 *      @param v1 first to add
 *      @param v2 second
 *    @return new Vector3D<T>  based on dot
 *    product of both vectors
 */
template<typename T>
T  Vector3D<T>::dot(const Vector3D<T> v2) const
{

  return (x*v2.x) + (y*v2.y) + (z*v2.z);
}

//**********************************************************
/**
 *  Cross product of vectors
 *
 *  @param v1 first to add
 *  @param v2 second
 *  @return new Vector3D<T>  based on cross
 *    product of both vectors
 */
template<typename T>
Vector3D<T>  Vector3D<T>::cross(const Vector3D<T> v1) const
{


  // Here is our good ol' cross product determinate
  return -Vector3D(v1.y*z-y*v1.z   /* ^x unit vec */,
      -(v1.x*z-x*v1.z)      /* ^y  unit vec*/,
      v1.x*y-x*v1.y)        /* ^z  unit vec*/;
}

//**********************************************************
/**
 *    Cross product of vectors
 *      @param v1 first to add
 *      @param v2 second
 *    @return new Vector3D<T>  based on cross
 *    product of both vectors
 */
template<typename T>
Vector3D<T>  Vector3D<T>::operator% (const Vector3D<T> v1) const
{
  //    const Vector3D<T>& v2 = *(this);
  //
  //    // Here is our good ol' cross product determinate
  //    return Vector3D(v1.y*v2.z-v2.y*v1.z   /* ^x */,
  //            -(v1.x*v2.z-v2.x*v1.z)/* ^y */,
  //            v1.x*v2.y-v2.x*v1.y)  /* ^z */;

  return cross(v1);
}

//**********************************************************
/**
 *     angle between 2 vectors
 *    (Will be doing normalization on both to avoid
 *  NOT-A-NUMBER issues with trigonometry, e.g. #acos(n),n>1||n<-1)
 *
 * @param v1  vector to find angle between (between this vector and v)
 *    @return new T based on dot product of both vectors
 */
  template<typename T>
T Vector3D<T>::angleInDegree(const Vector3D<T> v1)
{

  Vector3D<T> temp = v1;
  //printf("v2 ^ v1: %f\n",normalize() ^ temp.normalize());
  //printf("acos(v2 ^ v1): %f\n",acos(normalize() ^ temp.normalize()));
  // Here is our good ol' cross product determinate
  return M_180_OVER_PI*acos(normalize() ^ temp.normalize());
}

//************************************************************
/**
 *   Normalize the vector and save to a T
 */
  template<typename T>
void Vector3D<T>::normalizeToFloat(T* n)
{
  normalize().toFloatArray(n);
}

//************************************************************
  template<typename T>
Vector3D<T> Vector3D<T>::normalize()
{

  // Magnitude
  //    (zeroed to shutup profiler)
  T mag = 0;
  mag =(T)magnitude();

  // Div by zero is bad, M'Kay
  if(mag != 0)    
  {

    return (*this)/ mag;

  }
  else
  {

    return *this;

  }

}

//************************************************************
  template<typename T>
T Vector3D<T>::magnitude()
{

  return sqrt(x*x + y*y + z*z);
}

//************************************************************
/**
 *   Scalar division of vector by n
 */
  template<typename T>
Vector3D<T>  Vector3D<T>::operator/ (T n)
{
  return Vector3D(x/n, y/n, z/n);
}

//************************************************************
/**
 *   Scalar division of vector by n
 */
  template<typename T>
Vector3D<T>  Vector3D<T>::operator/= (const T n)
{
  x /= n;
  y /= n;
  z /= n;
  return *this;
}

//**********************************************************
  template<typename T>
void Vector3D<T>::printVector()
{
  std::cout << "{" << x <<", "<<  y <<", " <<
    z <<"} "<< std::endl;
}

//**********************************************************
/**
 *  Friend function for printing Vector3D to output streams
 */

  template<typename T>
std::ostream& operator<< (std::ostream& o, const Vector3D<T> v)
{

  return o << "{" << v.x <<", "<<  v.y <<", " <<
    v.z <<"} ";

}

//**********************************************************
/**
 *  Friend function for printing Vector3D to output streams
 */

template<typename T>
std::string Vector3D<T>::toString()
{

  std::ostringstream o;
  
  o << "{" << x <<", "<<  y <<", " <<
    z <<"} ";

  return o.str();

}


//**********************************************************
/**
 *  Find closest point of a box and this vertex.
 *
 *  @param min min cord of a Cube
 *  @param max max cord of a Cube
 */

  template<typename T>
Vector3D<T> Vector3D<T>::closestCubePoint(Vector3D<T> min, Vector3D<T> max)
{

  Vector3D<T> temp = min;
  Vector3D<T> close = Vector3D<T>(max.x,min.y, min.z);
  if(temp.distanceBetweenSquared(*this) <
      close.distanceBetweenSquared(*this) )
  {
    close = temp;
  }

  if(temp.distanceBetweenSquared(*this) <
      close.distanceBetweenSquared(*this) )
  {
    close = temp;
  }

  temp = Vector3D<T>(min.x,max.y, min.z);
  if(temp.distanceBetweenSquared(*this) <
      close.distanceBetweenSquared(*this) )
  {
    close = temp;
  }

  temp = Vector3D<T>(max.x,max.y, min.z);
  if(temp.distanceBetweenSquared(*this) <
      close.distanceBetweenSquared(*this) )
  {
    close = temp;
  }

  temp = Vector3D<T>(min.x,min.y, max.z);
  if(temp.distanceBetweenSquared(*this) <
      close.distanceBetweenSquared(*this) )
  {
    close = temp;
  }

  temp = Vector3D<T>(max.x,min.y, max.z);
  if(temp.distanceBetweenSquared(*this) <
      close.distanceBetweenSquared(*this) )
  {
    close = temp;
  }

  temp = Vector3D<T>(max.x,max.y, max.z);
  if(temp.distanceBetweenSquared(*this) <
      close.distanceBetweenSquared(*this) )
  {
    close = temp;
  }

  return close;

}


//**********************************************************
/**
 *   Distance between two Verticies
 */

  template<typename T>
T Vector3D<T>::distanceBetween(Vector3D<T> v)
{

  return sqrt(distanceBetweenSquared(v));
  //  return sqrt(pow((this->x-v.x),2) +
  //      pow(this->y-v.y,2) + pow(this->z-v.z,2));

}

//**********************************************************
/**
 *   Distance squared between two Verticies,
 *   this is VERY usefull to avoid a square root when accurate
 *   distance is not needed (eg for comparisons, etc).
 */

  template<typename T>
T Vector3D<T>::distanceBetweenSquared(Vector3D<T> v)
{
  return (pow((this->x - v.x),2) +
      pow(this->y - v.y, 2) + pow(this->z - v.z,2));
}



typedef Vector3D<float> Vector3Df;


#endif /*VECTOR3D_H_*/