+ View the NASA Portal

Search JPL

Search CLARAty

- Algorithms- Infrastructure - Doxygen (public)- Doxygen (private)- Tutorials

- Repository- Nightly Build- Nightly Regression- Feature/Bug Tracking- Build Targets- Procedure- Conventions- Tools

2d_point.h

Go to the documentation of this file.

// -*-c++-*-
//---------------------------< /-/ CLARAty /-/ >------------------------------
/**
 * @file  2d_point.h
 *
 * Defines 2D point class for planar coordinate point manipulation.
 *
 * <br>@b Designer(s):  Stanley Lippman
 * <br>@b Author(s):    Stanley Lippman
 * <br>@b Date:         October 22, 2001
 *
 * <b>Software License:</b><br>
 *
 * <code> http://claraty.jpl.nasa.gov/license/open_src/  or 
 *        file: license/open_src.txt </code>
 *
 * &copy; 2006, Jet Propulsion Laboratory, California Institute of Technology<br>
 *
 * $Revision: 1.8 $
 */
//-----------------------------------------------------------------------------

#ifndef N_2D_POINT_H
#define N_2D_POINT_H

#include "claraty/share.h"
#include <iostream>
#include <math.h>

#include "claraty/fdm.h"
#include "claraty/math_limits.h"

namespace claraty {

/*
 * @defgroup points Points
 * @ingroup Base
 *
 * 2D Point Math
 */
//@{
//@}

// forward declarations

template < class T >
class N_2D_Point;

typedef N_2D_Point< double > N_2D_Point_d;
typedef N_2D_Point< float >  N_2D_Point_f;
typedef N_2D_Point< int >    N_2D_Point_i;


// independent operator functions
// for class N_2D_Point objects ...

template < class T >
inline bool operator==( const N_2D_Point< T >&, const N_2D_Point< T >& );

template < class T >
inline N_2D_Point<T> operator+( const N_2D_Point<T>&, const N_2D_Point<T>& );

template < class T >
inline N_2D_Point<T> operator-( const N_2D_Point<T>&, const N_2D_Point<T>& );

template < class T >
inline N_2D_Point<T> operator-( const N_2D_Point<T>& );

template < class T >
inline N_2D_Point<T> operator*( double, const N_2D_Point<T>& );

template < class T >
inline N_2D_Point<T> operator/( const N_2D_Point<T>&, double );


/*
 * 
 * @ingroup points
 */
template < class T >
class  N_2D_Point 
{
public:
  enum{ X, Y };
  
  /*
   * Constructs a 2D point setting the first element to e0 and the second
   * to e1, if given.
   * @param e0 the first element
   * @param e1 the second element
   */
  explicit N_2D_Point( T e0 = T(), T e1 = T() ) 
  { _elems[ 0 ] = e0; _elems[ 1 ] = e1; }

  /*
   * Returns the number of elements in the 2D vector (2).
   * @return the size of the vector (2)
   */
  int size() const { return 2; }

  /*
   * References a variable of the point by indexing
   * @param ix the index used to reference
   * @return the variable of point referenced by index
   */
  T  operator()( int ix ) const { return _elems[ ix ];  }
    
  /*
   * References a variable of the point by indexing
   * @param ix the index used to reference
   * @return the variable of point referenced by index
   */
  T& operator()( int ix )       { return _elems[ ix ];  }
    
  /*
   * Returns the x component of the point
   * @return the x component of the point
   */
  T  x() const { return _elems[ 0 ]; }
  
  /*
   *  Returns the x component of the point
   *  @return the x component of the point
   */
  T& x()       { return _elems[ 0 ]; }
  
  /*
   * Returns the y component of the point
   * @return the y component of the point
   */
  T& y()       { return _elems[ 1 ]; }
    
  /*
   * Returns the y component of the point
   * @return the y component of the point
   */
  T  y() const { return _elems[ 1 ]; }

  /*
   * Returns the x component of the point
   * @return the x component of the point
   */
  T get_x() const { return _elems[ 0 ]; }
    
  /*
   * Returns the y component of the point
   * @return the y component of the point
   */
  T get_y() const { return _elems[ 1 ]; }

  /*
   * Sets the first element of the point to x
   * @param x the x component of the point
   */
  void set( T x ) { _elems[ 0 ] = x; }

  /*
   * Sets the first and second elements of the point to x and y
   * @param x the x component of the point
   * @param y the y component of the point
   */
  void set( T x, T y ) { 
    _elems[ 0 ] = x;  _elems[ 1 ] = y; 
  }

  /*
   * Subtracts the y element from the variable inv_y.
   * @param inv_y variable by which y is subtracted
   */
  void      invert_y( T inv_y ) { _elems[ 1 ] = inv_y - _elems[ 1 ]; }

  T  length() const;
  T  length2() const;

  double angle() const;
  
  bool      normalize();
  N_2D_Point normalize_copy() const;

  T  dot( const N_2D_Point& ) const;
  
  T  distance ( const N_2D_Point& ) const;
  T  distance2( const N_2D_Point& ) const;
  N_2D_Point interpolate( const N_2D_Point&, T s ) const;
  N_2D_Point extrapolate( const T angle, T s ) const;
  
  bool intersect(const N_2D_Point &line1_a,
                 const N_2D_Point &line1_b,
                 const N_2D_Point &line2_a,
                 const N_2D_Point &line2_b,
                 N_2D_Point & result) const;
  
  N_2D_Point&   operator+=( const N_2D_Point& );
  N_2D_Point&   operator-=( const N_2D_Point& );
  N_2D_Point&   operator+=( T );
  N_2D_Point&   operator-=( T );
  N_2D_Point&   operator*=( T );
  N_2D_Point&   operator/=( T );
  
  N_2D_Point  rotate( double ang )     const;
  
  // Rotation -- from Ames ...
  /*
   * Rotates a point 90 degrees clockwise.
   * @return the rotated point
   */
  N_2D_Point rotate_cw_90()  const { return N_2D_Point(  _elems[1], -_elems[0] ); }
  
  /*
   * Rotates a point 90 degrees counter-clockwise.
   * @return the rotated point
   */
  N_2D_Point rotate_ccw_90() const { return N_2D_Point( -_elems[1],  _elems[0] ); }

  /*
   * Rotates a point by a given angle clockwise.
   * @param angle the angle the point is rotated
   * @return the rotated point
   */
  N_2D_Point rotate_cw( float angle ) const
  { 
    return N_2D_Point(
                     (T)(_elems[0] * cos(angle) +  _elems[1] * sin(angle)),
                     (T)(_elems[1] * cos(angle) -  _elems[0] * sin(angle))); 
  }

  /*
   * Rotates a point by a given angle counter-clockwise.
   * @param angle the angle the point is rotated
   * @return the rotated point
   */
  N_2D_Point rotate_ccw( float angle ) const
  { 
    return N_2D_Point(
                     (T)(_elems[0] * cos(angle) - _elems[1] * sin(angle)),
                     (T)(_elems[1] * cos(angle) + _elems[0] * sin(angle))); 
  }

  /*
   * Rotates point clockwise around a point p by a given angle.
   * @param angle the angle the point is rotated
   * @param p the point around which point is rotated
   * @return the rotated point
   */
  N_2D_Point rotate_cw_around_point( float angle, N_2D_Point p ) const
  { return ( *this-p ).rotate_cw( angle ) + p; }

  /*
   * Rotates point counter-clockwise around a point p by a given angle.
   * @param angle the angle the point is rotated
   * @param p the point around which point is rotated
   *  @return the rotated point
   */
  N_2D_Point rotate_ccw_around_point( float angle, N_2D_Point p) const
  { return ( *this-p ).rotate_ccw( angle ) + p; }

  T distance_to_unit_vector( const N_2D_Point& ) const;
  T closest_point_to_line( const N_2D_Point&, const N_2D_Point& ) const;
  T distance2_to_line( const N_2D_Point&, const N_2D_Point&, N_2D_Point* = 0 ) const;

  bool     is_origin() const;
  std::ostream& display(const char *pmsg, std::ostream &os = std::cout) const;
  std::ostream& display_unformatted(std::ostream &os) const;

  inline double angle_to(const N_2D_Point &a) const;

private:
  T _elems[ 2 ];
};

/*
 * Returns the length of the point.
 * @return the length of the point
*/
template < class T >
inline T 
N_2D_Point<T>::
length() const 
{ 
  return (T)(sqrt( _elems[0]*_elems[0] + _elems[1]*_elems[1])); 
}

/*
 * Returns the length of the point squared.
 * @return the squared length of the point
 */
template < class T >
inline T N_2D_Point<T>::
length2() const 
{ 
  return _elems[0]*_elems[0] + _elems[1]*_elems[1]; 
}

/*
 *   Returns the angle of the point
 *  @return the angle of the point
 */
template < class T >
inline double N_2D_Point<T>::
angle() const
{
  // TODO: handle x==y==0 gracefully (return an arbitrary angle)
  // Under linux (glibc2.2), atan2(0,0) appears to return 0, which is great
  return atan2(y(), x());
}

/*
 * Returns the angle from existing point to another point.
 *  @param a point to which the delta angle is calculated
 *  @return the angle from existing point to point a
 */
template<class T>
inline double N_2D_Point<T>::
angle_to(const N_2D_Point<T> &a) const
{
  double delta_angle= a.angle() - angle();
  if (delta_angle <= -M_PI) delta_angle += 2*M_PI;
  else if (delta_angle >= M_PI) delta_angle -= 2*M_PI;
  return delta_angle;
}

/*
 * Returns the dot product of 2 points.
 * @param v the second point
 * @return the dog product
 */
template < class T >
inline T N_2D_Point<T>::
dot( const N_2D_Point &v ) const 
{ 
  return ( _elems[0]*v._elems[0] + _elems[1]*v._elems[1] ); 
}

/*
 *   Returns the distance between two points squared.
 *  @param p the point to which distance is measured
 *  @return the squared distance between the points
 */
template < class T >
inline T N_2D_Point<T>::
distance2( const N_2D_Point &p ) const 
{
  double d0 = p._elems[0] - _elems[0];
  double d1 = p._elems[1] - _elems[1];
  
  return (T)(d0*d0 + d1*d1);
}

// Return is -pi to pi
// Positive angle means A is clockwise from point
// Negative means A is CCW from point

/*
 *  Returns the distance between two points.
 *  @param p the point to which distance is measured
 *  @return the distance between the points
 */
template < class T >
inline T N_2D_Point<T>::
distance( const N_2D_Point &p ) const 
{
  double d0 = p._elems[0] - _elems[0];
  double d1 = p._elems[1] - _elems[1];
  
  return (T)(sqrt( d0*d0 + d1*d1 ));
}

//
//  Linearly interpolates between line endpoints:
//    np.x = (p'.x - p.x)*s + p', ... etc
//    where np = new point, p' = instance point,
//           p = supplied endpoint, and
//           s = interpolation fraction
//

/*
 *  Linearly interpolates between line endpoints (already supplied).
 *  @param p the instance point
 *  @param s the interpolation fraction (function assumes this is between 0 and 1)
 *  @return new point after interpolation
*/
template < class T >
inline N_2D_Point<T> N_2D_Point<T>::
interpolate( const N_2D_Point &p, T s ) const 
{
  return N_2D_Point<T>
    (( p._elems[0]-_elems[0] ) * s+_elems[0],
     ( p._elems[1]-_elems[1] ) * s+_elems[1] );
}

/*
 * Linearly extrapolate from a point along a line that is at a given
 *  angle for a distance s. 
 *  Note that the angle is measured as zero at the X-axis and
 *  increasing positively as the angle rotates about the origin to the
 *  Y-axis.
 *    np.x = p'.x + s*cos(angle), ... etc
 *    where np     = new point, p' = instance point,
 *           angle = supplied angle, and
 *           s     = extrapolation distance
 */
template < class T >
inline N_2D_Point<T> N_2D_Point<T>::
extrapolate( const T angle, T s ) const 
{
  return N_2D_Point<T> (
                              _elems[0] + s*cos(angle),
                              _elems[1] + s*sin(angle));
}

/*
 * Compute the intersection point of 2 lines. Each line is defined by
 * 2 points. If the lines are parallel, the intersection is at
 * infinity so return false. If an intersection point is found,
 * return true.
 */
template < class T >
inline bool N_2D_Point<T>::
intersect(const N_2D_Point &line1_a,
          const N_2D_Point &line1_b,
          const N_2D_Point &line2_a,
          const N_2D_Point &line2_b,
          N_2D_Point & result) const 
{
  // If lines are parallel, return false indicating that they do not
  // intersect.
  T delta_x1 = line1_a.x() - line1_b.x();
  T delta_x2 = line2_a.x() - line2_b.x();
  T delta_y1 = line1_a.y() - line1_b.y();
  T delta_y2 = line2_a.y() - line2_b.y();

  T angle_difference =
    atan2(delta_y1, delta_x1) - atan2(delta_y2, delta_x2);
  
  while (angle_difference > M_PI)
    angle_difference -= M_2_PI;
  while (angle_difference <= -M_PI)
    angle_difference += M_2_PI;

  if (fabs(angle_difference) < DOUBLE_EPSILON)
    return false;

  if (fabs(fabs(angle_difference) - M_PI) < DOUBLE_EPSILON)
    return false;

  // Compute the denominator determinant
  T denominator = delta_x1 * delta_y2 - delta_x2 * delta_y1;

  // Verify that the denominator is not near singular
  if (fabs(denominator) < DOUBLE_EPSILON)
    return false;

  // Compute the intersection point
  T sub_det_1 = line1_a.x() * line1_b.y() - line1_b.x() * line1_a.y();
  T sub_det_2 = line2_a.x() * line2_b.y() - line2_b.x() * line2_a.y();

  result._elems[0]
    = ((sub_det_1*delta_x2)-(sub_det_2*delta_x1))/denominator;
  result._elems[1]
    = ((sub_det_1*delta_y2)-(sub_det_2*delta_y1))/denominator;

  return true;
}




//
// Returns perpendicular distance from this point to 
// given unit vector which passes through origin.
//
/*
 *   Calculates the distance from point to given unit vector.
 *  @param v the unit vector
 *  @return perpendicular distance from this point to given unit vector
 */
template < class T >
inline T N_2D_Point<T>::
distance_to_unit_vector( const N_2D_Point &v) const 
{
  T d = _elems[0]*v._elems[1] - _elems[1]*v._elems[0];
  return d < 0 ? -d : d ;
}

/*
 * Normalizes this vector.
 * @return true if the vector has been normalized; false otherwise
 */
template < class T >
inline bool N_2D_Point<T>::
normalize() 
{
  bool   status = false;
  double vec_len = length();
  
  if ( vec_len > EPSILON ) {
    _elems[0] = (T)(_elems[0] / vec_len); 
    _elems[1] = (T)(_elems[1] / vec_len); 
    status = true;
  }
  
  return status;
}

/*
 *   Returns a copy of the normalized vector.
 * @return a copy of the normalize vector
 */
template < class T >
inline N_2D_Point<T> N_2D_Point<T>::
normalize_copy() const 
{
  N_2D_Point<T> test;
  double vec_len = length();
  if (vec_len > EPSILON){
    
    test(0) = (T)(_elems[0]/vec_len);
    test(1) = (T)(_elems[1]/vec_len);
  }
  return test;
  // this was the original code. it executes both branches of the if statement.  but i don't know why.
  //    return vec_len > EPSILON
  //    ? N_2D_Point( (T)(_elems[0]/vec_len, _elems[1]/vec_len))
  //    : N_2D_Point();
}

/*
 * Rotates a point by a specified angle.
 * @param ang angle by which the point is rotated
 * @return the rotated point
 */
template < class T >
inline N_2D_Point<T> N_2D_Point<T>::
rotate( double ang ) const 
{
  double cosa = cos( ang ); double sina = sin( ang );
  
  return N_2D_Point( (T)(_elems[0]*cosa - _elems[1]*sina),
                    (T)(_elems[0]*sina + _elems[1]*cosa) );
}

/*
 *  Checks that this point is the origin.
 *  @return true if this is the origin; false otherwise 
 */
template < class T >
inline bool N_2D_Point<T>::
is_origin() const 
{ 
  return ! _elems[0] && ! _elems[1]; 
}

/*
 * Checks that 2 2D points are the same.
 * @param v0 the first 2D point
 * @param v1 the second 2D point
 * @return true if the 2 points are the same; false otherwise
 */
template < class T >
inline bool 
operator==( const N_2D_Point<T> &v0, const N_2D_Point<T> &v1 ) 
{
  return ( v0(0)==v1(0) ) && ( v0(1)==v1(1) );
}

/*
 *  Adds 2 2D points together.
 *  @param v0 the first 2D point
 *  @param v1 the second 2D point
 *  @return the point resulting from addition
 */
template < class T >
inline N_2D_Point<T> 
operator+( const N_2D_Point<T> &v0, const N_2D_Point<T> &v1 ) 
{
  return N_2D_Point<T>( v0(0)+v1(0), v0(1)+v1(1) );
}

/*
 * Subtracts one point from the other.
 * @param v0 the point subtracted from
 * @param v1 the point being subtracted
 * @return the point resulting from subtraction
 */
template < class T >
inline N_2D_Point<T>
operator-( const N_2D_Point<T> &v0, const N_2D_Point<T> &v1 ) 
{
  return N_2D_Point<T>(v0(0)-v1(0), v0(1)-v1(1));
}

/*
 * Negates the x and y comp. of a point
 * @param v the point negated
 * @return the negated point
 */
template < class T >
inline N_2D_Point<T>
operator-( const N_2D_Point<T> &v ) 
{ 
  return N_2D_Point<T>( -v(0), -v(1)); 
}

/*
 * Adds given point (v1) to existing point on left-hand side.
 * @param v1 the vector to be added
 * @return the resulting point from addition
 */
template < class T >
inline N_2D_Point<T>& N_2D_Point<T>::
operator+=( const N_2D_Point<T> &v1 ) 
{
  _elems[0] += v1(0); _elems[1] += v1(1); 
  return *this;
}

/*
 * Subtracts given point (v1) from existing point on left-hand side.
 *  @param v1 the vector to be subtracted
 *  @return the point resulting from subtraction
 */
template < class T >
inline N_2D_Point<T>& N_2D_Point<T>::
operator-=( const N_2D_Point &v1 ) 
{
  _elems[0] -= v1(0); _elems[1] -= v1(1); return *this;
}

/*
 * Adds given scalar (s) to existing point on left-hand side.
 * @param s the scalar to be added
 * @return the resulting point from addition
 */
template < class T >
inline N_2D_Point<T>& N_2D_Point<T>::
operator+=( T s ) 
{
  _elems[0] += s; _elems[1] += s; 
  return *this;
}

/*
 * Subtracts given scalar (s) from existing point on left-hand side.
 * @param s the scalar to be subtracted
 * @return the resulting point from addition
 */
template < class T >
inline N_2D_Point<T>& N_2D_Point<T>::
operator-=( T s ) 
{
  _elems[0] -= s; _elems[1] -= s; 
  return *this;
}

/*
 *  Multiplies the 2D point by a given number. 
 *  @param v the 2D point multiplied
 *  @param s number the point is multiplied by
 *  @return the point resulting from multiplication
 */
template < class T >
inline N_2D_Point<T>
operator*( const N_2D_Point<T> &v, double s ) 
{
    return N_2D_Point<T>(v(0)*s, v(1)*s);
}

/*
 * Multiplies the 2D point by a given number.
 * @param s number the point is multiplied by
 * @param v the 2D point multiplied
 * @return the point resulting from multiplication
 */
template < class T >
inline N_2D_Point<T>
operator*( double s, const N_2D_Point<T> &v ) 
{
  return N_2D_Point<T>( (T)(v(0)*s), 
                              (T)(v(1)*s) );
}

/*
 * Multiplies the point on left-hand side by given value (s).
 * @param s value by which point is multiplied
 * @return the point resulting from multiplication
*/
template < class T >
inline N_2D_Point<T>& N_2D_Point<T>::
operator*=( T s )
{
  _elems[0] *= s; _elems[1] *= s; return *this;
}

/*
 * Divides the 2D point by a given number
 * @param v the 2D point divided
 * @param s number the point is divided  by
 * @return the point resulting from division
 */
template < class T >
inline N_2D_Point<T>
operator/( const N_2D_Point<T> &v, double s ) 
{
  return N_2D_Point<T>( v.x()/s, v.y()/s );
}

/*
 * Divides the point on left-hand side by given value (s).
 * @param s value by which point is divided
 * @return the point resulting from division
 */
template < class T >
inline N_2D_Point<T>& N_2D_Point<T>::
operator/=( T s ) 
{
  _elems[0] /= s; _elems[1] /= s; return *this;
}

/*
 * Finds the closest point on the line ab to this point and returns
 * the distance to the point from a.
 * @param a one endpoint of the line
 * @param b another endpoint of the line
 * @return the distance from the closest point on the line to this point from a
 */
template < class T >
inline T N_2D_Point<T>::
closest_point_to_line( const N_2D_Point<T> &a, const N_2D_Point<T> &b ) const
{
  N_2D_Point<T> v( b - a );
  double d2 = v.length2();
  return (T)(d2 < 1.0E-6
                    ? 0.
                    : v.dot( *this-a )/d2);
}

/*
 * Returns the square of the distance from this position
 * to the line from 'a' to 'b'. If 'p' is a non-zero pointer,
 * the closest position on the line is set in 'p'.
 * @param a one point on the line ab
 * @param b second point on the line ab
 * @param p if p the closest position on the line if p is non-zero
 * @return the square of the distance from this position to the line ab
 */
template < class T >
inline T N_2D_Point<T>::
distance2_to_line( const N_2D_Point<T> &a, const N_2D_Point<T> &b, N_2D_Point<T> *p ) const {
  double s = closest_point_to_line( a, b );
  N_2D_Point<T> p_on_line = a + s*( b - a );
  
  if ( p != 0 )
    *p = p_on_line;
  
  return distance2( p_on_line );
}

template < typename elem_type >
inline std::ostream& N_2D_Point<elem_type>::display_unformatted(std::ostream &os)
  const
{
  return os  << _elems[0] << " " << _elems[1];
}

template < typename elem_type >
inline std::ostream& N_2D_Point<elem_type>::
display( const char* pmsg, std::ostream &os ) const
{
  if ( pmsg )
    os << pmsg;
  
  os  << "[x: "
      << _elems[0] << "  y: "
      << _elems[1] << "]";
  
  return os;
}

template < typename elem_type >
inline std::ostream& operator<<(std::ostream &os, const N_2D_Point<elem_type> &p)
{
  return p.display( NULL, os );
}

template <class T>
inline std::istream& operator>>(std::istream& is, N_2D_Point<T>& p)
{
  return is >> p.x() >> p.y();
}

template <class T>
bool io_object(FDM_Untyped_Node node, N_2D_Point<T>& rhs)
{
  FDM_Array a(node);
  bool ok = a.element(rhs(0));
  ok &= a.element(rhs(1));
  return ok;
}

template <class T>
inline bool cl_isnan(const N_2D_Point<T> &pt) {
  return cl_isnan(pt.x()) || cl_isnan(pt.y());
}

template <class T>
inline void cl_setnan(N_2D_Point<T> &pt) {
  cl_setnan(pt.x());
  cl_setnan(pt.y());
}

//-----------------------------------------------------------------------------

#include "claraty/operator_def.h"

#define CL_OP_GENERATE          ( CL_OP_NOT_EQUAL )
#define CL_OP_TEMPLATE          template < typename T >
#define CL_OP_CLASS             N_2D_Point<T>
#include "claraty/operator.h"

#define CL_OP_TEMPLATE          template < typename T >
#define CL_OP_CLASS             N_2D_Point<T>
#define CL_OP_SCALAR            T
#include "claraty/scalar_operator.h"

} // namespace claraty

#endif // N2D_POINT_H

Last modified: June 05 2007 10:19:53

JPL PRIVACY STATEMENT GLOSSARY SITEMAP FEEDBACK CONTACT US

Site Manager: Issa A.D. Nesnas
Site Maintainer: Kelly Breed
Document Clearance #: CL 07-0635