00001 #ifndef PL_RRT_CLOSEDCHAIN
00002 #define PL_RRT_CLOSEDCHAIN 1
00003
00004 #include "PL_PRM_ClosedChain.h"
00005
00006
00007 class PL_RRT_ClosedChain: public PL_PRM_ClosedChain
00008 {
00009 public:
00010 PL_RRT_ClosedChain();
00011 ~PL_RRT_ClosedChain();
00012
00013 void SetCollisionDetector(CD_BasicStyle* collisionDetector);
00014 virtual bool DrawExplicit () const;
00015 virtual bool Plan ();
00016
00017 protected:
00018 vertex m_goalVert;
00019 vertex m_rootVert;
00020
00021
00022 private:
00023 dynamic_trees m_trajTree;
00024 std::vector<vertex> m_vertices;
00025
00026 bool CreateTree(Configuration &conf);
00027 void ClearTree();
00028 vertex AddNodeInTree(vertex parentVertex, Configuration &childConf, PA_Points &localPath);
00029 vertex FindClosestInTree(Configuration &conf);
00030 Configuration& GetConfigurationFromTree(vertex vert);
00031 PA_Points& GetPathFromTree(vertex vert);
00032 double Distance(const Configuration &conf1, const Configuration &conf2);
00033 int Extend(Configuration &fromConf, Configuration &toConf, Configuration dirConf, PA_Points &edgePath);
00034 int ExtendJacobian(Configuration &fromConf, Configuration &toConf, Configuration dirConf, PA_Points &edgePath);
00035 int ConnectToGoal(vertex fromVert);
00036 int ConnectToGoal();
00037
00038 void CompareLocalPlanners();
00039 bool TestRGDConnection(Configuration &testStartConf, Configuration &testGoalConf, PA_Points &localPath);
00040 bool TestJacobianConnection(Configuration &testStartConf, Configuration &testGoalConf, PA_Points &localPath);
00041 bool TestAPDecompConnection(Configuration &testStartConf, Configuration &testGoalConf, PA_Points &localPath);
00042
00043 private:
00044 void RetrievePath(vertex &goalVert);
00045 inline void CopyPath(PA_Points &target, PA_Points &source){target = source;};
00046 void AppendPath(PA_Points &collect, PA_Points &local);
00047 void AppendPath(PA_Points &collect, PA_Points *local);
00048 void InsertPath(PA_Points &target, PA_Points &source);
00049 void OutputStatistics();
00050 };
00051 #endif
