geometry.c

RoboCup 2D 仿真组冠军源代码之1998年冠军队——CMUnited98源代码

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

/* geometry.C
 * CMUnited98 (soccer client for Robocup98)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1998 Peter Stone
 *
 * CMUnited-98 was created by Peter Stone, Manuela Veloso, and Patrick Riley
 *
 * 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.
 */

/* geometry.C defines several constructs for doing geometric reasoning on the soccer field
 */

#include "geometry.h"

#ifdef DEBUG_OUTPUT
#define DebugGeom(x) 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());
}

/********************************************************************************/
/********************************************************************************/
/********************************************************************************/
/*                         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(Vector orig, Vector dir)   
{
  if (fabs(dir.y) < FLOAT_EPS && fabs(dir.x) < FLOAT_EPS)
    my_error("LineFromRay: dir can not be zero");
  LineFromTwoPoints(orig, orig + dir);
}

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

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);