rtcGeometry2D.cpp
Go to the documentation of this file.
00001 /*
00002  * Copyright (C) 2008
00003  * Robert Bosch LLC
00004  * Research and Technology Center North America
00005  * Palo Alto, California
00006  *
00007  * All rights reserved.
00008  *
00009  *------------------------------------------------------------------------------
00010  * project ....: PUMA: Probablistic Unsupervised Model Acquisition
00011  * file .......: Geometry2D.cpp
00012  * authors ....: Benjamin Pitzer
00013  * organization: Robert Bosch LLC
00014  * creation ...: 07/25/2006
00015  * modified ...: $Date: 2009-08-25 17:32:44 -0700 (Tue, 25 Aug 2009) $
00016  * changed by .: $Author: benjaminpitzer $
00017  * revision ...: $Revision: 893 $
00018  */
00019 
00020 //== INCLUDES ==================================================================
00021 #include <cassert>
00022 #include "rtc/rtcGeometry2D.h"
00023 #include <rtc/rtcRotation2D.h>
00024 
00025 //== NAMESPACES ================================================================
00026 namespace rtc {
00027 
00028 //== CONSTANTS =================================================================
00029 #define INSIDE  false
00030 #define OUTSIDE true
00031 
00032 float dist2(const Vec2f &a, const Vec2f &b)
00033 {
00034   return (a-b).normSqr();
00035 }
00036 
00037 float dist(const Vec2f &a, const Vec2f &b)
00038 {
00039   return (a-b).norm();
00040 }
00041 
00042 void dist_to_line(const Vec2f &x, const Vec2f &a, const Vec2f &b, float &d, Vec2f &cp)
00043 {
00044   Vec2f ba(b[0]-a[0], b[1]-a[1]);
00045   Vec2f xa(x[0]-a[0], x[1]-a[1]);
00046 
00047   float xa_ba = xa.dot(ba);
00048   // if the dot product is negative, the point is closest to a
00049   if (xa_ba < 0.0) {
00050     float nd = dist(x,a);
00051     cp = a;
00052     d = nd;
00053     return;
00054   }
00055 
00056   // if the dot product is greater than squared segment length,
00057   // the point is closest to b
00058   float fact = xa_ba/ba.normSqr();
00059   if (fact >= 1.0) {
00060     float nd = dist(x,b);
00061     cp = b;
00062     d = nd;
00063     return;
00064   }
00065 
00066   // take the squared dist x-a, squared dot of x-a to unit b-a,
00067   // use Pythagoras' rule
00068   float nd = xa.normSqr() - xa_ba*fact;
00069   d = sqrt(nd);
00070   cp[0] = a[0] + fact * ba[0];
00071   cp[1] = a[1] + fact * ba[1];
00072 }
00073 
00074 float dist_to_line(const Vec2f &x, const Vec2f &a, const Vec2f &b)
00075 {
00076   Vec2f ba(b[0]-a[0], b[1]-a[1]);
00077   Vec2f xa(x[0]-a[0], x[1]-a[1]);
00078 
00079   float xa_ba = xa.dot(ba);
00080   // if the dot product is negative, the point is closest to a
00081   if (xa_ba < 0.0) {
00082     return dist(x,a);
00083   }
00084 
00085   // if the dot product is greater than squared segment length,
00086   // the point is closest to b
00087   float fact = xa_ba/ba.normSqr();
00088   if (fact >= 1.0) {
00089     return dist(x,b);
00090   }
00091 
00092   // take the squared dist x-a, squared dot of x-a to unit b-a,
00093   // use Pythagoras' rule
00094   return sqrt(xa.normSqr() - xa_ba*fact);
00095 }
00096 
00097 float proj_dist_to_line(const Vec2f &x, const Vec2f &a, const Vec2f &b)
00098 {
00099   Vec2f v(b[1]-a[1], a[0]-b[0]);
00100   v.normalize();
00101   Vec2f xa(x[0]-a[0], x[1]-a[1]);
00102   return abs(v.dot(xa));
00103 }
00104 
00105 
00106 bool closer_on_line(const Vec2f &x, const Vec2f &a, const Vec2f &b, float &d2, Vec2f &cp)
00107 {
00108   Vec2f ba(b[0]-a[0], b[1]-a[1]);
00109   Vec2f xa(x[0]-a[0], x[1]-a[1]);
00110 
00111   float xa_ba = xa.dot(ba);
00112   // if the dot product is negative, the point is closest to a
00113   if (xa_ba < 0.0) {
00114     float nd = dist2(x, a);
00115     if (nd < d2) {
00116       cp = a;
00117       d2 = nd;
00118       return true;
00119     }
00120     return false;
00121   }
00122 
00123   // if the dot product is greater than squared segment length,
00124   // the point is closest to b
00125   float fact = xa_ba/ba.normSqr();
00126   if (fact >= 1.0) {
00127     float nd = dist2(x,b);
00128     if (nd < d2) {
00129       cp = b;
00130       d2 = nd;
00131       return true;
00132     }
00133     return false;
00134   }
00135 
00136   // take the squared dist x-a, squared dot of x-a to unit b-a,
00137   // use Pythagoras' rule
00138   float nd = xa.normSqr() - xa_ba*fact;
00139   if (nd<0)
00140     nd=0.0f;
00141   if (nd < d2) {
00142     d2 = nd;
00143     cp[0] = a[0] + fact * ba[0];
00144     cp[1] = a[1] + fact * ba[1];
00145     return true;
00146   }
00147   return false;
00148 }
00149 
00150 // is the circle centered at b with radius r
00151 // fully within the rectangle centered at bc, with radius br?
00152 bool circle_within_bounds(const Vec2f &b, float r, const Vec2f &bc, float br)
00153 {
00154   r -= br;
00155   if ((b[0]  - bc[0] <= r) ||
00156       (bc[0] - b[0]  <= r) ||
00157       (b[1]  - bc[1] <= r) ||
00158       (bc[1] - b[1]  <= r)) return false;
00159   return true;
00160 }
00161 
00162 // is the circle centered at b with radius r
00163 // fully within the rectangle centered from min to max?
00164 bool circle_within_bounds(const Vec2f &b, float r, const Vec2f &min, const Vec2f &max)
00165 {
00166   if ((b[0] - min[0] <= r) ||
00167     (max[0] - b[0] <= r) ||
00168     (b[1] - min[1] <= r) ||
00169     (max[1] - b[1] <= r)) return false;
00170   return true;
00171 }
00172 
00173 // does the circle centered at b, with radius r,
00174 // intersect the rectangle centered at bc, with radius br?
00175 bool bounds_overlap_circle(const Vec2f &b, float r, const Vec2f &bc, float br)
00176 {
00177   float r2 = rtc_sqr(r);
00178   float R_max_x,R_max_y;
00179   float R_min_x,R_min_y;
00180 
00181   /* Translate coordinates, placing the circle at the origin. */
00182   R_max_x = bc[0]+br-b[0];  R_max_y = bc[1]+br-b[1];
00183   R_min_x = bc[0]-br-b[0];  R_min_y = bc[1]-br-b[1];
00184 
00185   if (R_max_x < 0)      /* R to left of circle center */
00186     if ( R_max_y < 0)     /* R in lower left corner */
00187         return ((R_max_x * R_max_x + R_max_y * R_max_y) < r2);
00188     else if ( R_min_y > 0)  /* R in upper left corner */
00189         return ((R_max_x * R_max_x +  R_min_y *  R_min_y) < r2);
00190     else          /* R due West of circle */
00191         return(abs(R_max_x) < r);
00192    else if ( R_min_x > 0)    /* R to right of circle center */
00193       if ( R_max_y < 0)   /* R in lower right corner */
00194           return (( R_min_x *  R_min_x + R_max_y * R_max_y) < r2);
00195     else if ( R_min_y > 0)    /* R in upper right corner */
00196         return (( R_min_x *  R_min_x +  R_min_y *  R_min_y) < r2);
00197     else        /* R due East of circle */
00198         return ( R_min_x < r);
00199    else        /* R on circle vertical centerline */
00200       if ( R_max_y < 0)   /* R due South of circle */
00201         return (abs(R_max_y) < r);
00202     else if ( R_min_y > 0)    /* R due North of circle */
00203         return ( R_min_y < r);
00204     else        /* R contains circle centerpoint */
00205         return(true);
00206 }
00207 
00210 //bool bounds_overlap_circle(const Vec2f &b, float r, const Vec2f &bc, float br)
00211 //  {
00212 //    float sum = 0.0, tmp;
00213 //    if        ((tmp = bc[0]-br - b[0]) > 0.0) {
00214 //      if (tmp>r) return false; sum += tmp*tmp;
00215 //    } else if ((tmp = b[0] - (bc[0]+br)) > 0.0) {
00216 //      if (tmp>r) return false; sum += tmp*tmp;
00217 //    }
00218 //    if        ((tmp = bc[1]-br - b[1]) > 0.0) {
00219 //      if (tmp>r) return false; sum += tmp*tmp;
00220 //    } else if ((tmp = b[1] - (bc[1]+br)) > 0.0) {
00221 //      if (tmp>r) return false; sum += tmp*tmp;
00222 //    }
00223 //    return (sum < r*r);
00224 //  }
00225 
00226  /* Which of the four edges is point P outside of? */
00227 long bevel_1d(const Vec2f &p)
00228 {
00229   long outcode = 0;
00230   if (p[0] >  .5) outcode |= 0x01;
00231   if (p[0] < -.5) outcode |= 0x02;
00232   if (p[1] >  .5) outcode |= 0x04;
00233   if (p[1] < -.5) outcode |= 0x08;
00234   return outcode;
00235 }
00236 
00237 /* Which of the four corner lines is point P outside of? */
00238 long bevel_2d(const Vec2f &p)
00239 {
00240   long outcode = 0;
00241   if ( p[0] + p[1] > 1.0) outcode |= 0x01;
00242   if ( p[0] - p[1] > 1.0) outcode |= 0x02;
00243   if (-p[0] + p[1] > 1.0) outcode |= 0x04;
00244   if (-p[0] - p[1] > 1.0) outcode |= 0x08;
00245   return outcode;
00246 }
00247 
00248 // 2D linear interpolation
00249 Vec2f lerp(float t, const Vec2f &a, const Vec2f &b)
00250 {
00251   float v[2];
00252   float u = 1.0 - t;
00253   v[0]=u*a[0]+t*b[0];
00254   v[1]=u*a[1]+t*b[1];
00255   return Vec2f(v[0],v[1]);
00256 }
00257 
00258 /* Test the point "alpha" of the way from P1 to P2 */
00259 /* See if it is on a edge of the rectangle         */
00260 /* Consider only faces in "mask"                   */
00261 bool point_on_edge(const Vec2f &p1, const Vec2f &p2, float alpha, long mask)
00262 {
00263   Vec2f line_point;
00264   line_point = lerp(alpha, p1, p2);
00265   long l = bevel_1d(line_point) & mask;
00266   return (l==0?INSIDE:OUTSIDE);
00267 }
00268 
00269 /* Compute intersection of P1 --> P2 line segment with edge lines  */
00270 /* Then test intersection point to see if it is on cube face       */
00271 /* Consider only face planes in "outcode_diff"                     */
00272 /* Note: Zero bits in "outcode_diff" means edge is outside of      */
00273 bool segment_on_edge(const Vec2f &p1, const Vec2f &p2, long outcode_diff)
00274 {
00275   if (0x01 & outcode_diff)
00276     if (point_on_edge(p1,p2,( .5-p1[0])/(p2[0]-p1[0]),0xE) == INSIDE) return INSIDE;
00277   if (0x02 & outcode_diff)
00278     if (point_on_edge(p1,p2,(-.5-p1[0])/(p2[0]-p1[0]),0xD) == INSIDE) return INSIDE;
00279   if (0x04 & outcode_diff)
00280     if (point_on_edge(p1,p2,( .5-p1[1])/(p2[1]-p1[1]),0xB) == INSIDE) return INSIDE;
00281   if (0x08 & outcode_diff)
00282     if (point_on_edge(p1,p2,(-.5-p1[1])/(p2[1]-p1[1]),0x7) == INSIDE) return INSIDE;
00283   return OUTSIDE;
00284 }
00285 
00286 
00287 // the main routine
00288 // true if line t1,t2 is outside a rectangle
00289 // centered at c with length of a side s,
00290 // false if the line intersects rectangle
00291 //
00292 bool line_outside_of_rect(const Vec2f &c, float s, const Vec2f &t1, const Vec2f &t2)
00293 {
00294 
00295   long v1_test,v2_test;
00296 
00297   // First compare both points tih all four rectangle edges
00298   // If any point is inside the rectangle, return immediately!
00299   Vec2f v1((t1[0]-c[0])/s, (t1[1]-c[1])/s);
00300   if (!(v1_test = bevel_1d(v1))) return INSIDE;
00301   Vec2f v2((t2[0]-c[0])/s, (t2[1]-c[1])/s);
00302   if (!(v2_test = bevel_1d(v2))) return INSIDE;
00303   // If both points were outside of one or more edges,
00304   // return immediately with a trivial rejection!
00305   if ((v1_test & v2_test) != 0) return OUTSIDE;
00306 
00307   // Now do the same trivial rejection test for the four corner lines
00308   v1_test |= bevel_2d(v1) << 8;
00309   v2_test |= bevel_2d(v2) << 8;
00310   if ((v1_test & v2_test) != 0) return OUTSIDE;
00311 
00312   /* If point 1 and 2, as a pair, cannot be trivially rejected    */
00313   /* by the above tests, then see if the v1-->v2 segment          */
00314   /* intersects the rectangle.                                    */
00315   /* Pass to the intersection algorithm the "OR" of the outcode   */
00316   /* bits, so that only those rectangle edges which are spanned   */
00317   /* by each triangle edge need be tested.                        */
00318   if (segment_on_edge(v1,v2,v1_test|v2_test) == INSIDE) return INSIDE;
00319 
00320 //  if (point_on_edge(v1,v2,( .5-v1[0])/(v2[0]-v1[0])) == INSIDE) return INSIDE;
00321 //  if (point_on_edge(v1,v2,(-.5-v1[0])/(v2[0]-v1[0])) == INSIDE) return INSIDE;
00322 //  if (point_on_edge(v1,v2,( .5-v1[1])/(v2[1]-v1[1])) == INSIDE) return INSIDE;
00323 //  if (point_on_edge(v1,v2,(-.5-v1[1])/(v2[1]-v1[1])) == INSIDE) return INSIDE;
00324 
00325 //  /* No line touched the rectangle                                   */
00326 //  /* We're done...there was no intersection.                         */
00327   return OUTSIDE;
00328 }
00329 
00330 // true if line p is outside a rectangle
00331 // centered at c with length of a side s,
00332 // false if the line intersects rectangle
00333 bool point_outside_of_rect(const Vec2f &c, float s, const Vec2f &p)
00334 {
00335   Vec2f v1((p[0]-c[0])/s, (p[1]-c[1])/s);
00336   if (!bevel_1d(v1)) return INSIDE;
00337   return OUTSIDE;
00338 }
00339 
00340 // true if line p is outside a rectangle
00341 // centered at c with length of x side xlen and length of y side ylen,
00342 // rotated rot_angle degree,
00343 // false if the line intersects rectangle
00344 bool point_outside_of_rect(const Vec2f &c, float xlen, float ylen, float rot_angle, const Vec2f &p)
00345 {
00346   Vec2f v(p[0]-c[0], p[1]-c[1]);
00347   Rotation2D<float> r(rot_angle);
00348   v=r*v;
00349   v.x[0]/=xlen;v.x[1]/=ylen;
00350   if (!bevel_1d(v)) return INSIDE;
00351   return OUTSIDE;
00352 }
00353 
00354 // true if rectangle 1 intersects rectangle 2
00355 // c == center
00356 // theta == angle
00357 // w == width
00358 // l == length
00359 bool rect_rect_X(const Vec2f &c1, float theta1, float w1, float l1,
00360                  const Vec2f &c2, float theta2, float w2, float l2) {
00361 
00362   float x,y,sintheta2,costheta2;
00363   float tx, ty, scale_x, scale_y,sintheta1,costheta1;
00364   assert(l1);assert(w1);
00365   assert(l2);assert(w2);
00366 
00367   sintheta2 = sin(theta2);
00368   costheta2 = cos(theta2);
00369 
00370   // calculate the coordinates of all four corners of rectangle 2
00371   float rect2_x[4];
00372   float rect2_y[4];
00373 
00374   x = -l2/2.0; y = -w2/2.0;
00375   rect2_x[0] = costheta2*(x) - sintheta2*(y) + c2[0];
00376   rect2_y[0] = sintheta2*(x) + costheta2*(y) + c2[1];
00377 
00378   x = l2/2.0; y = -w2/2.0;
00379   rect2_x[1] = costheta2*(x) - sintheta2*(y) + c2[0];
00380   rect2_y[1] = sintheta2*(x) + costheta2*(y) + c2[1];
00381 
00382   x = l2/2.0; y = w2/2.0;
00383   rect2_x[2] = costheta2*(x) - sintheta2*(y) + c2[0];
00384   rect2_y[2] = sintheta2*(x) + costheta2*(y) + c2[1];
00385 
00386   x = -l2/2.0; y = w2/2.0;
00387   rect2_x[3] = costheta2*(x) - sintheta2*(y) + c2[0];
00388   rect2_y[3] = sintheta2*(x) + costheta2*(y) + c2[1];
00389 
00390   // normalize all coordinates so that rectangle 1 ist axis aligned and of length 1
00391   tx = c1[0];
00392   ty = c1[1];
00393   scale_x = l1;
00394   scale_y = w1;
00395   sintheta1 = sin(-theta1);
00396   costheta1 = cos(-theta1);
00397 
00398   for(int i=0;i<4;++i) {
00399     // transform coordinates of rectangle 2
00400     x = rect2_x[i] - tx;
00401     y = rect2_y[i] - ty;
00402     rect2_x[i] = (costheta1*(x) - sintheta1*(y))/scale_x;
00403     rect2_y[i] = (sintheta1*(x) + costheta1*(y))/scale_y;
00404   }
00405 
00406   Vec2f t0(rect2_x[0], rect2_y[0]);
00407   Vec2f t1(rect2_x[1], rect2_y[1]);
00408   Vec2f t2(rect2_x[2], rect2_y[2]);
00409   Vec2f t3(rect2_x[3], rect2_y[3]);
00410 
00411   // perform line rectangle intersection for all four edges of rectangle 2
00412   Vec2f c((float)0);
00413   bool boutside;
00414 
00415   boutside = line_outside_of_rect(c, 1, t0, t1);
00416   if(!boutside) return true;
00417 
00418   boutside = line_outside_of_rect(c, 1, t1, t2);
00419   if(!boutside) return true;
00420 
00421   boutside = line_outside_of_rect(c, 1, t2, t3);
00422   if(!boutside) return true;
00423 
00424   boutside = line_outside_of_rect(c, 1, t3, t0);
00425   if(!boutside) return true;
00426 
00427   return false;
00428 }
00429 
00430 // true if line intersects rectangle
00431 // line t1-->t2
00432 // c == center
00433 // theta == angle
00434 // w == width
00435 // l == length
00436 bool line_rect_X(const Vec2f &t1, const Vec2f &t2,
00437                  const Vec2f &c, float theta, float w, float l) {
00438   float x,y,sintheta,costheta;
00439   float tx, ty, scale_x, scale_y;
00440   Vec2f t1n,t2n;
00441   assert(l);assert(w);
00442 
00443   // normalize all coordinates so that rectangle 1 ist axis aligned and of length 1
00444   tx = c[0];
00445   ty = c[1];
00446   scale_x = l;
00447   scale_y = w;
00448   sintheta = sin(-theta);
00449   costheta = cos(-theta);
00450 
00451   // transform coordinates of t1
00452   x = t1[0] - tx;
00453   y = t1[1] - ty;
00454   t1n[0] = (costheta*(x) - sintheta*(y)) / scale_x;
00455   t1n[1] = (sintheta*(x) + costheta*(y)) / scale_y;
00456 
00457   // transform coordinates of t2
00458   x = t2[0] - tx;
00459   y = t2[1] - ty;
00460   t2n[0] = (costheta*(x) - sintheta*(y)) / scale_x;
00461   t2n[1] = (sintheta*(x) + costheta*(y)) / scale_y;
00462 
00463   // perform line rectangle intersection for all four edges of rectangle 2
00464   Vec2f cn((float)0);
00465   bool boutside = line_outside_of_rect(cn, 1, t1n, t2n);
00466   if(!boutside) return true;
00467   return false;
00468 }
00469 
00470 //==============================================================================
00471 } // namespace rtc
00472 //==============================================================================


rtc
Author(s): Benjamin Pitzer
autogenerated on Thu Jan 2 2014 11:04:53