geometry.c

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

/* -*- Mode: C++ -*- */

/* geometry.C
 * CMUnited99 (soccer client for Robocup99)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1999 Peter Stone
 *
 * CMUnited-99 was created by Peter Stone, Patrick Riley, and Manuela Veloso
 *
 * You may copy and distribute this program freely as long as you retain this notice.
 * If you make any changes or have any comments we would appreciate a message.
 * For more information, please see http://www.cs.cmu.edu/~robosoccer/
 */


#include "geometry.h"

#ifdef DEBUG_OUTPUT
#define DebugGeom(x) 
#else
#define DebugGeom(x)
#endif

/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                        Rectangle Class                                       */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/

Rectangle::Rectangle()
{
  left_x = right_x = top_y = bottom_y = 0;
}

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

Rectangle::Rectangle(const float l, const float r, const float t, const float b) 
{
  if ( l >= r ) my_error("width must be positive");
  if ( t >= b ) my_error("height must be positive");

  left_x = l ; right_x = r ;
  top_y = t ; bottom_y = b ;
}

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

Rectangle::Rectangle(const Vector center,const Vector size)
{
  left_x = center.x - size.x/2.0 ;
  right_x = center.x + size.x/2.0 ;
  top_y = center.y - size.y/2.0 ;
  bottom_y = center.y + size.y/2.0 ;
}

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

Bool Rectangle::IsWithin(const Vector &p)
{
  return (Bool) ((p.x >= left_x) && (p.x <= right_x) && (p.y >= top_y) && (p.y <= bottom_y)) ;
}

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

Vector Rectangle::AdjustToWithin(const Vector &p){

  Vector r = p;
  
  if (r.y - top_y    <0) r.y = top_y;
  if (bottom_y - r.y <0) r.y = bottom_y;
  if (right_x - r.x  <0) r.x = right_x;
  if (r.x - left_x   <0) r.x = left_x;

  return r;
}

/********************************************************************************/
//order: top, right, bot, left
Line Rectangle::GetEdge(int n)
{
  switch (n % 4) {
  case 0: return TopEdge();
  case 1: return RightEdge();
  case 2: return BottomEdge();
  case 3: return LeftEdge();
  }
  my_error("Rectangle::GetEdge: how did I get here");
  return Line();
}

/********************************************************************************/
//order: TL, TR, BR, BL
Vector Rectangle::GetPoint(int n)
{
  switch (n % 4) {
  case 0: return TopLeftCorner();
  case 1: return TopRightCorner();
  case 2: return BottomRightCorner();
  case 3: return BottomLeftCorner();
  }    
  my_error("Rectangle::GetPoint: how did I get here");
  return Vector(0,0);
}

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

Vector Rectangle::nearestHEdge(const Vector &p) 
     /* find nearest horizontal line */
{
  static Vector r ;

  r.x = Min(Max(p.x,left_x),right_x) ;
  r.y = ((p.y - top_y) < (bottom_y - p.y)) ? top_y : bottom_y ;

  return r ;
}

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

Vector Rectangle::nearestVEdge(const Vector &p) 
     /* find nearest vertical line */
{
  static Vector r ;

  r.x = ((p.x - left_x) < (right_x - p.x)) ? left_x : right_x ;
  r.y = Min(Max(p.y,top_y),bottom_y) ;

  return r ;
}

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

Vector Rectangle::nearestEdge(const Vector &p) 
{
  if(Min((p.x-left_x),(right_x-p.x)) < Min((p.y-top_y),(bottom_y-p.y)))
    return nearestVEdge(p) ;
  else
    return nearestHEdge(p) ;
}

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

Line Rectangle::nearestHEdgeLine(const Vector& p) 
{
  return ((p.y - top_y) < (bottom_y - p.y)) ? TopEdge() : BottomEdge() ;
}


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

Line Rectangle::nearestVEdgeLine(const Vector& p) 
{
  return ((p.x - left_x) < (right_x - p.x)) ? LeftEdge() : RightEdge() ;
}


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

Line Rectangle::nearestEdgeLine(const Vector& p) 
{
  if(Min((p.x-left_x),(right_x-p.x)) < Min((p.y-top_y),(bottom_y-p.y)))
    return nearestVEdgeLine(p) ;
  else
    return nearestHEdgeLine(p) ;  
}


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

float Rectangle::DistanceToEdge(const Vector &p) 
{
  if ( !IsWithin(p) ){
    Vector q = AdjustToWithin(p);
    return -(q.dist(p)); /* distance outside is a negative number */
  }
   
  return Min((p.x-left_x),Min((right_x-p.x),Min((p.y-top_y),(bottom_y-p.y))));
}

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

Vector Rectangle::random() 
{
  static Vector r ;

  r.x = range_random(left_x, right_x) ;
  r.y = range_random(bottom_y, top_y) ;

  return r ;
}

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

Rectangle Rectangle::expand(float val)
{
  return Rectangle(left_x - val, right_x + val, top_y - val, bottom_y + val);
}

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

void  Rectangle::Print(){
  printf("RECTANGLE:  x = %.1f to %.1f   y = %.1f to %.1f\n",
	 LeftX(),RightX(),TopY(),BottomY());
}

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

Vector Rectangle::RayIntersection(Ray r)
{
  if (!IsWithin(r.origin)) {
    my_error("Rectangle/Ray intersection only handles rays inside the rectangle!");
    return 0;
  }

  Vector int_pt[2];
  int num_int = 0;
  for (int i=0; i < 4; i++) {
    Vector pt;
    if (GetEdge(i).RayIntersection(r, &pt))
      int_pt[num_int++] = pt;
  }

  if (num_int == 0) {
    my_error("Rectangle ray intersection: no edge intersection?");
    return 0;
  } else if (num_int == 1) {
    return int_pt[0]; // same slope as one pair of edges 
  } else if (num_int == 2) {
    Rectangle temp_rect = expand(FLOAT_EPS);
    for (int j = 0; j < 2; j++)
      if (temp_rect.IsWithin(int_pt[j]))
	return int_pt[j];
    my_error("Rectangle ray intersection: how did I get here");
    return 0;
  } else {
    my_error("Rectangle ray intersection: how did num_int get so big: %d", num_int);
    return 0;
  }
}


/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                         Line Class                                           */
/********************************************************************************/
/********************************************************************************/
/********************************************************************************/

Line::Line(float x_coef, float y_coef, float constant) 
{
  A = x_coef;
  B = y_coef;
  C = constant;
}

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

void Line::LineFromTwoPoints(Vector pt1, Vector pt2)
{
  float temp = (pt2.x - pt1.x);
  if (fabs(temp) < FLOAT_EPS) {
    if (fabs(pt2.y - pt1.y) < FLOAT_EPS)
      my_error("LineFromTwoPoints: points can not be the same!");
    A = 1;
    B = 0;
  } else {
    float m = (pt2.y - pt1.y) / temp;
    A = -m;
    B = 1;
  }  
  C = -(A * pt2.x + B * pt2.y);
}

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

void Line::LineFromRay(Ray r)   
{
  if (fabs(r.direction.y) < FLOAT_EPS && fabs(r.direction.x) < FLOAT_EPS)
    my_error("LineFromRay: dir can not be zero");
  LineFromTwoPoints(r.origin, r.origin + r.direction);
}

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

Bool Line::PointOnLine(float x, float y)
{
  return (Bool)(fabs(A * x + B * y + C) < FLOAT_EPS);
}

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

float Line::dist(Vector pt)     
{
  return fabs( (A*pt.x + B*pt.y + C) / sqrt(Sqr(A) + Sqr(B)) );
}

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

float Line::dist2(Vector pt)     
{
  return fabs( Sqr(A*pt.x + B*pt.y + C) / (Sqr(A) + Sqr(B)) );
}

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

float Line::angle()
{
  return ATan(-A / B);
}

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

Vector Line::ProjectPointUsingCircle(Vector pt)
{
  /* here's the idea:
     first get the plane equation for the ray
     then use the closest distance formula to get the closest distance
     then using the eq for that line and the eq for a circle of the dist radius
       around our curretn position, find the nearest point.
       Whew! */

  /* compute the dist */
  Vector retPt;
  float d = dist(pt); /* the min distance */

  /* intersect the circle and the line */
  float a,b,c; /* coefficent in quadratic to solve for x */
  float disc; /* discriminant in quadratic equation */
  a = 1 + Sqr(A);
  b = 2 * (A * (pt.y + C) - pt.x);
  c = Sqr(pt.x) + Sqr(pt.y + C) - Sqr(d);
  disc = Sqr(b) - 4 * a * c;
  /* the discriminant should be zero since this is the radius
     is the closest distance between the center and the line */
  if (fabs(disc) > FLOAT_EPS)
    fprintf(stderr, "GetClosestPointToBallPath: discrimannt is bad! %f\n", disc);
  retPt.x = - b / (2 * a);

  /* we compute two possible solutions for y and then see which one is on the
     line of the ray's path */
  float sol1, sol2;
  sol1 = sqrt(Sqr(d) - Sqr(retPt.x - pt.x));
  sol2 = -sol1;
  sol1 += pt.y;
  sol2 += pt.y;

  if (fabs(A * (retPt.x) + B * sol1 + C) < FLOAT_EPS ){
    /* sol1 is on line */
    retPt.y = sol1;
  } else if (fabs(A * (retPt.x) + B * sol2 + C) < FLOAT_EPS ){
    /* sol2 is on line */
    retPt.y = sol2;
  } else
    fprintf(stderr, "GetClosestPointToBallPath: neither solution works!\n");

  DebugGeom(printf("  dist: %f\t ptMod: %f\n", d,
		  sqrt(Sqr(pt.x - retPt.x) + Sqr(pt.y - retPt.y))));

  return retPt;
}

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

float Line::get_y(float x)
{
  if ( B != 0 ) 
    return (-A*x - C)/B;
  
  my_error("can't get y"); 
  return 0;
}

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

float Line::get_x(float y)
{
  if ( A != 0 ) 
    return (-B*y - C)/A;
  
  my_error("can't get x"); 
  return 0;
}

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

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

  pt = ProjectPoint(pt);
  float dist2 = end1.dist2(end2);
  
  return (pt.dist2(end1) <= dist2 && pt.dist2(end2) <= dist2) ? TRUE : FALSE;
}

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