UnitVec.h

Go to the documentation of this file.

#ifndef SimTK_UNITVEC_H 
#define SimTK_UNITVEC_H 

/* -------------------------------------------------------------------------- *
 *                       Simbody(tm): SimTKcommon                             *
 * -------------------------------------------------------------------------- *
 * This is part of the SimTK 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/home/simbody.  *
 *                                                                            *
 * Portions copyright (c) 2005-12 Stanford University and the Authors.        *
 * Authors: Michael Sherman                                                   *
 * Contributors: Paul Mitiguy                                                 *
 *                                                                            *
 * Licensed under the Apache License, Version 2.0 (the "License"); you may    *
 * not use this file except in compliance with the License. You may obtain a  *
 * copy of the License at http://www.apache.org/licenses/LICENSE-2.0.         *
 *                                                                            *
 * Unless required by applicable law or agreed to in writing, software        *
 * distributed under the License is distributed on an "AS IS" BASIS,          *
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.   *
 * See the License for the specific language governing permissions and        *
 * limitations under the License.                                             *
 * -------------------------------------------------------------------------- */

#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) { }

    static const UnitVec& getAs(const RealP* p)  
    {   return *reinterpret_cast<const UnitVec*>(p); }
};


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;

    UnitRow( const BaseRow& v, bool ) : BaseRow(v) { }
    template <int S2> UnitRow( const Row<3,P,S2>& v, bool ) : BaseRow(v) { }

    static const UnitRow& getAs(const RealP* p)  
    {   return *reinterpret_cast<const UnitRow*>(p); }
};

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_
//--------------------------------------------------------------------------