dp.h
Go to the documentation of this file.
00001 /*
00002  * Copyright (c) 2008, AIST, the University of Tokyo and General Robotix Inc.
00003  * All rights reserved. This program is made available under the terms of the
00004  * Eclipse Public License v1.0 which accompanies this distribution, and is
00005  * available at http://www.eclipse.org/legal/epl-v10.html
00006  * Contributors:
00007  * The University of Tokyo
00008  */
00016 #ifndef __DP_H__
00017 #define __DP_H__
00018 
00019 #include <iostream>
00020 using namespace std;
00021 
00022 class dpQueue;
00023 class dpMain;
00024 
00029 class dpNode 
00030 {
00031         friend class dpQueue;
00032         friend class dpMain;
00033 public:
00034         dpNode() {
00035                 cost = 0.0;
00036                 astar_cost = 0.0;
00037                 total_cost = 0.0;
00038                 dp_main = 0;
00039                 parent = 0;
00040                 child = 0;
00041                 brother = 0;
00042                 queue = 0;
00043                 id = -1;
00044                 depth = 0;
00045                 active = true;
00046         }
00047         virtual ~dpNode() {
00048                 if(brother) delete brother;
00049                 if(child) delete child;
00050         }
00051 
00052         dpNode* Parent() {
00053                 return parent;
00054         }
00055         dpNode* Brother() {
00056                 return brother;
00057         }
00058         dpNode* Child() {
00059                 return child;
00060         }
00061         
00062         int Depth(dpNode* target_p = 0) {
00063                 int ret;
00064                 dpNode* p;
00065                 for(p=this, ret=0; p && p!=target_p; p=p->parent, ret++);
00066                 if(target_p) ret++;
00067                 return ret;
00068         }
00069         dpNode* GetParent(int id) {
00070                 int i;
00071                 dpNode* cur;
00072                 for(i=0, cur=this; cur && i<id; i++, cur=cur->parent);
00073                 return cur;
00074         }
00075         
00076         double TotalCost() {
00077                 return total_cost;
00078         }
00079         double Cost() {
00080                 return cost;
00081         }
00082         double TotalAstarCost() {
00083                 return total_astar_cost;
00084         }
00085         double AstarCost() {
00086                 return astar_cost;
00087         }
00088 
00089         dpQueue* Queue() {
00090                 return queue;
00091         }
00092 
00093         int ID() {
00094                 return id;
00095         }
00096         
00097 protected:
00102 
00103 
00107         virtual int create_child_nodes(dpNode**& nodes) = 0;
00109         virtual double calc_cost() = 0;
00111         virtual double calc_astar_cost(dpNode* potential_parent) {
00112                 return 0.0;
00113         }
00115         virtual int same_state(dpNode* refnode) = 0;
00117         virtual int is_goal() = 0;
00120         int open(int _max_nodes, int _max_goals);
00121         void add_child(dpNode* n);
00122         void remove_single();
00123         void remove();
00124 
00125         dpNode* next_depth(dpNode* first_child = 0) {
00126                 dpNode* c, *ret = 0;
00127                 for(c = first_child ? first_child->brother : child; c && !ret; c=c->brother)
00128                 {
00129                         if(c->active) return c;
00130                         else if((ret = c->next_depth())) return ret;
00131                 }
00132                 if(parent) return parent->next_depth(this);
00133                 return 0;
00134         }
00135 
00136         dpNode* next_breadth(dpNode* refnode) {
00137                 if(depth >= refnode->depth && active) return this;
00138                 dpNode* ret = 0;
00139                 if((ret = brother->next_breadth(refnode))) return ret;
00140                 return child->next_breadth(refnode);
00141         }
00142 
00143         virtual void dump(ostream& ost) {
00144         }
00145         void dump_trajectory(ostream& ost) {
00146                 parent->dump_trajectory(ost);
00147                 dump(ost);
00148         }
00149         void dump_all(ostream& ost) {
00150                 for(int i=0; i<depth; i++) ost << " " << flush;
00151                 dump(ost);
00152                 child->dump_all(ost);
00153                 brother->dump_all(ost);
00154         }
00155 
00156         double cost;  // cost from parent to this node
00157         double astar_cost;
00158 
00159         // automatically set
00160         dpNode* parent;
00161         dpNode* brother;
00162         dpNode* child;
00163         double total_cost; // total cost from start to this node
00164         double total_astar_cost;
00165         dpMain* dp_main;
00166         dpQueue* queue;
00167         int id;
00168         int depth;
00169         int active;
00170 };
00171 
00177 class dpQueue 
00178 {
00179         friend class dpNode;
00180         friend class dpMain;
00181 public:
00182         dpQueue(dpNode* n) {
00183                 node = n;
00184                 parent = 0;
00185                 smaller_child = 0;
00186                 larger_child = 0;
00187                 active = true;
00188         }
00189         ~dpQueue() {
00190                 if(larger_child) delete larger_child;
00191                 if(smaller_child) delete smaller_child;
00192         }
00193 
00194 protected:
00195         void add_queue(dpQueue* q) {
00196                 if(q->node->total_astar_cost >= node->total_astar_cost)
00197                 {
00198                         if(!larger_child)
00199                         {
00200                                 larger_child = q;
00201                                 q->parent = this;
00202                         }
00203                         else larger_child->add_queue(q);
00204                 }
00205                 else
00206                 {
00207                         if(!smaller_child)
00208                         {
00209                                 smaller_child = q;
00210                                 q->parent = this;
00211                         }
00212                         else smaller_child->add_queue(q);
00213                 }
00214                 
00215         }
00216         dpQueue* smallest() {
00217                 dpQueue* ret = 0;
00218 //              cerr << "smallest: " << flush;
00219 //              node->dump(cerr);
00220                 if(smaller_child) ret = smaller_child->smallest();
00221                 if(!ret)
00222                 {
00223                         if(active)
00224                         {
00225                                 ret = this;
00226                         }
00227                         else if(larger_child) ret = larger_child->smallest();
00228                 }
00229                 return ret;
00230         }
00231         dpQueue* larger_goal() {
00232                 dpQueue* ret = larger_goal_sub();
00233                 dpQueue* q = this;
00234                 while(!ret && q->parent)
00235                 {
00236                         if(q->parent->node->is_goal() &&
00237                            q->parent->smaller_child == q)
00238                         {
00239                                 ret = q->parent;
00240                         }
00241                         else
00242                         {
00243                                 if(q->parent->smaller_child == q)
00244                                         ret = q->parent->larger_goal_sub();
00245                         }
00246                         q = q->parent;
00247                 }
00248                 return ret;
00249         }
00250         dpQueue* larger_goal_sub() {
00251                 if(larger_child) return larger_child->smallest_goal();
00252                 return 0;
00253         }
00254 
00255         dpQueue* smallest_goal() {
00256                 dpQueue* ret = 0;
00257                 if(smaller_child) ret = smaller_child->smallest_goal();
00258                 if(!ret)
00259                 {
00260                         if(node->is_goal()) ret = this;
00261                         else if(larger_child) ret = larger_child->smallest_goal();
00262                 }
00263                 return ret;
00264         }
00265         
00266         void dump(ostream& ost) {
00267                 ost << "--- id = " << node->id << endl;
00268                 ost << " cost = " << node->total_cost << ", goal = " << node->is_goal() << endl;
00269                 ost << " smaller = " << (smaller_child ? smaller_child->node->id : -1) << ", larger = " << (larger_child ? larger_child->node->id : -1) << endl;
00270                 smaller_child->dump(ost);
00271                 larger_child->dump(ost);
00272         }
00273         
00274         dpNode* node;
00275         dpQueue* parent;
00276         dpQueue* smaller_child;
00277         dpQueue* larger_child;
00278 
00279         int active;
00280 };
00281 
00286 class dpMain
00287 {
00288         friend class dpQueue;
00289         friend class dpNode;
00290 public:
00291         dpMain() {
00292                 start_node = 0;
00293                 top_queue = 0;
00294                 n_goals = 0;
00295                 n_nodes = 0;
00296         }
00297         virtual ~dpMain() {
00298                 if(start_node) delete start_node;
00299                 if(top_queue) delete top_queue;
00300         }
00301 
00303         void SetStartNode(dpNode* _n) {
00304                 start_node = _n;
00305                 add_node(_n);
00306                 if(_n->is_goal()) n_goals++;
00307         }
00308 
00313 
00314 
00319         int Search(int _max_nodes = -1, int _max_goals = -1);
00321         int Search(double max_time);
00323         int SearchDepthFirst(int _max_nodes = -1, int _max_goals = -1);
00325         int SearchBreadthFirst(int _max_nodes = -1, int _max_goals = -1);
00328         // clear all nodes and edges
00329         virtual void ClearAll() {
00330                 reset();
00331         }
00332 
00333         void DumpTrajectory(ostream& ost, dpNode* goal);
00334 
00336         dpNode* BestGoal(dpNode* ref = 0) {
00337                 if(!top_queue) return 0;
00338                 dpQueue* g = 0;
00339                 if(ref) g = ref->queue->larger_goal();
00340                 else g = top_queue->smallest_goal();
00341                 if(g) return g->node;
00342                 return 0;
00343         }
00344         
00345         int NumNodes() {
00346                 return n_nodes;
00347         }
00348         int NumGoals() {
00349                 return n_goals;
00350         }
00351         void DumpAll(ostream& ost) {
00352                 cerr << "--" << endl;
00353                 start_node->dump_all(ost);
00354                 cerr << "--" << endl;
00355         }
00356         void DumpQueue(ostream& ost) {
00357                 top_queue->dump(ost);
00358         }
00359 
00360 protected:
00361         void reset() {
00362                 if(start_node) delete start_node;
00363                 if(top_queue) delete top_queue;
00364                 n_nodes = 0;
00365                 n_goals = 0;
00366         }
00367         void add_node(dpNode* _n) {
00368                 _n->dp_main = this;
00369                 _n->id = n_nodes;
00370                 n_nodes++;
00371                 dpQueue* q = new dpQueue(_n);
00372                 add_queue(q);
00373                 _n->queue = q;
00374         }
00375         void add_queue(dpQueue* _q) {
00376                 if(!top_queue)
00377                 {
00378                         top_queue = _q;
00379                         return;
00380                 }
00381                 top_queue->add_queue(_q);
00382         }
00383         dpQueue* smallest_queue() {
00384                 if(!top_queue) return 0;
00385                 return top_queue->smallest();
00386         }
00387         void remove_node(dpNode* node) {
00388                 node->queue->active = false;
00389                 node->remove();  // remove all child nodes
00390         }
00391         void remove_node_single(dpNode* node) {
00392                 node->queue->active = false;
00393                 node->remove_single();  // remove all child nodes
00394         }
00395         int is_best(dpNode* ref, dpNode* target);
00396         
00397         dpNode* next_breadth(dpNode* refnode) {
00398                 return start_node->next_breadth(refnode);
00399         }
00400 
00401         dpQueue* top_queue;
00402         dpNode* start_node;
00403         int n_goals;
00404         int n_nodes;
00405 };
00406 
00407 
00408 #endif


openhrp3
Author(s): AIST, General Robotix Inc., Nakamura Lab of Dept. of Mechano Informatics at University of Tokyo
autogenerated on Sun Apr 2 2017 03:43:53