AnimaL

Tutorial

Documentation

---

Documentation

• Overview
• Modules
• Namespace List
• Class Hierarchy
• Alphabetical List
• Compound List
• File List
• Namespace Members
• Compound Members
• File Members
• Examples

---

rotation2d.h

Go to the documentation of this file.

#ifndef _ANIMAL_ROTATION_2D_
#define _ANIMAL_ROTATION_2D_

#include <iostream>
#include <animal/array.h>
//#include <animal/numerics.h>
#include <cmath>

namespace animal {


template < class Real = double > class Rotation2d_Traits;


// -----------------------------------------------------------
//  
//  Rotation2d class.
//  
// -----------------------------------------------------------
template 
    <
    class RealT = double,
    class TraitsT = Rotation2d_Traits<RealT>
    >
class Rotation2d 
{
public:


    typedef TraitsT Traits;

    typedef RealT Real;
    



    Rotation2d(){}
    
    Rotation2d( const Real angle )
    {
        _matrix[0][0] = cos(angle);
        _matrix[0][1] = sin(angle);
        _matrix[1][0] = -_matrix[0][1];
        _matrix[1][1] = _matrix[0][0];
    }
    
    static Rotation2d identity() { return Rotation2d( 1, 0, 0, 1 ); }

    
    
    Real angle() const { 
        return _matrix[0][1] > 0 ? 
            acos
                ( _matrix[0][0] > 1 ? 1 : _matrix[0][0] < -1 ? -1 : _matrix[0][0] )
            :
          -acos
                ( _matrix[0][0] > 1 ? 1 : _matrix[0][0] < -1 ? -1 : _matrix[0][0] )
        ;
    }
    
    Rotation2d& setAngle( const Real angle )
    {
        _matrix[0][0] = cos(angle);
        _matrix[0][1] = sin(angle);
        _matrix[1][0] = - _matrix[0][1];
        _matrix[1][1] = _matrix[0][0];
        return (*this);
    }
    
    Rotation2d inverse() const 
    {
        return Rotation2d( 
            _matrix[0][0], _matrix[1][0], 
            _matrix[0][1], _matrix[1][1] 
        );
    }
    
    Rotation2d operator * ( const Real u ) const
    {
        return Rotation2d
        (
            angle() * u
        );
    }
    
    
    
    
    Rotation2d operator * ( const Rotation2d& r ) const
    {
        return Rotation2d(
            _matrix[0][0] * r._matrix[0][0] + _matrix[0][1] * r._matrix[1][0],
            _matrix[0][0] * r._matrix[0][1] + _matrix[0][1] * r._matrix[1][1],
            _matrix[1][0] * r._matrix[0][0] + _matrix[1][1] * r._matrix[1][0],
            _matrix[1][0] * r._matrix[0][1] + _matrix[1][1] * r._matrix[1][1]
        );
    }

    Rotation2d& operator *= ( const Rotation2d& r )
    {
        (*this) = Rotation2d(
            _matrix[0][0] * r._matrix[0][0] + _matrix[0][1] * r._matrix[1][0],
            _matrix[0][0] * r._matrix[0][1] + _matrix[0][1] * r._matrix[1][1],
            _matrix[1][0] * r._matrix[0][0] + _matrix[1][1] * r._matrix[1][0],
            _matrix[1][0] * r._matrix[0][1] + _matrix[1][1] * r._matrix[1][1]
        );
        return (*this);
    }

    Rotation2d operator / ( const Rotation2d& r ) const
    {
        return Rotation2d(
            _matrix[0][0] * r._matrix[0][0] + _matrix[0][1] * r._matrix[0][1],
            _matrix[0][0] * r._matrix[1][0] + _matrix[0][1] * r._matrix[1][1],
            _matrix[1][0] * r._matrix[0][0] + _matrix[1][1] * r._matrix[0][1],
            _matrix[1][0] * r._matrix[1][0] + _matrix[1][1] * r._matrix[1][1]
        );
    }

    Rotation2d& operator /= ( const Rotation2d& r )
    {
        (*this) = Rotation2d(
            _matrix[0][0] * r._matrix[0][0] + _matrix[0][1] * r._matrix[0][1],
            _matrix[0][0] * r._matrix[1][0] + _matrix[0][1] * r._matrix[1][1],
            _matrix[1][0] * r._matrix[0][0] + _matrix[1][1] * r._matrix[0][1],
            _matrix[1][0] * r._matrix[1][0] + _matrix[1][1] * r._matrix[1][1]
        );
        return (*this);
    }
    
    
    
    template< class RotationVelocity >
    static Rotation2d eulerIncrement( const RotationVelocity& omega, const double h )
    {
        return Rotation2d( h * omega );
    }
    


    template< class R, class T, class Vector >
    friend Vector operator * ( const Vector&, const Rotation2d<R,T>& );

    template< class R, class T, class Vector >
    friend Vector& operator *= ( Vector&, const Rotation2d<R,T>& );

    template< class R, class T, class Vector >
    friend Vector operator / ( const Vector&, const Rotation2d<R,T>& );
    
    template< class R, class T, class Vector >
    friend Vector& operator /= ( Vector&, const Rotation2d<R,T>& );
    
    

    template< class R, class T >
    friend std::istream& operator >> ( std::istream&, Rotation2d<R,T>& );

    template< class R, class T >
    friend std::ostream& operator << ( std::ostream&, const Rotation2d<R,T>& );
    


private:

    Array<2 , Array< 2, Real> > _matrix;
    
    Rotation2d( const Real xx, const Real xy, const Real yx, const Real yy )
    {
        _matrix[0][0] = xx; _matrix[0][1] = xy; 
        _matrix[1][0] = yx; _matrix[1][1] = yy;
    }
    

};
// -----------------------------------------------------------
//  
//  
// -----------------------------------------------------------

template< class R, class T, class Vector >
inline Vector
operator / (
    const Vector& v,
    const Rotation2d<R,T>& m
    )
{
    return Vector(
        m._matrix[0][0] * T::x(v) + m._matrix[0][1] * T::y(v),
        m._matrix[1][0] * T::x(v) + m._matrix[1][1] * T::y(v)
    );
}

template< class R, class T, class Vector >
inline Vector&
operator /= (
    Vector& v,
    const Rotation2d<R,T>& m
    )
{
    v = Vector(
        m._matrix[0][0] * T::x(v) + m._matrix[0][1] * T::y(v),
        m._matrix[1][0] * T::x(v) + m._matrix[1][1] * T::y(v)
    );
    return v;
}

template< class R, class T, class Vector >
inline Vector
operator * (
    const Vector& v,
    const Rotation2d<R,T>& m
    )
{
    return Vector(
        m._matrix[0][0] * T::x(v) + m._matrix[1][0] * T::y(v),
        m._matrix[0][1] * T::x(v) + m._matrix[1][1] * T::y(v)
    );
}

template< class R, class T, class Vector >
inline Vector&
operator *= (
    Vector& v,
    const Rotation2d<R,T>& m
    )
{
    v = Vector(
        m._matrix[0][0] * T::x(v) + m._matrix[1][0] * T::y(v),
        m._matrix[0][1] * T::x(v) + m._matrix[1][1] * T::y(v)
    );
    return v;
}

template< class R, class T >
std::istream& operator >> ( 
    std::istream& stream, 
    Rotation2d<R,T>& r
    )
{
    R angle;
    stream >> angle;
    r.setAngle( angle );
    return stream;
}

template< class R, class T >
std::ostream& operator << ( 
    std::ostream& stream,
    const Rotation2d<R,T>& r
    )
{
    stream << r.angle();
    return stream;
}




// -----------------------------------------------------------
//  
//  Rotation2d_Traits class.
//  
// -----------------------------------------------------------



template <class Real>
class Rotation2d_Traits
{
public:

    
    template< class Vector >
    static Real
    x( const Vector& v )
    {
        return v[0];
    }
    
    template< class Vector >
    static Real
    y( const Vector& v )
    {
        return v[1];
    }
    
    
};


} // end namespace animal

#endif

---