Main Page | Namespace List | Class Hierarchy | Alphabetical List | Class List | File List | Namespace Members | Class Members | File Members | Related Pages

# Tools/PotentialFields/PfieldGeometry.cpp

Go to the documentation of this file.

    00001 /**
    00002 * @file PfieldGeometry.cpp
    00003 *
    00004 * Implementation of several geometric functions and objects used by potential fields
    00005 *
    00006 * @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
    00007 */
    00008
    00009
    00010 #include "PfieldGeometry.h"
    00011
    00012
    00013
    00014 inline bool pointPerpendicularToLine(const PfVec& l1, const PfVec& l2, const PfVec& p,
    00015                               double& distance, PfVec& perpendicularPoint)
    00016 {
    00017   PfVec v = (l2 - l1);
    00018   double r = (v.scalarProduct(p) - v.scalarProduct(l1)) / (v.x*v.x + v.y*v.y);
    00019   PfVec pp = (l1 + (v*r));
    00020   if(fabs(pp.distanceTo(l1) + pp.distanceTo(l2) - l1.distanceTo(l2))<EPSILON)
    00021   {
    00022     perpendicularPoint = pp;
    00023     distance = pp.distanceTo(p);
    00024     return true;
    00025   }
    00026   else
    00027   {
    00028     return false;
    00029   }
    00030 }
    00031
    00032
    00033 /** Only used by reduceToConvexHullByWrapping*/
    00034 inline double theta(const PfVec& p1, const PfVec& p2)
    00035 {
    00036   double dx(p2.x-p1.x);
    00037   double dy(p2.y-p1.y);
    00038   double ax(fabs(dx));
    00039   double ay(fabs(dy));
    00040   double t;
    00041   if((ax<EPSILON) && (ay<EPSILON))
    00042   {
    00043     t = 0.0;
    00044   }
    00045   else
    00046   {
    00047     t = (dy / (ax + ay));
    00048   }
    00049   if(dx < 0.0)
    00050   {
    00051     t = 2.0 - t;
    00052   }
    00053   else if(dy < 0.0)
    00054   {
    00055     t = 4.0 + t;
    00056   }
    00057   return t*pi_2;
    00058 }
    00059
    00060
    00061 void reduceToConvexHullByWrapping(Polygon& p)
    00062 {
    00063   unsigned int minIdx(0);
    00064   int lastHullPoint(-1);
    00065   double minAngle(0.0), v(0.0);
    00066   PfVec t;
    00067   for(unsigned int j=1; j<p.pts.size(); j++)
    00068   {
    00069     if(p.pts[j].y < p.pts[minIdx].y)
    00070     {
    00071       minIdx = j;
    00072     }
    00073   }
    00074   p.pts.push_back(p.pts[minIdx]);
    00075   unsigned int lastIdx(p.pts.size()-1);
    00076   do
    00077   {
    00078     ++lastHullPoint;
    00079     t = p.pts[(unsigned int)lastHullPoint];
    00080     p.pts[(unsigned int)lastHullPoint] = p.pts[minIdx];
    00081     p.pts[minIdx] = t;
    00082     minIdx = lastIdx;
    00083     v = minAngle;
    00084     minAngle = pi2;
    00085     for(unsigned int i = (lastHullPoint+1); i <= lastIdx; i++)
    00086     {
    00087       if(theta(p.pts[(unsigned int)lastHullPoint], p.pts[i]) > v)
    00088       {
    00089         if(theta(p.pts[(unsigned int)lastHullPoint], p.pts[i]) < minAngle)
    00090         {
    00091           minIdx = i;
    00092           minAngle = theta(p.pts[(unsigned int)lastHullPoint], p.pts[minIdx]);
    00093         }
    00094       }
    00095     }
    00096   } while (minIdx != lastIdx);
    00097   for(int k = lastIdx; k > lastHullPoint; k--)
    00098   {
    00099     p.pts.pop_back();
    00100   }
    00101 }
    00102
    00103
    00104 /** Checks if three points are ordered counter clockwise*/
    00105 inline int ccw(const PfVec& p0, const PfVec& p1, const PfVec& p2)
    00106 {
    00107   double dx1 = p1.x - p0.x;
    00108   double dy1 = p1.y - p0.y;
    00109   double dx2 = p2.x - p0.x;
    00110   double dy2 = p2.y - p0.y;
    00111   if(dx1*dy2 > dy1*dx2)
    00112   {
    00113     return 1;
    00114   }
    00115   if(dx1*dy2 < dy1*dx2)
    00116   {
    00117     return -1;
    00118   }
    00119   // Now (dx1*dy2 == dy1*dx2) must be true
    00120   if((dx1*dx2 < 0.0) || (dy1*dy2 < 0.0))
    00121   {
    00122     return -1;
    00123   }
    00124   if((dx1*dx1 + dy1*dy1) >= (dx2*dx2 + dy2*dy2))
    00125   {
    00126     return 0;
    00127   }
    00128   return 1;
    00129 }
    00130
    00131
    00132 inline bool testIntersection(const Line& l1, const Line& l2)
    00133 {
    00134   return (((ccw(l1.p1, l1.p2, l2.p1) * ccw(l1.p1,l1.p2,l2.p2)) <= 0)
    00135           && ((ccw(l2.p1, l2.p2, l1.p1) * ccw(l2.p1,l2.p2,l1.p2)) <= 0));
    00136 }
    00137
    00138
    00139 void intersectLineAndLine(const Line& l1, const Line& l2,
    00140                           std::vector<PfVec>& intersections)
    00141 {
    00142   if(testIntersection(l1,l2))
    00143   {
    00144     PfVec p = l1.p1;
    00145     PfVec v = (l1.p2 - l1.p1);
    00146     PfVec q = l2.p1;
    00147     PfVec w = (l2.p2 - l2.p1);
    00148     PfVec u(q-p);
    00149     double denominator = w.x*v.y - w.y*v.x;
    00150     if(denominator != 0.0)
    00151     {
    00152       double s = (u.y*v.x - u.x*v.y) / denominator;
    00153       PfVec pt(q);
    00154       pt += (w*s);
    00155       intersections.push_back(pt);
    00156     }
    00157   }
    00158 }
    00159
    00160
    00161 void intersectLineAndPolygon(const Line& line, const Polygon& poly,
    00162                             std::vector<PfVec>& intersections)
    00163 {
    00164   Line polyLine;
    00165   int numberOfPoints = poly.pts.size();
    00166   for(int i=0; i<numberOfPoints; i++)
    00167   {
    00168     polyLine.p1 = poly.pts[i];
    00169     polyLine.p2 = poly.pts[(i+1) % numberOfPoints];
    00170     intersectLineAndLine(line, polyLine, intersections);
    00171   }
    00172 }
    00173
    00174
    00175 void intersectLineAndCircle(const Line& line, const Circle& circle,
    00176                             std::vector<PfVec>& intersections)
    00177 {
    00178   double distLineToCircle = (circle.position - line.position).length();
    00179   if(distLineToCircle > (circle.radiusOfCollisionCircle + line.radiusOfCollisionCircle))
    00180   {
    00181     return;
    00182   }
    00183   PfVec v(line.p2-line.p1);
    00184   PfVec d(line.p1-circle.position);
    00185   double r = circle.radiusOfCollisionCircle;
    00186   double vv = v.scalarProduct(v);
    00187   double dv = d.scalarProduct(v);
    00188   double dd = d.scalarProduct(d);
    00189   if(vv != 0.0)
    00190   {
    00191     double term = - dv/vv;
    00192     double denominator = (vv*(r*r - dd) + dv*dv) / (vv*vv);
    00193     if(denominator < 0.0)
    00194     {
    00195       return;
    00196     }
    00197     double sqrtValue = sqrt(denominator);
    00198     double s = term + sqrtValue;
    00199     if((s < 0.0) || (s > line.radiusOfCollisionCircle))
    00200     {
    00201       return;
    00202     }
    00203     PfVec p1(line.p1 + (v*s));
    00204     intersections.push_back(p1);
    00205     if(sqrtValue != 0.0)
    00206     {
    00207       s = term - sqrtValue;
    00208       PfVec p2(line.p1 + (v*s));
    00209       intersections.push_back(p2);
    00210     }
    00211   }
    00212 }
    00213
    00214
    00215 void intersectPolygonAndCircle(const Polygon& polygon, const Circle& circle,
    00216                               std::vector<PfVec>& intersections)
    00217 {
    00218   Line polyLine;
    00219   int numberOfPoints = polygon.pts.size();
    00220   for(int i=0; i<numberOfPoints; i++)
    00221   {
    00222     polyLine.p1 = polygon.pts[i];
    00223     polyLine.p2 = polygon.pts[(i+1) % numberOfPoints];
    00224     intersectLineAndCircle(polyLine, circle, intersections);
    00225   }
    00226 }
    00227
    00228
    00229 void intersectCircleAndCircle(const Circle& circle1, const Circle& circle2,
    00230                               std::vector<PfVec>& intersections)
    00231 {
    00232   double xt(circle2.position.x - circle1.position.x);
    00233   double yt(circle2.position.y - circle1.position.y);
    00234   double r0Quad(circle1.radius*circle1.radius);
    00235   double r1Quad(circle2.radius*circle2.radius);
    00236   PfVec newPoint;
    00237   if(fabs(xt) < EPSILON)
    00238   {
    00239     if(fabs(yt) < EPSILON)
    00240     {
    00241       return;
    00242     }
    00243     else
    00244     {
    00245       double y((r1Quad - r0Quad - yt*yt) / (-2.0 * yt));
    00246       double denominator(r0Quad - y*y);
    00247       if(denominator < 0.0)
    00248       {
    00249         return;
    00250       }
    00251       else if(fabs(denominator) < EPSILON)
    00252       {
    00253         newPoint.y = y;
    00254         newPoint.x = 0.0;
    00255         intersections.push_back(newPoint);
    00256       }
    00257       else
    00258       {
    00259         newPoint.y = y;
    00260         newPoint.x = sqrt(denominator);
    00261         intersections.push_back(newPoint);
    00262         newPoint.x = -sqrt(denominator);
    00263         intersections.push_back(newPoint);
    00264       }
    00265     }
    00266   }
    00267   else
    00268   {
    00269     double k((r1Quad - r0Quad - xt*xt - yt*yt) / -2.0);
    00270     double ytxtQuad(yt*yt + xt*xt);
    00271     double p((-2.0*yt*k) / ytxtQuad);
    00272     double q((k*k - r0Quad*xt*xt) / ytxtQuad);
    00273     double denominator((p*p/4.0) - q);
    00274     if(denominator < 0.0)
    00275     {
    00276       return;
    00277     }
    00278     else if(fabs(denominator) < EPSILON)
    00279     {
    00280       newPoint.y = -p/2.0;
    00281       newPoint.x = (k - newPoint.y*yt) / xt;
    00282       intersections.push_back(newPoint);
    00283     }
    00284     else
    00285     {
    00286       newPoint.y = (-pi/2.0) + sqrt(denominator);
    00287       newPoint.x = (k - newPoint.y*yt) / xt;
    00288       intersections.push_back(newPoint);
    00289       newPoint.y = (-pi/2.0) - sqrt(denominator);
    00290       newPoint.x = (k - newPoint.y*yt) / xt;
    00291       intersections.push_back(newPoint);
    00292     }
    00293   }
    00294 }
    00295
    00296
    00297 void intersectGeometricObjects(PfieldGeometricObject* g1, PfieldGeometricObject* g2,
    00298                                std::vector<PfVec>& intersections)
    00299 {
    00300   if(!(g1->intersectable && g2->intersectable))
    00301   {
    00302     return;
    00303   }
    00304   PfVec distVec(g2->position - g1->position);
    00305   if(distVec.length() <= (g2->radiusOfCollisionCircle + g1->radiusOfCollisionCircle))
    00306   {
    00307     if(g1->getType() == LINE)
    00308     {
    00309       switch(g2->getType())
    00310       {
    00311       case POLYGON: intersectLineAndPolygon(*(Line*)g1, *(Polygon*)g2, intersections);
    00312         break;
    00313       case LINE: intersectLineAndLine(*(Line*)g1, *(Line*)g2, intersections);
    00314         break;
    00315       case CIRCLE: intersectLineAndCircle(*(Line*)g1, *(Circle*)g2, intersections);
    00316         break;
    00317       case NONE: break;
    00318       };
    00319     }
    00320     else if(g1->getType() == CIRCLE)
    00321     {
    00322       switch(g2->getType())
    00323       {
    00324       case POLYGON: intersectPolygonAndCircle(*(Polygon*)g2, *(Circle*)g1, intersections);
    00325         break;
    00326       case LINE: intersectLineAndCircle(*(Line*)g2, *(Circle*)g1, intersections);
    00327         break;
    00328       case CIRCLE: intersectCircleAndCircle(*(Circle*)g1, *(Circle*)g2, intersections);
    00329       default: break;
    00330       };
    00331     }
    00332   }
    00333 }
    00334
    00335
    00336 double Circle::distanceTo(const PfPose& base, const PfVec& pos, PfVec& contact) const
    00337 {
    00338   double distToCenter = base.pos.distanceTo(pos);
    00339   PfVec vecCenterToPos = (pos - base.pos);
    00340   vecCenterToPos.normalize();
    00341   contact = (base.pos + (vecCenterToPos*radius));
    00342   if(distToCenter <= radius)
    00343   {
    00344     return 0.0;
    00345   }
    00346   else
    00347   {
    00348     return (distToCenter -radius);
    00349   }
    00350 }
    00351
    00352
    00353 void Circle::initRadiusOfCollisionCircle()
    00354 {
    00355   radiusOfCollisionCircle = radius;
    00356 }
    00357
    00358
    00359 PfieldGeometricObject* Circle::getAbs(const PfPose& base) const
    00360 {
    00361   PfieldGeometricObject* absGeo = clone();
    00362   absGeo->position = base.pos;
    00363   return absGeo;
    00364 }
    00365
    00366
    00367 void Circle::setAbsoluteFromOther(const PfPose& base, PfieldGeometricObject* other)
    00368 {
    00369   position = base.pos;
    00370 }
    00371
    00372
    00373 PfieldGeometricObject* Circle::clone() const
    00374 {
    00375     Circle* newCircle = new Circle();
    00376     newCircle->radius = radius;
    00377     PfieldGeometricObject* geo = newCircle;
    00378     geo->radiusOfCollisionCircle = radiusOfCollisionCircle;
    00379     geo->intersectable = intersectable;
    00380     geo->position = position;
    00381     return geo;
    00382 }
    00383
    00384
    00385 void Circle::getPoints(std::vector<PfVec>& points)
    00386 {
    00387   points.push_back(position);
    00388 }
    00389
    00390
    00391 double Line::distanceTo(const PfPose& base, const PfVec& pos, PfVec& contact) const
    00392 {
    00393   PfVec p = (pos - base.pos);
    00394   p.rotate(-base.rotation);
    00395   double distance;
    00396   PfVec pTest;
    00397   if(pointPerpendicularToLine(p1, p2, p, distance, pTest))
    00398   {
    00399     contact = pTest;
    00400   }
    00401   else
    00402   {
    00403     distance = p.distanceTo(p1);
    00404     double distance2 = p.distanceTo(p2);
    00405     if(distance2 < distance)
    00406     {
    00407       contact = p2;
    00408       distance = distance2;
    00409     }
    00410     else
    00411     {
    00412       contact = p1;
    00413     }
    00414   }
    00415   contact.rotate(base.rotation);
    00416   contact += base.pos;
    00417   return distance;
    00418 }
    00419
    00420
    00421 void Line::initRadiusOfCollisionCircle()
    00422 {
    00423   radiusOfCollisionCircle = (p2-p1).length()*0.5;
    00424 }
    00425
    00426
    00427 void Line::setAbsoluteFromOther(const PfPose& base, PfieldGeometricObject* other)
    00428 {
    00429   position = base.pos;
    00430   Line* otherLine = (Line*)other;
    00431   p1 = otherLine->p1;
    00432   p2 = otherLine->p2;
    00433   p1.rotate(base.rotation);
    00434   p1 += base.pos;
    00435   p2.rotate(base.rotation);
    00436   p2 += base.pos;
    00437 }
    00438
    00439
    00440 PfieldGeometricObject* Line::getAbs(const PfPose& base) const
    00441 {
    00442   PfieldGeometricObject* absGeo = this->clone();
    00443   Line* line = (Line*)(absGeo);
    00444   line->p1.rotate(base.rotation);
    00445   line->p1 += base.pos;
    00446   line->p2.rotate(base.rotation);
    00447   line->p2 += base.pos;
    00448   return line;
    00449 }
    00450
    00451
    00452 PfieldGeometricObject* Line::clone() const
    00453 {
    00454     Line* newLine = new Line();
    00455     newLine->p1 = p1;
    00456     newLine->p2 = p2;
    00457     PfieldGeometricObject* geo = newLine;
    00458     geo->radiusOfCollisionCircle = radiusOfCollisionCircle;
    00459     geo->intersectable = intersectable;
    00460     geo->position = position;
    00461     return geo;
    00462 }
    00463
    00464
    00465 void Line::getPoints(std::vector<PfVec>& points)
    00466 {
    00467   points.push_back(p1);
    00468   points.push_back(p2);
    00469 }
    00470
    00471
    00472 double Polygon::distanceTo(const PfPose& base, const PfVec& pos, PfVec& contact) const
    00473 {
    00474   PfVec p = (pos - base.pos);
    00475   p.rotate(-base.rotation);
    00476   PfVec pTest;
    00477   PfVec pPoint = pts[0];
    00478   double distance;
    00479   double minDistance = p.distanceTo(pts[0]);
    00480   unsigned int i;
    00481   //Check distances to points
    00482   for(i=1; i<pts.size(); i++)
    00483   {
    00484     distance = p.distanceTo(pts[i]);
    00485     if(distance<minDistance)
    00486     {
    00487       minDistance = distance;
    00488       pPoint = pts[i];
    00489     }
    00490   }
    00491   //Check distances to lines
    00492   unsigned int numOfPoints = pts.size();
    00493   for(i=0; i<pts.size(); i++)
    00494   {
    00495     if(pointPerpendicularToLine(pts[i], pts[(i+1) % numOfPoints], p, distance, pTest))
    00496     {
    00497       if(distance<minDistance)
    00498       {
    00499         minDistance = distance;
    00500         pPoint = pTest;
    00501       }
    00502     }
    00503   }
    00504   pPoint.rotate(base.rotation);
    00505   pPoint += base.pos;
    00506   contact = pPoint;
    00507   if(pointInside(p))
    00508   {
    00509     minDistance = 0.0;
    00510   }
    00511   return minDistance;
    00512 }
    00513
    00514
    00515 void Polygon::initRadiusOfCollisionCircle()
    00516 {
    00517   radiusOfCollisionCircle = pts[0].squareLength();
    00518   double dist;
    00519   for(unsigned int i=0; i<pts.size(); i++)
    00520   {
    00521     dist = pts[i].squareLength();
    00522     if(dist > radiusOfCollisionCircle)
    00523     {
    00524       radiusOfCollisionCircle = dist;
    00525     }
    00526   }
    00527   radiusOfCollisionCircle = sqrt(radiusOfCollisionCircle);
    00528 }
    00529
    00530
    00531 bool Polygon::pointInside(const PfVec& p) const
    00532 {
    00533   if(p.length() > radiusOfCollisionCircle)
    00534   {
    00535     return false;
    00536   }
    00537   double totalAngle = 0;
    00538   PfVec a,b;
    00539   unsigned int numOfPoints = pts.size();
    00540   for(unsigned int i=0; i<numOfPoints; i++)
    00541   {
    00542     a = (pts[i]-p);
    00543     b = (pts[(i+1) % numOfPoints]-p);
    00544     totalAngle += acos(a.scalarProduct(b)/(a.length()*b.length()));
    00545   }
    00546   if(totalAngle < (pi2 - EPSILON))
    00547   {
    00548     return false;
    00549   }
    00550   else
    00551   {
    00552     return true;
    00553   }
    00554 }
    00555
    00556
    00557 PfieldGeometricObject* Polygon::getAbs(const PfPose& base) const
    00558 {
    00559   PfieldGeometricObject* absGeo = this->clone();
    00560   Polygon* poly = (Polygon*)(absGeo);
    00561   for(unsigned int i=0; i<pts.size(); i++)
    00562   {
    00563     poly->pts[i].rotate(base.rotation);
    00564     poly->pts[i] += base.pos;
    00565   }
    00566   return poly;
    00567 }
    00568
    00569
    00570 void Polygon::setAbsoluteFromOther(const PfPose& base, PfieldGeometricObject* other)
    00571 {
    00572   position = base.pos;
    00573   Polygon* otherPoly = (Polygon*)(other);
    00574   for(unsigned int i=0; i<pts.size(); i++)
    00575   {
    00576     pts[i] = otherPoly->pts[i];
    00577     pts[i].rotate(base.rotation);
    00578     pts[i] += base.pos;
    00579   }
    00580 }
    00581
    00582
    00583 PfieldGeometricObject* Polygon::clone() const
    00584 {
    00585    Polygon* newPoly = new Polygon();
    00586    newPoly->pts = pts;
    00587    PfieldGeometricObject* geo = newPoly;
    00588    geo->radiusOfCollisionCircle = radiusOfCollisionCircle;
    00589    geo->intersectable = intersectable;
    00590    geo->position = position;
    00591    return geo;
    00592 }
    00593
    00594
    00595 void Polygon::getPoints(std::vector<PfVec>& points)
    00596 {
    00597   std::vector< PfVec >::const_iterator point;
    00598   for(point = pts.begin(); point != pts.end(); ++point)
    00599   {
    00600     points.push_back((*point));
    00601   }
    00602 }
    00603
    00604
    00605 double NoGeometry::distanceTo(const PfPose& base, const PfVec& pos, PfVec& contact) const
    00606 {
    00607   contact = base.pos;
    00608   return base.pos.distanceTo(pos);
    00609 }
    00610
    00611
    00612 PfieldGeometricObject* NoGeometry::getAbs(const PfPose& base) const
    00613 {
    00614   PfieldGeometricObject* absGeo = clone();
    00615   absGeo->position = base.pos;
    00616   return absGeo;
    00617 }
    00618
    00619
    00620 void NoGeometry::setAbsoluteFromOther(const PfPose& base, PfieldGeometricObject* other)
    00621 {
    00622   position = base.pos;
    00623 }
    00624
    00625
    00626 PfieldGeometricObject* NoGeometry::clone() const
    00627 {
    00628     NoGeometry* newNoGeometry = new NoGeometry();
    00629     PfieldGeometricObject* geo = newNoGeometry;
    00630     geo->radiusOfCollisionCircle = radiusOfCollisionCircle;
    00631     geo->intersectable = intersectable;
    00632     geo->position = position;
    00633     return geo;
    00634 }
    00635
    00636
    00637 bool Sector::pointInside(const PfPose& base, const PfVec& pos) const
    00638 {
    00639   double angleToPos(base.getAngleTo(pos));
    00640   return (fabs(angleToPos) <= openingAngle/2.0);
    00641 }