geometry.c

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

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

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

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::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;    
}




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

/* intersects a ray and a cricle */
/* return the number of solutions */
int RayCircleIntersect(Vector orig, Vector dir, float rad, Vector center,
		       Vector* psol1, Vector* psol2)
{
  DebugGeom(cout << "RCI: orig: " << orig << "\tdir: " << dir << endl
	    << "rad: " << rad << "\tcenter: " << center << endl);
  float a,b,c,disc;
  float t1, t2;
  a = Sqr(dir.x) + Sqr(dir.y);
  b = 2.0 * ((orig.x-center.x) * dir.x + (orig.y-center.y) * dir.y);
  c = Sqr(orig.x-center.x) + Sqr(orig.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 = orig + dir * t1;
      *psol2 = orig + dir * t2;
      DebugGeom(printf(" RCI:two sols\n"));
      return 2;
    } else {
      my_error("RayCircleIntersect: weird roots");
      return 0;
    }
  } else if (t2 > 0.0) {
    *psol1 = orig + dir * t2;
    DebugGeom(printf(" RCI:t2 only sol\n"));
    return 1;
  } else
    return 0;

  return 0;
}

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

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)) {
      // 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)
{
  pt = LineFromTwoPoints(end1, end2).ProjectPoint(pt);
  float dist2 = end1.dist2(end2);
  
  return (pt.dist2(end1) <= dist2 && pt.dist2(end2) <= dist2) ? TRUE : FALSE;
}

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

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