geometry.c

足球机器人仿真组CMU97的源码

/* geometry.c
 * CMUnited-97 (soccer client for Robocup-97)
 * Peter Stone <[e-mail]>
 * Computer Science Department
 * Carnegie Mellon University
 * Copyright (C) 1997 Peter Stone
 *
 * CMUnited-97 was created by Peter Stone 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.
 */

#include "global.h"

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

float mod2pi(float angle)
{
  while (angle < 0.0) angle = angle + M_PI*2;
  while (angle >= M_PI*2) angle = angle - M_PI*2;
  return angle;
}

LINEP LnFrom2Pts (float x1, float y1, float x2, float y2){
  LINEP ln = (LINEP) new LINE;

  if ( x1==x2 && y1==y2){
    printf("CAREFUL:  Can't make line from 2 of the same point (LnFrom2Pts):  %.1f %.1f\n",x1,y1);
    //my_error("bla");
    ln->slope = 0;
    ln->y_intcpt = y1 - ln->slope * x1; 
    return ln;
  }

  if (x1==x2){
    ln->slope = HUGE;
    ln->y_intcpt = x1;
  }
  else{
    ln->slope = ((float)y2-y1)/(x2-x1);
    ln->y_intcpt = y1 - ln->slope * x1; 
  }

  return ln;
}


LINEP LnFromSlopeAndPoint (float slope, float x, float y){
  LINEP ln = (LINEP) new LINE;
  
  ln->slope = slope;
  ln->y_intcpt = y - slope*x;

  return ln;
}


float AngleFromSlope(float slope, float dir){

    /*dir==1 means moving towards more positive x*/
  float angle;

  angle = (float)atan(slope);
  if (angle < 0) angle += M_PI*2;
  if (dir < 0) {
    if (angle >= M_PI)
      angle -= M_PI;
    else 
      angle += M_PI;
  }

  return angle;
}


POSNP OffsetAlongLine(POSNP pt, float ln_slope, float dist, float dir){
  POSNP offset_pt = (POSNP) new POSN;
  float ln_angle;

    /*dir==1 means moving towards more negative x*/
  ln_angle = AngleFromSlope(ln_slope,-dir);

  offset_pt->x = pt->x + dist * cos(ln_angle);
  offset_pt->y = pt->y + dist * sin(ln_angle);
  //offset_pt->theta = (int)mod2pi(ln_angle+M_PI);

  return offset_pt;
}
  
POSNP OffsetAlongLineTowards(POSNP origin, float ln_slope, POSNP towards, float dist){

  POSNP pt1 = OffsetAlongLine(origin,ln_slope,dist,1);
  POSNP pt2 = OffsetAlongLine(origin,ln_slope,dist,-1);

  float dist1 = PtDistanceSqr(towards,pt1);
  float dist2 = PtDistanceSqr(towards,pt2);
  
  if ( dist < 0 ){ /* Actually want to go AWAY from the towards point: swap */
    POSNP tmp = pt1;
    pt1 = pt2;
    pt2 = tmp;
  }

  if ( dist1 < dist2 ){ /* pt 1 is closer to the goal point */
    delete pt2;
    return pt1;
  }
  else {
    delete pt1;
    return pt2;
  }
}

POSNP OffsetTowards(POSNP origin, POSNP towards, float dist){
  LINEP ConnectLine = LnFrom2Pts(origin->x,origin->y,towards->x,towards->y);

  POSNP result = OffsetAlongLineTowards(origin,ConnectLine->slope,towards,dist);

  delete ConnectLine;
  return result;
}

float PtDistanceSqr (POSNP a, POSNP b){
  return (a->x - b->x)*(a->x - b->x) + (a->y - b->y)*(a->y - b->y);
}

float PtDistance (POSNP a, POSNP b){
  return (float)sqrt( PtDistanceSqr(a,b) );
}


void Intersection (LINEP line1, LINEP line2, POSNP intrsct){
  if (line1->slope == line2->slope) 
    my_error("same line or parallel\n");

  intrsct->x = (line2->y_intcpt - line1->y_intcpt)/(line1->slope - line2->slope);
  intrsct->y = line1->slope * intrsct->x + line1->y_intcpt;
}


void GetPerpLine(POSNP point, LINEP line, LINEP perp){
  if (!line->slope){
    perp->slope = HUGE;
    perp->y_intcpt = point->x;
    return;
  }

  if (fabs(line->slope) == HUGE){
    perp->slope = 0;
    perp->y_intcpt = point->y;
    return;
  }

  perp->slope = -1/line->slope;
  perp->y_intcpt = point->y - perp->slope * point->x;
}


void Projection(POSNP point, LINEP line, POSNP intrsct){
  LINE perp;
  GetPerpLine(point, line, &perp);
  Intersection(line,&perp,intrsct);
}

void Reflection(POSNP point, LINEP line, POSNP reflct){
  POSN intrsct;
  Projection(point,line,&intrsct);
  reflct->x = intrsct.x + (intrsct.x - point->x);
  reflct->y = intrsct.y + (intrsct.y - point->y);
}

float ComputeAngleDeg(POSNP point1, POSNP vertex, POSNP point3){
  
  float aa = PtDistanceSqr(point1,vertex);
  float bb = PtDistanceSqr(point3,vertex);
  float cc = PtDistanceSqr(point3,point1);
  float a  = (float)sqrt(aa);
  float b  = (float)sqrt(bb);

  float ang = acos((aa + bb - cc)/(2*a*b));
  return rad_to_deg(ang);
}

/* Distance from a line */
void LnDistance (POSNP point, POSNP pointOnLine, LINE *line, float *DistanceToLine, float *DistanceAlongLine){
  LINE *perp;
  POSNP intersept;

  if (line->slope == 0){
    *DistanceToLine = point->y - line->y_intcpt;
    *DistanceAlongLine = point->x - pointOnLine->x; 
    return;
  }
                             /* For vertical lines, use y_intcpt as x intercept */
  else if (fabs(line->slope) == HUGE){
    *DistanceToLine = line->y_intcpt - point->x;
    *DistanceAlongLine = point->y - pointOnLine->y;   /* Negative number means up */ 
    return;
  }  

  intersept = (POSNP) new POSN;
  perp = (LINEP) new LINE;

  Projection(point,line,intersept);
  *DistanceToLine = PtDistance(point,intersept);
  /* Distance is negative if the point is below the line*/
  /* If you change this, be sure to change the earlier return value too*/
  if (point->y < line->slope * point->x + line->y_intcpt) 
    *DistanceToLine *= -1; 
    
  *DistanceAlongLine = PtDistance(intersept,pointOnLine);
  /* Distance is negative if the point is to the left on the line*/
  /* If you change this, be sure to change the earlier return value too*/
  if (point->x - pointOnLine->x < 0)
  	*DistanceAlongLine *= -1;

  delete(perp);
  delete(intersept);
}

/* Distance from a line -- FALSE if not between 2 points */ 
int LnDistanceIfBetween (POSNP point, POSNP pointOnLine, POSNP boundaryPoint, LINE *line, float *DistanceToLine, float *DistanceAlongLine){
  LINE *perp;
  POSNP intersept;

  int done = FALSE;
  int result = TRUE;

  if (line->slope == 0){
    *DistanceToLine = point->y - line->y_intcpt;
    *DistanceAlongLine = point->x - pointOnLine->x; 
    if ( fabs(*DistanceAlongLine) + fabs(point->x - boundaryPoint->x) > 
	 fabs(pointOnLine->x - boundaryPoint->x) + .01 )
      /* Intercept isn't between the 2 points */
      result = FALSE;
    done = TRUE;
  }
                             /* For vertical lines, use y_intcpt as x intercept */
  else if (fabs(line->slope) == HUGE){
    *DistanceToLine = line->y_intcpt - point->x;
    *DistanceAlongLine = point->y - pointOnLine->y;   /* Negative number means up */ 
    if ( fabs(*DistanceAlongLine) + fabs(point->y - boundaryPoint->y) > 
	 fabs(pointOnLine->y - boundaryPoint->y) + .01 )
      /* Intercept isn't between the 2 points */
      result = FALSE;
    done = TRUE;
  }  

  if ( !done ){
    intersept = (POSNP) new POSN;
    perp = (LINEP) new LINE;

    Projection(point,line,intersept);
    *DistanceToLine = PtDistance(point,intersept);
    /* Distance is negative if the point is below the line*/
    /* If you change this, be sure to change the earlier return value too*/
    if (point->y < line->slope * point->x + line->y_intcpt) 
      *DistanceToLine *= -1; 
    
    *DistanceAlongLine = PtDistance(intersept,pointOnLine);
    /* Distance is negative if the point is to the left on the line*/
    /* If you change this, be sure to change the earlier return value too*/
    if (point->x - pointOnLine->x < 0)
      *DistanceAlongLine *= -1;

    if ( fabs(*DistanceAlongLine) + PtDistance(intersept,boundaryPoint) > 
	 PtDistance(pointOnLine,boundaryPoint) + .01 )
      /* Intercept isn't between the 2 points */
      result = FALSE;
  }

  if (!done){
    delete(perp);
    delete(intersept);
  }
  return result;
}