```//* Utility classes for analytical geometry - 11/17/2010 2:13AM *
#ifndef _NAV_GEOMETRY
#define _NAV_GEOMETRY

#include "../Aria/include/Aria.h"

class NAV_Line
{
public:
NAV_Line() : A(0), B(0), C(0) {}
NAV_Line(double a, double b, double c) : A(a), B(b), C(c) {}
NAV_Line(double x1, double y1, double x2, double y2);
void    reset()                                         { A=B=C=0.0; } // invalidates the line
void    setABC(double a, double b, double c)            { A=a; B=b; C=c; }
bool    isValid() const { return(A!=0||B!=0); }
double  getA() const    { return(A); }
double  getB() const    { return(B); }
double  getC() const    { return(C); }
void    normalize();

double  distance        (double x, double y) const      { return(ArMath::fabs(A*x+B*y+C)); }
double  distance        (const ArPose& pose) const      { return(ArMath::fabs(distance(pose.getX(), pose.getY()))); }
double  signed_distance (double x, double y) const      { return(A*x+B*y+C); }
double  signed_distance (const ArPose& pose) const      { return(signed_distance(pose.getX(), pose.getY())); }
bool    isParallelTo    (const NAV_Line &l) const       { return( ArMath::fabs( A*l.getB()-B*l.getA() )<1E-6 ); }
protected:
double A, B, C;
};

class NAV_Quadrants : protected ArPose {
public:
NAV_Quadrants   (const ArPose &p)               : ArPose(p) { compute_lines();        }
NAV_Quadrants   (double x, double y, double th) : ArPose(x,y,th) { compute_lines();   }
void    setPose         (const ArPose &p)               { ArPose::setPose(p); compute_lines();}
void    setPose         (double x, double y, double th) { ArPose::setPose(x, y, th); compute_lines(); }
double  findAngleTo     (const ArPose &p) const         { return(ArPose::findAngleTo(p));     }
double  findDistanceTo  (const ArPose &p) const         { return(ArPose::findDistanceTo(p));  }

bool    isToLeft        (double x, double y) const      { return(robotXaxis.signed_distance(x,y)>0); }
bool    isToRight       (double x, double y) const      { return(robotXaxis.signed_distance(x,y)<0); }
bool    isInFront       (double x, double y) const      { return(robotYaxis.signed_distance(x,y)<0); }
bool    isInBack        (double x, double y) const      { return(robotYaxis.signed_distance(x,y)>0); }
bool    isNarrowLeft    (double x, double y) const      { return(robotXP45axis.signed_distance(x,y)>0 && robotXN45axis.signed_distance(x,y)>0 ); }
bool    isNarrowRight   (double x, double y) const      { return(robotXP45axis.signed_distance(x,y)<0 && robotXN45axis.signed_distance(x,y)<0 ); }
bool    isNarrowFront   (double x, double y) const      { return(robotXP45axis.signed_distance(x,y)>0 && robotXN45axis.signed_distance(x,y)<0 ); }
bool    isNarrowBack    (double x, double y) const      { return(robotXP45axis.signed_distance(x,y)<0 && robotXN45axis.signed_distance(x,y)>0 ); }
protected:
void    compute_lines();
NAV_Line robotXaxis;    // to tell between left and right of the robot
NAV_Line robotYaxis;    // to tell between behind and ahead of the robot
NAV_Line robotXP45axis; // XP45 && XN445 to do fine tuning 135 to 45, 45 to -45, -45 to -135, -135 to 135
NAV_Line robotXN45axis; // XP45 && XN445 to do fine tuning 135 to 45, 45 to -45, -45 to -135, -135 to 135
};

class NAV_Proximity : protected ArPose {
public:
NAV_Proximity   (const ArPose &p=ArPose(), double robotradius=150 ) : ArPose(p), close_enough(robotradius), was_gone(FALSE) {}
void    setPose         (const ArPose &p)               { ArPose::setPose(p); resetReturned(); }
bool    is_close_now    (const ArPose &p) const         { return(ArPose::findDistanceTo(p)<=close_enough); }
public:
void    resetReturned   ()                              { was_gone=FALSE; }
bool    checkReturned   (const ArPose &p);
// reports TRUE when went away and returned (multiple times) until departed away again or resetReturned is called

protected:
double  close_enough;
bool    was_gone;
};

class NAV_LineCrossing : protected NAV_Line {
public:
NAV_LineCrossing(double x1, double y1, double x2, double y2)
: NAV_Line(x1, y1, x2, y2), last_sign(0) {}
NAV_LineCrossing(double x, double y, double th);
public:
bool    checkCrossed    (const ArPose &p);  // reports each crossing of the line only once and resets internal state
protected:
double  last_sign;
};

#endif

```