vector_2d.h

Go to the documentation of this file.

// -*-c++-*-

 
/*
 *Copyright:
 
 Copyright (C) Hidehisa AKIYAMA, Hiroki Shimora
 
 This code is free software; you can redistribute it and/or
 modify it under the terms of the GNU Lesser General Public
 License as published by the Free Software Foundation; either
 version 3 of the License, or (at your option) any later version.
 
 This library is distributed in the hope that it will be useful,
 but WITHOUT ANY WARRANTY; without even the implied warranty of
 MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
 Lesser General Public License for more details.
 
 You should have received a copy of the GNU Lesser General Public
 License along with this library; if not, write to the Free Software
 Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  USA
 
 *EndCopyright:
 */

 
#ifndef RCSC_GEOM_VECTOR2D_H
#define RCSC_GEOM_VECTOR2D_H
 
#include <geom/angle_deg.h>
 
#include <functional>
#include <iostream>
#include <cmath>
 
namespace rcsc {

class Vector2D {
    // : public boost::addable< Vector2D >
    // , public boost::subtractable< Vector2D >
    // , public multipliable2< Vector2D, double >
    // , public dividable2< Vector2D, double >
 
public:

    static const double EPSILON;

    static const double ERROR_VALUE;

    static const Vector2D INVALIDATED;
 
    double x; 
    double y; 

    Vector2D()
        : x( 0.0 ),
          y( 0.0 )
      { }

    Vector2D( const double & xx,
              const double & yy )
        : x( xx ),
          y( yy )
      { }

    bool isValid() const
      {
          return ( ( x != ERROR_VALUE ) && ( y != ERROR_VALUE ) );
      }

    Vector2D & assign( const double & xx,
                       const double & yy )
      {
          x = xx;
          y = yy;
          return *this;
      }

    Vector2D & setPolar( const double & radius,
                         const AngleDeg & dir )
      {
          x = radius * dir.cos();
          y = radius * dir.sin();
          return *this;
      }

    const
    Vector2D & invalidate()
      {
          x = ERROR_VALUE;
          y = ERROR_VALUE;
          return *this;
      }

    double r2() const
      {
          return x * x + y * y;
      }

    double r() const
      {
          //return std::hypot( x, y );
          return std::sqrt( r2() );
      }

    double norm() const
      {
          return r();
      }

    double norm2() const
      {
          return r2();
      }

    double length() const
      {
          return r();
      }

    double length2() const
      {
          return r2();
      }

    AngleDeg th() const
      {
          return AngleDeg( AngleDeg::atan2_deg( y, x ) );
      }

    AngleDeg dir() const
      {
          return th();
      }

    Vector2D abs() const
      {
          return Vector2D( std::fabs( x ), std::fabs( y ) );
      }

    double absX() const
      {
          return std::fabs( x );
      }

    double absY() const
      {
          return std::fabs( y );
      }

    Vector2D & add( const Vector2D & v )
      {
          x += v.x;
          y += v.y;
          return *this;
      }

    Vector2D & add( const double & xx,
                    const double & yy )
      {
          x += xx;
          y += yy;
          return *this;
      }

    Vector2D & scale( const double & scalar )
      {
          x *= scalar;
          y *= scalar;
          return *this;
      }

    const
    Vector2D & operator+() const
      {
          return *this;
      }

    Vector2D operator-() const
      {
          return Vector2D( -x, -y );
      }

    const
    Vector2D & operator+=( const Vector2D & v )
      {
          x += v.x;
          y += v.y;
          return *this;
      }

    const
    Vector2D & operator-=( const Vector2D & v )
      {
          x -= v.x;
          y -= v.y;
          return *this;
      }

    const
    Vector2D & operator*=( const double & scalar )
      {
          //x *= scalar;
          //y *= scalar;
          //return *this;
          return scale( scalar );
      }

    const
    Vector2D & operator/=( const double & scalar )
      {
          //if ( scalar != 0 )
          if ( std::fabs( scalar ) > EPSILON )
          {
              x /= scalar;
              y /= scalar;
          }
          return *this;
      }

    double dist2( const Vector2D & p ) const
      {
          //return ( Vector2D( *this ) -= p ).r2();
          return ( std::pow( this->x - p.x, 2 )
                   + std::pow( this->y - p.y, 2 ) );
      }

    double dist( const Vector2D & p ) const
      {
          //return std::hypot( this->x - p.x,
          //                   this->y - p.y );
          return std::sqrt( dist2( p ) );
      }

    Vector2D & reverse()
      {
          x = -x;
          y = -y;
          return *this;
      }

    Vector2D reversedVector() const
      {
          return Vector2D( *this ).reverse();
      }

    Vector2D & setLength( const double & len )
      {
          double mag = this->r();
          //if ( mag == 0 )
          if ( mag < EPSILON )
          {
              return *this;
          }
          //return ( (*this) *= ( len / mag ) );
          return scale( len / mag );
      }

    Vector2D setLengthVector( const double & len ) const
      {
          return Vector2D( *this ).setLength( len );
      }

    Vector2D & normalize()
      {
          return setLength( 1.0 );
      }

    Vector2D normalizedVector() const
      {
          return Vector2D( *this ).normalize();
      }

    Vector2D & rotate( const double & deg )
      {
          double c = std::cos( deg * AngleDeg::DEG2RAD );
          double s = std::sin( deg * AngleDeg::DEG2RAD );
          return assign( this->x * c - this->y * s,
                         this->x * s + this->y * c );
      }

    Vector2D & rotate( const AngleDeg & angle )
      {
          return rotate( angle.degree() );
      }

    Vector2D rotatedVector( const double & deg ) const
      {
          return Vector2D( *this ).rotate( deg );
      }

    Vector2D rotatedVector( const AngleDeg & angle ) const
      {
          return Vector2D( *this ).rotate( angle.degree() );
      }

    Vector2D & setDir( const AngleDeg & dir )
      {
          double radius = this->r();
          x = radius * dir.cos();
          y = radius * dir.sin();
          return *this;
      }

    double innerProduct( const Vector2D & v ) const
      {
          return this->x * v.x + this->y * v.y;
          // ==  |this| * |v| * (*this - v).th().cos()
      }

    double outerProduct( const Vector2D & v ) const
      {
          /*---------------------*
           * assume virtual 3D environment.
           * calculate Z-coordinate of outer product in right hand orientation.
           * For the time being, Input Vector's Z-coordinate is set to ZERO.
           *---------------------*/
          // Normal 3D outer product
          //   xn = this->y * v.z - this->z * v.y;
          //   yn = this->z * v.x - this->x * v.z;
          // # zn = this->x * v.y - this->y * v.x;
          return this->x * v.y - this->y * v.x;
          // == |this| * |v| * (*this - v).th().sin()
      }

    bool equals( const Vector2D & other ) const
      {
          return this->x == other.x
              && this->y == other.y;
      }

    bool equalsWeakly( const Vector2D & other ) const
      {
          //return dist2( other ) < EPSILON * EPSILON;
          return std::fabs( this->x - other.x ) < EPSILON
              && std::fabs( this->y - other.y ) < EPSILON;
      }

    // static utility

    inline
    static
    Vector2D polar2vector( const double & mag,
                           const AngleDeg & theta )
      {
          return Vector2D( mag * theta.cos(), mag * theta.sin() );
      }

    inline
    static
    Vector2D from_polar( const double & mag,
                         const AngleDeg & theta )
      {
          return Vector2D( mag * theta.cos(), mag * theta.sin() );
      }

    inline
    static
    double inner_product( const Vector2D & v1,
                          const Vector2D & v2 )
      {
          return v1.innerProduct( v2 );
      }

    inline
    static
    double outer_product( const Vector2D & v1,
                          const Vector2D & v2 )
      {
          return v1.outerProduct( v2 );
      }

    // stream utility

    std::ostream & print( std::ostream & os ) const
      {
          os << '(' << x << ", " << y << ')';
          return os;
      }

    std::ostream & printRound( std::ostream & os,
                               const double & prec = 0.1 ) const
      {
          os << '('  << rint( x / prec ) * prec
             << ", " << rint( y / prec ) * prec << ')';
          return os;
      }

    // functors for comparison

    class XCmp
        : public std::binary_function< Vector2D, Vector2D, bool > {
    public:
        result_type operator()( const first_argument_type & lhs,
                                const second_argument_type & rhs ) const
          {
              return lhs.x < rhs.x;
          }
    };

    class YCmp
        : public std::binary_function< Vector2D, Vector2D, bool > {
    public:
        result_type operator()( const first_argument_type & lhs,
                                const second_argument_type & rhs ) const
          {
              return lhs.y < rhs.y;
          }
    };

    class AbsXCmp
        : public std::binary_function< Vector2D, Vector2D, bool > {
    public:
        result_type operator()( const first_argument_type & lhs,
                                const second_argument_type & rhs ) const
          {
              return lhs.absX() < rhs.absX();
          }
    };

    class AbsYCmp
        : public std::binary_function< Vector2D, Vector2D, bool > {
    public:
        result_type operator()( const first_argument_type & lhs,
                                const second_argument_type & rhs ) const
          {
              return lhs.absY() < rhs.absY();
          }
    };

    class XYCmp
        : public std::binary_function< Vector2D, Vector2D, bool > {
    public:
        result_type operator()( const first_argument_type & lhs,
                                const second_argument_type & rhs ) const
          {
              return ( lhs.x < rhs.x
                       ? true
                       : lhs.x > rhs.x
                       ? false
                       : lhs.y < rhs.y );
              // if ( lhs.x < rhs.x ) return true;
              // else if ( lhs.x > rhs.x ) return false;
              // else return lhs.y < rhs.y;
          }
 
    };

    class YXCmp
        : public std::binary_function< Vector2D, Vector2D, bool > {
    public:
        result_type operator()( const first_argument_type & lhs,
                                const second_argument_type & rhs ) const
          {
              return ( lhs.y < rhs.y
                       || ( lhs.y == rhs.y && lhs.x < rhs.x ) );
          }
    };

    class Equal
        : public std::binary_function< Vector2D, Vector2D, bool > {
    public:
        result_type operator()( const first_argument_type & lhs,
                                const second_argument_type & rhs ) const
          {
              return lhs.equals( rhs );
          }
    };

    // functor for region

    template < typename REGION >
    class IsWithin
        : public std::unary_function< Vector2D, bool > {
    private:
        const REGION M_region; 
    public:
        explicit
        IsWithin( const REGION & region )
            : M_region( region )
          { }

        result_type operator()( const argument_type & position ) const
          {
              return M_region.contains( position );
          }
    };
};
 
} // end of namespace
 

// comparison operators
inline
bool
operator==( const rcsc::Vector2D & lhs,
            const rcsc::Vector2D & rhs )
{
    //return lhs.x == rhs.x
    //    && lhs.y == rhs.y;
    return lhs.equals( rhs );
}

inline
bool
operator!=( const rcsc::Vector2D & lhs,
            const rcsc::Vector2D & rhs )
{
    return ! operator==( lhs, rhs );
}
 

// arithmetic operators

inline
const
rcsc::Vector2D
operator+( const rcsc::Vector2D & lhs,
           const rcsc::Vector2D & rhs )
{
    return rcsc::Vector2D( lhs ) += rhs;
}

inline
const
rcsc::Vector2D
operator-( const rcsc::Vector2D & lhs,
           const rcsc::Vector2D & rhs )
{
    return rcsc::Vector2D( lhs ) -= rhs;
}

inline
const
rcsc::Vector2D
operator*( const rcsc::Vector2D & lhs,
           const double & rhs )
{
    return rcsc::Vector2D( lhs ) *= rhs;
}

inline
const
rcsc::Vector2D
operator/( const rcsc::Vector2D & lhs,
           const double & rhs )
{
    return rcsc::Vector2D( lhs ) /= rhs;
}

template < typename T >
bool
operator<( const rcsc::Vector2D & lhs,
           const T & rhs );

template < typename T >
bool
operator<=( const rcsc::Vector2D & lhs,
            const T & rhs );

template < typename T >
bool
operator>( const rcsc::Vector2D & lhs,
           const T & rhs );

template < typename T >
bool
operator>=( const rcsc::Vector2D & lhs,
           const T & rhs );

template < typename T >
bool
operator<( const T & lhs,
           const rcsc::Vector2D & rhs );
 

template < typename T >
bool
operator<=(const T & lhs,
           const rcsc::Vector2D & rhs );
 

template < typename T >
bool
operator>( const T & lhs,
           const rcsc::Vector2D & rhs );

template < typename T >
bool
operator>=( const T & lhs,
            const rcsc::Vector2D & rhs );

template < typename T >
bool
operator==( const T & lhs,
            const rcsc::Vector2D & rhs );

template < typename T >
bool
operator!=( const T & lhs,
            const rcsc::Vector2D & rhs );
 
 


inline
std::ostream &
operator<<( std::ostream & os,
            const rcsc::Vector2D & v )
{
    return v.print( os );
}
 
 
#endif