geometry.h

#ifndef INCLUDE_NAVLIB_GEOMETRY
#define INCLUDE_NAVLIB_GEOMETRY

#include "wykobi/wykobi.hpp"

namespace navlib
{

const static double PI = static_cast<double>(wykobi::PI);
const static double PI2 = static_cast<double>(wykobi::PI2);

class Circle;
class Polar;
class Segment;
class Sector;

template <class T>
T maximum(const T& lhs, const T& rhs)
{
        if (lhs > rhs)
                return lhs;
        return rhs;
}

template <class T>
T minimum(const T& lhs, const T& rhs)
{
        if (lhs < rhs)
                return lhs;
        return rhs;
}

double pi2pi(const double& angle);

double sign(const double& num);

bool sortSectors(const Sector& lhs, const Sector& rhs, const double& goalHeading);

class Point : public wykobi::point2d<double> 
{
public:
        Point(double xi=0.0f, double yi=0.0f);
        Point(wykobi::point2d<double> point);
        bool operator ==(const Point& point) const;
        double getDistance(const Point& point) const;
        double getAngleTo(const Point& point) const;
};

class Pose
{
public:
        double x;                                                                       
        double y;                                                                       
        double theta;                                                           
        
        Pose(double xi=0.0f, double yi=0.0f, double thetai=0.0);
        bool operator ==(const Pose& pose) const;
        Point globalize(const Point& point) const;
        Pose globalize(const Pose& pose) const;
        Point localize(const Point& point) const;
        Pose localize(const Pose& pose) const;
        Pose rotate(const double& dtheta);
        Pose translate(const double& dx, const double& dy);

private:
        double _ct;                                                                     
        double _st;                                                                     
};

class Shape
{
public:
        virtual ~Shape() {};
        virtual std::vector<Segment> clip(std::vector<Segment> segments)=0;
        virtual bool contains(Point& p) const = 0;
        virtual bool collides(std::vector<Segment> segments);
        virtual Circle getBoundingCircle();
        virtual double getDistance(std::vector<Point> points);
        virtual std::vector<Point> getPoints() const=0;
        virtual Pose getPose() const=0;
        virtual std::vector<Segment> getSegments() const;
        void globalize(const Pose& pose);
        void localize(const Pose& pose);
        virtual void setPose(const Pose& pose)=0;
};

class Circle : public wykobi::circle<double>, public Shape
{
public:
        Circle(double x, double y, double radiusi, unsigned int ni=16);
        virtual ~Circle() {};
        virtual std::vector<Segment> clip(std::vector<Segment> segments);
        virtual bool contains(Point& p) const;
        virtual Circle getBoundingCircle();
        virtual std::vector<Point> getPoints() const;
        virtual Pose getPose() const;
        virtual void setPose(const Pose& pose);

protected:
        double _theta;                                                          
        unsigned int _n;                                                        
};

class Gap
{
public:
        double lRho;                                                            
        double rRho;                                                            
        double theta;                                                           
        int index;                                                                      

        Gap(double lRhoi, double rRhoi, double thetai, int indexi);
        Gap(const Polar& lhs, const Polar& rhs, int indexi);
        bool operator ==(const Gap& gap) const;
};

class Polar
{
public:
        double rho;                                                                     
        double theta;                                                           

        Polar(double rhoi=0.0f, double thetai=0.0f);
        Polar(Point p, Pose origin=Pose(0.0f, 0.0f, 0.0f));
        bool operator < (const Polar& polar) const;
        bool operator ==(const Polar& polar) const;
        Point toPoint() const;
        Point toPoint(const Pose& origin) const;
};

class Polygon : public wykobi::polygon<double, 2>, public Shape
{
public:
        Polygon(std::vector<Point> points);
        virtual ~Polygon() {};
        virtual std::vector<Segment> clip(std::vector<Segment> segments);
        virtual bool contains(Point& p) const;
        virtual std::vector<Point> getPoints() const;
        virtual Pose getPose() const;
        virtual void setPose(const Pose& pose);

protected:
        Pose _pose;                                                                     
};

class Sector
{
public:
        Gap lGap;                                                                       
        Gap rGap;                                                                       

        Sector(const Gap& lGapi, const Gap& rGapi);
        bool operator ==(const Sector& sector) const;
};

class Segment : public wykobi::segment<double, 2>
{
public:
        Segment(double x1, double y1, double x2, double y2);
        Segment(Point p1, Point p2);
        bool operator ==(const Segment& segment) const;
        double getDistance(const Point& point) const;
        double getLength() const;
        Point getPoint1() const { return (*this)[0]; }
        Point getPoint2() const { return (*this)[1]; }
        void setPoint1(const Point& p) { (*this)[0].x = p.x; (*this)[0].y = p.y; }
        void setPoint2(const Point& p) { (*this)[1].x = p.x; (*this)[1].y = p.y; }
};

}

#endif

---