path.h

#ifndef INCLUDE_NAVLIB_PATHPLANNER_PATH
#define INCLUDE_NAVLIB_PATHPLANNER_PATH

#include "geometry.h"
#include <assert.h>
#include <iostream>
#include <sstream>
#include <string>

using std::istream;
using std::ostream;

namespace navlib
{
        class PathPoint : public Pose
        {
        public:
                PathPoint();
                PathPoint(double x0, double y0, double theta0, double c0);
                PathPoint(const Pose& p0, double c0);
                double c;
                
                double getDistance(const PathPoint& wp) const;
                bool operator==(const PathPoint &wp) const;
                bool operator<(const PathPoint &wp) const;
        };

        istream& operator >>(istream& is, PathPoint& p);
        ostream& operator <<(ostream& os, const PathPoint& p);
        
        class Path
        {
        public:
                Path();
                virtual ~Path() {}
                virtual bool collides(const Shape* body, std::vector<Point> obstacles);
                virtual Path* copy() = 0;
                virtual PathPoint getPathPoint(double d) const = 0;
                virtual std::vector<PathPoint> getPathPoints() const;
                PathPoint getInitPathPoint() const;
                PathPoint getFinalPathPoint() const;
                virtual double getLength() const = 0;
                virtual bool isReverse(double d) const;
                virtual void read(istream& is);
                virtual void write(ostream& os) const;

        protected:
                PathPoint _wp0; 
        };
        
        istream& operator >>(istream& is, Path& path);
        ostream& operator <<(ostream& os, const Path& path);

        class PathChain : public Path
        {
        public:
                PathChain();
                virtual ~PathChain();
                bool collides(const Shape* body, std::vector<Point> obstacles);
                double getLength() const;
                PathPoint getPathPoint(double d) const;
                std::vector<PathPoint> getPathPoints() const;
                bool append(Path* T);
                Path* copy();
                bool insert(Path* T);
                bool isReverse(double d) const;

        private:
                std::vector<Path*> _pathList;
                double _length;
        };

        class ReversePath : public Path
        {
        public:
                ReversePath(Path* path);
                virtual ~ReversePath();
                Path* copy();
                PathPoint getPathPoint(double d) const;
                std::vector<PathPoint> getPathPoints() const;
                double getLength() const;
                bool isReverse(double d) const;

        private:
                Path* _path;
                double _length;
        };
}

#endif

---