vect3.hpp

//
// Copyright (c) Singular Inversions Inc. 1998.
//
 
#ifndef FR2_VECT3_HPP
#define FR2_VECT3_HPP
 
#include "FgSerial.hpp"
 
namespace Fg {
 
template<class T> class FR2Vect3TC
{
public:
    T      x1,x2,x3;
 
                // Constructors.
    FR2Vect3TC() : x1(0), x2(0), x3(0) {};                                      // default
        FR2Vect3TC(T a) : x1(a), x2(a), x3(a) {};                               // init single val.
        FR2Vect3TC(T y1,T y2,T y3) : x1(y1), x2(y2), x3(y3) {}; // init each val.
        template<class U>
    explicit FR2Vect3TC(const U &y) :  x1(T(y.x1)), x2(T(y.x2)), x3(T(y.x3)) {}
 
        T&                                              operator[](uint n)
                                                        {FGASSERT_FAST(n < 3);return (&x1)[n];}
        T const&                                operator[](uint n) const
                                                        {FGASSERT_FAST(n < 3);return (&x1)[n];}
 
        void                            set(T a) {x1 = a;x2 = a;x3 = a;}
    const FR2Vect3TC    operator-() const;                  // unary operator
    void                operator-=(FR2Vect3TC);
    void                operator+=(FR2Vect3TC);
    const FR2Vect3TC    operator+(const FR2Vect3TC&) const;
    const FR2Vect3TC    operator-(const FR2Vect3TC&) const;
    const FR2Vect3TC    operator*(T) const;
    const FR2Vect3TC    operator/(T) const;
    void                operator*=(T);
    void                operator/=(T);
    bool                operator==(const FR2Vect3TC &v) const
                        {return (x1==v.x1 && x2==v.x2 && x3==v.x3);}
    bool                operator!=(const FR2Vect3TC &v) const
                        {return (x1!=v.x1 || x2!=v.x2 || x3!=v.x3);}
    T const             len() const {return (T)(sqrt(x1*x1+x2*x2+x3*x3));}
    T const             mag() const {return(x1*x1+x2*x2+x3*x3);}
    const FR2Vect3TC    rotateX(T) const;
    const FR2Vect3TC    rotateY(T) const;
    const FR2Vect3TC    rotateZ(T) const;
    const FR2Vect3TC    rotateK(FR2Vect3TC,T) const;
    const FR2Vect3TC    cross(FR2Vect3TC) const;
    T const dot(FR2Vect3TC vec) const {return(x1*vec.x1+x2*vec.x2+x3*vec.x3);}
};
 
// **********************************************************************
// **********************************************************************
 
typedef FR2Vect3TC<int>         FR2Vect3IC;
typedef FR2Vect3TC<float>       FR2Vect3FC;
typedef FR2Vect3TC<int>         FutVect3IC;
typedef FR2Vect3TC<float>       FutVect3FC;
typedef FR2Vect3TC<double>      FutVect3DC;
 
template <class T>
struct FR2Vect3LessThanByAxisT
{
    bool operator()(const FR2Vect3TC<T> &x, const FR2Vect3TC<T> &y) const
            { return ((x.x3 < y.x3) ||
                      (x.x3 == y.x3 && x.x2 < y.x2) ||
                      (x.x3 == y.x3 && x.x2 == y.x2 && x.x1 < y.x1)); }
};
 
 
// Definitions are the in the 'cpp' file since they are written separately
// for each template type.
template<class T> std::ostream& operator<<(std::ostream&,FR2Vect3TC<T>);
 
template<class T>
std::istream&   operator>>(std::istream& s,FR2Vect3TC<T> &v)
{
        s >> v.x1;
        s >> v.x2;
        s >> v.x3;
        return s;
}
 
template<class T>
const inline FR2Vect3TC<T>  FR2Vect3TC<T>::operator-() const
{
    return FR2Vect3TC<T>(-x1,-x2,-x3);
}
 
template<class T>
const inline FR2Vect3TC<T>  FR2Vect3TC<T>::operator+(
    const FR2Vect3TC<T>   &v1)
        const
{
    return FR2Vect3TC<T>(x1 + v1.x1,
                         x2 + v1.x2,
                         x3 + v1.x3);
}
 
template<class T>
const inline FR2Vect3TC<T>  FR2Vect3TC<T>::operator-(
    const FR2Vect3TC<T>   &v1)
        const
{
    return FR2Vect3TC<T>(x1 - v1.x1,
                         x2 - v1.x2,
                         x3 - v1.x3);
}
 
template<class T>
inline void FR2Vect3TC<T>::operator-=(
    FR2Vect3TC<T>   v)
{
    x1 -= v.x1;
    x2 -= v.x2;
    x3 -= v.x3;
}
 
template<class T>
inline void FR2Vect3TC<T>::operator+=(
    FR2Vect3TC<T>   v)
{
    x1 += v.x1;
    x2 += v.x2;
    x3 += v.x3;
}
 
template<class T>
const inline FR2Vect3TC<T>  FR2Vect3TC<T>::operator*(
    T      val)
        const
{
    return FR2Vect3TC<T>(x1*val, x2*val, x3*val);
}
 
template<class T>
const inline FR2Vect3TC<T>  FR2Vect3TC<T>::operator/(
    T      val)         // Good form to create inverse operator, even if not used.
        const
{
    FGASSERT_FAST(val != T(0));
 
    return FR2Vect3TC<T>(x1/val, x2/val, x3/val);
}
 
template<class T>
inline void FR2Vect3TC<T>::operator*=(
    T  val)
{
    x1 *= val;
    x2 *= val;
    x3 *= val;
}
 
template<class T>
inline void FR2Vect3TC<T>::operator/=(
    T  val)
{
    FGASSERT_FAST(val != T(0));
 
    x1 /= val;
    x2 /= val;
    x3 /= val;
}
 
//***********************************************************************
//                      Rotations
//***********************************************************************
// All rotations act according to the right hand rule around the axis
// about which they are named. Angles are given in radians.
 
template<class T>
const inline FR2Vect3TC<T> FR2Vect3TC<T>::rotateX(
 
        T       theta)
        const
{
    T                   cosine  = (T)cos(theta), // Casting is safe here to float since
                sine    = (T)sin(theta); // range of sin/cos is [-1,1].
 
    return FR2Vect3TC<T>(x1,
                         x2 * cosine - x3 * sine,
                         x3 * cosine + x2 * sine);
}
 
template<class T>
const inline FR2Vect3TC<T> FR2Vect3TC<T>::rotateY(
 
        T       theta)
        const
{
    T                   cosine = (T)cos(theta),
                sine   = (T)sin(theta);
 
    return FR2Vect3TC<T>(x1 * cosine + x3 * sine,
                         x2,
                         x3 * cosine - x1 * sine);
}
 
template<class T>
const inline FR2Vect3TC<T> FR2Vect3TC<T>::rotateZ(
 
    T   theta)
        const
{
    T                   cosine = (T)cos(theta),
                sine   = (T)sin(theta);
 
    return FR2Vect3TC<T>(x1 * cosine - x2 * sine,
                         x2 * cosine + x1 * sine,
                         x3);
}
 
template<class T>
const FR2Vect3TC<T>    FR2Vect3TC<T>::rotateK(
    FR2Vect3TC<T>   K,          // arbitrary axis of rotation
    T       theta)      // angle of rotation
        const
{
    T      cosine=cos(theta),
                sine=sin(theta);
    T      vtheta = 1.0-cosine;
    FR2Vect3TC<T>       retval, k;
 
    // Make sure that the given vector is a unit vector
        T               ln = K.len();
    FGASSERT_FAST(ln != T(0));
    k = K / ln;
 
    retval.x1 =
        (k.x1*k.x1*vtheta+cosine)    * x1 +
        (k.x1*k.x2*vtheta-k.x3*sine) * x2 +
        (k.x1*k.x3*vtheta+k.x2*sine) * x3;
    retval.x2 =
        (k.x1*k.x2*vtheta+k.x3*sine) * x1 +
        (k.x2*k.x2*vtheta+cosine)    * x2 +
        (k.x2*k.x3*vtheta-k.x1*sine) * x3;
    retval.x3 =
        (k.x1*k.x3*vtheta-k.x2*sine) * x1 +
        (k.x2*k.x3*vtheta+k.x1*sine) * x2 +
        (k.x3*k.x3*vtheta+cosine)    * x3;
 
    return(retval);
}
 
template<class T>
const inline FR2Vect3TC<T> FR2Vect3TC<T>::cross(
 
        FR2Vect3TC<T> vec)
        const
{
    return FR2Vect3TC<T>(x2 * vec.x3 - x3 * vec.x2,
                         x3 * vec.x1 - x1 * vec.x3,
                         x1 * vec.x2 - x2 * vec.x1);
}
 
 
// Given a list of vector sorted by z coord, then y cood, then x coord,
// returns the index position of where the new vector should be inserted.
// If there are exact matches between the new vector and elements in the
// list, the function will return the index position of one of the exact
// matches.  This function uses binary search to find the insertion position.
//
// NOTE: The algorithm for this function is exactly the same as the one in
// algs.hpp's fr2FindSortedListInsertPos.  If there are any efficiency
// issues, I should change fr2FindSortedListInsertPos to accept a predicate
// as template argument and then define a special predicate for vector3 class.
//
template <class T>
int fr2FindSortedVect3InsertPos(
    const std::vector< FR2Vect3TC<T> > &list, const FR2Vect3TC<T> &pt)
{
    int numPts = list.size();
    if (numPts == 0) return 0;
 
    int lo = 0;
    int hi = numPts-1;
 
    if ((list[0].x3 > pt.x3) ||
        (list[0].x3 == pt.x3 && list[0].x2 > pt.x2) ||
        (list[0].x3 == pt.x3 && list[0].x2 == pt.x2 && list[0].x1 >= pt.x1))
        return 0;
 
    if ((list[hi].x3 < pt.x3) ||
        (list[hi].x3 == pt.x3 && list[hi].x2 < pt.x2) ||
        (list[hi].x3 == pt.x3 && list[hi].x2 == pt.x2 && list[hi].x1 < pt.x1))
        return numPts;
 
    int idx = (hi+lo+1)/2;
 
    while (idx != hi)
    {
        if ((list[idx].x3 > pt.x3) ||
            (list[idx].x3 == pt.x3 && list[idx].x2 > pt.x2) ||
            (list[idx].x3 == pt.x3 && list[idx].x2 == pt.x2 &&
             list[idx].x1 > pt.x1))
            hi = idx;
        else if ((list[idx].x3 < pt.x3) ||
                 (list[idx].x3 == pt.x3 && list[idx].x2 < pt.x2) ||
                 (list[idx].x3 == pt.x3 && list[idx].x2 == pt.x2 &&
                  list[idx].x1 < pt.x1))
            lo = idx;
        else // all three coordinates are equal
            hi = lo = idx;
 
        idx = (hi+lo+1)/2;
    }
 
    return idx;
}
 
}
 
#endif