Geometry.cc
Go to the documentation of this file.
00001 
00010 #include <blort/Recognizer3D/Geometry.hh>
00011 
00012 
00013 namespace P 
00014 {
00015 
00016 #define NONE  (-1)
00017 
00018 //for convex hull
00019 typedef struct range_bin Bin;
00020  struct range_bin {
00021      int    min;    // index of min point P[] in bin (>=0 or NONE)
00022      int    max;    // index of max point P[] in bin (>=0 or NONE)
00023  };
00024 
00025 
00026 
00041 void ChainHull2D( Array<Vector2> &p, Array<Vector2> &h)
00042 {
00043   h.Clear();               //delete hull
00044   if (p.Size()>0){
00045     int    minmin=0,  minmax=0;
00046     int    maxmin=0,  maxmax=0;
00047     int k = 200;                 //CHULL_MAX;
00048     Vector2* cP;                 // the current point being considered
00049     int    bot=0, top=(-1);  // indices for bottom and top of the stack
00050 
00051     int n=p.Size();
00052     float  xmin=p[0].x,  xmax=p[0].x;
00053 
00054     // Get the points with (1) min-max x-coord, and (2) min-max y-coord
00055     for (int i=1; i<n; i++) {
00056         cP = &p[i];
00057         if (cP->x <= xmin) {
00058             if (cP->x < xmin) {        // new xmin
00059                 xmin = cP->x;
00060                 minmin = minmax = i;
00061             }
00062             else {                      // another xmin
00063                 if (cP->y < p[minmin].y)
00064                     minmin = i;
00065                 else if (cP->y > p[minmax].y)
00066                     minmax = i;
00067             }
00068         }
00069 
00070         if (cP->x >= xmax) {
00071              if (cP->x > xmax) {        // new xmax
00072                  xmax = cP->x;
00073                  maxmin = maxmax = i;
00074              }
00075              else {                      // another xmax
00076                  if (cP->y < p[maxmin].y)
00077                      maxmin = i;
00078                  else if (cP->y > p[maxmax].y)
00079                      maxmax = i;
00080              }
00081         }
00082     }
00083 
00084     if (xmin == xmax) {      // degenerate case: all x-coords == xmin
00085          h.PushBack(p[minmin]);             // a point, or
00086          top++;
00087          if (minmax != minmin){           // a nontrivial segment
00088              top++;
00089              h.PushBack(p[minmax]);
00090          }
00091 
00092     }else{
00093        // Next, get the max and min points in the k range bins
00094        Bin*   B = new Bin[k+2];   // first allocate the bins
00095        B[0].min = minmin;         B[0].max = minmax;        // set bin 0
00096        B[k+1].min = maxmin;       B[k+1].max = maxmax;      // set bin k+1
00097        for (int b=1; b<=k; b++) { // initially nothing is in the other bins
00098            B[b].min = B[b].max = NONE;
00099        }
00100 
00101        for (int b, i=0; i<n; i++) {
00102           cP = &p[i];
00103           if (cP->x == xmin || cP->x == xmax) // already have bins 0 and k+1
00104               continue;
00105 
00106           // check if a lower or upper point
00107           if (IsLeft( p[minmin], p[maxmin], *cP) < 0) {  // below lower line
00108               b = (int)( k * (cP->x - xmin) / (xmax - xmin) ) + 1;  // bin #
00109 
00110               if (B[b].min == NONE){       // no min point in this range
00111                   B[b].min = i;           // first min
00112               }else if (cP->y < p[B[b].min].y){
00113                   B[b].min = i;           // new min
00114               }
00115               continue;
00116           }
00117 
00118           if (IsLeft( p[minmax], p[maxmax], *cP) > 0) {  // above upper line
00119               b = (int)( k * (cP->x - xmin) / (xmax - xmin) ) + 1;  // bin #
00120 
00121               if (B[b].max == NONE){       // no max point in this range
00122                   B[b].max = i;           // first max
00123               }else if (cP->y > p[B[b].max].y){
00124                   B[b].max = i;           // new max
00125               }
00126               continue;
00127           }
00128        }
00129 
00130        // Now, use the chain algorithm to get the lower and upper hulls
00131        // the output array H[] will be used as the stack
00132        // First, compute the lower hull on the stack H
00133        int idx;
00134        for (int i=0; i <= k+1; ++i)
00135        {
00136          if (B[i].min == NONE)  // no min point in this range
00137                continue;
00138            cP = &p[ B[i].min ];   // select the current min point
00139            idx=B[i].min;
00140 
00141            while (top > 0)        // there are at least 2 points on the stack
00142            {
00143                // test if current point is left of the line at the stack top
00144                if (IsLeft( h[top-1], h[top], *cP) > 0)
00145                   break;         // cP is a new hull vertex
00146                else{
00147                    top--;         // pop top point off stack
00148                    h.EraseLast();
00149                }
00150            }
00151            top++;
00152 
00153            h.PushBack(p[idx]);        // push current point onto stack
00154        }
00155 
00156        // Next, compute the upper hull on the stack H above the bottom hull
00157        if (maxmax != maxmin){      // if distinct xmax points
00158            top++;
00159 
00160            h.PushBack(p[maxmax]);   // push maxmax point onto stack
00161        }
00162 
00163        bot = top;                 // the bottom point of the upper hull stack
00164        for (int i=k; i >= 0; --i)
00165        {
00166          if (B[i].max == NONE)  // no max point in this range
00167                continue;
00168            cP = &p[ B[i].max ];   // select the current max point
00169            idx=B[i].max;
00170 
00171            while (top > bot)      // at least 2 points on the upper stack
00172            {
00173                // test if current point is left of the line at the stack top
00174                if (IsLeft( h[top-1], h[top], *cP) > 0)
00175                    break;         // current point is a new hull vertex
00176                else{
00177                    top--;         // pop top point off stack
00178                    h.EraseLast();
00179                }
00180            }
00181            top++;
00182            h.PushBack(p[idx]);      // push current point onto stack
00183        }
00184 
00185        if (minmax != minmin){
00186            top++;
00187            h.PushBack(p[minmin]);   // push joining endpoint onto stack
00188        }
00189        delete[] B;                  // free bins before returning
00190     }
00191 
00192     //ups a bug
00193     if (h.Size()>0)
00194       h.Erase(h.Size()-1);      //now it is fixed ;-)
00195   }
00196 }
00197 
00198 void ConvexHull( Array<Vector2> &p, Array<Vector2> &h)
00199 {
00200   h.Clear();
00201   CvPoint* points = (CvPoint*)malloc( p.Size() * sizeof(points[0]));
00202   int* hull = (int*)malloc( p.Size() * sizeof(hull[0]));
00203 
00204   for( unsigned i = 0; i < p.Size(); i++ )
00205   {
00206     points[i].x = (int)p[i].x;
00207     points[i].y = (int)p[i].y;
00208   }
00209 
00210   CvMat hullMat = cvMat( 1, p.Size(), CV_32SC1, hull );
00211   CvMat pointMat = cvMat( 1, p.Size(), CV_32SC2, points);
00212 
00213   cvConvexHull2( &pointMat, &hullMat, CV_CLOCKWISE, 0 );
00214   int hullcount = hullMat.cols;
00215 
00216   for (int i=0; i<hullcount; i++)
00217     h.PushBack(p[hull[i]]);
00218 
00219   free( hull );
00220   free( points);
00221 }
00222 
00234 void AngleHalf(const Vector2 &p1, const Vector2 &p2, const Vector2 &p3, 
00235                Vector2 &p4)
00236 {
00237   double norm;
00238 
00239   norm = sqrt ( ( p1.x - p2.x ) * ( p1.x - p2.x )
00240               + ( p1.y - p2.y ) * ( p1.y - p2.y ) );
00241 
00242 
00243   p4.x = ( p1.x - p2.x ) / norm;
00244   p4.y = ( p1.y - p2.y ) / norm;
00245 
00246   norm = sqrt ( ( p3.x - p2.x ) * ( p3.x - p2.x )
00247               + ( p3.y - p2.y ) * ( p3.y - p2.y ) );
00248 
00249   p4.x = p4.x + ( p3.x - p2.x ) / norm;
00250   p4.y = p4.y + ( p3.y - p2.y ) / norm;
00251 
00252   p4.x = 0.5 * p4.x;
00253   p4.y = 0.5 * p4.y;
00254 
00255   norm = sqrt ( p4.x * p4.x + p4.y * p4.y );
00256 
00257   p4.x = p2.x + p4.x / norm;
00258   p4.y = p2.y + p4.y / norm;
00259 }
00260 
00273 double AngleRAD(const Vector2 &p1, const Vector2 &p2, const Vector2 &p3)
00274 {
00275 #define _PI 3.141592653589793
00276 
00277   Vector2 p;
00278   double value;
00279 
00280   p.x = ( p3.x - p2.x ) * ( p1.x - p2.x )
00281        + ( p3.y - p2.y ) * ( p1.y - p2.y );
00282 
00283 
00284   p.y = ( p3.x - p2.x ) * ( p1.y - p2.y )
00285        - ( p3.y - p2.y ) * ( p1.x - p2.x );
00286 
00287   if ( p.x == 0.0 && p.y == 0.0 )
00288   {
00289     value = 0.0;
00290     return value;
00291   }
00292 
00293   value = atan2 ( p.y, p.x );
00294 
00295   if ( value < 0.0 )
00296   {
00297     value = value + 2.0 * _PI;
00298   }
00299 
00300   return value;
00301 # undef _PI
00302 }
00303 
00304 }
00305 


blort
Author(s): Thomas Mörwald , Michael Zillich , Andreas Richtsfeld , Johann Prankl , Markus Vincze , Bence Magyar
autogenerated on Wed Aug 26 2015 15:24:12