SPolygon.cc
Go to the documentation of this file.
00001 
00008 #include <blort/Recognizer3D/SPolygon.hh>
00009 #include <blort/Recognizer3D/Geometry.hh>
00010 #include <float.h>
00011 #include <stdio.h>
00012 #include <fstream>
00013 #include <iostream>
00014 
00015 namespace P 
00016 {
00017 
00018 SPolygon::SPolygon()
00019 {
00020   p = UNDEF_ID;
00021   center = Vector2(DBL_MAX,DBL_MAX);
00022 }
00023 
00024 #ifdef USE_VS_CLOSURES
00025 SPolygon::SPolygon(Closure *c)
00026 {
00027   p = UNDEF_ID;
00028   v = c->v;
00029   center = Vector2(DBL_MAX,DBL_MAX);
00030 
00031 }
00032 #endif
00033 
00034 SPolygon::SPolygon(Array<Vector2> &p)
00035 {
00036   v = p;
00037   center = Vector2(DBL_MAX,DBL_MAX);
00038   //SetConvexHull(p);
00039 }
00040 
00041 SPolygon& SPolygon::operator=(const SPolygon &p)
00042 {
00043   this->v = p.v;
00044   this->p = p.p;
00045   this->center = p.center;
00046   return *this;
00047 }
00048 
00053 void SPolygon::SetConvexHull(Array<Vector2> &p)
00054 {
00055   ChainHull2D( p, v);
00056 }
00057 
00058 void SPolygon::Expand(double d)
00059 {
00060   double angle;
00061   double h2;
00062   int j;
00063   int jm1;
00064   int jp1;
00065   Vector2 p4;
00066   int n=v.Size();
00067 
00068   Array<Vector2> eh;
00069 
00070   //  Consider each angle, formed by the nodes P(I-1), P(I), P(I+1).
00071   for ( j = 0; j < n; j++ )
00072   {
00073     jm1 = IWrap ( j-1, 0, n-1 );
00074     jp1 = IWrap ( j+1, 0, n-1 );
00075 //
00076 //        P1
00077 //        /
00078 //       /   P4
00079 //      /  .
00080 //     / .
00081 //    P2--------->P3
00082     AngleHalf (v[jm1],v[j],v[jp1],p4);
00083 
00084 //  Compute the value of the half angle.
00085     angle = AngleRAD (v[jm1],v[j],p4);
00086 //  The stepsize along the ray must be adjusted so that the sides
00087 //  move out by H.
00088     h2 = d / sin(angle);
00089     eh.PushBack(Vector2(v[j].x-h2*(p4.x-v[j].x), v[j].y-h2*(p4.y-v[j].y)));
00090   }
00091   v=eh;
00092 }
00093 
00114 bool SPolygon::Inside(double x, double y)
00115 {
00116   int i, j, npol = v.Size();
00117   bool c = false;
00118   for(i = 0, j = npol-1; i < npol; j = i++)
00119   {
00120     if((((v[i].y <= y) && (y < v[j].y)) ||
00121         ((v[j].y <= y) && (y < v[i].y))) &&
00122        (x < (v[j].x - v[i].x) * (y - v[i].y) / (v[j].y - v[i].y) + v[i].x))
00123       c = !c;
00124   }
00125   return c;
00126 }
00127 
00128 bool SPolygon::OnContour(double x, double y, double eps)
00129 {
00130   for (unsigned i=0; i<v.Size(); i++)
00131   {
00132     if (Distance(v[i],P::Vector2(x,y)) < eps)
00133       return true;
00134   }
00135   return false;
00136 }
00137 
00138 bool SPolygon::Inside(Array<Vector2> &vs, Vector2 &p)
00139 {
00140   int i, j, npol = vs.Size();
00141   bool c = false;
00142   for(i = 0, j = npol-1; i < npol; j = i++)
00143   {
00144     if((((vs[i].y <= p.y) && (p.y < vs[j].y)) ||
00145         ((vs[j].y <= p.y) && (p.y < vs[i].y))) &&
00146        (p.x < (vs[j].x - vs[i].x) * (p.y - vs[i].y) / (vs[j].y - vs[i].y) + vs[i].x))
00147       c = !c;
00148   }
00149   return c;
00150 }
00151 
00155 double SPolygon::NearestNeighbour(Vector2 &p, Array<Vector2> &vs, unsigned &id)
00156 {
00157   double dist, min_dist=DBL_MAX;
00158   for (unsigned i=0; i<vs.Size(); i++){
00159     dist = Distance(p, vs[i]);
00160     if (dist<min_dist){
00161       min_dist = dist;
00162       id = i;
00163     }
00164   }
00165   return min_dist;
00166 }
00167 
00171 double SPolygon::NearestLine(Vector2 &p, Array<Vector2> &vs, unsigned &id)
00172 {
00173   id=UINT_MAX;
00174   double dist, min_dist=DBL_MAX;
00175   Vector2 d, l2p;
00176 
00177   for (unsigned i=0; i<vs.Size(); i++){
00178     d = vs[vs.CircularNext(i)]-vs[i];
00179     d.Normalise();
00180     l2p = p-vs[i];
00181 
00182     dist = fabs(Cross(l2p, d));
00183     if (Dot(l2p,d)>0. && dist<min_dist){
00184       min_dist=dist;
00185       id=i;
00186     }
00187   }
00188 
00189   return min_dist;
00190 }
00191 
00195 double SPolygon::Match(Array<Vector2> &vs1, Array<Vector2> &vs2)
00196 {
00197   unsigned z;
00198   double dist, h_dist=0., min_dist=DBL_MAX;
00199 
00200   //search for each vs1 vertex in vs2 
00201   for (unsigned i=0; i<vs1.Size(); i++){
00202     //search for nearest neighbour to vertex
00203     min_dist = NearestNeighbour(vs1[i], vs2, z);
00204     //search for nearest line (normal distance between two end points)
00205     dist = NearestLine(vs1[i], vs2, z);
00206     if (dist<min_dist) min_dist=dist;
00207     //check for minimum
00208     if (min_dist>h_dist) h_dist=min_dist;
00209   }
00210 
00211   //search for each vs2 vertex in vs1 
00212   for (unsigned i=0; i<vs2.Size(); i++){
00213     //search for nearest neighbour to vertex
00214     min_dist = NearestNeighbour(vs2[i], vs1, z);
00215     //search for nearest line (normal distance between two end points)
00216     dist = NearestLine(vs2[i], vs1, z);
00217     if (dist<min_dist) min_dist=dist;
00218     //check for minimum
00219     if (min_dist>h_dist) h_dist=min_dist;
00220   }
00221 
00222   return h_dist;
00223 }
00224 
00228 void SPolygon::CenterOfGravity(Array<Vector2> &vs, Vector2 &vc)
00229 {
00230   vc=P::Vector2(0.,0.);
00231   if (vs.Size()>0)
00232   {
00233     for (unsigned i=0; i<vs.Size(); i++)
00234       vc+=vs[i];
00235     vc/=vs.Size();
00236   }
00237 }
00238 
00242 double SPolygon::Area(Array<Vector2> &vs)
00243 {
00244   double area;
00245   int im1;
00246   int ip1;
00247 
00248   area = 0.0;
00249 
00250   for ( unsigned i = 1; i <= vs.Size(); i++ )
00251   {
00252     im1 = IWrap ( i-1, 1, vs.Size() );
00253     ip1 = IWrap ( i+1, 1, vs.Size() );
00254 
00255     area = area + vs[i-1].x * ( vs[ip1-1].y - vs[im1-1].y );
00256   }
00257 
00258   area = 0.5 * fabs ( area );
00259 
00260   return area;
00261 
00262 }
00263 
00264 
00265 void SPolygon::Save(ofstream &os, const SPolygon &p)
00266 {
00267   os<<p.v.Size();
00268 
00269   for (unsigned i=0; i<p.v.Size(); i++)
00270     os<<' '<<p.v[i].x<<' '<<p.v[i].y;
00271 
00272   os<<' '<<p.center.x<<' '<<p.center.y;
00273   os<<' '<<p.p<<'\n';
00274 }
00275 
00276 void SPolygon::Load(ifstream &is, SPolygon &p)
00277 {
00278   unsigned size;
00279   is >> size;
00280   p.v.Resize(size);
00281   for (unsigned i=0; i<size; i++)
00282     is >>p.v[i].x>>p.v[i].y;
00283   is >>p.center.x>>p.center.y;
00284   is >>p.p;
00285 }
00286 
00287 
00288 
00289 
00290 
00291 }
00292 


blort
Author(s): Michael Zillich, Thomas Mörwald, Johann Prankl, Andreas Richtsfeld, Bence Magyar (ROS version)
autogenerated on Thu Jan 2 2014 11:38:25