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 <rcsc/geom/angle_deg.h>
 
#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 ) );
      }
 
    double dist2( const double xx,
                  const double yy ) const
      {
          return ( std::pow( this->x - xx, 2 )
                   + std::pow( this->y - yy, 2 ) );
      }
 
    double dist( const double xx,
                 const double yy ) const
      {
          //return std::hypot( this->x - xx,
          //                   this->y - yy );
          return std::sqrt( dist2( xx, yy ) );
      }
 
    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
 
    struct XCmp {
        bool operator()( const Vector2D & lhs,
                         const Vector2D & rhs ) const
          {
              return lhs.x < rhs.x;
          }
    };
 
    struct YCmp {
        bool operator()( const Vector2D & lhs,
                         const Vector2D & rhs ) const
          {
              return lhs.y < rhs.y;
          }
    };
 
    struct AbsXCmp {
        bool operator()( const Vector2D & lhs,
                         const Vector2D & rhs ) const
          {
              return lhs.absX() < rhs.absX();
          }
    };
 
    struct AbsYCmp {
        bool operator()( const Vector2D & lhs,
                         const Vector2D & rhs ) const
          {
              return lhs.absY() < rhs.absY();
          }
    };
 
    struct XYCmp {
        bool operator()( const Vector2D & lhs,
                         const Vector2D & rhs ) const
          {
              return ( lhs.x < rhs.x
                       ? true
                       : lhs.x > rhs.x
                       ? false
                       : lhs.y < rhs.y );
          }
 
    };
 
    struct YXCmp {
        bool operator()( const Vector2D & lhs,
                         const Vector2D & rhs ) const
          {
              return ( lhs.y < rhs.y
                       || ( lhs.y == rhs.y && lhs.x < rhs.x ) );
          }
    };
 
};
 
} // 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