Tools/PotentialFields/PfieldGeometry.cpp

Go to the documentation of this file.

/**
* @file PfieldGeometry.cpp
* 
* Implementation of several geometric functions and objects used by potential fields
*
* @author <a href="mailto:timlaue@informatik.uni-bremen.de">Tim Laue</a>
*/


#include "PfieldGeometry.h"



inline bool pointPerpendicularToLine(const PfVec& l1, const PfVec& l2, const PfVec& p,
                              double& distance, PfVec& perpendicularPoint)
{
  PfVec v = (l2 - l1);
  double r = (v.scalarProduct(p) - v.scalarProduct(l1)) / (v.x*v.x + v.y*v.y);
  PfVec pp = (l1 + (v*r));
  if(fabs(pp.distanceTo(l1) + pp.distanceTo(l2) - l1.distanceTo(l2))<EPSILON)
  {
    perpendicularPoint = pp;
    distance = pp.distanceTo(p);
    return true;
  }
  else
  {
    return false;
  }
}


/** Only used by reduceToConvexHullByWrapping*/
inline double theta(const PfVec& p1, const PfVec& p2)
{
  double dx(p2.x-p1.x);
  double dy(p2.y-p1.y);
  double ax(fabs(dx));
  double ay(fabs(dy));
  double t;
  if((ax<EPSILON) && (ay<EPSILON))
  {
    t = 0.0;
  }
  else
  {
    t = (dy / (ax + ay));
  }
  if(dx < 0.0)
  {
    t = 2.0 - t;
  }
  else if(dy < 0.0)
  {
    t = 4.0 + t;
  }
  return t*pi_2;
}


void reduceToConvexHullByWrapping(Polygon& p)
{
  unsigned int minIdx(0);
  int lastHullPoint(-1);
  double minAngle(0.0), v(0.0);
  PfVec t;
  for(unsigned int j=1; j<p.pts.size(); j++)
  {
    if(p.pts[j].y < p.pts[minIdx].y)
    {
      minIdx = j;
    }
  }
  p.pts.push_back(p.pts[minIdx]);
  unsigned int lastIdx(p.pts.size()-1);
  do
  {
    ++lastHullPoint;
    t = p.pts[(unsigned int)lastHullPoint];
    p.pts[(unsigned int)lastHullPoint] = p.pts[minIdx];
    p.pts[minIdx] = t;
    minIdx = lastIdx;
    v = minAngle;
    minAngle = pi2;
    for(unsigned int i = (lastHullPoint+1); i <= lastIdx; i++)
    {
      if(theta(p.pts[(unsigned int)lastHullPoint], p.pts[i]) > v)
      {
        if(theta(p.pts[(unsigned int)lastHullPoint], p.pts[i]) < minAngle)
        {
          minIdx = i;
          minAngle = theta(p.pts[(unsigned int)lastHullPoint], p.pts[minIdx]);
        }
      }
    }
  } while (minIdx != lastIdx);
  for(int k = lastIdx; k > lastHullPoint; k--)
  {
    p.pts.pop_back();
  }
}


/** Checks if three points are ordered counter clockwise*/
inline int ccw(const PfVec& p0, const PfVec& p1, const PfVec& p2)
{
  double dx1 = p1.x - p0.x; 
  double dy1 = p1.y - p0.y;
  double dx2 = p2.x - p0.x;
  double dy2 = p2.y - p0.y;
  if(dx1*dy2 > dy1*dx2)
  {
    return 1;
  }
  if(dx1*dy2 < dy1*dx2)
  {
    return -1;
  }
  // Now (dx1*dy2 == dy1*dx2) must be true
  if((dx1*dx2 < 0.0) || (dy1*dy2 < 0.0))
  {
    return -1;
  }
  if((dx1*dx1 + dy1*dy1) >= (dx2*dx2 + dy2*dy2))
  {
    return 0;
  }
  return 1;
}


inline bool testIntersection(const Line& l1, const Line& l2)
{
  return (((ccw(l1.p1, l1.p2, l2.p1) * ccw(l1.p1,l1.p2,l2.p2)) <= 0)
          && ((ccw(l2.p1, l2.p2, l1.p1) * ccw(l2.p1,l2.p2,l1.p2)) <= 0));
}


void intersectLineAndLine(const Line& l1, const Line& l2, 
                          std::vector<PfVec>& intersections)
{
  if(testIntersection(l1,l2))
  {
    PfVec p = l1.p1;
    PfVec v = (l1.p2 - l1.p1);
    PfVec q = l2.p1;
    PfVec w = (l2.p2 - l2.p1);
    PfVec u(q-p);
    double denominator = w.x*v.y - w.y*v.x;
    if(denominator != 0.0)
    {
      double s = (u.y*v.x - u.x*v.y) / denominator;
      PfVec pt(q);
      pt += (w*s);
      intersections.push_back(pt);
    }
  }
}


void intersectLineAndPolygon(const Line& line, const Polygon& poly, 
                            std::vector<PfVec>& intersections)
{
  Line polyLine;
  int numberOfPoints = poly.pts.size();
  for(int i=0; i<numberOfPoints; i++)
  {
    polyLine.p1 = poly.pts[i];
    polyLine.p2 = poly.pts[(i+1) % numberOfPoints];
    intersectLineAndLine(line, polyLine, intersections);
  }
}


void intersectLineAndCircle(const Line& line, const Circle& circle, 
                            std::vector<PfVec>& intersections)
{
  double distLineToCircle = (circle.position - line.position).length();
  if(distLineToCircle > (circle.radiusOfCollisionCircle + line.radiusOfCollisionCircle))
  {
    return;
  }
  PfVec v(line.p2-line.p1);
  PfVec d(line.p1-circle.position);
  double r = circle.radiusOfCollisionCircle;
  double vv = v.scalarProduct(v);
  double dv = d.scalarProduct(v);
  double dd = d.scalarProduct(d);
  if(vv != 0.0)
  {
    double term = - dv/vv;
    double denominator = (vv*(r*r - dd) + dv*dv) / (vv*vv);
    if(denominator < 0.0)
    {
      return;
    }
    double sqrtValue = sqrt(denominator);
    double s = term + sqrtValue;
    if((s < 0.0) || (s > line.radiusOfCollisionCircle))
    {
      return;
    }
    PfVec p1(line.p1 + (v*s));
    intersections.push_back(p1);
    if(sqrtValue != 0.0)
    {
      s = term - sqrtValue;
      PfVec p2(line.p1 + (v*s));
      intersections.push_back(p2);
    }
  }
}


void intersectPolygonAndCircle(const Polygon& polygon, const Circle& circle, 
                              std::vector<PfVec>& intersections)
{
  Line polyLine;
  int numberOfPoints = polygon.pts.size();
  for(int i=0; i<numberOfPoints; i++)
  {
    polyLine.p1 = polygon.pts[i];
    polyLine.p2 = polygon.pts[(i+1) % numberOfPoints];
    intersectLineAndCircle(polyLine, circle, intersections);
  }
}


void intersectCircleAndCircle(const Circle& circle1, const Circle& circle2, 
                              std::vector<PfVec>& intersections)
{
  double xt(circle2.position.x - circle1.position.x);
  double yt(circle2.position.y - circle1.position.y);
  double r0Quad(circle1.radius*circle1.radius);
  double r1Quad(circle2.radius*circle2.radius);
  PfVec newPoint;
  if(fabs(xt) < EPSILON)
  {
    if(fabs(yt) < EPSILON)
    {
      return;
    }
    else
    {    
      double y((r1Quad - r0Quad - yt*yt) / (-2.0 * yt));
      double denominator(r0Quad - y*y);
      if(denominator < 0.0)
      {
        return;
      }
      else if(fabs(denominator) < EPSILON)
      {
        newPoint.y = y;
        newPoint.x = 0.0;
        intersections.push_back(newPoint);
      }
      else
      {
        newPoint.y = y;
        newPoint.x = sqrt(denominator);
        intersections.push_back(newPoint);
        newPoint.x = -sqrt(denominator);
        intersections.push_back(newPoint);
      }
    }
  }
  else
  {
    double k((r1Quad - r0Quad - xt*xt - yt*yt) / -2.0);
    double ytxtQuad(yt*yt + xt*xt);
    double p((-2.0*yt*k) / ytxtQuad);
    double q((k*k - r0Quad*xt*xt) / ytxtQuad);
    double denominator((p*p/4.0) - q);
    if(denominator < 0.0)
    {
      return;
    }
    else if(fabs(denominator) < EPSILON)
    {
      newPoint.y = -p/2.0;
      newPoint.x = (k - newPoint.y*yt) / xt;
      intersections.push_back(newPoint);
    }
    else
    {
      newPoint.y = (-pi/2.0) + sqrt(denominator);
      newPoint.x = (k - newPoint.y*yt) / xt;
      intersections.push_back(newPoint);
      newPoint.y = (-pi/2.0) - sqrt(denominator);
      newPoint.x = (k - newPoint.y*yt) / xt;
      intersections.push_back(newPoint);
    }
  }
}


void intersectGeometricObjects(PfieldGeometricObject* g1, PfieldGeometricObject* g2, 
                               std::vector<PfVec>& intersections)
{
  if(!(g1->intersectable && g2->intersectable))
  {
    return;
  }
  PfVec distVec(g2->position - g1->position);
  if(distVec.length() <= (g2->radiusOfCollisionCircle + g1->radiusOfCollisionCircle))
  {
    if(g1->getType() == LINE)
    {
      switch(g2->getType())
      {
      case POLYGON: intersectLineAndPolygon(*(Line*)g1, *(Polygon*)g2, intersections);
        break;
      case LINE: intersectLineAndLine(*(Line*)g1, *(Line*)g2, intersections);
        break;
      case CIRCLE: intersectLineAndCircle(*(Line*)g1, *(Circle*)g2, intersections); 
        break;
      case NONE: break;
      };
    }
    else if(g1->getType() == CIRCLE)
    {
      switch(g2->getType())
      {
      case POLYGON: intersectPolygonAndCircle(*(Polygon*)g2, *(Circle*)g1, intersections);
        break;
      case LINE: intersectLineAndCircle(*(Line*)g2, *(Circle*)g1, intersections); 
        break;
      case CIRCLE: intersectCircleAndCircle(*(Circle*)g1, *(Circle*)g2, intersections);
      default: break;
      };
    }
  }
}


double Circle::distanceTo(const PfPose& base, const PfVec& pos, PfVec& contact) const
{
  double distToCenter = base.pos.distanceTo(pos);
  PfVec vecCenterToPos = (pos - base.pos);
  vecCenterToPos.normalize();
  contact = (base.pos + (vecCenterToPos*radius));
  if(distToCenter <= radius)
  {
    return 0.0;
  }
  else
  {  
    return (distToCenter -radius);
  }
}


void Circle::initRadiusOfCollisionCircle()
{
  radiusOfCollisionCircle = radius;
}


PfieldGeometricObject* Circle::getAbs(const PfPose& base) const
{
  PfieldGeometricObject* absGeo = clone();
  absGeo->position = base.pos;
  return absGeo;
}


void Circle::setAbsoluteFromOther(const PfPose& base, PfieldGeometricObject* other)
{
  position = base.pos;
}


PfieldGeometricObject* Circle::clone() const
{
    Circle* newCircle = new Circle();
    newCircle->radius = radius;
    PfieldGeometricObject* geo = newCircle;
    geo->radiusOfCollisionCircle = radiusOfCollisionCircle;
    geo->intersectable = intersectable;
    geo->position = position;
    return geo;
}


void Circle::getPoints(std::vector<PfVec>& points)
{
  points.push_back(position);
}


double Line::distanceTo(const PfPose& base, const PfVec& pos, PfVec& contact) const
{
  PfVec p = (pos - base.pos);
  p.rotate(-base.rotation);
  double distance;
  PfVec pTest;
  if(pointPerpendicularToLine(p1, p2, p, distance, pTest))
  {
    contact = pTest;
  }
  else
  {
    distance = p.distanceTo(p1);
    double distance2 = p.distanceTo(p2);
    if(distance2 < distance)
    {
      contact = p2;
      distance = distance2;
    }
    else
    {
      contact = p1;
    }
  }
  contact.rotate(base.rotation);
  contact += base.pos;
  return distance;
}


void Line::initRadiusOfCollisionCircle()
{
  radiusOfCollisionCircle = (p2-p1).length()*0.5;
}


void Line::setAbsoluteFromOther(const PfPose& base, PfieldGeometricObject* other)
{
  position = base.pos;
  Line* otherLine = (Line*)other;
  p1 = otherLine->p1;
  p2 = otherLine->p2;
  p1.rotate(base.rotation);
  p1 += base.pos;
  p2.rotate(base.rotation);
  p2 += base.pos;
}


PfieldGeometricObject* Line::getAbs(const PfPose& base) const
{
  PfieldGeometricObject* absGeo = this->clone();
  Line* line = (Line*)(absGeo);
  line->p1.rotate(base.rotation);
  line->p1 += base.pos;
  line->p2.rotate(base.rotation);
  line->p2 += base.pos;
  return line;
}


PfieldGeometricObject* Line::clone() const
{
    Line* newLine = new Line();
    newLine->p1 = p1;
    newLine->p2 = p2;
    PfieldGeometricObject* geo = newLine;
    geo->radiusOfCollisionCircle = radiusOfCollisionCircle;
    geo->intersectable = intersectable;
    geo->position = position;
    return geo;
}


void Line::getPoints(std::vector<PfVec>& points)
{
  points.push_back(p1);
  points.push_back(p2);
}


double Polygon::distanceTo(const PfPose& base, const PfVec& pos, PfVec& contact) const
{
  PfVec p = (pos - base.pos);
  p.rotate(-base.rotation);
  PfVec pTest;
  PfVec pPoint = pts[0];
  double distance;
  double minDistance = p.distanceTo(pts[0]);
  unsigned int i;
  //Check distances to points
  for(i=1; i<pts.size(); i++)
  {
    distance = p.distanceTo(pts[i]);
    if(distance<minDistance)
    {
      minDistance = distance;
      pPoint = pts[i];
    }
  }
  //Check distances to lines
  unsigned int numOfPoints = pts.size();
  for(i=0; i<pts.size(); i++)
  {
    if(pointPerpendicularToLine(pts[i], pts[(i+1) % numOfPoints], p, distance, pTest))
    {
      if(distance<minDistance)
      {
        minDistance = distance;
        pPoint = pTest;
      }
    }
  }
  pPoint.rotate(base.rotation);
  pPoint += base.pos;
  contact = pPoint;
  if(pointInside(p))
  {
    minDistance = 0.0;
  }
  return minDistance;
}


void Polygon::initRadiusOfCollisionCircle()
{
  radiusOfCollisionCircle = pts[0].squareLength();
  double dist;
  for(unsigned int i=0; i<pts.size(); i++)
  {
    dist = pts[i].squareLength();
    if(dist > radiusOfCollisionCircle)
    {
      radiusOfCollisionCircle = dist;
    }
  }
  radiusOfCollisionCircle = sqrt(radiusOfCollisionCircle);
}


bool Polygon::pointInside(const PfVec& p) const
{
  if(p.length() > radiusOfCollisionCircle)
  {
    return false;
  }
  double totalAngle = 0;
  PfVec a,b;
  unsigned int numOfPoints = pts.size();
  for(unsigned int i=0; i<numOfPoints; i++)
  {
    a = (pts[i]-p);
    b = (pts[(i+1) % numOfPoints]-p);
    totalAngle += acos(a.scalarProduct(b)/(a.length()*b.length()));
  }
  if(totalAngle < (pi2 - EPSILON))
  {
    return false;
  }
  else
  {
    return true;
  }
}


PfieldGeometricObject* Polygon::getAbs(const PfPose& base) const
{
  PfieldGeometricObject* absGeo = this->clone();
  Polygon* poly = (Polygon*)(absGeo);
  for(unsigned int i=0; i<pts.size(); i++)
  {
    poly->pts[i].rotate(base.rotation);
    poly->pts[i] += base.pos;
  }
  return poly;
}


void Polygon::setAbsoluteFromOther(const PfPose& base, PfieldGeometricObject* other)
{
  position = base.pos;
  Polygon* otherPoly = (Polygon*)(other);
  for(unsigned int i=0; i<pts.size(); i++)
  {
    pts[i] = otherPoly->pts[i];
    pts[i].rotate(base.rotation);
    pts[i] += base.pos;
  }
}


PfieldGeometricObject* Polygon::clone() const
{
   Polygon* newPoly = new Polygon();
   newPoly->pts = pts;
   PfieldGeometricObject* geo = newPoly;
   geo->radiusOfCollisionCircle = radiusOfCollisionCircle;
   geo->intersectable = intersectable;
   geo->position = position;
   return geo;
}


void Polygon::getPoints(std::vector<PfVec>& points)
{
  std::vector< PfVec >::const_iterator point;
  for(point = pts.begin(); point != pts.end(); ++point)
  {
    points.push_back((*point));
  }
}


double NoGeometry::distanceTo(const PfPose& base, const PfVec& pos, PfVec& contact) const
{
  contact = base.pos;
  return base.pos.distanceTo(pos);
}


PfieldGeometricObject* NoGeometry::getAbs(const PfPose& base) const
{
  PfieldGeometricObject* absGeo = clone();
  absGeo->position = base.pos;
  return absGeo;
}


void NoGeometry::setAbsoluteFromOther(const PfPose& base, PfieldGeometricObject* other)
{
  position = base.pos;
}


PfieldGeometricObject* NoGeometry::clone() const
{
    NoGeometry* newNoGeometry = new NoGeometry();
    PfieldGeometricObject* geo = newNoGeometry;
    geo->radiusOfCollisionCircle = radiusOfCollisionCircle;
    geo->intersectable = intersectable;
    geo->position = position;
    return geo;
}


bool Sector::pointInside(const PfPose& base, const PfVec& pos) const
{
  double angleToPos(base.getAngleTo(pos));
  return (fabs(angleToPos) <= openingAngle/2.0);
}

---