00001 #ifndef PL_RRT_CONNECT_h
00002 #define PL_RRT_CONNECT_h 1
00003
00004 #pragma warning( disable : 4250 )
00005
00006
00007 #include "Planners/PL_Boolean_Output.h"
00008
00009 #include "Planners/PL_OpenGL.h"
00010 #include "planners/aca/JMA_aca_structs.h"
00011
00012 class PL_RRT_CONNECT : public PL_Boolean_Output,
00013 virtual public PL_OpenGL
00014 {
00015
00016 public:
00017 ~PL_RRT_CONNECT();
00018 virtual bool Plan();
00019 virtual void SetStart_and_GoalConfig (const Configuration& configuration1,const Configuration& configuration2);
00020 bool DrawExplicit () const;
00021 protected:
00022 double MyDistanceFunc (const Configuration& conf1, const Configuration& conf2) const;
00023 int Extend_trapped(JMA_Roadmap_Tree* Ptr_tree, int* the_n_nodes, Configuration* the_random_config);
00024 int Simple_Extend_trapped(JMA_Roadmap_Tree* Ptr_tree,
00025 int* the_n_nodes, Configuration old_new_config);
00026 bool connect(JMA_Roadmap_Tree* My_tree, Configuration old_new_config);
00027 int Get_new_config(Configuration* the_random_config, Configuration* the_near_config, Configuration* the_new_config);
00028
00029 int Get_nearest_neighbour(JMA_Roadmap_Tree* Ptr_tree, Configuration* the_random_config);
00030 void InitRoadmapTree( JMA_Roadmap_Tree* tree1, JMA_Roadmap_Tree* tree2, const int n_dof1, const int n_dof2, const JMA_Configuration init_conf, const JMA_Configuration goal_conf ) const;
00031 static void GetIdsPath (JMA_Roadmap_Tree* tree1,JMA_Roadmap_Tree* tree2, const int landmark_id1, const int landmark_id2, int ids_list1[], int* n_landmarks1, int ids_list2[], int* n_landmarks2);
00032 int Get_random_config(Configuration* the_random_config);
00033 private:
00034 int ids_list1[ 1000 ], ids_list2[1000];
00035 JMA_Roadmap_Tree tree1, tree2;
00036 JMA_Configuration init_conf, goal_conf;
00037 int numberOfEmbryos,near_neighbour_id;
00038 int numberOfLandmarks;
00039 Configuration random_config, near_config, new_config, old_new_config;
00040
00041 private:
00042
00043 };
00044 #endif