vector3.h

// -*- Mode: C++; tab-width: 2; -*-
// vi: set ts=2:
//
// $Id: vector3.h,v 1.77 2006/01/09 20:14:59 oliver Exp $
//

#ifndef BALL_MATHS_VECTOR3_H
#define BALL_MATHS_VECTOR3_H

#ifndef BALL_COMMON_EXCEPTION_H
# include <BALL/COMMON/exception.h>
#endif

#ifndef BALL_CONCEPT_PERSISTENCEMANAGER_H
# include <BALL/CONCEPT/persistenceManager.h>
#endif

#ifndef BALL_MATHS_ANGLE_H
# include <BALL/MATHS/angle.h>
#endif

#ifndef BALL_MATHS_COMMON_H
# include <BALL/MATHS/common.h>
#endif

#ifdef BALL_HAS_IEEEFP_H
# include <ieeefp.h>
#endif 


namespace BALL 
{

  template <typename T>
  class TVector3;


  template <typename T>
  BALL_INLINE 
  TVector3<T> operator * (const T& a, const TVector3<T>& b)
    ;

  template <typename T>
  std::istream& operator >> (std::istream& s, TVector3<T>& vector)
    ;

  template <typename T>
  std::ostream& operator << (std::ostream& s, const TVector3<T>& vector)
    ;


  template <typename T>
  class TVector3
  {
    public:


    TVector3()
      ;

    TVector3(const T* ptr)
      throw(Exception::NullPointer);

    explicit TVector3(const T& value)
      ;

    TVector3(const T& vx, const T& vy, const T& vz)
      ;

    TVector3(const TVector3& vector)
      ;

    TVector3(const T& r, const TAngle<T>& phi, const TAngle<T>& theta)
      ;

    ~TVector3()
      ;

    void clear()
      ;



    void set(const T* ptr)
      throw(Exception::NullPointer);

    void set(const T& value)
      ;

    void set(const T& vx, const T& vy, const T& vz)
      ;

    void set(const TVector3& vector)
      ;

    void set(const T& r, const TAngle<T>& phi, const TAngle<T>& theta)
      ;

    TVector3& operator = (const TVector3& v)
      ;

    TVector3& operator = (T value)
      ;

    TVector3& operator = (const T* ptr)
      throw(Exception::NullPointer);

    void get(T* ptr) const
      throw(Exception::NullPointer);

    void get(T& x, T& y, T& z) const
      ;

    void get(TVector3& vector) const
      ;

    void get(T& r, TAngle<T>& phi, TAngle<T>& theta) const
      ;

    void swap(TVector3& vector)
      ;

    T getLength() const
      ;

    T getSquareLength() const
      ;

    TVector3& normalize()
      throw(Exception::DivisionByZero);

    TVector3& negate()
      ;

    static const TVector3& getZero()
      ;

    static const TVector3& getUnit()
      ;

    T& operator [] (Position position)
      throw(Exception::IndexOverflow);

    const T& operator [] (Position position) const
      throw(Exception::IndexOverflow);
    

    const TVector3& operator + () const
      ;

    TVector3 operator - () const
      ;

    TVector3 operator + (const TVector3& b) const
      ;

    TVector3 operator - (const TVector3& b) const
      ;

    TVector3& operator += (const TVector3& vector)
      ;

    TVector3& operator -= (const TVector3& vector)
      ;

    TVector3 operator * (const T& scalar) const
      ;

    TVector3& operator *= (const T& scalar)
      ;

    TVector3 operator / (const T& lambda) const
      throw(Exception::DivisionByZero);

    TVector3& operator /= (const T& lambda)
      throw(Exception::DivisionByZero);

    T operator * (const TVector3& vector) const
      ;

    TVector3 operator % (const TVector3& vector) const
      ;

    TVector3& operator %= (const TVector3& vector)
      ;



    T getDistance(const TVector3& vector) const
      ;

    T getSquareDistance(const TVector3& vector) const
      ;

    TAngle<T> getAngle(const TVector3& vector) const
      throw(Exception::DivisionByZero);

    TVector3 getOrthogonalProjection(const TVector3& direction) const
      ;

    static TVector3 getPerpendicularNormalization
      (const TVector3& a, const TVector3& b, const TVector3& c)
      ;

    static T getTripleProduct (const TVector3<T>& a, const TVector3<T>& b, const TVector3<T>& c)
      ;

  

    bool operator == (const TVector3& vector) const
      ;
  
    bool operator != (const TVector3& vector) const
      ;

    bool operator < (const TVector3& vector) const
      ;


    bool isZero() const
      ;

    bool isOrthogonalTo(const TVector3& vector) const
      ;




    void write(PersistenceManager& pm) const
      ;

    bool read(PersistenceManager& pm)
      ;



    void dump(std::ostream& s = std::cout, Size depth = 0) const
      ;

    bool isValid() const
      ;



  
    T x;

    T y;

    T z;

    private:

    TAngle<T> getAngle_(const T& a, const T& b) const
      
    {
      TAngle<T> angle;
  
      if (Maths::isNotZero(a))
      {
        angle = atan(b / a);
      } 
      else  
      {
        angle = BALL_SGN(b) * Constants::PI / 2;
      }

      if (Maths::isLess(a, 0)) 
      {
        angle += Constants::PI;
      }

      if (Maths::isLess(angle.value, 0)) 
      {
        return (Angle)(angle.value += 2.0 * Constants::PI);
      } 
      else 
      {
        return angle;
      }
    }
  };

  template <typename T>
  BALL_INLINE
  TVector3<T>::TVector3()
    
    : x(0),
      y(0),
      z(0)
  {
  }

  template <typename T>
  BALL_INLINE
  TVector3<T>::TVector3(const T* ptr)
    throw(Exception::NullPointer)
  {
    if (ptr == 0) 
    {
      throw Exception::NullPointer(__FILE__, __LINE__);
    }

    x = *ptr++;
    y = *ptr++;
    z = *ptr;
  }

  template <typename T>
  BALL_INLINE
  TVector3<T>::TVector3(const T& value)
    
    : x(value),
      y(value),
      z(value)
  {
  }

  template <typename T>
  BALL_INLINE
  TVector3<T>::TVector3(const T& vx, const T& vy, const T& vz)
    
    : x(vx),
      y(vy),
      z(vz)
  {
  }

  template <typename T>
  BALL_INLINE
  TVector3<T>::TVector3(const TVector3& vector)
    
    : x(vector.x),
      y(vector.y),
      z(vector.z)
  {
  }

  template <typename T>
  BALL_INLINE
  TVector3<T>::TVector3(const T& r, const TAngle<T>& phi, const TAngle<T>& theta)
    
    : x(r * cos(phi) * sin(theta)),
      y(r * sin(phi) * sin(theta)),
      z(r * cos(theta))
  {
  }

  template <typename T>
  BALL_INLINE
  TVector3<T>::~TVector3()
    
  {
  }

  template <typename T>
  BALL_INLINE
  void TVector3<T>::clear()
    
  {
    x = y = z = (T)0;
  }

  template <typename T>
  BALL_INLINE 
  void TVector3<T>::set(const T* ptr)
    throw(Exception::NullPointer)
  {
    if (ptr == 0) 
      throw Exception::NullPointer(__FILE__, __LINE__);
    
    x = *ptr++;
    y = *ptr++;
    z = *ptr;
  }

  template <typename T>
  BALL_INLINE 
  void TVector3<T>::set(const T& value)
    
  {
    x = value;
    y = value;
    z = value;
  }

  template <typename T>
  BALL_INLINE 
  void TVector3<T>::set(const T& vx, const T& vy, const T& vz)
    
  {
    x = vx;
    y = vy;
    z = vz;
  }

  template <typename T>
  BALL_INLINE 
  void TVector3<T>::set(const TVector3<T>& vector)
    
  {
    x = vector.x;
    y = vector.y;
    z = vector.z;
  }

  template <typename T>
  BALL_INLINE
  void TVector3<T>::set(const T& r, const TAngle<T> &phi, const TAngle<T> &theta)
    
  {
    x = r * cos(phi) * sin(theta);
    y = r * sin(phi) * sin(theta);
    z = r * cos(theta);
  }

  template <typename T>
  BALL_INLINE
  TVector3<T>& TVector3<T>::operator = (const T* ptr)
    throw(Exception::NullPointer)
  {
    if (ptr == 0)
    {
      throw Exception::NullPointer(__FILE__, __LINE__);
    }

    x = *ptr++;
    y = *ptr++;
    z = *ptr;

    return *this;
  }

  template <typename T>
  BALL_INLINE 
  TVector3<T>& TVector3<T>::operator = (const TVector3<T>& vector)
    
  {
    x = vector.x;
    y = vector.y;
    z = vector.z;

    return *this;
  }

  template <typename T>
  BALL_INLINE 
  TVector3<T>& TVector3<T>::operator = (T value)
    
  {
    x = y = z = value;

    return *this;
  }

  template <typename T>
  BALL_INLINE 
  void TVector3<T>::get(T* ptr) const
    throw(Exception::NullPointer)
  {
    if (ptr == 0)
    {
      throw Exception::NullPointer(__FILE__, __LINE__);
    }

    *ptr++ = x;
    *ptr++ = y;
    *ptr   = z;
  }

  template <typename T>
  BALL_INLINE 
  void TVector3<T>::get(T& new_x, T& new_y, T& new_z) const
    
  {
    new_x = x;
    new_y = y;
    new_z = z;
  }

  template <typename T>
  BALL_INLINE 
  void TVector3<T>::get(TVector3<T>& vector) const
    
  {
    vector.x = x;
    vector.y = y;
    vector.z = z;
  }

  template <typename T>
  BALL_INLINE 
  void TVector3<T>::get(T& r, TAngle<T>& phi, TAngle<T>& theta) const
    
  {
    r     = sqrt(x * x + y * y + z * z);
    phi   = (Angle)getAngle_(x, y);
    theta = getAngle_(z, sqrt(x * x + y * y));
  }

  template <typename T>
  BALL_INLINE
  void TVector3<T>::swap(TVector3<T>& vector)
    
  {
    T temp = x;
    x = vector.x;
    vector.x = temp;

    temp = y;
    y = vector.y;
    vector.y = temp;

    temp = z;
    z = vector.z;
    vector.z = temp;
  }

  template <typename T>
  BALL_INLINE 
  T TVector3<T>::getLength() const
    
  {
    return (T)sqrt(x * x + y * y + z * z);
  }

  template <typename T>
  BALL_INLINE 
  T TVector3<T>::getSquareLength() const
    
  {
    return (x * x + y * y + z * z);
  }

  template <typename T>
  TVector3<T>& TVector3<T>::normalize()
    throw(Exception::DivisionByZero)
  {
    T len = sqrt(x * x + y * y + z * z);

    if (Maths::isZero(len)) 
    {
      throw Exception::DivisionByZero(__FILE__, __LINE__);
    }
    
    x /= len;
    y /= len;
    z /= len;

    return *this;
  }

  template <typename T>
  BALL_INLINE
  TVector3<T>& TVector3<T>::negate()
    
  {
    x *= -1;
    y *= -1;
    z *= -1;
    return *this;
  }

  template <typename T>
  BALL_INLINE 
  const TVector3<T>& TVector3<T>::getZero()
    
  {
    static TVector3<T> null_vector(0, 0, 0);
    return null_vector;
  }

  template <typename T>
  BALL_INLINE 
  const TVector3<T>& TVector3<T>::getUnit()
    
  {
    static TVector3<T> unit_vector(1, 1, 1);
    return unit_vector;
  }

  template <typename T>
  BALL_INLINE 
  T& TVector3<T>::operator [] (Position position)
    throw(Exception::IndexOverflow)
  {
    if (position > 2)
    {
      throw Exception::IndexOverflow(__FILE__, __LINE__, position);
    }
    switch (position) 
    {
      case 0: return x;
      case 1: return y;
      case 2:
      default:
        return z;
    }
  }

  template <typename T>
  BALL_INLINE 
  const T& TVector3<T>::operator [] (Position position) const
    throw(Exception::IndexOverflow)
  {
    if (position > 2)
    {
      throw Exception::IndexOverflow(__FILE__, __LINE__);
    }
    switch (position) 
    {
      case 0: return x;
      case 1: return y;
      case 2:
      default:
        return z;
    }
  }

  template <typename T>
  BALL_INLINE
  const TVector3<T>& TVector3<T>::operator + () const 
    
  {
    return *this;
  }

  template <typename T>
  BALL_INLINE
  TVector3<T> TVector3<T>::operator - () const  
    
  {
    return TVector3<T>(-x, -y, -z);
  }

  template <typename T>
  BALL_INLINE 
  TVector3<T>& TVector3<T>::operator += (const TVector3<T>& vector)
    
  {
    x += vector.x;
    y += vector.y;
    z += vector.z;

    return *this;
  }

  template <typename T>
  BALL_INLINE 
  TVector3<T>& TVector3<T>::operator -= (const TVector3<T>& vector)
    
  {
    x -= vector.x;
    y -= vector.y;
    z -= vector.z;

    return *this;
  }

  template <typename T>
  BALL_INLINE 
  TVector3<T> TVector3<T>::operator * (const T& scalar) const 
    
  {
    return TVector3<T>(x * scalar, y * scalar, z * scalar);
  }

  template <typename T>
  BALL_INLINE 
  TVector3<T>& TVector3<T>::operator *= (const T &scalar)
    
  {
    x *= scalar;
    y *= scalar;
    z *= scalar;

    return *this;
  }

  template <typename T>
  TVector3<T> TVector3<T>::operator / (const T& lambda) const
    throw(Exception::DivisionByZero)
  {
    if (lambda == (T)0)
    {
      throw Exception::DivisionByZero(__FILE__, __LINE__);
    }
    return TVector3<T>(x / lambda, y / lambda, z / lambda);
  }

  template <typename T>
  TVector3<T>& TVector3<T>::operator /= (const T& lambda)
    throw(Exception::DivisionByZero)
  {
    if (lambda == (T)0)
    {
      throw Exception::DivisionByZero(__FILE__, __LINE__);    
    }
    x /= lambda;
    y /= lambda;
    z /= lambda;

    return *this;
  }

  template <typename T>
  BALL_INLINE 
  T TVector3<T>::operator * (const TVector3<T>& vector) const
    
  {
    return (x * vector.x + y * vector.y + z * vector.z);
  }

  template <typename T>
  TVector3<T> TVector3<T>::operator % (const TVector3<T>& v) const
    
  {
    return TVector3(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
  }

  template <typename T>
  BALL_INLINE 
  TVector3<T>& TVector3<T>::operator %= (const TVector3<T>& v)
    
  {
    set(y * v.z - z * v.y, z * v.x - x * v.z, x * v.y - y * v.x);
    return *this;
  }

  template <typename T>
  BALL_INLINE 
  T TVector3<T>::getDistance(const TVector3<T>& v) const
    
  {
    T dx = x - v.x;
    T dy = y - v.y;
    T dz = z - v.z;
    
    return (T)sqrt(dx * dx + dy * dy + dz * dz); 
  }

  template <typename T>
  BALL_INLINE T
  TVector3<T>::getSquareDistance(const TVector3<T>& v) const
    
  {
    T dx = x - v.x;
    T dy = y - v.y;
    T dz = z - v.z;
    
    return (dx * dx + dy * dy + dz * dz); 
  }

  template <typename T>
  BALL_INLINE 
  TAngle<T> TVector3<T>::getAngle(const TVector3<T>& vector) const
    throw(Exception::DivisionByZero)
  {
    T length_product = getSquareLength() * vector.getSquareLength();

    if (length_product == (T)0) 
    {
      throw Exception::DivisionByZero(__FILE__, __LINE__);
    }
    
    T acos_arg = ((*this) * vector) / sqrt(length_product);

    // ensure that the argument of acos is in the correct range
    // (might happen if the angle between the two vectors is
    // very close to zero)
    if (fabs(acos_arg) > 1.0)
    { 
      return (TAngle<T>)0.0;
    }
    
    return (TAngle<T>)acos(acos_arg);
  }

  template <typename T>
  BALL_INLINE 
  TVector3<T> TVector3<T>::getOrthogonalProjection(const TVector3<T>& direction) const
    
  {
    return ((direction * (*this)) / (direction * direction) * direction);
  }

  template <typename T>
  TVector3<T> TVector3<T>::getPerpendicularNormalization
    (const TVector3<T> &a, const TVector3<T> &b, const TVector3<T> &c)
    
  {
    TVector3 diff1(b.x - a.x, b.y - a.y, b.z - a.z);
    TVector3 diff2(b.x - c.x, b.y - c.y, b.z - c.z);

    return TVector3
      (diff1.y * diff2.z - diff1.z * diff2.y,
       diff1.z * diff2.x - diff1.x * diff2.z,
       diff1.x * diff2.y - diff1.y * diff2.x);
  }

  template <typename T>
  BALL_INLINE
  T TVector3<T>::getTripleProduct
    (const TVector3<T>& a,
     const TVector3<T>& b,
     const TVector3<T>& c)
    
  {
    return (  a.x * (b.y * c.z - b.z * c.y)
            + a.y * (b.z * c.x - b.x * c.z)
            + a.z * (b.x * c.y - b.y * c.x));
  }

  template <typename T>
  BALL_INLINE 
  bool TVector3<T>::operator == (const TVector3<T>& v) const
    
  {
    return (Maths::isEqual(x, v.x) && Maths::isEqual(y, v.y) && Maths::isEqual(z, v.z));
  }

  template <typename T>
  BALL_INLINE 
  bool TVector3<T>::operator < (const TVector3<T>& v) const
    
  {
    return (x < v.x || y < v.y || z < v.z);
  }


  template <typename T>
  BALL_INLINE 
  bool TVector3<T>::operator != (const TVector3<T>& v) const
    
  {
    return (Maths::isNotEqual(x, v.x) || Maths::isNotEqual(y, v.y) || Maths::isNotEqual(z, v.z));
  }

  template <typename T>
  BALL_INLINE 
  bool TVector3<T>::isOrthogonalTo(const TVector3<T>& v) const
    
  {
    return Maths::isZero((*this) * v);
  }

  template <typename T>
  BALL_INLINE 
  bool TVector3<T>::isValid() const
    
  {
    return true;
  }

  template <typename T>
  BALL_INLINE 
  bool TVector3<T>::isZero() const
    
  {
    return (Maths::isZero(x) && Maths::isZero(y) && Maths::isZero(z));
  }

  template <typename T>
  void TVector3<T>::dump(std::ostream& s, Size depth) const
    
  {
    BALL_DUMP_STREAM_PREFIX(s);

    BALL_DUMP_HEADER(s, this, this);

    BALL_DUMP_DEPTH(s, depth);
    s << "  (x =  " << x << ", y = " << y << ", z = " << z << ")" << std::endl;

    BALL_DUMP_STREAM_SUFFIX(s);
  }

  typedef TVector3<float> Vector3;

  template <typename T>
  BALL_INLINE 
  TVector3<T> TVector3<T>::operator + (const TVector3<T>& b) const
    
  {
    return TVector3<T>(x + b.x, y + b.y, z + b.z);
  }
  
  template <typename T>
  BALL_INLINE
  TVector3<T> TVector3<T>::operator - (const TVector3<T>& b) const
    
  {
    return TVector3<T>(x - b.x, y - b.y, z - b.z);
  }

  template <typename T>
  void TVector3<T>::write(PersistenceManager& pm) const
    
  {
    pm.writePrimitive(x, "x");
    pm.writePrimitive(y, "y");
    pm.writePrimitive(z, "z");
  }

  template <typename T>     
  bool TVector3<T>::read(PersistenceManager& pm)
    
  {
    pm.readPrimitive(x, "x");
    pm.readPrimitive(y, "y");
    pm.readPrimitive(z, "z");

    return true;
  }


  template <typename T>
  BALL_INLINE 
  TVector3<T> operator * (const T& scalar, const TVector3<T>& vector)
    
  {
    return TVector3<T>(scalar * vector.x, scalar * vector.y, scalar * vector.z);
  }

  template <typename T>
  std::istream& operator >> (std::istream& s, TVector3<T>& v)
    
  {
    char c;
    s >> c >> v.x >> v.y >> v.z >> c;

    return s;
  }

  template <typename T>
  std::ostream& operator << (std::ostream& s, const TVector3<T>& v)
    
  {
    s << "(" << v.x << ' ' << v.y << ' ' << v.z << ')';

    return s;
  }

}// namespace BALL

#endif // BALL_MATHS_VECTOR3_H

---