UnitVec.h

Go to the documentation of this file.

#ifndef SimTK_UNITVEC_H 
#define SimTK_UNITVEC_H 

/* -------------------------------------------------------------------------- *
 *                      SimTK Core: SimTK Simmatrix(tm)                       *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK Core biosimulation toolkit originating from      *
 * Simbios, the NIH National Center for Physics-Based Simulation of           *
 * Biological Structures at Stanford, funded under the NIH Roadmap for        *
 * Medical Research, grant U54 GM072970. See https://simtk.org.               *
 *                                                                            *
 * Portions copyright (c) 2005-10 Stanford University and the Authors.        *
 * Authors: Michael Sherman                                                   *
 * Contributors: Paul Mitiguy                                                 *
 *                                                                            *
 * Permission is hereby granted, free of charge, to any person obtaining a    *
 * copy of this software and associated documentation files (the "Software"), *
 * to deal in the Software without restriction, including without limitation  *
 * the rights to use, copy, modify, merge, publish, distribute, sublicense,   *
 * and/or sell copies of the Software, and to permit persons to whom the      *
 * Software is furnished to do so, subject to the following conditions:       *
 *                                                                            *
 * The above copyright notice and this permission notice shall be included in *
 * all copies or substantial portions of the Software.                        *
 *                                                                            *
 * THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR *
 * IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,   *
 * FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL    *
 * THE AUTHORS, CONTRIBUTORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM,    *
 * DAMAGES OR OTHER LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR      *
 * OTHERWISE, ARISING FROM, OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE  *
 * USE OR OTHER DEALINGS IN THE SOFTWARE.                                     *
 * -------------------------------------------------------------------------- */

#include "SimTKcommon/SmallMatrix.h"
#include "SimTKcommon/internal/CoordinateAxis.h"

#include <iosfwd>  // Forward declaration of iostream

namespace SimTK {

//-----------------------------------------------------------------------------
// Forward declarations. These are templatized by precision P and and stride S
// but always have length 3. TODO: this should be generalized to other lengths.
template <class P, int S> class UnitVec;
template <class P, int S> class UnitRow;

// UnitVec3 is more intelligible name for UnitVec<Real,1>.
typedef UnitVec<Real,1>     UnitVec3;
typedef UnitVec<float,1>    fUnitVec3;
typedef UnitVec<double,1>   dUnitVec3;

//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
template <class P, int S>
class UnitVec : public Vec<3,P,S> {
    typedef P   RealP;
public:
    typedef Vec<3,P,S>      BaseVec;
    typedef UnitRow<P,S>    TransposeType;

    UnitVec() : BaseVec(NTraits<P>::getNaN()) {}

    UnitVec(const UnitVec& u) 
    :   BaseVec( static_cast<const BaseVec&>(u) ) {}

    template <int S2> UnitVec(const UnitVec<P,S2>& u) 
    :   BaseVec( static_cast<const typename UnitVec<P,S2>::BaseVec&>(u) ) {}

    explicit UnitVec(const BaseVec& v) : BaseVec(v/v.norm()) {}
    template <int S2> 
    explicit UnitVec(const Vec<3,P,S2>& v) : BaseVec(v/v.norm())  {}

    UnitVec(const RealP& x, const RealP& y, const RealP& z) : BaseVec(x,y,z)  
    {   static_cast<BaseVec&>(*this) /= BaseVec::norm(); }

    UnitVec(const CoordinateAxis& axis) : BaseVec(0) 
    {   BaseVec::operator[](axis) = 1; }

    UnitVec(const CoordinateDirection& dir) : BaseVec(0) 
    {   BaseVec::operator[](dir.getAxis()) = RealP(dir.getDirection()); }

    explicit UnitVec(int axis) : BaseVec(0) 
    {   assert(0 <= axis && axis <= 2);
        BaseVec::operator[](axis) = 1; }

    UnitVec& operator=(const UnitVec& u) 
    {   BaseVec::operator=(static_cast<const BaseVec&>(u)); 
        return *this; }

    template <int S2> UnitVec& operator=(const UnitVec<P,S2>& u) 
    {   BaseVec::operator=(static_cast<const typename UnitVec<P,S2>::BaseVec&>(u));
        return *this; }

    const BaseVec& asVec3() const {return static_cast<const BaseVec&>(*this);}

    // Override Vec3 methods which preserve length. These return a 
    // packed UnitVec regardless of our stride.

    UnitVec<P,1> negate()    const {return UnitVec<P,1>(-asVec3(),true);}
    UnitVec<P,1> operator-() const {return negate();}

    const TransposeType& operator~() const {return *reinterpret_cast<const TransposeType*>(this);}
    TransposeType& operator~() {return *reinterpret_cast<TransposeType*>(this);}

    // We have to define these here so that the non-const ones won't be
    // inherited. We don't trust anyone to write on one element of a UnitVec!

    const RealP&  operator[](int i) const  { return BaseVec::operator[](i); }
    const RealP&  operator()(int i) const  { return BaseVec::operator()(i); }

    UnitVec<P,1> abs() const {return UnitVec<P,1>( asVec3().abs(), true );}

    inline UnitVec<P,1> perp() const;

    UnitVec(const BaseVec& v, bool) : BaseVec(v) {}
    template <int S2> UnitVec(const Vec<3,RealP,S2>& v, bool) : BaseVec(v) { }
};


template <class P, int S> inline UnitVec<P,1> 
UnitVec<P,S>::perp() const {
    // Choose the coordinate axis which makes the largest angle
    // with this vector, that is, has the "least u" along it.
    const UnitVec<P,1> u(abs());    // reflect to first octant
    const int minAxis = u[0] <= u[1] ? (u[0] <= u[2] ? 0 : 2)
                                     : (u[1] <= u[2] ? 1 : 2);
    // Cross returns a Vec3 result which is then normalized.
    return UnitVec<P,1>( *this % UnitVec<P,1>(minAxis) );
}

template <class P, int S1, int S2> inline bool
operator==(const UnitVec<P,S1>& u1, const UnitVec<P,S2>& u2)
{   return u1.asVec3() == u2.asVec3(); }

template <class P, int S1, int S2> inline bool
operator!=(const UnitVec<P,S1>& u1, const UnitVec<P,S2>& u2)
{   return !(u1==u2); }

//-----------------------------------------------------------------------------
//-----------------------------------------------------------------------------
template <class P, int S>
class UnitRow : public Row<3,P,S> {
    typedef P   RealP;
public:
    typedef Row<3,P,S>      BaseRow;
    typedef UnitVec<P,S>    TransposeType;

    UnitRow() : BaseRow(NTraits<P>::getNaN()) { }

    UnitRow(const UnitRow& u) 
    :   BaseRow(static_cast<const BaseRow&>(u)) {}

    template <int S2> UnitRow(const UnitRow<P,S2>& u)
    :   BaseRow(static_cast<const typename UnitRow<P,S2>::BaseRow&>(u)) { }

    UnitRow& operator=(const UnitRow& u) 
    {   BaseRow::operator=(static_cast<const BaseRow&>(u)); 
        return *this; }

    template <int S2> UnitRow& operator=(const UnitRow<P,S2>& u) 
    {   BaseRow::operator=(static_cast<const typename UnitRow<P,S2>::BaseRow&>(u));
        return *this; }

    explicit UnitRow(const BaseRow& v) : BaseRow(v/v.norm()) {}
    template <int S2> 
    explicit UnitRow(const Row<3,P,S2>& v) : BaseRow(v/v.norm()) {}

    UnitRow(const RealP& x, const RealP& y, const RealP& z)
    :   BaseRow(x,y,z)
    {   static_cast<BaseRow&>(*this) /= BaseRow::norm(); }

    explicit UnitRow(int axis) : BaseRow(0) 
    {   assert(0 <= axis && axis <= 2);
        BaseRow::operator[](axis) = 1; }

    const BaseRow& asRow3() const  {return static_cast<const BaseRow&>(*this);}

    // Override Row3 methods which preserve length. These return the 
    // packed UnitRow regardless of our stride.

    UnitRow<P,1> negate()    const  { return UnitRow<P,1>(-asRow3(),true); }
    UnitRow<P,1> operator-() const  { return negate();}

    const TransposeType&  operator~() const {return *reinterpret_cast<const TransposeType*>(this);}
    TransposeType& operator~() {return *reinterpret_cast<TransposeType*>(this);}

    // We have to define these here so that the non-const ones won't be
    // inherited. We don't trust anyone to write on one element of a UnitRow!

    const RealP&  operator[](int i) const  { return BaseRow::operator[](i); }
    const RealP&  operator()(int i) const  { return BaseRow::operator()(i); }

    UnitRow<P,1> abs() const {return UnitRow<P,1>(asRow3().abs(),true);}

    inline UnitRow<P,1> perp() const;

    // This constructor is only for our friends whom we trust to
    // give us an already-normalized vector.
    UnitRow( const BaseRow& v, bool ) : BaseRow(v) { }
    template <int S2> UnitRow( const Row<3,P,S2>& v, bool ) : BaseRow(v) { }
};

template <class P, int S>
inline UnitRow<P,1> UnitRow<P,S>::perp() const {
    // Choose the coordinate axis which makes the largest angle
    // with this vector, that is, has the "least u" along it.
    const UnitRow<P,1> u(abs());    // reflect to first octant
    const int minAxis = u[0] <= u[1] ? (u[0] <= u[2] ? 0 : 2)
                                     : (u[1] <= u[2] ? 1 : 2);
    // Cross returns a Row3 result which is then normalized.
    return UnitRow<P,1>(*this % UnitRow<P,1>(minAxis));
}


template <class P, int S1, int S2> inline bool
operator==(const UnitRow<P,S1>& u1, const UnitRow<P,S2>& u2)
{   return u1.asRow3() == u2.asRow3(); }

template <class P, int S1, int S2> inline bool
operator!=(const UnitRow<P,S1>& u1, const UnitRow<P,S2>& u2)
{   return !(u1==u2); }

//------------------------------------------------------------------------------
}  // End of namespace SimTK

//--------------------------------------------------------------------------
#endif // SimTK_UNITVEC_H_
//--------------------------------------------------------------------------