vector3.cpp

#include <math.h>
#include <assert.h>
#include "vector3.h"


using namespace Geometry;
using namespace std;



// Aux. function that translates a value to a valid interval
double normalize(double min, double max, double value)
{
  double distance = max - min;
  if (distance <= 0)
    {
      cerr << __FILE__ << ": min > max in \"normalize\" function" << endl;
      return value;
    }

  while (value > max)
    value -= distance;

  while (value < min)
    value += distance;

  return value;
}



// Default constructor
template <class T> Vector3D<T>::Vector3D()
{
        for (unsigned i=0 ; i<DIM ; i++)
                _data[i] = (T) 0.0;
}

// Constructor
template <class T> Vector3D<T>::Vector3D(const T& x, const T& y, const T& z)
{
  _data[0] = x;
  _data[1] = y;
  _data[2] = z;
}

// Constructor
template <class T> Vector3D<T>::Vector3D(const T data[3])
{
        for (unsigned i=0 ; i<DIM ; i++)
                _data[i] = data[i];
}

// Copy constructor
template <class T> Vector3D<T>::Vector3D(const Vector3D<T>& v)
{
        for (unsigned i=0 ; i<DIM ; i++)
                _data[i] = v._data[i];
}

// Copy constructor
template <class T> Vector3D<T>::Vector3D(const vector<T>& v)
{
        assert(v.size() >= DIM);
        for (unsigned i=0 ; i<DIM ; i++)
                _data[i] = v[i];
}

// Constructor
template <class T> Vector3D<T>::Vector3D(const Vector3D<T>* v)
{
        for (unsigned i=0 ; i<DIM ; i++)
                _data[i] = v->_data[i];
}


// Simple global "set" method
template <class T> void Vector3D<T>::set(const T&x, const T& y, const T& z)
{
  _data[0] = x;
  _data[1] = y;
  _data[2] = z;
}


// Simple global "set" method
template <class T> void Vector3D<T>::set(const T data[3])
{
        for (unsigned i=0 ; i<DIM ; i++)
                _data[i] = data[i];
}


// Copy operator
template <class T> Vector3D<T>& Vector3D<T>::operator=(const Vector3D<T>& src)
{
        for (unsigned i=0 ; i<DIM ; i++)
                _data[i] = src._data[i];

  return *this;
}

// Comparison operator
template <class T> bool Vector3D<T>::operator==(const Vector3D<T>& src) const
{
        for (unsigned i=0 ; i<DIM ; i++)
                if (ABS(_data[i] - src._data[i]) > ALMOST_ZERO)
                        return false;

        return true;
}

// Comparison operator, admittedly arbitrary, yet necessary for
// using the class in many STL algorithms and containers.
template <class T> bool Vector3D<T>::operator<(const Vector3D<T>& src) const
{
        for (unsigned i=0 ; i<DIM ; i++)
                {
                        if (_data[i] < src._data[i])
                                return true;
                        if (_data[i] > src._data[i])
                                return false;
                }

        return false;
}

// Normalizes a vector (sets the module to 1)
template <class T> void Vector3D<T>::normalize()
{
  double mod = module();
        *this /= mod;
}

// Sets the length of a vector 
template <class T> void Vector3D<T>::setLength(T len)
{
  double mod = module();
        *this *= len/mod;
}


// Returns the square of the module of the vector.
// Equivalent to module(), faster when only relative comparisons are needed.
template <class T> double Vector3D<T>::squaredModule() const
{
        T sum(0);
        for (unsigned i=0 ; i<DIM ; i++)
                sum += _data[i]*_data[i];

  return sum;
}


// Accesses an element of the vector
template <class T> T& Vector3D<T>::operator[](int index)
{
  return _data[index];
}

// Accesses an element of the vector
template <class T> const T& Vector3D<T>::operator[](int index) const
{
  return _data[index];
}

// Calculates the angle between 2 vectors
template <class T> double Vector3D<T>::angle(const Vector3D<T>& v) const
{
  Vector3D<T> local(*this);
  Vector3D<T> param(v);

  double m = module();
  assert(!isnan(m));

  double mv = v.module();
  assert(!isnan(mv));

  double mod = m*mv;
  assert(!isnan(mod));

  if ( mod < ALMOST_ZERO )
    return 0.0;

  local /= m;
  param /= mv;

  double dp = local*param;
  assert(!isnan(dp));

        // Clamp the value to [-1,1], for sanity
  if (dp > 1.0)
    dp = 1.0;
  else if (dp < -1.0)
    dp = -1.0;

  assert(!isnan(acos(dp)));
  return acos( dp );
}


// Calculates the square of the distance between 2 vectors.
// Faster than ::distance, for when only comparisons are needed.
template <class T> double Vector3D<T>::squaredDistance(const Vector3D<T>& v) const
{
        T sum(0);
        for (unsigned i=0 ; i<DIM ; i++)
                {
                        T diff = _data[i] - v._data[i];
                        sum += diff * diff;
                }

  return sum;
}

// Computes the infinite-degree distance to 'v'
template <class T> double Vector3D<T>::infDistance(const Vector3D<T>& v) const
{
        T soFar(0);
        for (unsigned i=0 ; i<DIM ; i++)
                {
                        T diff = ABS(_data[i] - v._data[i]);
                        if (diff > soFar)
                                soFar = diff;
                }

  return soFar;
}

// Computes the degree-1 (Manhattan) distance to 'v'
template <class T> double Vector3D<T>::manhattanDistance(const Vector3D<T>& v) const
{
        T sum(0);
        for (unsigned i=0 ; i<DIM ; i++)
                sum += ABS(_data[i] - v._data[i]);

  return sum;
}

// Calculates the distance between a point in space and a plane represented
// as a point on that plane and the plane normal.
// This distance is signed, as it considers the positive and negative sides of the plane.
template <class T> double Vector3D<T>::distanceToPlane(const Vector3D<T>& P, const Vector3D<T>& N) const
{
  assert( fabs(N.squaredModule() - 1) < ALMOST_ZERO );

  return (P-*this) * N;
}



// Rotates a vector around the X axis
template <class T> void Vector3D<T>::rotateX(double angle)
{
        double y = _data[1];
        double z = _data[2];
        double cosine = cos(angle);
        double sine = sin(angle);

        _data[1] = (T) (y*cosine - z*sine);
        _data[2] = (T) (y*sine + z*cosine);
}


// Rotates a vector around the Y axis
template <class T> void Vector3D<T>::rotateY(double angle)
{
  double x = _data[0];
  double z = _data[2];
  double cosine = cos(angle);
  double sine = sin(angle);

  _data[2] = (T) (z*cosine - x*sine);
  _data[0] = (T) (z*sine + x*cosine);
}

// Rotates a vector around the Z axis
template <class T> void Vector3D<T>::rotateZ(double angle)
{
  double x = _data[0];
  double y = _data[1];
  double cosine = cos(angle);
  double sine = sin(angle);

  _data[0] = (T) (x*cosine - y*sine);
  _data[1] = (T) (x*sine + y*cosine);
}


// Produces the rotation of the vector around an axis.
// Precondition: *this and axis are unit vectors
template <class T> Vector3D<T> Vector3D<T>::rotation(const Vector3D<T>& axis, double angle) const
{
        Vector3D<T> Y = axis ^ (*this);
        assert(fabs(Y.squaredModule() - 1) < 0.0001);
        
        double cosine = cos(angle);
        double sine = sin(angle);

        return Y*sine + (*this)*cosine;
}

// Calculates the dot product of 2 vectors
template <class T> T Vector3D<T>::operator*(const Vector3D<T>& v) const
{
        T sum(0);
        for (unsigned i=0 ; i<DIM ; i++)
                sum += _data[i] * v._data[i];

  return sum;
}

// Calculates the dot product of 2 vectors, the second being an STD vector
template <class T> T Vector3D<T>::operator*(const std::vector<T>& v) const
{
        assert(v.size() >= (unsigned) dim());
        T sum(0);
        for (unsigned i=0 ; i<DIM ; i++)
                sum += _data[i] * v[i];

  return sum;
}

// Divides a vector by a scalar factor
template <class T> Vector3D<T> Vector3D<T>::operator/(T factor) const
{
  return Vector3D<T>(_data[0]/factor, _data[1]/factor, _data[2]/factor);
}

// Multiplies a vector by a scalar factor and assigns the result to itself
template <class T> const Vector3D<T>& Vector3D<T>::operator/=(T factor)
{
        for (unsigned i=0 ; i<DIM ; i++)
                _data[i] /= factor;

  return *this;
}

// Multiplies a vector by a scalar factor
template <class T> Vector3D<T> Vector3D<T>::operator*(T factor) const
{
  return Vector3D<T>(_data[0]*factor, _data[1]*factor, _data[2]*factor);
}

#if 0
// Multiplies a vector by a scalar factor
template <class T> Vector3D<T> operator*(T factor, const Vector3D<T>& v)
{
  return v*factor;
}
#endif

// Multiplies a vector by a scalar factor and assigns the result to itself
template <class T> const Vector3D<T>& Vector3D<T>::operator*=(T factor)
{
        for (unsigned i=0 ; i<DIM ; i++)
                _data[i] *= factor;

  return *this;
}

// Returns the cross product of 2 vectors
template <class T> Vector3D<T> Vector3D<T>::operator^(const Vector3D<T>& v) const
{
  return Vector3D<T>( (_data[1] * v._data[2]) - (v._data[1] * _data[2]),
                      (_data[2] * v._data[0]) - (v._data[2] * _data[0]),
                      (_data[0] * v._data[1]) - (v._data[0] * _data[1]) );
}


// Returns the difference vector between 2 vectors
template <class T> Vector3D<T> Vector3D<T>::operator-(const Vector3D<T>& v) const
{
  return Vector3D<T>( _data[0] - v._data[0],
                      _data[1] - v._data[1],
                      _data[2] - v._data[2] );
}


// Substracts one vector to another
template <class T> const Vector3D<T>& Vector3D<T>::operator-=(const Vector3D<T>& v)
{
        for (unsigned i=0 ; i<DIM ; i++)
                _data[i] -= v._data[i];

  return *this;
}


// Returns a vector summation
template <class T> Vector3D<T> Vector3D<T>::operator+(const Vector3D<T>& v) const
{
  return Vector3D<T>( _data[0] + v._data[0],
                      _data[1] + v._data[1],
                      _data[2] + v._data[2] );
}


// Adds a vector to another one
template <class T> const Vector3D<T>& Vector3D<T>::operator+=(const Vector3D<T>& v)
{
        for (unsigned i=0 ; i<DIM ; i++)
                _data[i] += v._data[i];

  return *this;
}

// Calculates the spherical coordinates for the vector, except the distance
template <class T> void Vector3D<T>::getSpheric(double& theta, double& phi) const
{
  double distance = sqrt( x()*x() + y()*y() + z()*z() );
  assert(distance != 0.0);

  theta = atan2(y(), x());
  phi = acos(z() / distance);

  //    theta = ::normalize(-M_PI, M_PI, theta);
  //    phi = ::normalize(0, M_PI, phi);
}

// Calculates the spherical coordinates for the vector
template <class T> void Vector3D<T>::getSpheric(double& distance, double& theta, double& phi) const
{
  distance = sqrt( x()*x() + y()*y() + z()*z() );
  theta = atan2(y(), x());
  phi = acos(z() / distance);

  assert( (theta >= -M_PI) && (theta <= M_PI) );
  assert( (phi >= 0) && (phi <= M_PI) );
}

// Sets the values of the vector given its spheric coordinates
template <class T> void Vector3D<T>::setSpheric(double distance, double theta, double phi)
{
  //    theta = ::normalize(-M_PI, M_PI, theta);
  //    phi = ::normalize(0, M_PI, phi);
  //assert( (theta >= -M_PI) && (theta <= M_PI) );
  //assert( (phi >= 0) && (phi <= M_PI) );

  double sinPhi = sin(phi);

  x( (T) (distance * cos(theta) * sinPhi) );
  y( (T) (distance * sin(theta) * sinPhi) );
  z( (T) (distance * cos(phi)) );
}


// Projects the vector onto plane X
template <class T> Vector3D<T> Vector3D<T>::projectX() const
{
  return Vector3D<T>(0, _data[1], _data[2]);
}

// Projects the vector onto plane Y
template <class T> Vector3D<T> Vector3D<T>::projectY() const
{
  return Vector3D<T>(_data[0], 0, _data[2]);
}

// Projects the vector onto plane Z
template <class T> Vector3D<T> Vector3D<T>::projectZ() const
{
  return Vector3D<T>(_data[0], _data[1], 0);
}

// Projects the vector onto a plane crossing the origin, with normal v.
// Precondition: n is assumed to be a unit vector.
template <class T> Vector3D<T> Vector3D<T>::project(const Vector3D<T>& n) const
{
        return (*this) - n * dotProduct(n);
}


// Project the Vector3D<T> onto a plane defined by a center and normal
// The vector is supposed to be origined at (0,0,0)
// Check <http://members.tripod.com/~Paul_Kirby/vector/Vplanelineint.html>
// for details (a=0, b=*this, c=center, n=normal).
template <class T> Vector3D<T>
                Vector3D<T>::intersect(const Vector3D<T>& center, const Vector3D<T>& normal) const
{
        return (*this) * ((center*normal) / ((*this)*normal));
}


// Returns a unit vector which is perpendicular to *this
template <class T> Vector3D<T>
                Vector3D<T>::perpendicular() const
{
        static const Vector3D<T> X(1, 0, 0);
        static const Vector3D<T> Y(0, 1, 0);
        
        const Vector3D<T>& base = (fabs((*this) * X) <  fabs((*this) * Y) ? X : Y);
        Vector3D<T> result = base ^ (*this);
        result.normalize();
        
        return result;
}


// Instantiation of templates, needed by gcc
// The fewer of these we compile, the faster the compilation
//template class Vector3D<double>;
template class Vector3D<float>;
//template class Vector3D<int>;
//template class Vector3D<short>;
//template class Vector3D<char>;

---