orca-robotics INTRODUCTION Overview Download and Install Documentation REPOSITORY Interfaces Drivers Libraries Utilities Software Map DEVELOPER Dashboard PEOPLE Contributors Users Project Download Mailing lists

geom2d.h /* * Orca-Robotics Project: Components for robotics * http://orca-robotics.sf.net/ * Copyright (c) 2004-2008 Alex Brooks * * This copy of Orca is licensed to you under the terms described in * the LICENSE file included in this distribution. * */ #ifndef GEOM2D_H #define GEOM2D_H #include <string> #include <cmath> #include <iostream> #include <hydroportability/sharedlib.h> namespace geom2d { class SOEXPORT Point { public: explicit Point() {} explicit Point( double x, double y ) { p_[0] = x; p_[1] = y; } double x() const { return p_[0]; } double y() const { return p_[1]; } double &x() { return p_[0]; } double &y() { return p_[1]; } // allow treatment as a vector double operator[]( int i ) const { return p_[i]; } double &operator[]( int i ) { return p_[i]; } // convert to polar double range() const { return std::sqrt(y()*y()+x()*x()); } double bearing() const { return std::atan2(y(),x()); } std::string toString() const; std::string toStringXY() const; std::string toStringRB() const; private: double p_[2]; }; class SOEXPORT PolarPoint { public: explicit PolarPoint() {} explicit PolarPoint( double r, double b ) { p_[0] = r; p_[1] = b; } double range() const { return p_[0]; } double bearing() const { return p_[1]; } double &range() { return p_[0]; } double &bearing() { return p_[1]; } // allow treatment as a vector double operator[]( int i ) const { return p_[i]; } // convert to cartesian double x() const { return range()*std::cos(bearing()); } double y() const { return range()*std::sin(bearing()); } std::string toString() const; std::string toStringXY() const; std::string toStringRB() const; private: double p_[2]; }; inline double dist( const Point &p1, const Point &p2 ) { return std::sqrt((p1.y()-p2.y())*(p1.y()-p2.y())+(p1.x()-p2.x())*(p1.x()-p2.x())); } inline double dist( const PolarPoint &p1, const PolarPoint &p2 ) { return std::sqrt((p1.y()-p2.y())*(p1.y()-p2.y())+(p1.x()-p2.x())*(p1.x()-p2.x())); } inline double distSq( const Point &p1, const Point &p2 ) { return ((p1.y()-p2.y())*(p1.y()-p2.y())+(p1.x()-p2.x())*(p1.x()-p2.x())); } inline double distSq( const PolarPoint &p1, const PolarPoint &p2 ) { return ((p1.y()-p2.y())*(p1.y()-p2.y())+(p1.x()-p2.x())*(p1.x()-p2.x())); } inline bool operator==( const Point &p1, const Point &p2 ) { return (p1.x()==p2.x()) && (p1.y()==p2.y()); } inline bool operator!=( const Point &p1, const Point &p2 ) { return !(p1==p2); } // in place inline void rotate( Point &p, double theta ) { const double ct = std::cos(theta); const double st = std::sin(theta); double xNew = p.x()*ct - p.y()*st; double yNew = p.x()*st + p.y()*ct; p.x()=xNew; p.y()=yNew; } // not in place inline void rotate( const Point &p, double theta, Point &q ) { const double ct = std::cos(theta); const double st = std::sin(theta); q.x() = p.x()*ct - p.y()*st; q.y() = p.x()*st + p.y()*ct; } // in place inline void addTransform( Point &p, double x, double y, double theta ) { rotate( p, theta ); p.x() += x; p.y() += y; } // not in place inline void addTransform( const Point &p, double x, double y, double theta, Point &tp ) { rotate( p, theta, tp ); tp.x() += x; tp.y() += y; } // in place inline void subtractTransform( Point &p, double x, double y, double theta ) { p.x() -= x; p.y() -= y; rotate( p, -theta ); } // not in place inline void subtractTransform( const Point &p, double x, double y, double theta, Point &tp ) { tp.x() = p.x()-x; tp.y() = p.y()-y; rotate( tp, -theta ); } inline std::ostream &operator<<( std::ostream &s, const Point &p ) { return s << p.toString(); } inline std::ostream &operator<<( std::ostream &s, const PolarPoint &p ) { return s << p.toString(); } SOEXPORT bool circleCircleIntersection( double a1, double b1, double r1, double a2, double b2, double r2, double &px, double &py, double &qx, double &qy ); inline bool circleCircleIntersection( const Point &c1, double r1, const Point &c2, double r2, Point &p, Point &q ) { return circleCircleIntersection( c1.x(),c1.y(),r1, c2.x(),c2.y(),r2, p.x(),p.y(), q.x(),q.y() ); } class SOEXPORT Line { public: Line( const Point &p1, const Point &p2 ); Line( const Point &p, double vx, double vy ); void getUnitVectorParallel( double v[2] ) const { v[0] = -b_; v[1] = a_; } void getUnitVectorPerpendicular( double v[2] ) const { v[0] = a_; v[1] = b_; } double a() const { return a_; } double b() const { return b_; } double c() const { return c_; } private: // Represent in general equation form: ax + by + c = 0 // (normalised such that hypot(a,b) = 1) double a_, b_, c_; }; std::string toString( const Line &l ); inline std::ostream &operator<<( std::ostream &s, const Line &l ) { return s << toString(l); } SOEXPORT double dist( const Line &l, const Point &p ); inline double dotProduct( double v1[2], double v2[2] ) { return v1[0]*v2[0] + v1[1]*v2[1]; } inline Point closestPointToOrigin( const Line &l ) { return Point( -l.a()*l.c(), -l.b()*l.c() ); } SOEXPORT Point closestPointOnLine( const Line &line, const Point &point ); SOEXPORT bool lineLineIntersection( const Line &l1, const Line &l2, Point &p ); SOEXPORT void convertToRhoAlpha( const Line &line, double &rho, double &alpha ); SOEXPORT double rho( const Line &line ); SOEXPORT double alpha( const Line &line ); } #endif

Webmaster: Tobias Kaupp (tobasco at users.sourceforge.net)

---