geometry.c

卡内基梅隆大学99年机器人足球世界杯2D仿真组源代码。卡内基梅隆大学在人工智能界的巨牛

Vector Line::GetClosestPtInBetween(Vector pt, Vector end1, Vector end2)
{
  if (!OnLine(end1) || !OnLine(end2))
    my_error("Line::InBetween: passed in points that weren't on line");

  if (InBetween(pt, end1, end2))
    return ProjectPoint(pt);
  if (end1.dist2(pt) < end2.dist2(pt))
    return end1;
  else
    return end2;
}


/********************************************************************************/

Vector Line::intersection(Line l)
{
  Vector result = 0;
  if (SameSlope(l)) {
    //if ( B == 0 && l.B == 0 || A/B == l.A/l.B ){
    my_error("Lines have same slope");
    DebugGeom(cout << "Lines have same slope" << endl);
    return result;
  }

  if ( B == 0 ){
    result.x = -C/A;
    result.y = l.get_y(result.x);
    return result;
  }

  if ( l.B == 0){
    result.x = -l.C/l.A;
    result.y = get_y(result.y);
    return result;
  }

  result.x = (C*l.B - B*l.C)/(l.A*B - A*l.B);
  result.y = get_y(result.x);
  return result;
}  

/********************************************************************************/
Bool Line::RayIntersection(Ray r, Vector *ppt)
{
  Line lRay(r);

  if (SameSlope(lRay))
    return FALSE;
  
  *ppt = intersection(lRay);
  return (r.InRightDir(*ppt))
      //fabs(GetNormalizeAngleDeg((*ppt - r.origin).dir() - r.direction.dir())) < 10)
    ? TRUE : FALSE;
}


/********************************************************************************/
Bool Line::IsPtCloserToPtOnLine(Vector pt1, Vector pt2, Vector targ_pt)
{
  if (!OnLine(targ_pt)) 
    my_error("IsPtCloserToPtOnLine: targ_pt not on line");

  pt1 = ProjectPoint(pt1);
  pt2 = ProjectPoint(pt2);

  return (pt1.dist(targ_pt) < pt2.dist(targ_pt)) ? TRUE : FALSE;  
}


/********************************************************************************/

//return TRUE on top/left part of plane
Bool Line::HalfPlaneTest(Vector pt)
{
  if (B==0)
    return (pt.x < -C/A) ? TRUE : FALSE;
  return (pt.y > get_y(pt.x)) ? TRUE : FALSE;
}

Bool Line::SameSlope(Line l)
{
  return ( B == 0 && l.B == 0 || A/B == l.A/l.B ) ? TRUE : FALSE;    
}


/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                       Ray Class                                              */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
Ray::Ray(Vector orig, Vector dir) 
{
  origin = orig;
  if (fabs(dir.y) < FLOAT_EPS && fabs(dir.x) < FLOAT_EPS) {
    my_error("Ray: dir can not be zero");
    direction = Vector (1,0);
  } else {
    direction = dir;
    direction = direction.Normalize();
  }
}

Bool Ray::OnRay(Vector pt)
{
  Vector v = pt - origin;
  return (fabs(Sin(v.dir() - direction.dir()) * v.mod()) < FLOAT_EPS)
    ? TRUE : FALSE;
}

Bool Ray::InRightDir(Vector pt)
{
  return (fabs(GetNormalizeAngleDeg((pt - origin).dir() - direction.dir())) < 10)
    ? TRUE : FALSE;
}

Bool Ray::intersection(Line l, Vector *pPt)
{
  return l.RayIntersection(*this, pPt);
}

Bool Ray::intersection(Ray r, Vector *pPt)
{
  Line thisLine(*this), argLine(r);

  if (thisLine.SameSlope(argLine))
    return FALSE;
  *pPt = thisLine.intersection(argLine);

  /* now make sure that the intersection is the correct direction on both lines */
  return  (InRightDir(*pPt) && r.InRightDir(*pPt))
	   //	   fabs(GetNormalizeAngleDeg((*pPt - origin).dir() - direction.dir())) < 10 &&
	   //fabs(GetNormalizeAngleDeg((*pPt - r.origin).dir() - r.direction.dir())) < 10)
    ? TRUE : FALSE;
}

/* intersects a ray and a cricle */
/* return the number of solutions */
/* psol1 1 is not as afar along the ray as psol2 */
int Ray::CircleIntersect(float rad, Vector center, Vector* psol1, Vector* psol2)
{
  DebugGeom(cout << "RCI: origin: " << origin << "\tdirection: " << direction << endl
	    << "rad: " << rad << "\tcenter: " << center << endl);
  float a,b,c,disc;
  float t1, t2;
  a = Sqr(direction.x) + Sqr(direction.y);
  b = 2.0 * ((origin.x-center.x) * direction.x + (origin.y-center.y) * direction.y);
  c = Sqr(origin.x-center.x) + Sqr(origin.y-center.y) - Sqr(rad);
  DebugGeom(printf(" RCI: a: %f\tb: %f\t c: %f\n", a,b,c));
  
  disc = Sqr(b) - 4 * a * c;
  if (disc < 0) {
    DebugGeom(printf(" RCI disc < 0: %f\n", disc));
    return 0;
  }
  
  disc = sqrt(disc);
  t1 = (-b + disc) / (2.0 * a);
  t2 = (-b - disc) / (2.0 * a);
  DebugGeom(printf(" RCI: t1: %f\tt2: %f\n", t1, t2));
  
  if (t1 > t2) {
    DebugGeom(printf(" RCI: reversing t1, t2\n"));
    float temp = t1;
    t1 = t2;
    t2 = temp;
  }
 
  if (t1 > 0.0) {
    if (t2 > 0.0) {
      *psol1 = origin + direction * t1;
      *psol2 = origin + direction * t2;
      DebugGeom(printf(" RCI:two sols\n"));
      return 2;
    } else {
      my_error("RayCircleIntersect: weird roots");
      return 0;
    }
  } else if (t2 > 0.0) {
    *psol1 = origin + direction * t2;
    DebugGeom(printf(" RCI:t2 only sol\n"));
    return 1;
  } else
    return 0;

  return 0;
}

Vector Ray::RectangleIntersection(Rectangle R)
{
  return R.RayIntersection(*this);
}

Vector Ray::GetClosestPoint(Vector pt)
{
  Line l(*this);
  Vector close_pt = l.ProjectPoint(pt);
  if (OnRay(close_pt))
    return close_pt;
  else
    return origin;
}



/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                  Miscellaneous Geometry Functions                            */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/

#ifdef OLD_CODE
/* intersects a ray and a cricle */
/* return the number of solutions */
/* psol1 1 is not as afar along the ray as psol2 */
int RayCircleIntersect(Ray r, float rad, Vector center,
		       Vector* psol1, Vector* psol2)
{
  DebugGeom(cout << "RCI: r.origin: " << r.origin << "\tr.direction: " << r.direction << endl
	    << "rad: " << rad << "\tcenter: " << center << endl);
  float a,b,c,disc;
  float t1, t2;
  a = Sqr(r.direction.x) + Sqr(r.direction.y);
  b = 2.0 * ((r.origin.x-center.x) * r.direction.x + (r.origin.y-center.y) * r.direction.y);
  c = Sqr(r.origin.x-center.x) + Sqr(r.origin.y-center.y) - Sqr(rad);
  DebugGeom(printf(" RCI: a: %f\tb: %f\t c: %f\n", a,b,c));
  
  disc = Sqr(b) - 4 * a * c;
  if (disc < 0) {
    DebugGeom(printf(" RCI disc < 0: %f\n", disc));
    return 0;
  }
  
  disc = sqrt(disc);
  t1 = (-b + disc) / (2.0 * a);
  t2 = (-b - disc) / (2.0 * a);
  DebugGeom(printf(" RCI: t1: %f\tt2: %f\n", t1, t2));
  
  if (t1 > t2) {
    DebugGeom(printf(" RCI: reversing t1, t2\n"));
    float temp = t1;
    t1 = t2;
    t2 = temp;
  }
 
  if (t1 > 0.0) {
    if (t2 > 0.0) {
      *psol1 = r.origin + r.direction * t1;
      *psol2 = r.origin + r.direction * t2;
      DebugGeom(printf(" RCI:two sols\n"));
      return 2;
    } else {
      my_error("RayCircleIntersect: weird roots");
      return 0;
    }
  } else if (t2 > 0.0) {
    *psol1 = r.origin + r.direction * t2;
    DebugGeom(printf(" RCI:t2 only sol\n"));
    return 1;
  } else
    return 0;

  return 0;
}
#endif

/********************************************************************************/

int QuadraticFormula(float a, float b, float c, float* psol1, float* psol2)
{
  float d = Sqr(b) - 4*a*c;
  if (fabs(d) < FLOAT_EPS) {
    *psol1 = -b / (2 * a);
    return 1;
  } else if (d < 0) {
    return 0;
  } else {
    d = sqrt(d);
    *psol1 = (-b + d ) / (2 * a);
    *psol2 = (-b - d ) / (2 * a);
    return 2;
  }
}

/********************************************************************************/

/* some test code
   Vector sol1, sol2;
  int num;
  num = LineCircleIntersect(LineFromTwoPoints(Vector(-10, 2), Vector(10, 2)),
					      1, Vector(1,2), &sol1, &sol2);
  cout << "num: " << num << "\tsol1: " << sol1 << "\tsol2: " << sol2 << endl;
  num = LineCircleIntersect(LineFromTwoPoints(Vector(2, 10), Vector(2, -10)),
					      1, Vector(2,1), &sol1, &sol2);
  cout << "num: " << num << "\tsol1: " << sol1 << "\tsol2: " << sol2 << endl;
  num = LineCircleIntersect(LineFromTwoPoints(Vector(-10, -10), Vector(10, 10)),
					      sqrt(2), Vector(1,1), &sol1, &sol2);
  cout << "num: " << num << "\tsol1: " << sol1 << "\tsol2: " << sol2 << endl;
  exit(1);
  */
int LineCircleIntersect(Line l, float rad, Vector center,
			Vector* psol1, Vector* psol2)
{
  *psol1 = *psol2 = Vector(0,0);
  if (fabs(l.A) > FLOAT_EPS) {
    float a,b,c;
    a = 1 + Sqr(l.B/l.A);
    b = 2*(-center.y + (l.C/l.A + center.x)*(l.B/l.A));
    c = -Sqr(rad) + Sqr(l.C/l.A + center.x) + Sqr(center.y);
    int numSol = QuadraticFormula(a, b, c, &(psol1->y), &(psol2->y));
    psol1->x = - ( (l.B * psol1->y + l.C) / l.A );
    psol2->x = - ( (l.B * psol2->y + l.C) / l.A );
    return numSol;
  } else {
    int numSol = QuadraticFormula(1, -2*center.x,
				  Sqr(center.x) + Sqr(l.C/l.B + center.y) - Sqr(rad),
				  &(psol1->x), &(psol2->x));
    psol1->y = psol2->y = - l.C / l.B;
    return numSol;
  }
}


/********************************************************************************/

/* loop over rectangle edges. See if we're outside of the edge.
   If so, try to intersect with edge to move it inside */
Vector AdjustPtToRectOnLine(Vector pt, Rectangle r, Line l)
{
  DebugGeom(cout << "Adjust: " << pt << "\t" << r << "\t" << l << endl);
  Vector c = r.Center();
  for (int i = 0; i < 4; i++) {
    Line edge = r.GetEdge(i);
    if (edge.HalfPlaneTest(pt) != edge.HalfPlaneTest(c) && !edge.SameSlope(l)) {
      // pt is outside this edge
      DebugGeom(printf("AdjustPtToRectOnLine: HalfPlaneTest failed for %d\n", i));
      Vector newPt = edge.intersection(l);
      if (edge.InBetween(newPt, r.GetPoint(i), r.GetPoint(i+1))) {
	DebugGeom(printf("AdjustPtToRectOnLine: Intersection okay for %d\n", i));	
	return newPt;
      }      
    }    
  }
  return pt;
}

/********************************************************************************/

Bool InBetween(Vector pt, Vector end1, Vector end2)
{
  Line l = LineFromTwoPoints(end1, end2);
  return l.InBetween(pt, end1, end2);  
}

/********************************************************************************/

Vector PointInBetween(Vector pt1, Vector pt2, float pt1dist)
{
  if ( pt1dist < 0 ) my_error ("no neg dists");
  Vector targ = pt2 - pt1;
  targ *= pt1dist / targ.mod();
  return(pt1 + targ);
}

/********************************************************************************/

AngleDeg AngleBisect(AngleDeg a1, AngleDeg a2)
{
  if ( fabs(a1-a2) > 180 )
    return GetNormalizeAngleDeg((Min(a1,a2)+360+Max(a1,a2))/2);
  
  return GetNormalizeAngleDeg((a1+a2)/2);
}

/********************************************************************************/

Vector GetClosestPtInBetween(Vector pt, Vector end1, Vector end2)
{
  Line l;
  l.LineFromTwoPoints(end1, end2);
  return l.GetClosestPtInBetween(pt, end1, end2);
}

/********************************************************************************/

Bool IsPointInCone(Vector pt, float wid_dist_ratio, Vector end, Vector vert)
{
  Line l = LineFromTwoPoints(vert, end);
  Vector proj_pt = l.ProjectPoint(pt);
  return  ((proj_pt.dist2(pt) < proj_pt.dist2(vert)*wid_dist_ratio*wid_dist_ratio &&
	   l.InBetween(proj_pt, vert, end)))
    ? TRUE : FALSE;
}