00001 #ifndef COVERING_MATCHER_H
00002 #define COVERING_MATCHER_H
00003
00004 #include "polygonal_curve.h"
00005 #include "rescutil.h"
00006 #include "rescuenumerics.h"
00007 #include "rigid_motion_interval.h"
00008 #include "point2.h"
00009
00010 namespace rescue
00011 {
00012
00013
00019
00020
00021
00022 template<class arithmetic, class interval_container>
00023 class covering_matcher
00024 {
00025
00026 public:
00027
00028 typedef unit_circle_angle<arithmetic> angle;
00029 typedef rigid_motion<arithmetic> motion;
00030 typedef rigid_motion_interval<arithmetic> motion_interval;
00031 typedef Point2<arithmetic> point;
00032 typedef polygonal_curve<point> curve;
00033 typedef curve::size_t index_t;
00034 typedef polygonal_curve<point>::vertex vertex;
00035 typedef polygonal_curve<point>::edge edge;
00036
00037 private:
00038
00039 index_t m;
00040 index_t n;
00041
00042 index_t m_prime;
00043 index_t n_prime;
00044
00045 index_t* c;
00046 index_t* d;
00047
00048 index_t* kappa;
00049 index_t* lambda;
00050
00051 point* P;
00052 point* Q;
00053 point* P_prime;
00054 point* Q_prime;
00055
00056 arithmetic eps;
00057 arithmetic delta;
00058
00059 bool matched;
00060 bool match_found;
00061
00062 public:
00063
00064 covering_matcher(curve&, curve&, const arithmetic&, const arithmetic&);
00065
00066 covering_matcher();
00067
00068 ~covering_matcher();
00069
00070 bool match();
00071
00072 bool dynamic_match();
00073
00074 rigid_motion<arithmetic> get_match();
00075
00076 private:
00077
00078 void construct_coverings();
00079
00080 motion match_result;
00081
00082 bool match(index_t i, index_t j, index_t i_prime, index_t j_prime);
00083
00084 bool dynamic_match(index_t i, index_t j, index_t i_prime, index_t j_prime);
00085
00086 bool approx_intersect(index_t);
00087
00088 bool dynamic_intersect(index_t);
00089
00090 void dynamic_delete(index_t);
00091
00092 static inline arithmetic dist(const point&, const point&);
00093
00094 public:
00095
00096 void clear();
00097
00098 private:
00099
00100 interval_container dynamic_intervals;
00101
00102 };
00103
00104
00105
00106
00107
00108
00109 template<class arithmetic, class interval_container>
00110 covering_matcher<arithmetic,interval_container>::covering_matcher()
00111
00112 {
00113
00114 m = 0;
00115 n = 0;
00116
00117 m_prime = 0;
00118 n_prime = 0;
00119
00120 c = NULL;
00121 d = NULL;
00122
00123 kappa = NULL;
00124 lambda = NULL;
00125
00126 P = NULL;
00127 Q = NULL;
00128 P_prime = NULL;
00129 Q_prime = NULL;
00130
00131 eps = 0.0;
00132 delta = 0.0;
00133
00134 }
00135
00136
00137 template<class arithmetic, class interval_container>
00138 covering_matcher<arithmetic,interval_container>::covering_matcher(curve& cP, curve& cQ, const arithmetic& ee, const arithmetic& dd)
00139
00140 {
00141
00142 eps = ee;
00143 delta = dd;
00144
00145 m = cP.length();
00146 n = cQ.length();
00147 P = new point[m+1];
00148 Q = new point[n+1];
00149
00150 curve::vertex_iterator v_it, v_end;
00151 v_it = cP.vertex_begin();
00152 v_end = cP.vertex_end();
00153 index_t i=0;
00154 for (; v_it!=v_end; v_it++)
00155 {
00156 P[i] = (*v_it).pos;
00157 i++;
00158 }
00159
00160 v_it = cQ.vertex_begin();
00161 v_end = cQ.vertex_end();
00162 index_t j=0;
00163 for (; v_it!=v_end; v_it++)
00164 {
00165 Q[j] = (*v_it).pos;
00166 j++;
00167 }
00168
00169 c = new index_t[m+1];
00170 d = new index_t[n+1];
00171
00172 kappa = new index_t[m+n+2];
00173 lambda = new index_t[m+n+2];
00174
00175 construct_coverings();
00176
00177 }
00178
00179
00180 template<class arithmetic, class interval_container>
00181 void covering_matcher<arithmetic,interval_container>::construct_coverings()
00182
00183 {
00184
00185 std::list<point> P_cp;
00186 P_cp.push_back(P[0]);
00187 c[0] = 0;
00188
00189 for (index_t i=0; i<m; i++)
00190 {
00191
00192
00193 Point P0 = P[i];
00194 Point P1 = P[i+1];
00195
00196 arithmetic length = sqrt((P0.x-P1.x)*(P0.x-P1.x)+(P0.y-P1.y)*(P0.y-P1.y));
00197 arithmetic Dx = 2.0*delta*(P1.x-P0.x)/length;
00198 arithmetic Dy = 2.0*delta*(P1.y-P0.y)/length;
00199
00200 arithmetic covered = 2.0*delta;
00201
00202 while (covered<=length)
00203 {
00204 P0.x += Dx;
00205 P0.y += Dy;
00206 P_cp.push_back(P0);
00207 covered += 2.0*delta;
00208 }
00209
00210 P_cp.push_back(P1);
00211 c[i+1] = P_cp.size()-1;
00212
00213 }
00214
00215 m_prime = P_cp.size()-1;
00216 P_prime = new point[P_cp.size()];
00217 std::list<point>::iterator p_it = P_cp.begin();
00218 i=0;
00219
00220 for (; p_it!=P_cp.end(); p_it++)
00221 {
00222 P_prime[i]=*p_it;
00223 i++;
00224 }
00225
00226 std::list<point> Q_cp;
00227 Q_cp.push_back(Q[0]);
00228 d[0] = 0;
00229
00230 for (index_t j=0; j<n; j++)
00231 {
00232
00233
00234 Point Q0 = Q[j];
00235 Point Q1 = Q[j+1];
00236
00237 arithmetic length = sqrt((Q0.x-Q1.x)*(Q0.x-Q1.x)+(Q0.y-Q1.y)*(Q0.y-Q1.y));
00238 arithmetic Dx = 2.0*delta*(Q1.x-Q0.x)/length;
00239 arithmetic Dy = 2.0*delta*(Q1.y-Q0.y)/length;
00240
00241 arithmetic covered = 2.0*delta;
00242
00243 while (covered<=length)
00244 {
00245 Q0.x += Dx;
00246 Q0.y += Dy;
00247 Q_cp.push_back(Q0);
00248 covered += 2.0*delta;
00249 }
00250
00251 Q_cp.push_back(Q1);
00252 d[j+1] = Q_cp.size()-1;
00253
00254 }
00255
00256 n_prime = Q_cp.size()-1;
00257 Q_prime = new point[Q_cp.size()];
00258 std::list<point>::iterator q_it = Q_cp.begin();
00259 j=0;
00260
00261 for (; q_it!=Q_cp.end(); q_it++)
00262 {
00263 Q_prime[j]=*q_it;
00264 j++;
00265 }
00266
00267 }
00268
00269
00270 template<class arithmetic, class interval_container>
00271 bool covering_matcher<arithmetic,interval_container>::match()
00272
00273 {
00274 return match(0,0,0,0);
00275 }
00276
00277
00278 template<class arithmetic, class interval_container>
00279 bool covering_matcher<arithmetic,interval_container>::match(index_t i, index_t j, index_t i_prime, index_t j_prime)
00280
00281 {
00282
00283 if (i==m && j==n)
00284 {
00285 kappa[i+j] = c[m];
00286 lambda[i+j] = d[n];
00287 return approx_intersect(i+j);
00288 }
00289
00290 if (i==m)
00291 {
00292 kappa[i+j] = c[m];
00293 lambda[i+j] = j_prime;
00294 return match(m,j+1,c[m],d[j+1]);
00295 }
00296
00297 if (j==n)
00298 {
00299 kappa[i+j] = i_prime;
00300 lambda[i+j] = d[n];
00301 return match(i+1,n,c[i+1],d[n]);
00302 }
00303
00304 std::list<index_t> A;
00305 std::list<index_t> B;
00306
00307 for (index_t a=i_prime; a<=c[i+1] ; a++)
00308 if (dist(P_prime[i_prime],P_prime[a]) - dist(Q_prime[j_prime],Q_prime[d[j+1]])
00309 <=2.0*(eps+delta))
00310 A.push_back(a);
00311 for (index_t b=j_prime; b<=d[j+1]; b++)
00312 if (dist(P_prime[i_prime],P_prime[c[i+1]]) - dist(Q_prime[j_prime],Q_prime[b])
00313 <=2.0*(eps+delta))
00314 B.push_back(b);
00315
00316 kappa[i+j] = i_prime;
00317 lambda[i+j] = j_prime;
00318
00319 std::list<index_t>::iterator k;
00320 for (k = A.begin(); k!=A.end(); k++)
00321 if (match(i,j+1,*k,d[j+1])) return true;
00322
00323 std::list<index_t>::iterator l;
00324 for (l = B.begin(); l!=B.end(); l++)
00325 if (match(i+1,j,c[i+1],*l)) return true;
00326
00327 return false;
00328
00329 }
00330
00331
00332
00333 template<class arithmetic, class interval_container>
00334 bool covering_matcher<arithmetic,interval_container>::approx_intersect(index_t N)
00335
00336 {
00337
00338 interval_container I;
00339
00340 for (int i=0; i<=N; i++)
00341 {
00342 std::cerr <<"("<<P_prime[kappa[i]].x<<","<<P_prime[kappa[i]].y<<")::("
00343 <<Q_prime[lambda[i]].x<<","<<Q_prime[lambda[i]].y<<")\n";
00344 I.push_back(std::pair<point,point>(P_prime[kappa[i]],Q_prime[lambda[i]]),eps+delta);
00345 }
00346
00347 bool res = I.is_empty();
00348 if (res)
00349 std::cerr<<"true\n";
00350 else
00351 std::cerr<<"false\n";
00352 std::cerr.flush();
00353
00354 match_result = I.where();
00355
00356 return res;
00357
00358 }
00359
00360
00361 template<class arithmetic, class interval_container>
00362 covering_matcher<arithmetic,interval_container>::motion covering_matcher<arithmetic,interval_container>::get_match()
00363
00364 {
00365 return match_result;
00366 }
00367
00368
00369 template<class arithmetic, class interval_container>
00370 void covering_matcher<arithmetic,interval_container>::clear()
00371
00372 {
00373 delete[] P;
00374 delete[] Q;
00375 delete[] P_prime;
00376 delete[] Q_prime;
00377 delete[] c;
00378 delete[] d;
00379 delete[] kappa;
00380 delete[] lambda;
00381 }
00382
00383
00384 template<class arithmetic, class interval_container>
00385 covering_matcher<arithmetic,interval_container>::~covering_matcher()
00386
00387 {
00388 clear();
00389 }
00390
00391
00392 template<class arithmetic, class interval_container>
00393 inline arithmetic covering_matcher<arithmetic,interval_container>::dist(const point& P, const point& Q)
00394
00395 {
00396 return sqrt((P.x-Q.x)*(P.x-Q.x)+(P.y-Q.y)*(P.y-Q.y));
00397 }
00398
00399
00400
00401
00402 }
00403
00404 #endif