//* Utility classes for analytical geometry - 11/17/2010 2:13AM *

#include "nav_geometry.h" #include <cmath> NAV_Line::NAV_Line(double x1, double y1, double x2, double y2) { A = y1-y2; B = x2-x1; C = y2*x1 - x2*y1; normalize(); } void NAV_Line::normalize() { double coef = sqrt(A*A+B*B); A /= coef; B /= coef; C /= coef; } void NAV_Quadrants::compute_lines() { double sinth = ArMath::sin(getTh()); double costh = ArMath::cos(getTh()); robotXaxis.setABC( sinth, costh, -sinth*getX() - costh*getY() ); robotYaxis.setABC( costh, -sinth, -costh*getX() + sinth*getY() ); // rotate Xaxix +90deg double th=ArMath::fixAngle(getTh()+45); sinth = ArMath::sin(th); costh = ArMath::cos(th); robotXP45axis.setABC( sinth, costh, -sinth*getX() - costh*getY() ); robotXN45axis.setABC( costh, sinth, -costh*getX() - sinth*getY() ); // rotate XP45axis -90deg } bool NAV_Proximity::checkReturned(const ArPose &p) { bool result; if (ArPose::findDistanceTo(p)<=close_enough) { result = was_gone; // if (was_gone) return(true); else return(false); } else { was_gone=true; result = false; // return(false); } return(result); } NAV_LineCrossing::NAV_LineCrossing(double x, double y, double th) : NAV_Line(0, 0, 0), last_sign(0) { double sinth = ArMath::sin(th); double costh = ArMath::cos(th); setABC( sinth, costh, -sinth*x - costh*x ); } bool NAV_LineCrossing::checkCrossed(const ArPose &p) { double new_sign = NAV_Line::signed_distance(p); bool result = last_sign*new_sign<0; if (new_sign!=0) last_sign = new_sign; return(result); }