00001 #ifndef RESCUE__FREE_SPACE_GRAPH_H
00002 #define RESCUE__FREE_SPACE_GRAPH_H
00003
00004 #include "rescutil.h"
00005
00006 namespace rescue
00007 {
00008
00013
00014
00015
00016 template<class transporter_edge>
00017 class free_space_graph
00018 {
00019
00020 friend class edge_iterator;
00021
00022 protected:
00023
00024 public:
00025
00026 typedef transporter_edge transporter_edge_class;
00027 typedef transporter_edge::index_t index_t;
00028 typedef transporter_edge::arithmetic_t arithmetic;
00029 typedef transporter_edge::interval_border interval_border;
00030 typedef transporter_edge::border_iterator edge_border_iterator;
00031 typedef transporter_edge::point_t point;
00032 typedef transporter_edge::motion motion;
00033
00034 typedef arithmetic arithmetic_t;
00035
00036 typedef std::pair<point,point> point_pair;
00037 typedef transporter_edge::border_value border_value;
00038 typedef std::list<interval_border> border_list;
00039 typedef border_list::iterator border_iterator;
00040
00046 typedef std::pair<index_t,index_t> discrete_free_space_point;
00047
00052 typedef std::list<discrete_free_space_point> discrete_reparametrization_pair;
00053
00054
00058 free_space_graph();
00059
00062 ~free_space_graph();
00063
00068 void build(point* P,point* Q,index_t m_P,index_t n_Q,
00069 const arithmetic& eps, const arithmetic& delta);
00070
00080 bool update(const interval_border& e);
00081
00088 motion get_match();
00089
00092 bool get_reparametrizations(discrete_reparametrization_pair& kappa_lambda);
00093
00096 void clear();
00097
00102 index_t m;
00103
00108 index_t n;
00109
00113 transporter_edge*** edges;
00114
00115 private:
00116
00120 void save_borders(transporter_edge& e, index_t col, index_t row, graph_direction dir);
00121
00125 border_list left_border_list;
00126
00131 point_pair reference_points;
00132
00142 border_value match_pos;
00143
00147 bool match_found;
00148
00149
00156
00157
00158 class edge_iterator
00159 {
00160
00161 protected:
00162
00163 transporter_edge*** edges;
00164 index_t i;
00165 index_t j;
00166 index_t m;
00167 index_t n;
00168 graph_direction dir;
00169
00170
00171 public:
00172
00173
00174 inline edge_iterator()
00175
00176 {
00177 }
00178
00179
00180 inline edge_iterator(index_t it_i, index_t it_j,
00181 graph_direction it_dir, const free_space_graph<transporter_edge>& Gamma)
00182
00183 {
00184 i = it_i;
00185 j = it_j;
00186 dir = it_dir;
00187 m = Gamma.m;
00188 n = Gamma.n;
00189 edges = Gamma.edges;
00190 }
00191
00192
00193 inline edge_iterator& operator++()
00194
00195 {
00196
00197 if (dir<vertical)
00198 {
00199 dir++;
00200 return *this;
00201 }
00202 dir = horizontal;
00203
00204 if (i<m-1)
00205 {
00206 i++;
00207 return *this;
00208 }
00209 i=0;
00210
00211 if (j<=n-1)
00212 {
00213 j++;
00214 return *this;
00215 }
00216
00217 return *this;
00218
00219 }
00220
00221
00222 inline edge_iterator& operator++(int)
00223
00224 {
00225
00226 if (j==n)
00227 {
00228 if (i<m)
00229 {
00230 i++;
00231 j=0;
00232 dir=horizontal;
00233 }
00234 return *this;
00235 }
00236
00237 if (i==m)
00238 {
00239 if (j<n)
00240 {
00241 j++;
00242 dir=vertical;
00243 }
00244 return *this;
00245 }
00246
00247 if (dir==horizontal)
00248 {
00249 dir=diagonal;
00250 return *this;
00251 }
00252 if (dir==diagonal)
00253 {
00254 dir=vertical;
00255 return *this;
00256 }
00257 dir = horizontal;
00258
00259 if (j<n)
00260 {
00261 j++;
00262 return *this;
00263 }
00264
00265 if (i<m)
00266 {
00267 i++;
00268 j=0;
00269 if (i==m)
00270 dir=vertical;
00271 return *this;
00272 }
00273
00274 return *this;
00275
00276 }
00277
00278
00279 inline transporter_edge& operator*()
00280
00281 {
00282 return edges[i][j][dir];
00283 }
00284
00285
00286 inline bool operator==(const edge_iterator& e_it)
00287
00288 {
00289 return dir==e_it.dir && i==e_it.i && j==e_it.j;
00290 }
00291
00292
00293 inline bool operator!=(const edge_iterator& e_it)
00294
00295 {
00296 return !(dir==e_it.dir && i==e_it.i && j==e_it.j);
00297 }
00298
00299 };
00300
00301
00302 public:
00303
00304 edge_iterator edge_begin();
00305 edge_iterator edge_end();
00306
00307 border_iterator border_begin();
00308 border_iterator border_end();
00309
00310 };
00311
00312
00313
00314
00315
00316
00317
00318 template<class transporter_edge>
00319 free_space_graph<transporter_edge>::
00320 free_space_graph()
00321
00322 {
00323 m=0;
00324 n=0;
00325 edges = NULL;
00326 match_found = false;
00327 }
00328
00329
00330 template<class transporter_edge>
00331 free_space_graph<transporter_edge>::
00332 ~free_space_graph()
00333
00334 {
00335 clear();
00336 }
00337
00338
00339 template<class transporter_edge>
00340 free_space_graph<transporter_edge>::edge_iterator
00341 free_space_graph<transporter_edge>::edge_begin()
00342
00343 {
00344 if (m>0)
00345 return edge_iterator(0,0,horizontal,*this);
00346 else
00347
00348
00349
00350 return edge_iterator(0,0,vertical,*this);
00351 }
00352
00353
00354 template<class transporter_edge>
00355 free_space_graph<transporter_edge>::edge_iterator
00356 free_space_graph<transporter_edge>::edge_end()
00357
00358 {
00359 if (n>0)
00360 return edge_iterator(m,n,vertical,*this);
00361 else
00362
00363
00364
00365 return edge_iterator(m,n,horizontal,*this);
00366 }
00367
00368
00369
00370 template<class transporter_edge>
00371 free_space_graph<transporter_edge>::border_iterator
00372 free_space_graph<transporter_edge>::border_begin()
00373
00374 {
00375 return left_border_list.begin();
00376 }
00377
00378
00379 template<class transporter_edge>
00380 free_space_graph<transporter_edge>::border_iterator
00381 free_space_graph<transporter_edge>::border_end()
00382
00383 {
00384 return left_border_list.end();
00385 }
00386
00387
00388 template<class transporter_edge>
00389 void free_space_graph<transporter_edge>::
00390 clear()
00391
00392 {
00393
00394 if (edges!=NULL)
00395 {
00396 for (index_t i=0; i<=m; i++)
00397 {
00398 for (index_t j=0; j<=n; j++)
00399 {
00400 delete[] edges[i][j];
00401 }
00402 delete[] edges[i];
00403 }
00404 delete[] edges;
00405 edges = NULL;
00406 }
00407
00408 m = 0;
00409 n = 0;
00410 match_found = false;
00411
00412 }
00413
00414
00415 template<class transporter_edge>
00416 void free_space_graph<transporter_edge>::
00417 build(point* P,point* Q,index_t m_P,index_t n_Q,
00418 const arithmetic& eps, const arithmetic& delta)
00419
00420 {
00421
00422 clear();
00423
00424 m = m_P;
00425 n = n_Q;
00426
00427 edges = new transporter_edge**[m+1];
00428
00429 reference_points = point_pair(P[0],Q[0]);
00430
00431 for (index_t i=0; i<m; i++)
00432 {
00433 edges[i] = new transporter_edge*[n+1];
00434 for (index_t j=0; j<n; j++)
00435 {
00436 edges[i][j] = new transporter_edge[3];
00437 edges[i][j][horizontal].assign_transporter
00438 (point_pair(P[i],P[i+1]),point_pair(Q[j],Q[j]),reference_points,eps,delta);
00439 save_borders(edges[i][j][horizontal],i,j,horizontal);
00440
00441 edges[i][j][diagonal].assign_transporter
00442 (point_pair(P[i],P[i+1]),point_pair(Q[j],Q[j+1]),reference_points,eps,delta);
00443 save_borders(edges[i][j][diagonal],i,j,diagonal);
00444
00445 edges[i][j][vertical].assign_transporter
00446 (point_pair(P[i],P[i]),point_pair(Q[j],Q[j+1]),reference_points,eps,delta);
00447 save_borders(edges[i][j][vertical],i,j,vertical);
00448 }
00449 edges[i][n] = new transporter_edge[3];
00450 edges[i][n][horizontal].assign_transporter
00451 (point_pair(P[i],P[i+1]),point_pair(Q[n],Q[n]),reference_points,eps,delta);
00452 save_borders(edges[i][n][horizontal],i,n,horizontal);
00453 }
00454
00455 edges[m] = new transporter_edge*[n+1];
00456 for (index_t j=0; j<n; j++)
00457 {
00458 edges[m][j] = new transporter_edge[3];
00459 edges[m][j][vertical].assign_transporter
00460 (point_pair(P[m],P[m]),point_pair(Q[j],Q[j+1]),reference_points,eps,delta);
00461 save_borders(edges[m][j][vertical],m,j,vertical);
00462
00463 }
00464 edges[m][n] = new transporter_edge[3];
00465
00466 }
00467
00468
00469 template<class transporter_edge>
00470 void free_space_graph<transporter_edge>::
00471 save_borders(transporter_edge& e, index_t col, index_t row, graph_direction dir)
00472
00473 {
00474 transporter_edge::border_iterator b_it = e.left_borders_begin();
00475 transporter_edge::border_iterator b_end = e.left_borders_end();
00476
00477 for (; b_it!=b_end; b_it++)
00478 {
00479 left_border_list.push_back(interval_border(*b_it,LEFT_BORDER,col,row,dir));
00480 }
00481 }
00482
00483
00484 template<class transporter_edge>
00485 bool free_space_graph<transporter_edge>::
00486 update(const free_space_graph<transporter_edge>::interval_border& ib)
00487
00488 {
00489
00490 bool* last_column = new bool[n+1];
00491 bool* curr_column = new bool[n+1];
00492
00493 bool path_found;
00494 match_found = false;
00495
00496 bool b_v, b_h, b_d;
00497
00498
00499 last_column[0] = true;
00500 curr_column[0] = true;
00501 for (index_t j=1; j<=n; j++)
00502 {
00503 b_v = edges[0][j-1][vertical].contains(ib.val());
00504 last_column[j] = last_column[j-1] && b_v;
00505 }
00506
00507 curr_column[n] = last_column[n];
00508
00509
00510 path_found = true;
00511 index_t i;
00512 for (i=1; i<=m && path_found; i++)
00513 {
00514 path_found = false;
00515 curr_column[0] = false;
00516 b_h = edges[i-1][0][horizontal].contains(ib.val());
00517 if (last_column[0] && b_h)
00518 {
00519 curr_column[0] = true;
00520 path_found = true;
00521 }
00522 for (index_t j=1; j<=n; j++)
00523 {
00524 curr_column[j] = false;
00525
00526
00527 b_d = edges[i-1][j-1][diagonal].contains(ib.val());
00528 if (last_column[j-1] && b_d)
00529 {
00530 curr_column[j] = true;
00531 path_found = true;
00532 } else {
00533 b_v = edges[i][j-1][vertical].contains(ib.val());
00534 if (curr_column[j-1] && b_v)
00535 {
00536 curr_column[j] = true;
00537 path_found = true;
00538 } else {
00539 b_h = edges[i-1][j][horizontal].contains(ib.val());
00540 if (last_column[j] && b_h)
00541 {
00542 curr_column[j] = true;
00543 path_found = true;
00544 }
00545 }
00546 }
00547 last_column[j-1] = curr_column[j-1];
00548 }
00549 last_column[n-1] = curr_column[n-1];
00550 last_column[n] = curr_column[n];
00551
00552 }
00553
00554 if (curr_column[n])
00555 {
00556 delete[] curr_column;
00557 delete[] last_column;
00558 match_found = true;
00559 match_pos = ib.val();
00560 return true;
00561 }
00562
00563 delete[] curr_column;
00564 delete[] last_column;
00565 return false;
00566 }
00567
00568
00569
00570 template<class transporter_edge>
00571 free_space_graph<transporter_edge>::motion
00572 free_space_graph<transporter_edge>::get_match()
00573
00574 {
00575
00576 if (m==0 && n==0)
00577 {
00578 transporter_edge te;
00579 te.assign_transporter
00580 (point_pair(reference_points.first,reference_points.first),
00581 point_pair(reference_points.second,reference_points.second),
00582 reference_points,1.0,1.0);
00583 return transporter_edge::compute_match(*(te.left_borders_begin()),reference_points);
00584
00585 }
00586
00587 if (match_found)
00588 {
00589 return transporter_edge::compute_match(match_pos,reference_points);
00590 }
00591 motion match_motion;
00592 return match_motion;
00593
00594 }
00595
00596
00597 template<class transporter_edge>
00598 bool free_space_graph<transporter_edge>::get_reparametrizations
00599 (free_space_graph<transporter_edge>::discrete_reparametrization_pair& kappa_lambda)
00600
00601 {
00602
00603 kappa_lambda.clear();
00604 return false;
00605
00606 }
00607
00608
00609
00610
00611 }
00612
00613 #endif