//* 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