// vecpos.h
// by Glenn G. Chappell <chappellg@member.ams.org>
// and Chris Hartman    <hartman@cs.uaf.edu>
// VERSION 0.6
// 23 Jan 2004
//
// Part of the TRANSF package
// Copyright (c) 2003 Glenn G. Chappell and Chris Hartman.
// This file, and the TRANSF package, are provided with no warranties
// whatsoever. The TRANSF package may be freely copied, modified, and used
// for any purpose, as long as the following conditions are met.
// (1) This notice must be retained.
// (2) If the TRANSF files are modified, or if portions of the files are
//     reused in other files, then the modified/reused code must be marked
//     as such.
//
// This file defines classes
//    tf::vec
//    tf::pos
// along with associated global functions in namespace tf.

#ifndef TF_FILE_VECPOS_H_INCLUDED
#define TF_FILE_VECPOS_H_INCLUDED

// ************************************************************************
// Includes & macro definitions
// ************************************************************************

// All macros ending in "_INTERNAL" are #undef'd at end of file.

#ifdef __sgi
# include <math.h>  // For sqrt
# define TF_SQRT_INTERNAL (sqrt)
#else
# include <cmath>   // For std::sqrt
# if defined(_MSC_VER) && (_MSC_VER <= 1200)
   // MS-Vis C++ before 7.0 <cmath> does not use namespace std
#  define TF_SQRT_INTERNAL (sqrt)
# else
#  define TF_SQRT_INTERNAL (std::sqrt)
# endif
#endif

#define TF_INLINE_INTERNAL inline
   // Dirty trick to avoid redefined-function compiler complaints

// ************************************************************************
// Begin namespace tf
// ************************************************************************

namespace tf {

// ************************************************************************
// class vec - Declaration
// ************************************************************************

// class vec
// 3-D vector of scalars.
class vec {

// ***** vec: types *****
public:
   typedef double real_type;      // For lengths, scalar mult,
                                  //  interpolation param's.
   typedef real_type value_type;  // For coord's & dot products.
   // Above is also used as value_type by class tf::pos, and, in file
   //  transf.h, classes tf::tfquat, tf::rot, tf::orient, tf::transf,
   //  tf::frameref.
   // Note: This code is written to allow for the future possibility of
   //  vectors of (say) int's. In this case, value_type would be int, but
   //  real_type would still be double. At present, however, for the
   //  classes defined in transf.h to work properly, vec::value_type must
   //  be a floating-point type.

// ***** vec: friend declarations
   friend class pos;
   friend bool operator==(const vec & lhs,    // uses d_[]
                          const vec & rhs);
   friend bool operator!=(const vec & lhs,    // uses d_[]
                          const vec & rhs);
   friend vec operator-(const vec & thevec);  // uses d_[]

// ***** vec: internal-use utility functions *****
private:
   // We also put utility functions for class pos here.
   // (But there are none at present.)
   static real_type valuesqrt(value_type s)
   { return TF_SQRT_INTERNAL(s); }

// ***** vec: ctors, dctor, op=, swap *****
public:
   vec()
   { d_[0] = 0.; d_[1] = 0.; d_[2] = 0.; }
   vec(value_type thex,  // No 1-param ctor from numeric
       value_type they,
       value_type thez = 0.)
   { d_[0] = thex; d_[1] = they; d_[2] = thez; }
   // Copy ctor, copy assn, dctor: use compiler-generated
   inline explicit vec(const pos & p);
      // Implemented outside of class declaration
   void swap(vec & other)
   {
      value_type tmp = d_[0];
      d_[0] = other.d_[0];
      other.d_[0] = tmp;
      tmp = d_[1];
      d_[1] = other.d_[1];
      other.d_[1] = tmp;
      tmp = d_[2];
      d_[1] = other.d_[2];
      other.d_[2] = tmp;
   }

// ***** vec: element access *****
public:
   const value_type * c_array() const
   { return d_; }

   value_type & operator[](int subs)              // No bounds check
   { return d_[subs]; }
   const value_type & operator[](int subs) const  // No bounds check
   { return d_[subs]; }

   value_type & x()
   { return d_[0]; }
   const value_type & x() const
   { return d_[0]; }
   value_type & y()
   { return d_[1]; }
   const value_type & y() const
   { return d_[1]; }
   value_type & z()
   { return d_[2]; }
   const value_type & z() const
   { return d_[2]; }

   value_type & r()                               // Same as x
   { return d_[0]; }
   const value_type & r() const                   // Same as x
   { return d_[0]; }
   value_type & g()                               // Same as y
   { return d_[1]; }
   const value_type & g() const                   // Same as y
   { return d_[1]; }
   value_type & b()                               // Same as z
   { return d_[2]; }
   const value_type & b() const                   // Same as z
   { return d_[2]; }

// ***** vec: functions dealing with zero *****
public:
   static vec zero()
   { return vec(0., 0., 0.); }

// ***** vec: arithmetic/manipulation *****
public:
   vec & operator+=(const vec & rhs)
   {
      d_[0] += rhs.d_[0];
      d_[1] += rhs.d_[1];
      d_[2] += rhs.d_[2];
      return *this;
   }
   vec & operator-=(const vec & rhs)
   {
      d_[0] -= rhs.d_[0];
      d_[1] -= rhs.d_[1];
      d_[2] -= rhs.d_[2];
      return *this;
   }
   vec & operator*=(real_type s)
   { d_[0] *= s; d_[1] *= s; d_[2] *= s; return *this; }
   vec & operator/=(real_type s)         // Not safe; may divide by zero
   { d_[0] /= s; d_[1] /= s; d_[2] /= s; return *this; }
   vec coordwiseprod(const vec & rhs) const
   { return vec(d_[0]*rhs.d_[0], d_[1]*rhs.d_[1], d_[2]*rhs.d_[2]); }
   value_type dot(const vec & rhs) const
   { return d_[0]*rhs.d_[0] + d_[1]*rhs.d_[1] + d_[2]*rhs.d_[2]; }
   // lengthsqr: Length (Euclidean norm) squared.
   // More efficient than computing the length and squaring.
   value_type lengthsqr() const
   { return dot(*this); }
   // length: Euclidean norm
   real_type length() const
   { return valuesqrt(lengthsqr()); }
   // norm1: 1-norm (sum of absolute values of components).
   value_type norm1() const
   {
      return (d_[0] >= 0. ? d_[0] : -d_[0])
           + (d_[1] >= 0. ? d_[1] : -d_[1])
           + (d_[2] >= 0. ? d_[2] : -d_[2]);
   }
   // norminf: Infinity-norm (max of the absolute values of components).
   value_type norminf() const
   {
      // Avoid problematic implementations of max on some compilers.
      value_type maxabsval = (d_[0] >= 0. ? d_[0] : -d_[0]);
      value_type absval = (d_[1] >= 0. ? d_[1] : -d_[1]);
      if (absval > maxabsval) maxabsval = absval;
      absval = (d_[2] >= 0. ? d_[2] : -d_[2]);
      if (absval > maxabsval) maxabsval = absval;
      return maxabsval;
   }
   vec normalized() const  // Safe; will not divide by zero
   {
      const real_type lensqr = lengthsqr();
      return (lensqr != 0.) ? vec(*this) /= valuesqrt(lensqr)
                            : vec(1.,0.,0.);
   }
   vec cross(const vec & rhs) const      // UNIQUE to 3-D vec
   {
      return vec(d_[1]*rhs.d_[2]-d_[2]*rhs.d_[1],
                 d_[2]*rhs.d_[0]-d_[0]*rhs.d_[2],
                 d_[0]*rhs.d_[1]-d_[1]*rhs.d_[0]);
   }
   vec component(const vec & dir) const  // Safe; will not divide by zero
   {
      const value_type lsdir = dir.lengthsqr();
      return (lsdir != 0.) ? vec(dir) *= ((*this).dot(dir)/lsdir)
                           : zero();
   }

// ***** vec: functions for vec's as transformations *****
public:
   vec & compose_assign(const vec & rhs)
   { return *this += rhs; }
   vec & lcompose_assign(const vec & rhs)
   { return *this += rhs; }
   vec compose(const vec & other) const
   { return vec(*this) += other; }
   vec applyto(const vec & operand) const
   { return vec(operand) += *this; }
   inline pos applyto(const pos & operand) const;
      // Implemented outside of class declaration
   vec inverse() const
   { return vec(-d_[0], -d_[1], -d_[2]); }
   vec power(real_type s) const
   { return vec(*this) *= s; }

   // Column-major ordering matrix creation
#define TF_MM16C_VEC_BODY_INTERNAL(T)                     \
   m[ 0]=T(1.); m[ 4]=T(0.); m[ 8]=T(0.); m[12]=T(d_[0]); \
   m[ 1]=T(0.); m[ 5]=T(1.); m[ 9]=T(0.); m[13]=T(d_[1]); \
   m[ 2]=T(0.); m[ 6]=T(0.); m[10]=T(1.); m[14]=T(d_[2]); \
   m[ 3]=T(0.); m[ 7]=T(0.); m[11]=T(0.); m[15]=T(1.);

   void makematrix16c(double * m) const
   { TF_MM16C_VEC_BODY_INTERNAL(double) }
   void makematrix16c(float * m) const
   { TF_MM16C_VEC_BODY_INTERNAL(float) }
      // "float" here eliminates double-to-float conversion warnings
   template<typename M>
   void makematrix16c(M & m) const
   { TF_MM16C_VEC_BODY_INTERNAL(double) }
   template<typename M>
   void makematrix16c(const M & m) const
   { TF_MM16C_VEC_BODY_INTERNAL(double) }
#undef TF_MM16C_VEC_BODY_INTERNAL

   // Row-major ordering matrix creation
#define TF_MM16R_VEC_BODY_INTERNAL(T)                     \
   m[ 0]=T(1.); m[ 1]=T(0.); m[ 2]=T(0.); m[ 3]=T(d_[0]); \
   m[ 4]=T(0.); m[ 5]=T(1.); m[ 6]=T(0.); m[ 7]=T(d_[1]); \
   m[ 8]=T(0.); m[ 9]=T(0.); m[10]=T(1.); m[11]=T(d_[2]); \
   m[12]=T(0.); m[13]=T(0.); m[14]=T(0.); m[15]=T(1.);

   void makematrix16r(double * m) const
   { TF_MM16R_VEC_BODY_INTERNAL(double) }
   void makematrix16r(float * m) const
   { TF_MM16R_VEC_BODY_INTERNAL(float) }
      // "float" here eliminates double-to-float conversion warnings
   template<typename M>
   void makematrix16r(M & m) const
   { TF_MM16R_VEC_BODY_INTERNAL(double) }
   template<typename M>
   void makematrix16r(const M & m) const
   { TF_MM16R_VEC_BODY_INTERNAL(double) }
#undef TF_MM16R_VEC_BODY_INTERNAL

// ***** vec: data members *****
private:
   value_type d_[3];  // x, y, z components

};  // End class vec

// ************************************************************************
// class vec - Related global operators
// ************************************************************************

// Note: operator== and operator!= are included here for completeness;
// however, like all floating-point equality checks, they should be used
// sparingly, if at all. One good way to check equality or near equality is
// to use something like
//    (v1-v2).norminf() < epsilon
// where epsilon is a small, positive floating-point number.

TF_INLINE_INTERNAL
bool operator==(const vec & lhs,   // friend of class vec
                const vec & rhs)
{
   return lhs.d_[0] == rhs.d_[0]
       && lhs.d_[1] == rhs.d_[1]
       && lhs.d_[2] == rhs.d_[2];
}

TF_INLINE_INTERNAL
bool operator!=(const vec & lhs,   // friend of class vec
                const vec & rhs)
{
   return lhs.d_[0] != rhs.d_[0]
       || lhs.d_[1] != rhs.d_[1]
       || lhs.d_[2] != rhs.d_[2];
}

TF_INLINE_INTERNAL
vec operator+(const vec & lhs,
              const vec & rhs)
{ return vec(lhs) += rhs; }

TF_INLINE_INTERNAL
vec operator-(const vec & lhs,
              const vec & rhs)
{ return vec(lhs) -= rhs; }

TF_INLINE_INTERNAL
vec operator*(const vec & thevec,
              const vec::real_type s)
{ return vec(thevec) *= s; }

TF_INLINE_INTERNAL
vec operator*(const vec::real_type s,
              const vec & thevec)
{ return vec(thevec) *= s; }

TF_INLINE_INTERNAL
vec operator/(const vec & thevec,  // Not safe; may divide by zero
              const vec::real_type s)
{ return vec(thevec) /= s; }

TF_INLINE_INTERNAL
vec operator-(const vec & thevec)  // friend of class vec
{ return vec(-thevec.d_[0], -thevec.d_[1], -thevec.d_[2]); }

// ************************************************************************
// class vec - Related global functions
// ************************************************************************

TF_INLINE_INTERNAL
void swap(vec & v1, vec & v2)
{ v1.swap(v2); }

TF_INLINE_INTERNAL
vec coordwiseprod(const vec & lhs,
                  const vec & rhs)
{ return lhs.coordwiseprod(rhs); }

TF_INLINE_INTERNAL
vec::value_type dot(const vec & lhs,
                    const vec & rhs)
{ return lhs.dot(rhs); }

TF_INLINE_INTERNAL
vec cross(const vec & lhs,
          const vec & rhs)  // UNIQUE to 3-D vec
{ return lhs.cross(rhs); }

TF_INLINE_INTERNAL
vec compose(const vec & lhs,
            const vec & rhs)
{ return lhs.compose(rhs); }

TF_INLINE_INTERNAL
vec power(const vec & thevec,
          vec::real_type s)
{ return thevec.power(s); }

TF_INLINE_INTERNAL
vec applyto(const vec & thevec,
            const vec & operand)
{ return thevec.applyto(operand); }

TF_INLINE_INTERNAL
vec interpolate(const vec & a,
                const vec & b,
                vec::real_type t)
{ return t*(b-a) + a; }

TF_INLINE_INTERNAL
vec affinecomb(vec::real_type c1,  // Safe; will not divide by zero
               const vec & v1,
               vec::real_type c2 = 0.,
               const vec & v2 = vec::zero(),
               vec::real_type c3 = 0.,
               const vec & v3 = vec::zero(),
               vec::real_type c4 = 0.,
               const vec & v4 = vec::zero())
{
   const vec::real_type sum = c1+c2+c3+c4;
   if (sum != 0.)
      return (c1*v1 + c2*v2 + c3*v3 + c4*v4) / sum;
   else
      return v1;
}

// ************************************************************************
// class pos - Declaration
// ************************************************************************

// class pos
// 3-D position.
// Absolute version of class vec.
class pos {

// ***** pos: types *****
public:
   typedef vec::value_type value_type;  // For coord's.
   typedef vec::real_type real_type;    // For interpolation param's.

// ***** pos: friend declarations *****
   friend class vec;
   friend bool operator==(const pos & lhs,  // Uses d_[]
                          const pos & rhs);
   friend bool operator!=(const pos & lhs,  // Uses d_[]
                          const pos & rhs);
   friend vec operator-(const pos & lhs,    // Uses d_[]
                        const pos & rhs);

// ***** pos: ctors, dctor, op=, swap *****
public:
   pos()
   { d_[0] = 0.; d_[1] = 0.; d_[2] = 0.; }
   pos(value_type thex,  // No 1-param ctor from numeric
       value_type they,
       value_type thez = 0.)
   { d_[0] = thex; d_[1] = they; d_[2] = thez; }
   // Copy ctor, copy assn, dctor: use compiler-generated
   explicit pos(const vec & v)
   { d_[0] = v.d_[0]; d_[1] = v.d_[1]; d_[2] = v.d_[2]; }
   void swap(pos & other)
   {
      value_type tmp = d_[0];
      d_[0] = other.d_[0];
      other.d_[0] = tmp;
      tmp = d_[1];
      d_[1] = other.d_[1];
      other.d_[1] = tmp;
      tmp = d_[2];
      d_[1] = other.d_[2];
      other.d_[2] = tmp;
   }

// ***** pos: element access *****
public:
   const value_type * c_array() const
   { return d_; }

   value_type & operator[](int subs)              // No bounds check
   { return d_[subs]; }
   const value_type & operator[](int subs) const  // No bounds check
   { return d_[subs]; }

   value_type & x()
   { return d_[0]; }
   const value_type & x() const
   { return d_[0]; }
   value_type & y()
   { return d_[1]; }
   const value_type & y() const
   { return d_[1]; }
   value_type & z()
   { return d_[2]; }
   const value_type & z() const
   { return d_[2]; }

// ***** pos: functions dealing with zero *****
public:
   static pos zero()
   { return pos(0., 0., 0.); }

// ***** pos: arithmetic/manipulation *****
public:
   pos & operator+=(const vec & rhs)
   {
      d_[0] += rhs.d_[0];
      d_[1] += rhs.d_[1];
      d_[2] += rhs.d_[2];
      return *this;
   }
   pos & operator-=(const vec & rhs)
   {
      d_[0] -= rhs.d_[0];
      d_[1] -= rhs.d_[1];
      d_[2] -= rhs.d_[2];
      return *this;
   }

// ***** pos: data members *****
private:
   value_type d_[3];  // x, y, z components

};  // End class pos

// ************************************************************************
// class pos - Related global operators
// ************************************************************************

// Note: operator== and operator!= are included here for completeness;
// however, like all floating-point equality checks, they should be used
// sparingly, if at all. One good way to check equality or near equality is
// to use something like
//    (p1-p2).norminf() < epsilon
// where epsilon is a small, positive floating-point number.

TF_INLINE_INTERNAL
bool operator==(const pos & lhs,  // friend of class pos
                const pos & rhs)
{
   return lhs.d_[0] == rhs.d_[0]
       && lhs.d_[1] == rhs.d_[1]
       && lhs.d_[2] == rhs.d_[2];
}

TF_INLINE_INTERNAL
bool operator!=(const pos & lhs,  // friend of class pos
                const pos & rhs)
{
   return lhs.d_[0] != rhs.d_[0]
       || lhs.d_[1] != rhs.d_[1]
       || lhs.d_[2] != rhs.d_[2];
}

TF_INLINE_INTERNAL
pos operator+(const pos & lhs,
              const vec & rhs)
{ return pos(lhs) += rhs; }

TF_INLINE_INTERNAL
pos operator+(const vec & lhs,
              const pos & rhs)
{ return pos(rhs) += lhs; }

TF_INLINE_INTERNAL
pos operator-(const pos & lhs,
              const vec & rhs)
{ return pos(lhs) -= rhs; }

TF_INLINE_INTERNAL
vec operator-(const pos & lhs,    // friend of class pos
              const pos & rhs)
{
   return vec(lhs.d_[0]-rhs.d_[0],
              lhs.d_[1]-rhs.d_[1],
              lhs.d_[2]-rhs.d_[2]);
}

// ************************************************************************
// class pos - Related global functions
// ************************************************************************

TF_INLINE_INTERNAL
void swap(pos & p1, pos & p2)
{ p1.swap(p2); }

TF_INLINE_INTERNAL
pos applyto(const vec & thevec,
            const pos & operand)
{ return thevec.applyto(operand); }

TF_INLINE_INTERNAL
pos interpolate(const pos & a,
                const pos & b,
                pos::real_type t)
{ return t*(b-a) + a; }

TF_INLINE_INTERNAL
pos affinecomb(pos::real_type c1,  // Safe; will not divide by zero
               const pos & p1,
               pos::real_type c2 = 0.,
               const pos & p2 = pos::zero(),
               pos::real_type c3 = 0.,
               const pos & p3 = pos::zero(),
               pos::real_type c4 = 0.,
               const pos & p4 = pos::zero())
{
   const pos::real_type sum = c1+c2+c3+c4;
   if (sum != 0.)
      return p1 + (c2*(p2-p1) + c3*(p3-p1) + c4*(p4-p1))/sum;
   else
      return p1;
}

// ************************************************************************
// class vec - Member functions involving pos
// ************************************************************************

inline vec::vec(const pos & p)  // explicit
{ d_[0] = p.d_[0]; d_[1] = p.d_[1]; d_[2] = p.d_[2]; }

inline pos vec::applyto(const pos & operand) const
{ return pos(operand) += *this; }

// ************************************************************************
// End namespace tf
// ************************************************************************

}  // namespace tf

// ************************************************************************
// Internal-only macro undef's
// ************************************************************************

#undef TF_SQRT_INTERNAL
#undef TF_INLINE_INTERNAL

#endif  //#ifndef TF_FILE_VECPOS_H_INCLUDED