00001
00002 #ifndef vgl_homg_operators_2d_txx_
00003 #define vgl_homg_operators_2d_txx_
00004
00005
00006
00007 #include "vgl_homg_operators_2d.h"
00008
00009 #include <vcl_iostream.h>
00010 #include <vcl_limits.h>
00011 #include <vcl_cassert.h>
00012 #include <vcl_cmath.h>
00013 #include <vgl/vgl_homg_line_2d.h>
00014 #include <vgl/vgl_homg_point_2d.h>
00015 #include <vgl/vgl_point_2d.h>
00016 #include <vgl/vgl_conic.h>
00017 #include <vgl/vgl_box_2d.h>
00018 #include <vgl/vgl_homg.h>
00019
00020 #include <vnl/vnl_vector_fixed.h>
00021 #include <vnl/vnl_matrix.h>
00022 #include <vnl/vnl_matrix_fixed.h>
00023 #include <vnl/vnl_math.h>
00024 #include <vnl/algo/vnl_scatter_3x3.h>
00025 #include <vnl/algo/vnl_real_eigensystem.h>
00026 #include <vnl/vnl_diag_matrix.h>
00027
00028
00029
00030 template <class T>
00031 inline static vgl_homg_line_2d<T> cross(const vgl_homg_point_2d<T>& p1,
00032 const vgl_homg_point_2d<T>& p2)
00033 {
00034 return vgl_homg_line_2d<T> (p1.y() * p2.w() - p1.w() * p2.y(),
00035 p1.w() * p2.x() - p1.x() * p2.w(),
00036 p1.x() * p2.y() - p1.y() * p2.x());
00037 }
00038
00039 template <class T>
00040 inline static vgl_homg_point_2d<T> cross(const vgl_homg_line_2d<T>& l1,
00041 const vgl_homg_line_2d<T>& l2)
00042 {
00043 return vgl_homg_point_2d<T> (l1.b() * l2.c() - l1.c() * l2.b(),
00044 l1.c() * l2.a() - l1.a() * l2.c(),
00045 l1.a() * l2.b() - l1.b() * l2.a());
00046 }
00047
00048 template <class T>
00049 inline static T dot(vgl_homg_line_2d<T> const& l,
00050 vgl_homg_point_2d<T> const& p)
00051 {
00052 return l.a()*p.x() + l.b()*p.y() + l.c()*p.w();
00053 }
00054
00055
00056
00057 template <class T>
00058 vnl_vector_fixed<T,3> vgl_homg_operators_2d<T>::get_vector(vgl_homg_point_2d<T> const& p)
00059 {
00060 return vnl_vector_fixed<T,3>(p.x(),p.y(),p.w());
00061 }
00062
00063 template <class T>
00064 vnl_vector_fixed<T,3> vgl_homg_operators_2d<T>::get_vector(vgl_homg_line_2d<T> const& l)
00065 {
00066 return vnl_vector_fixed<T,3>(l.a(),l.b(),l.c());
00067 }
00068
00069 template <class T>
00070 vnl_vector_fixed<T,6> vgl_homg_operators_2d<T>::get_vector(vgl_conic<T> const& c)
00071 {
00072 vnl_vector_fixed<T,6> v;
00073 v.put(0,c.a());
00074 v.put(1,c.b());
00075 v.put(2,c.c());
00076 v.put(3,c.d());
00077 v.put(4,c.e());
00078 v.put(5,c.f());
00079
00080 return v;
00081 }
00082
00083
00084
00085 template <class T>
00086 void vgl_homg_operators_2d<T>::unitize(vgl_homg_point_2d<T>& a)
00087 {
00088 double norm = vcl_sqrt (a.x()*a.x() + a.y()*a.y() + a.w()*a.w());
00089
00090 if (norm == 0.0) {
00091 vcl_cerr << "vgl_homg_operators_2d<T>::unitize() -- Zero length vector\n";
00092 return;
00093 }
00094 norm = 1.0/norm;
00095 a.set(T(a.x()*norm), T(a.y()*norm), T(a.w()*norm));
00096 }
00097
00098
00099
00100 template <class T>
00101 T
00102 vgl_homg_operators_2d<T>::distance(const vgl_homg_point_2d<T>& point1,
00103 const vgl_homg_point_2d<T>& point2)
00104 {
00105 return vcl_sqrt(vgl_homg_operators_2d<T>::distance_squared(point1,point2));
00106 }
00107
00108 template <class T>
00109 T
00110 vgl_homg_operators_2d<T>::distance_squared(const vgl_homg_point_2d<T>& p1,
00111 const vgl_homg_point_2d<T>& p2)
00112 {
00113 if (p1 == p2) return T(0);
00114
00115 if (p1.w() == 0 || p2.w() == 0) {
00116 vcl_cerr << "vgl_homg_operators_2d<T>::distance_squared() -- point at infinity\n";
00117 return vcl_numeric_limits<T>::infinity();
00118 }
00119
00120 return vnl_math_sqr (p1.x() / p1.w() - p2.x() / p2.w()) +
00121 vnl_math_sqr (p1.y() / p1.w() - p2.y() / p2.w());
00122 }
00123
00124
00125
00126
00127
00128
00129
00130 template <class T>
00131 T
00132 vgl_homg_operators_2d<T>::perp_dist_squared(const vgl_homg_point_2d<T>& point,
00133 const vgl_homg_line_2d<T>& line)
00134 {
00135 if ((line.a()==0 && line.b()== 0) || point.w()==0) {
00136 vcl_cerr << "vgl_homg_operators_2d<T>::perp_dist_squared() -- line or point at infinity\n";
00137 return vgl_homg<T>::infinity;
00138 }
00139
00140 T numerator = vnl_math_sqr (dot(line,point) / point.w());
00141 if (numerator == 0) return T(0);
00142 T denominator = line.a()*line.a() + line.b()*line.b();
00143
00144 return numerator / denominator;
00145 }
00146
00147
00148
00149
00150
00151 template <class T>
00152 double
00153 vgl_homg_operators_2d<T>::line_angle(const vgl_homg_line_2d<T>& line)
00154 {
00155 return vcl_atan2 (line.b(), line.a());
00156 }
00157
00158
00159
00160 template <class T>
00161 double
00162 vgl_homg_operators_2d<T>::abs_angle(const vgl_homg_line_2d<T>& line1,
00163 const vgl_homg_line_2d<T>& line2)
00164 {
00165 double angle1 = line_angle(line1);
00166 double angle2 = line_angle(line2);
00167
00168 double diff = angle2 - angle1;
00169 if (diff > vnl_math::pi_over_2) diff -= vnl_math::pi;
00170 if (diff < -vnl_math::pi_over_2) diff += vnl_math::pi;
00171
00172 return vcl_fabs(diff);
00173 }
00174
00175
00176
00177
00178
00179
00180
00181
00182
00183 template <class T>
00184 double
00185 vgl_homg_operators_2d<T>::angle_between_oriented_lines(const vgl_homg_line_2d<T>& line1,
00186 const vgl_homg_line_2d<T>& line2)
00187 {
00188 return vcl_fmod(line_angle(line2)-line_angle(line1), 2*vnl_math::pi);
00189 }
00190
00191
00192
00193
00194
00195
00196
00197
00198 template <class T>
00199 vgl_homg_line_2d<T>
00200 vgl_homg_operators_2d<T>::join(const vgl_homg_point_2d<T>& p1,
00201 const vgl_homg_point_2d<T>& p2)
00202 {
00203 return cross(p1,p2);
00204 }
00205
00206
00207
00208
00209
00210
00211 template <class T>
00212 vgl_homg_line_2d<T>
00213 vgl_homg_operators_2d<T>::join_oriented(const vgl_homg_point_2d<T>&p1,
00214 const vgl_homg_point_2d<T>&p2)
00215 {
00216 int s1 = p1.w() < 0;
00217 int s2 = p2.w() < 0;
00218
00219 if (s1 ^ s2)
00220 return cross(p2,p1);
00221 else
00222 return cross(p1,p2);
00223 }
00224
00225
00226
00227
00228 template <class T>
00229 vgl_homg_point_2d<T>
00230 vgl_homg_operators_2d<T>::intersection(const vgl_homg_line_2d<T>& l1,
00231 const vgl_homg_line_2d<T>& l2)
00232 {
00233 return cross(l1,l2);
00234 }
00235
00236
00237
00238
00239
00240
00241
00242
00243
00244 template <class T>
00245 vgl_homg_line_2d<T>
00246 vgl_homg_operators_2d<T>::perp_line_through_point(const vgl_homg_line_2d<T>& l,
00247 const vgl_homg_point_2d<T>& p)
00248 {
00249 vgl_homg_point_2d<T> d(l.a(), l.b(), 0);
00250 return cross(d, p);
00251 }
00252
00253
00254
00255
00256 template <class T>
00257 vgl_homg_point_2d<T>
00258 vgl_homg_operators_2d<T>::perp_projection(const vgl_homg_line_2d<T>& line,
00259 const vgl_homg_point_2d<T>& point)
00260 {
00261 vgl_homg_line_2d<T> perpline = perp_line_through_point (line, point);
00262 vgl_homg_point_2d<T> answer = cross(line, perpline);
00263 return answer;
00264 }
00265
00266
00267
00268
00269 template <class T>
00270 vgl_homg_point_2d<T>
00271 vgl_homg_operators_2d<T>::midpoint(const vgl_homg_point_2d<T>& p1,
00272 const vgl_homg_point_2d<T>& p2)
00273 {
00274 T x = p1.x() * p2.w() + p2.x() * p1.w();
00275 T y = p1.y() * p2.w() + p2.y() * p1.w();
00276 T w = p1.w() * p2.w() + p2.w() * p1.w();
00277
00278 return vgl_homg_point_2d<T>(x,y,w);
00279 }
00280
00281
00282
00283
00284 template <class T>
00285 vnl_vector_fixed<T,3>
00286 vgl_homg_operators_2d<T>::most_orthogonal_vector(const vcl_vector<vgl_homg_line_2d<T> >& inpoints)
00287 {
00288 vnl_scatter_3x3<T> scatter_matrix;
00289
00290 for (typename vcl_vector<vgl_homg_line_2d<T> >::const_iterator i = inpoints.begin();
00291 i != inpoints.end(); ++i)
00292 scatter_matrix.add_outer_product(get_vector(*i));
00293
00294 return scatter_matrix.minimum_eigenvector();
00295 }
00296
00297 #include <vnl/algo/vnl_svd.h>
00298
00299 template <class T>
00300 vnl_vector_fixed<T,3>
00301 vgl_homg_operators_2d<T>::most_orthogonal_vector_svd(const vcl_vector<vgl_homg_line_2d<T> >& lines)
00302 {
00303 vnl_matrix<T> D(lines.size(), 3);
00304
00305 typename vcl_vector<vgl_homg_line_2d<T> >::const_iterator i = lines.begin();
00306 for (unsigned j = 0; i != lines.end(); ++i,++j)
00307 D.set_row(j, get_vector(*i).as_ref());
00308
00309 vnl_svd<T> svd(D);
00310 #ifdef DEBUG
00311 vcl_cout << "[movrank " << svd.W() << ']';
00312 #endif
00313
00314 return svd.nullvector();
00315 }
00316
00317
00318
00319
00320
00321
00322
00323 template <class T>
00324 vgl_homg_point_2d<T>
00325 vgl_homg_operators_2d<T>::lines_to_point(const vcl_vector<vgl_homg_line_2d<T> >& lines)
00326 {
00327
00328 assert(lines.size() >= 2);
00329
00330 #ifdef VGL_HOMG_OPERATORS_2D_LINES_TO_POINT_USE_SVD
00331 vnl_vector_fixed<T,3> mov = most_orthogonal_vector_svd(lines);
00332 #else
00333 vnl_vector_fixed<T,3> mov = most_orthogonal_vector(lines);
00334 #endif
00335 return vgl_homg_point_2d<T>(mov[0], mov[1], mov[2]);
00336 }
00337
00338
00339
00340
00341
00342
00343
00344
00345
00346
00347
00348
00349
00350
00351
00352
00353
00354
00355
00356
00357
00358 template <class T>
00359 double vgl_homg_operators_2d<T>::cross_ratio(const vgl_homg_point_2d<T>& a,
00360 const vgl_homg_point_2d<T>& b,
00361 const vgl_homg_point_2d<T>& c,
00362 const vgl_homg_point_2d<T>& d)
00363 {
00364 double x1 = a.x(), y1 = a.y(), w1 = a.w();
00365 double x2 = b.x(), y2 = b.y(), w2 = b.w();
00366 double x3 = c.x(), y3 = c.y(), w3 = c.w();
00367 double x4 = d.x(), y4 = d.y(), w4 = d.w();
00368 double x = x1 - x2; if (x<0) x = -x;
00369 double y = y1 - y2; if (y<0) y = -y;
00370 double n = (x>y) ? (x1*w3-x3*w1)*(x2*w4-x4*w2) : (y1*w3-y3*w1)*(y2*w4-y4*w2);
00371 double m = (x>y) ? (x1*w4-x4*w1)*(x2*w3-x3*w2) : (y1*w4-y4*w1)*(y2*w3-y3*w2);
00372 if (n == 0 && m == 0)
00373 vcl_cerr << "cross ratio not defined: three of the given points coincide\n";
00374 return n/m;
00375 }
00376
00377
00378
00379
00380 template <class T>
00381 vgl_homg_point_2d<T>
00382 vgl_homg_operators_2d<T>::conjugate(const vgl_homg_point_2d<T>& a,
00383 const vgl_homg_point_2d<T>& b,
00384 const vgl_homg_point_2d<T>& c,
00385 double cr)
00386
00387 {
00388 T x1 = a.x(), y1 = a.y(), w1 = a.w();
00389 T x2 = b.x(), y2 = b.y(), w2 = b.w();
00390 T x3 = c.x(), y3 = c.y(), w3 = c.w();
00391 T kx = x1*w3 - x3*w1, mx = x2*w3 - x3*w2, nx = T(kx*w2-cr*mx*w1);
00392 T ky = y1*w3 - y3*w1, my = y2*w3 - y3*w2, ny = T(ky*w2-cr*my*w1);
00393 return vgl_homg_point_2d<T>(T(x2*kx-cr*x1*mx)*ny,T(y2*ky-cr*y1*my)*nx,nx*ny);
00394 }
00395
00396
00397
00398
00399
00400
00401
00402 template <class T>
00403 vgl_conic<T>
00404 vgl_homg_operators_2d<T>::vgl_conic_from_matrix(vnl_matrix_fixed<T,3,3> const& mat)
00405 {
00406 return vgl_conic<T>(mat[0][0], mat[1][0]+mat[0][1], mat[1][1], mat[0][2]+mat[2][0], mat[1][2]+mat[2][1], mat[2][2]);
00407 }
00408
00409
00410
00411
00412
00413
00414
00415
00416 template <class T>
00417 vnl_matrix_fixed<T,3,3>
00418 vgl_homg_operators_2d<T>::matrix_from_conic(vgl_conic<T> const& c)
00419 {
00420 vnl_matrix_fixed<T,3,3> mat;
00421 T A = c.a(), B = c.b()/2, C = c.c(), D = c.d()/2, E = c.e()/2, F = c.f();
00422
00423 mat[0][0] = A; mat[0][1] = B; mat[0][2] = D;
00424 mat[1][0] = B; mat[1][1] = C; mat[1][2] = E;
00425 mat[2][0] = D; mat[2][1] = E; mat[2][2] = F;
00426
00427 return mat;
00428 }
00429
00430
00431
00432
00433
00434 template <class T>
00435 vnl_matrix_fixed<T,3,3>
00436 vgl_homg_operators_2d<T>::matrix_from_dual_conic(vgl_conic<T> const& c)
00437 {
00438 vnl_matrix_fixed<T,3,3> mat;
00439 T A = c.a(), B = c.b()/2, C = c.c(), D = c.d()/2, E = c.e()/2, F = c.f();
00440
00441 mat[0][0] = C*F-E*E; mat[0][1] = E*D-B*F; mat[0][2] = B*E-C*D;
00442 mat[1][0] = E*D-B*F; mat[1][1] = A*F-D*D; mat[1][2] = B*D-A*E;
00443 mat[2][0] = B*E-C*D; mat[2][1] = B*D-A*E; mat[2][2] = A*C-B*B;
00444
00445 return mat;
00446 }
00447
00448
00449
00450
00451 template <class T>
00452 vcl_list<vgl_homg_point_2d<T> >
00453 vgl_homg_operators_2d<T>::do_intersect(vgl_conic<T> const& c1,
00454 vgl_conic<T> const& c2)
00455 {
00456 if (c1==c2)
00457 {
00458 vcl_cerr << __FILE__
00459 << "Warning: the intersection of two identical conics is not a finite set of points.\n"
00460 << "Returning an empty list.\n";
00461 return vcl_list<vgl_homg_point_2d<T> >();
00462 }
00463 T A=c1.a(),B=c1.b(),C=c1.c(),D=c1.d(),E=c1.e(),F=c1.f();
00464 T a=c2.a(),b=c2.b(),c=c2.c(),d=c2.d(),e=c2.e(),f=c2.f();
00465
00466 T ab=a*B-A*b, ac=a*C-A*c, ad=a*D-A*d, ae=a*E-A*e, af=a*F-A*f, BD=b*D+B*d;
00467
00468
00469
00470
00471 if ((ab==0 && ac==0)
00472
00473
00474 || (ab==0 && ad==0)
00475
00476 || (ab==0 && ae==0)
00477
00478
00479
00480
00481 || (ac*ad*ad+af*ab*ab == ab*ad*ae))
00482 return intersection(vgl_conic<T>(0,ab,ac,ad,ae,af),c1);
00483
00484
00485 vnl_vector_fixed<T,5> coef;
00486 coef(0) = ac*ac-ab*(b*C-B*c);
00487 coef(1) = 2*ac*ae-ab*(b*E-B*e)-BD*(a*C+A*c)+2*A*b*C*d+2*a*B*c*D;
00488 coef(2) = ae*ae-ab*(b*F-B*f)+ad*(c*D-C*d)-BD*(a*E+A*e)+2*a*B*e*D+2*A*b*E*d+2*ac*af;
00489 coef(3) = 2*ae*af-ad*(d*E-D*e)-BD*(a*F+A*f)+2*A*b*d*F+2*a*B*D*f;
00490 coef(4) = af*af-ad*(d*F-D*f);
00491
00492
00493
00494
00495
00496
00497
00498
00499
00500
00501 if (coef(0) == 0 && coef(1) == 0) {
00502 if (coef(2) == 0 && coef(3) == 0) return vcl_list<vgl_homg_point_2d<T> >();
00503 T dis = coef(3)*coef(3)-4*coef(2)*coef(4);
00504 if (dis < 0) return vcl_list<vgl_homg_point_2d<T> >();
00505 T y;
00506 if (coef(2) == 0) dis=0, y = - coef(4) / coef(3);
00507 else y = - coef(3) / coef(2) / 2;
00508 T w = y*ab+ad;
00509 T x = -(y*y*ac+y*ae+af);
00510 if (x == 0 && w == 0) x = w = 1;
00511 if (dis == 0) {return vcl_list<vgl_homg_point_2d<T> >(2,vgl_homg_point_2d<T>(x,y*w,w));}
00512 dis = vcl_sqrt(dis) / coef(2) / 2;
00513 vcl_list<vgl_homg_point_2d<T> > solutions;
00514 y -= dis; w = y*ab+ad; x = -(y*y*ac+y*ae+af);
00515 if (x == 0 && w == 0) x = w = 1;
00516 solutions.push_back(vgl_homg_point_2d<T>(x,y*w,w));
00517 y += 2*dis; w = y*ab+ad; x = -(y*y*ac+y*ae+af);
00518 if (x == 0 && w == 0) x = w = 1;
00519 solutions.push_back(vgl_homg_point_2d<T>(x,y*w,w));
00520 return solutions;
00521 }
00522 if (coef(0) == 0) {
00523 coef /= -coef(1);
00524 double data[]={coef(2),coef(3),coef(4), 1,0,0, 0,1,0};
00525 vnl_matrix<double> M(data,3,3);
00526 vnl_real_eigensystem eig(M);
00527 vnl_diag_matrix<vcl_complex<double> > polysolutions = eig.D;
00528 vcl_list<vgl_homg_point_2d<T> > solutions;
00529 for (int i=0;i<3;++i)
00530 if (vcl_abs(vcl_imag(polysolutions(i))) < 1e-3) {
00531 T y = (T)vcl_real(polysolutions(i));
00532 T w = y*ab+ad;
00533 T x = -(y*y*ac+y*ae+af);
00534 if (x == 0 && w == 0) x = w = 1;
00535 solutions.push_back(vgl_homg_point_2d<T>(x,y*w,w));
00536 }
00537 return solutions;
00538 }
00539
00540 coef /= -coef(0);
00541 double data[]={coef(1),coef(2),coef(3),coef(4), 1,0,0,0, 0,1,0,0, 0,0,1,0};
00542 vnl_matrix<double> M(data,4,4);
00543 vnl_real_eigensystem eig(M);
00544
00545 vnl_diag_matrix<vcl_complex<double> > polysolutions = eig.D;
00546
00547
00548 vcl_list<T> yvals;
00549 for (int i=0;i<4;++i)
00550 if (vcl_abs(vcl_imag(polysolutions(i))) < 1e-7)
00551 yvals.push_back((T)vcl_real(polysolutions(i)));
00552
00553
00554
00555
00556 vcl_list<vgl_homg_point_2d<T> > solutions;
00557
00558
00559 typename vcl_list<T>::const_iterator it = yvals.begin();
00560 #if 0
00561 if (yvals(0) == yvals(1) || yvals(1) == yvals(2) || yvals(2) == yvals(3))
00562 {
00563 }
00564 #endif
00565
00566 for (it = yvals.begin(); it != yvals.end(); ++it) {
00567 T y = *it;
00568 T w = y*ab+ad;
00569 T x = -(y*y*ac+y*ae+af);
00570 if (x == 0 && w == 0) x = w = 1;
00571 solutions.push_back(vgl_homg_point_2d<T>(x,y*w,w));
00572 }
00573 return solutions;
00574 }
00575
00576
00577
00578
00579 template <class T>
00580 vcl_list<vgl_homg_point_2d<T> >
00581 vgl_homg_operators_2d<T>::do_intersect(vgl_conic<T> const& q,
00582 vgl_homg_line_2d<T> const& l)
00583 {
00584 T A=q.a(), B=q.b(), C=q.c(), D=q.d(), E=q.e(), F=q.f();
00585 T a=l.a(), b=l.b(), c=l.c();
00586
00587 if (a==0 && b==0) {
00588 if (A==0)
00589 return vcl_list<vgl_homg_point_2d<T> >(2,vgl_homg_point_2d<T>(1,0,0));
00590 T d = B*B-4*A*C;
00591 if (d < 0) return vcl_list<vgl_homg_point_2d<T> >();
00592 if (d == 0)
00593 return vcl_list<vgl_homg_point_2d<T> >(2,vgl_homg_point_2d<T>(-B,2*A,0));
00594 d = vcl_sqrt(d);
00595 vcl_list<vgl_homg_point_2d<T> > v(1, vgl_homg_point_2d<T>(-B+d, 2*A, 0));
00596 v.push_back(vgl_homg_point_2d<T>(-B-d, 2*A, 0));
00597 return v;
00598 }
00599 if (a==0) {
00600 T y = -c/b; B = B*y+D;
00601 T d = B*B-4*A*(C*y*y+E*y+F);
00602 if (d < 0) return vcl_list<vgl_homg_point_2d<T> >();
00603 if (d == 0 && A == 0)
00604 return vcl_list<vgl_homg_point_2d<T> >(2,vgl_homg_point_2d<T>(1,0,0));
00605 if (d == 0)
00606 return vcl_list<vgl_homg_point_2d<T> >(2,vgl_homg_point_2d<T>(-B,y*2*A,2*A));
00607 if (A == 0) {
00608 vcl_list<vgl_homg_point_2d<T> > v(1, vgl_homg_point_2d<T>(1,0,0));
00609 v.push_back(vgl_homg_point_2d<T>(C*y*y+E*y+F, -y*B, -B));
00610 return v;
00611 }
00612 d = vcl_sqrt(d);
00613 vcl_list<vgl_homg_point_2d<T> > v(1, vgl_homg_point_2d<T>(-B+d, y*2*A, 2*A));
00614 v.push_back(vgl_homg_point_2d<T>(-B-d, y*2*A, 2*A));
00615 return v;
00616 }
00617 b /= -a; c /= -a;
00618 T AA = A*b*b+B*b+C;
00619 B = 2*A*b*c+B*c+D*b+E;
00620 T d = B*B-4*AA*(A*c*c+D*c+F);
00621 if (d < 0) return vcl_list<vgl_homg_point_2d<T> >();
00622 if (d == 0 && AA == 0)
00623 return vcl_list<vgl_homg_point_2d<T> >(2,vgl_homg_point_2d<T>(b,1,0));
00624 if (d == 0)
00625 return vcl_list<vgl_homg_point_2d<T> >(2,vgl_homg_point_2d<T>(c*2*AA-b*B,-B,2*AA));
00626 if (AA == 0) {
00627 vcl_list<vgl_homg_point_2d<T> > v(1, vgl_homg_point_2d<T>(b,1,0));
00628 v.push_back(vgl_homg_point_2d<T>(b*A*c*c+b*D*c+b*F-c*B, A*c*c+D*c+F, -B));
00629 return v;
00630 }
00631 d = vcl_sqrt(d);
00632 vcl_list<vgl_homg_point_2d<T> > v(1, vgl_homg_point_2d<T>(c*2*AA-b*B+b*d, -B+d, 2*AA));
00633 v.push_back(vgl_homg_point_2d<T>(c*2*AA-b*B-b*d, -B-d, 2*AA));
00634 return v;
00635 }
00636
00637
00638 template <class T>
00639 vcl_list<vgl_homg_point_2d<T> >
00640 vgl_homg_operators_2d<T>::intersection(vgl_conic<T> const& c,
00641 vgl_homg_line_2d<T> const& l)
00642 {
00643 if (c.type()==vgl_conic<T>::no_type ||
00644 c.type()==vgl_conic<T>::complex_parallel_lines ||
00645 c.type()==vgl_conic<T>::complex_intersecting_lines ||
00646 c.type()==vgl_conic<T>::imaginary_ellipse ||
00647 c.type()==vgl_conic<T>::imaginary_circle)
00648 return vcl_list<vgl_homg_point_2d<T> >();
00649
00650
00651 if (c.type() == vgl_conic<T>::coincident_lines) {
00652 vgl_homg_point_2d<T> p = intersection(l, c.components().front());
00653 vcl_list<vgl_homg_point_2d<T> > list(2, p);
00654 return list;
00655 }
00656
00657 if (c.type() == vgl_conic<T>::real_intersecting_lines || c.type() == vgl_conic<T>::real_parallel_lines) {
00658 vcl_list<vgl_homg_point_2d<T> > list;
00659 list.push_back(intersection(l, c.components().front()));
00660 list.push_back(intersection(l, c.components().back()));
00661 return list;
00662 }
00663 return do_intersect(c, l);
00664 }
00665
00666 template <class T>
00667 vcl_list<vgl_homg_point_2d<T> >
00668 vgl_homg_operators_2d<T>::intersection(vgl_conic<T> const& c1, vgl_conic<T> const& c2)
00669 {
00670 if ((c1.type()==vgl_conic<T>::complex_parallel_lines ||
00671 c1.type()==vgl_conic<T>::complex_intersecting_lines)
00672 && c2.contains(c1.centre()))
00673 return vcl_list<vgl_homg_point_2d<T> >(2, c1.centre());
00674 if ((c2.type()==vgl_conic<T>::complex_parallel_lines ||
00675 c2.type()==vgl_conic<T>::complex_intersecting_lines)
00676 && c1.contains(c2.centre()))
00677 return vcl_list<vgl_homg_point_2d<T> >(2, c2.centre());
00678 if (c1.type() == vgl_conic<T>::no_type || c2.type() == vgl_conic<T>::no_type ||
00679 c1.type()==vgl_conic<T>::complex_parallel_lines||c2.type()==vgl_conic<T>::complex_parallel_lines ||
00680 c1.type()==vgl_conic<T>::complex_intersecting_lines||c2.type()==vgl_conic<T>::complex_intersecting_lines ||
00681 c1.type() == vgl_conic<T>::imaginary_ellipse|| c2.type() == vgl_conic<T>::imaginary_ellipse ||
00682 c1.type() == vgl_conic<T>::imaginary_circle || c2.type() == vgl_conic<T>::imaginary_circle)
00683 return vcl_list<vgl_homg_point_2d<T> >();
00684
00685 if (c1.type() == vgl_conic<T>::coincident_lines ||
00686 c1.type() == vgl_conic<T>::real_intersecting_lines ||
00687 c1.type() == vgl_conic<T>::real_parallel_lines) {
00688 vcl_list<vgl_homg_point_2d<T> > l1=intersection(c2,c1.components().front());
00689 vcl_list<vgl_homg_point_2d<T> > l2=intersection(c2,c1.components().back());
00690 l1.insert(l1.end(), l2.begin(), l2.end());
00691 return l1;
00692 }
00693
00694 if (c2.type() == vgl_conic<T>::coincident_lines ||
00695 c2.type() == vgl_conic<T>::real_intersecting_lines ||
00696 c2.type() == vgl_conic<T>::real_parallel_lines) {
00697 vcl_list<vgl_homg_point_2d<T> > l1=intersection(c1,c2.components().front());
00698 vcl_list<vgl_homg_point_2d<T> > l2=intersection(c1,c2.components().back());
00699 l1.insert(l1.end(), l2.begin(), l2.end());
00700 return l1;
00701 }
00702
00703 return do_intersect(c1, c2);
00704 }
00705
00706
00707
00708
00709
00710
00711
00712
00713
00714 template <class T>
00715 vcl_list<vgl_homg_line_2d<T> >
00716 vgl_homg_operators_2d<T>::tangent_from(vgl_conic<T> const& c,
00717 vgl_homg_point_2d<T> const& p)
00718 {
00719 if (c.is_degenerate()) {
00720 if (!c.contains(p)) return vcl_list<vgl_homg_line_2d<T> >();
00721 vcl_list<vgl_homg_line_2d<T> > v = c.components();
00722 if (c.type() == vgl_conic<T>::coincident_lines || p == c.centre())
00723 return v;
00724 if (v.size() > 0 && dot(v.front(),p) == 0)
00725 return vcl_list<vgl_homg_line_2d<T> >(1,v.front());
00726 else if (v.size() > 1 && dot(v.back(),p) == 0)
00727 return vcl_list<vgl_homg_line_2d<T> >(1,v.back());
00728 else
00729 return vcl_list<vgl_homg_line_2d<T> >();
00730 }
00731
00732 vgl_conic<T> C = c.dual_conic();
00733 vgl_homg_line_2d<T> l(p.x(),p.y(),p.w());
00734 vcl_list<vgl_homg_point_2d<T> > dualpts = intersection(C,l);
00735 vcl_list<vgl_homg_line_2d<T> > v;
00736 typename vcl_list<vgl_homg_point_2d<T> >::iterator it = dualpts.begin();
00737 for (; !(it == dualpts.end()); ++it)
00738 v.push_back(vgl_homg_line_2d<T>((*it).x(), (*it).y(), (*it).w()));
00739 return v;
00740 }
00741
00742
00743
00744
00745
00746 template <class T>
00747 vcl_list<vgl_homg_line_2d<T> >
00748 vgl_homg_operators_2d<T>::common_tangents(vgl_conic<T> const& c1,
00749 vgl_conic<T> const& c2)
00750 {
00751 if ((c1.type() != vgl_conic<T>::parabola && ! c1.is_central()) ||
00752 (c2.type() != vgl_conic<T>::parabola && ! c2.is_central()))
00753 return vcl_list<vgl_homg_line_2d<T> >();
00754
00755 vgl_conic<T> C1 = c1.dual_conic();
00756 vgl_conic<T> C2 = c2.dual_conic();
00757 vcl_list<vgl_homg_point_2d<T> > dualpts = intersection(C1,C2);
00758 vcl_list<vgl_homg_line_2d<T> > v;
00759 typename vcl_list<vgl_homg_point_2d<T> >::iterator it = dualpts.begin();
00760 for (; !(it == dualpts.end()); ++it)
00761 v.push_back(vgl_homg_line_2d<T>((*it).x(), (*it).y(), (*it).w()));
00762 return v;
00763 }
00764
00765
00766 template <class T>
00767 vgl_homg_point_2d<T>
00768 vgl_homg_operators_2d<T>::closest_point(vgl_conic<T> const& c,
00769 vgl_homg_point_2d<T> const& pt)
00770 {
00771
00772 if (c.contains(pt)) return pt;
00773
00774
00775
00776
00777 vcl_list<vgl_homg_point_2d<T> > candidates;
00778 if (pt.w() == 0)
00779 {
00780
00781 vgl_homg_line_2d<T> l(c.a()*pt.y()*2-c.b()*pt.x(),
00782 -c.c()*pt.x()*2+c.b()*pt.y(),
00783 c.d()*pt.y() -c.e()*pt.x());
00784 candidates = intersection(c, l);
00785 if (candidates.size() == 0)
00786 return vgl_homg_point_2d<T>(0,0,0);
00787
00788 else return candidates.front();
00789 }
00790 else if (c.b()==0 && c.a()==c.c())
00791 {
00792
00793 vgl_homg_point_2d<T> centre = c.centre();
00794 if (centre == pt)
00795 centre = vgl_homg_point_2d<T>(1,0,0);
00796 candidates = intersection(c, vgl_homg_line_2d<T>(centre,pt));
00797 }
00798 else
00799 {
00800 vgl_conic<T> conic(pt.w()*c.b(),
00801 pt.w()*(c.c()-c.a())*2,
00802 -pt.w()*c.b(),
00803 pt.y()*c.a()*2-pt.x()*c.b()+pt.w()*c.e(),
00804 -pt.x()*c.c()*2+pt.y()*c.b()-pt.w()*c.d(),
00805 pt.y()*c.d() -pt.x()*c.e());
00806
00807 candidates = intersection(c, conic);
00808 }
00809 if (candidates.size() == 0)
00810 {
00811 vcl_cerr << "Warning: vgl_homg_operators_2d<T>::closest_point: no intersection\n";
00812 return vgl_homg_point_2d<T>(0,0,0);
00813 }
00814
00815
00816 typename vcl_list<vgl_homg_point_2d<T> >::iterator it = candidates.begin();
00817 vgl_homg_point_2d<T> p = (*it);
00818 T dist = vgl_homg_operators_2d<T>::distance_squared(*it,pt);
00819 for (++it; it != candidates.end(); ++it) {
00820 if ((*it).w() == 0) continue;
00821 T d = vgl_homg_operators_2d<T>::distance_squared(*it,pt);
00822 if (d < dist) { p = (*it); dist = d; }
00823 }
00824 return p;
00825 }
00826
00827
00828
00829 template <class T>
00830 vgl_homg_point_2d<T>
00831 vgl_homg_operators_2d<T>::closest_point(vgl_conic<T> const& c,
00832 vgl_point_2d<T> const& pt)
00833 {
00834 return closest_point(c,vgl_homg_point_2d<T>(pt));
00835 }
00836
00837
00838
00839
00840 template <class T>
00841 vgl_box_2d<T>
00842 vgl_homg_operators_2d<T>::compute_bounding_box(vgl_conic<T> const& c)
00843 {
00844
00845
00846 if (c.real_type() == "complex intersecting lines") {
00847 vgl_homg_point_2d<T> pt = c.centre();
00848 return vgl_box_2d<T>(vgl_point_2d<T>(pt), vgl_point_2d<T>(pt));
00849 }
00850
00851 if (c.real_type() == "invalid conic" ||
00852 c.real_type() == "imaginary ellipse" ||
00853 c.real_type() == "imaginary circle" ||
00854 c.real_type() == "complex parallel lines")
00855 return vgl_box_2d<T>();
00856
00857 if (c.real_type() == "real parallel lines" ||
00858 c.real_type() == "coincident lines")
00859 {
00860
00861 vcl_list<vgl_homg_line_2d<T> > l = c.components();
00862 if (l.front().a() == 0)
00863 return vgl_box_2d<T>(vgl_point_2d<T>(T(1e33),-l.front().c()/l.front().b()),
00864 vgl_point_2d<T>(T(-1e33),-l.back().c()/l.back().b()));
00865 if (l.front().b() == 0)
00866 return vgl_box_2d<T>(vgl_point_2d<T>(-l.front().c()/l.front().b(),T(1e33)),
00867 vgl_point_2d<T>(-l.back().c()/l.back().b(),T(-1e33)));
00868
00869 }
00870
00871 if (c.real_type() != "real ellipse" && c.real_type() != "real circle")
00872 return vgl_box_2d<T>(T(-1e33), T(1e33), T(-1e33), T(1e33));
00873
00874
00875
00876 vgl_homg_point_2d<T> px (1,0,0);
00877 vgl_homg_point_2d<T> py (0,1,0);
00878
00879 vcl_list<vgl_homg_line_2d<T> > lx = vgl_homg_operators_2d<T>::tangent_from(c, px);
00880 vcl_list<vgl_homg_line_2d<T> > ly = vgl_homg_operators_2d<T>::tangent_from(c, py);
00881
00882 T y1 = - lx.front().c() / lx.front().b();
00883 T y2 = - lx.back().c() / lx.back().b();
00884 if (y1 > y2) { T t = y1; y1 = y2; y2 = t; }
00885 T x1 = - ly.front().c() / ly.front().a();
00886 T x2 = - ly.back().c() / ly.back().a();
00887 if (x1 > x2) { T t = x1; x1 = x2; x2 = t; }
00888
00889 return vgl_box_2d<T>(x1, x2, y1, y2);
00890 }
00891
00892
00893
00894 template <class T>
00895 vgl_homg_point_2d<T> operator*(vnl_matrix_fixed<T,3,3> const& m,
00896 vgl_homg_point_2d<T> const& p)
00897 {
00898 return vgl_homg_point_2d<T>(m(0,0)*p.x()+m(0,1)*p.y()+m(0,2)*p.w(),
00899 m(1,0)*p.x()+m(1,1)*p.y()+m(1,2)*p.w(),
00900 m(2,0)*p.x()+m(2,1)*p.y()+m(2,2)*p.w());
00901 }
00902
00903
00904
00905 template <class T>
00906 vgl_homg_line_2d<T> operator*(vnl_matrix_fixed<T,3,3> const& m,
00907 vgl_homg_line_2d<T> const& l)
00908 {
00909 return vgl_homg_line_2d<T>(m(0,0)*l.a()+m(0,1)*l.b()+m(0,2)*l.c(),
00910 m(1,0)*l.a()+m(1,1)*l.b()+m(1,2)*l.c(),
00911 m(2,0)*l.a()+m(2,1)*l.b()+m(2,2)*l.c());
00912 }
00913
00914
00915 template <class T>
00916 vgl_homg_point_2d<T> vgl_homg_operators_2d<T>::closest_point(vgl_homg_line_2d<T> const& l,
00917 vgl_homg_point_2d<T> const& p)
00918 {
00919
00920 if (l.a()*p.x()+l.b()*p.y()+l.c()*p.w() == 0) return p;
00921
00922 assert(!l.ideal());
00923
00924 vgl_homg_line_2d<T> o(l.b()*p.w(), -l.a()*p.w(), l.a()*p.y()-l.b()*p.x());
00925
00926 return vgl_homg_operators_2d<T>::intersection(l,o);
00927 }
00928
00929 #undef VGL_HOMG_OPERATORS_2D_INSTANTIATE
00930 #define VGL_HOMG_OPERATORS_2D_INSTANTIATE(T) \
00931 template class vgl_homg_operators_2d<T >; \
00932 template vgl_homg_point_2d<T > operator*(vnl_matrix_fixed<T,3,3> const& m, vgl_homg_point_2d<T > const& p); \
00933 template vgl_homg_line_2d<T > operator*(vnl_matrix_fixed<T,3,3> const& m, vgl_homg_line_2d<T > const& p)
00934
00935 #endif // vgl_homg_operators_2d_txx_