00001 #ifndef RESCUE__CIRCULAR_ARC_EDGE
00002 #define RESCUE__CIRCULAR_ARC_EDGE
00003
00004 #include <list>
00005 #include <utility>
00006 #include "rigid_motion.h"
00007 #include "compact_interval_border.h"
00008 #include "angle.h"
00009
00010
00011
00012
00013 namespace rescue
00014 {
00015
00016
00017
00018
00026 template <class index_class, class point>
00027 class circular_arc_edge
00028 {
00029
00030 public:
00031
00032 typedef point::arithmetic_t arithmetic_t;
00033 typedef arithmetic_t arithmetic;
00034 typedef index_class index_t;
00035 typedef unit_circle_angle<arithmetic> angle_t;
00036 typedef angle_t border_value;
00037 typedef unit_circle_angle<arithmetic> border_type;
00038 typedef point point_t;
00039 typedef std::pair<angle_t, angle_t > circular_arc;
00040 typedef std::pair<point,point> point_pair;
00041 typedef rigid_motion<arithmetic> motion;
00042 typedef compact_interval_border<angle_t,index_t> interval_border;
00043 typedef std::list<angle_t> interval_bounds;
00044 typedef std::list<angle_t>::iterator border_iterator;
00045
00048 circular_arc_edge();
00049
00052 ~circular_arc_edge();
00053
00057 inline border_iterator left_borders_begin();
00058
00061 inline border_iterator left_borders_end();
00062
00066 inline border_iterator right_borders_begin();
00067
00070 inline border_iterator right_borders_end();
00071
00075 inline bool contains(const angle_t& phi);
00076
00081 circular_arc_edge
00082 (const point_pair& P, const point_pair& Q, const point_pair& ref_points,
00083 const arithmetic& eps, const arithmetic& delta);
00084
00089 void assign_transporter
00090 (const point_pair& P, const point_pair& Q, const point_pair& ref_points,
00091 const arithmetic& eps, const arithmetic& delta);
00092
00096 static motion compute_match(const angle_t& phi, const point_pair& ref_points);
00097
00102 static bool empty(const circular_arc&);
00103
00108 static bool full(const circular_arc&);
00109
00110 private:
00111
00115 interval_bounds left_borders;
00116
00120 interval_bounds right_borders;
00121
00122 public:
00126 static circular_arc get_angle_interval(const point_pair& pp1, const point_pair& pp2, const arithmetic& eps);
00127
00128 };
00129
00130
00131
00132
00133
00134
00135
00136 template <class index_class, class point>
00137 circular_arc_edge<index_class,point>::circular_arc_edge()
00138
00139 {
00140
00141
00142 }
00143
00144
00145 template <class index_class, class point>
00146 circular_arc_edge<index_class,point>::~circular_arc_edge()
00147
00148 {
00149 left_borders.clear();
00150 right_borders.clear();
00151 }
00152
00153
00154 template <class index_class, class point>
00155 circular_arc_edge<index_class,point>::circular_arc_edge
00156 (const point_pair& P, const point_pair& Q, const point_pair& ref_points,
00157 const arithmetic& eps, const arithmetic& delta)
00158
00159 {
00160 assign_transporter(P,Q,ref_points,eps,delta);
00161 }
00162
00163
00164 template <class index_class, class point>
00165 void circular_arc_edge<index_class,point>::assign_transporter
00166 (const point_pair& P, const point_pair& Q, const point_pair& ref_points,
00167 const arithmetic& eps, const arithmetic& delta)
00168
00169 {
00170
00171 arithmetic eps_prime = eps+delta;
00172
00173 left_borders.clear();
00174 right_borders.clear();
00175
00176 angle_t angle_zero(false);
00177 angle_t angle_2pi(true);
00178
00179 point_pair pq(P.second,Q.second);
00180 point_pair pq_pred(P.first,Q.first);
00181
00182
00183
00184 circular_arc I1 = get_angle_interval(ref_points,pq,eps_prime);
00185 circular_arc I2 = get_angle_interval(pq_pred,pq,eps_prime);
00186
00187 if (empty(I1) || empty(I2))
00188 return;
00189
00190 if (full(I1) && full(I2))
00191 {
00192 left_borders.push_back(angle_zero);
00193 right_borders.push_back(angle_2pi);
00194 return;
00195 }
00196
00197
00198 if (I1.first<=I1.second)
00199 {
00200 if (I2.first<=I2.second)
00201 {
00202 if (I2.second<I1.first || I1.second<I2.first)
00203 return;
00204 if (I1.first<=I2.first)
00205 left_borders.push_back(I2.first);
00206 else
00207 left_borders.push_back(I1.first);
00208 if (I1.second<=I2.second)
00209 right_borders.push_back(I1.second);
00210 else
00211 right_borders.push_back(I2.second);
00212 } else {
00213 if (I2.first<=I1.second)
00214 {
00215 left_borders.push_back(I2.first);
00216 right_borders.push_back(I1.second);
00217 }
00218 if (I1.first<=I2.second)
00219 {
00220 left_borders.push_back(I1.first);
00221 right_borders.push_back(I2.second);
00222 }
00223 }
00224 } else {
00225 if (I2.first<=I2.second)
00226 {
00227 if (I1.first<=I2.second)
00228 {
00229 left_borders.push_back(I1.first);
00230 right_borders.push_back(I2.second);
00231 }
00232 if (I2.first<=I1.second)
00233 {
00234 left_borders.push_back(I2.first);
00235 right_borders.push_back(I1.second);
00236 }
00237 } else {
00238 if (I1.first<=I2.second)
00239 {
00240 left_borders.push_back(I1.first);
00241 right_borders.push_back(I2.second);
00242 } else if (I2.first<=I1.second)
00243 {
00244 left_borders.push_back(I2.first);
00245 right_borders.push_back(I1.second);
00246 }
00247 if (I1.first<=I2.first)
00248 {
00249 left_borders.push_back(I2.first);
00250 right_borders.push_back(angle_2pi);
00251 } else {
00252 left_borders.push_back(I1.first);
00253 right_borders.push_back(angle_2pi);
00254 }
00255 if (I1.second<=I2.second)
00256 {
00257 left_borders.push_back(angle_zero);
00258 right_borders.push_back(I1.second);
00259 } else {
00260 left_borders.push_back(angle_zero);
00261 right_borders.push_back(I2.second);
00262 }
00263 }
00264 }
00265
00266 }
00267
00268
00269
00270 template <class index_class, class point>
00271 circular_arc_edge<index_class,point>::circular_arc circular_arc_edge<index_class,point>::
00272 get_angle_interval(const point_pair& pp1, const point_pair& pp2, const arithmetic& eps)
00273
00274 {
00275
00276 point p, q, S0, S1;
00277
00278 arithmetic tp_x = (pp1.first.x+pp2.first.x) / 2.0;
00279 arithmetic tp_y = (pp1.first.y+pp2.first.y) / 2.0;
00280 arithmetic tq_x = (pp1.second.x+pp2.second.x) / 2.0;
00281 arithmetic tq_y = (pp1.second.y+pp2.second.y) / 2.0;
00282
00283 p.x = pp2.first.x-tp_x;
00284 p.y = pp2.first.y-tp_y;
00285 q.x = pp2.second.x-tq_x;
00286 q.y = pp2.second.y-tq_y;
00287
00288 arithmetic l_p_sq = p.x*p.x+p.y*p.y;
00289 arithmetic l_q_sq = q.x*q.x+q.y*q.y;
00290 arithmetic l_p = sqrt(l_p_sq);
00291 arithmetic l_q = sqrt(l_q_sq);
00292
00293 if (eps>l_p+l_q)
00294
00295 return circular_arc(angle_t(false),angle_t(true));
00296 if (l_p==0.0)
00297 if (l_q<=eps)
00298
00299 return circular_arc(angle_t(false),angle_t(true));
00300 else
00301
00302 return circular_arc(angle_t(true),angle_t(false));
00303 if (l_q==0.0)
00304 if (l_p<=eps)
00305
00306 return circular_arc(angle_t(false),angle_t(true));
00307 else
00308
00309 return circular_arc(angle_t(true),angle_t(false));
00310 arithmetic diff = (l_p-l_q);
00311 if (diff>eps || (-diff)>eps)
00312
00313 return circular_arc(angle_t(true),angle_t(false));
00314
00315 arithmetic cos_phi = (l_p_sq+l_q_sq-eps*eps)/(2.0*l_p*l_q);
00316 arithmetic sin_phi = sqrt(1-cos_phi*cos_phi);
00317
00318 angle_t phi_0(sin_phi,cos_phi);
00319 angle_t phi_1((-1.0)*sin_phi,cos_phi);
00320
00321 angle_t alpha(p.y/l_p,p.x/l_p);
00322 angle_t minus_beta((-1.0)*q.y/l_q,q.x/l_q);
00323
00324 angle_t theta_0, theta_1;
00325
00326 theta_0 = alpha;
00327 theta_0 += minus_beta;
00328 theta_0 += phi_0;
00329
00330 theta_1 = alpha;
00331 theta_1 += minus_beta;
00332 theta_1 += phi_1;
00333
00334 return circular_arc(theta_1,theta_0);
00335
00336 }
00337
00338
00339 template <class index_class, class point>
00340 circular_arc_edge<index_class,point>::border_iterator circular_arc_edge<index_class,point>::left_borders_begin()
00341
00342 {
00343 return left_borders.begin();
00344 }
00345
00346
00347 template <class index_class, class point>
00348 circular_arc_edge<index_class,point>::border_iterator circular_arc_edge<index_class,point>::left_borders_end()
00349
00350 {
00351 return left_borders.end();
00352 }
00353
00354
00355 template <class index_class, class point>
00356 circular_arc_edge<index_class,point>::border_iterator circular_arc_edge<index_class,point>::right_borders_begin()
00357
00358 {
00359 return right_borders.begin();
00360 }
00361
00362
00363 template <class index_class, class point>
00364 circular_arc_edge<index_class,point>::border_iterator circular_arc_edge<index_class,point>::right_borders_end()
00365
00366 {
00367 return right_borders.end();
00368 }
00369
00370
00371 template <class index_class, class point>
00372 inline bool circular_arc_edge<index_class,point>::contains(const angle_t& phi)
00373
00374 {
00375
00376 border_iterator l_it = left_borders.begin();
00377 border_iterator l_end = left_borders.end();
00378 border_iterator r_it = right_borders.begin();
00379 border_iterator r_end = right_borders.end();
00380
00381 for (; l_it!=l_end; l_it++, r_it++)
00382 {
00383 if (*l_it<=phi && phi<=*r_it)
00384 {
00385 return true;
00386 }
00387 }
00388 return false;
00389
00390 }
00391
00392
00393 template <class index_class, class point>
00394 bool circular_arc_edge<index_class,point>::full(const circular_arc_edge<index_class,point>::circular_arc& I)
00395
00396 {
00397 if (I.first==angle_t(false) && I.second==angle_t(true))
00398 return true;
00399 return false;
00400 }
00401
00402
00403 template <class index_class, class point>
00404 bool circular_arc_edge<index_class,point>::empty(const circular_arc_edge<index_class,point>::circular_arc& I)
00405
00406 {
00407 if (I.first==angle_t(true) && I.second==angle_t(false))
00408 return true;
00409 return false;
00410 }
00411
00412
00413 template <class index_class, class point>
00414 circular_arc_edge<index_class,point>::motion circular_arc_edge<index_class,point>::compute_match
00415 (const circular_arc_edge<index_class,point>::angle_t& phi, const circular_arc_edge<index_class,point>::point_pair& ref_points)
00416
00417 {
00418
00419 motion rho_phi = motion(phi);
00420
00421 point pp = ref_points.first;
00422 point qq = ref_points.second;
00423 qq *= rho_phi;
00424
00425 arithmetic t_x = pp.x-qq.x;
00426 arithmetic t_y = pp.y-qq.y;
00427
00428 motion there(t_x,t_y);
00429 there *= rho_phi;
00430
00431 return there;
00432 }
00433
00434
00435
00436
00437 }
00438
00439 #endif