utils.cpp

浙江大学 RoboCup3D 2006 源代码

    \return number of found x-coordinates */

int Geometry::abcFormula(double a, double b, double c, double *s1, double *s2)

{

  double dDiscr = b*b - 4*a*c;       // discriminant is b^2 - 4*a*c

  if (fabs(dDiscr) < EPS )       // if discriminant = 0

  {

    *s1 = -b / (2 * a);              //  only one solution

    return 1;

  }

  else if (dDiscr < 0)               // if discriminant < 0

    return 0;                        //  no solutions

  else                               // if discriminant > 0

  {

    dDiscr = sqrt(dDiscr);           //  two solutions

    *s1 = (-b + dDiscr ) / (2 * a);

    *s2 = (-b - dDiscr ) / (2 * a);

    return 2;

  }

}



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

/********************** CLASS CIRCLE ******************************************/

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



/*! This is the constructor of a circle.

    \param pos first point that defines the center of circle

    \param dR the radius of the circle

    \return circle with pos as center and radius as radius*/

Circle::Circle( Vector3 pos, double dR )

{

  setCircle( pos, dR );

}



/*! This is the constructor of a circle which initializes a circle with a

    radius of zero. */

Circle::Circle( )

{

  setCircle( Vector3(-1000.0,-1000.0), 0);

}



/*! This method prints the circle information to the specified output stream

    in the following format: "c: (c_x,c_y), r: rad" where (c_x,c_y) denotes

    the center of the circle and rad the radius.

    \param os output stream to which output is written. */

void Circle::show( ostream& os)

{

  os << "c:" << "( " << m_posCenter.x << ", " << m_posCenter.y << " )" << ", r:" << m_dRadius;

}



/*! This method sets the values of the circle.

    \param pos new center of the circle

    \param dR new radius of the circle

    ( > 0 )

    \return bool indicating whether radius was set */

bool Circle::setCircle( Vector3 pos, double dR )

{

  setCenter( pos );

  return setRadius( dR  );

}

/*! This method sets the radius of the circle.

    \param dR new radius of the circle ( > 0 )

    \return bool indicating whether radius was set */

bool Circle::setRadius( double dR )

{

  if( dR > 0 )

  {

    m_dRadius = dR;

    return true;

  }

  else

  {

    m_dRadius = 0.0;

    return false;

  }

}



/*! This method returns the radius of the circle.

    \return radius of the circle */

double Circle::getRadius()

{

  return m_dRadius;

}



/*! This method sets the center of the circle.

    \param pos new center of the circle

    \return bool indicating whether center was set */

bool Circle::setCenter( Vector3 pos )

{

  m_posCenter = pos;

  return true;

}



/*! This method returns the center of the circle.

    \return center of the circle */

Vector3 Circle::getCenter()

{

  return m_posCenter;

}



/*! This method returns the circumference of the circle.

    \return circumference of the circle */

double Circle::getCircumference()

{

  return 2.0*PI*getRadius();

}



/*! This method returns the area inside the circle.

    \return area inside the circle */

double Circle::getArea()

{

  return PI*getRadius()*getRadius();

}



/*! This method returns a boolean that indicates whether 'pos' is located inside

    the circle.

    \param pos position of which should be checked whether it is located in the

    circle

    \return bool indicating whether pos lies inside the circle */

bool Circle::isInside( Vector3 pos )

{

  return (m_posCenter - pos).mod() < getRadius() ;

}

/*! This method returns the two possible intersection points between two

    circles. This method returns the number of solutions that were found.

    \param c circle with which intersection should be found

    \param p1 will be filled with first solution

    \param p2 will be filled with second solution

    \return number of solutions. */

int Circle::getIntersectionPoints( Circle c, Vector3 *p1, Vector3 *p2)

{

    double x0, y0, r0;

    double x1, y1, r1;



    x0 = getCenter( ).x;

    y0 = getCenter( ).y;

    r0 = getRadius( );

    x1 = c.getCenter( ).x;

    y1 = c.getCenter( ).y;

    r1 = c.getRadius( );



    double      d, dx, dy, h, a, x, y, p2_x, p2_y;



    // first calculate distance between two centers circles P0 and P1.

    dx = x1 - x0;

    dy = y1 - y0;

    d = sqrt(dx*dx + dy*dy);



    // normalize differences

    dx /= d; dy /= d;



    // a is distance between p0 and point that is the intersection point P2

    // that intersects P0-P1 and the line that crosses the two intersection

    // points P3 and P4.

    // Define two triangles: P0,P2,P3 and P1,P2,P3.

    // with distances a, h, r0 and b, h, r1 with d = a + b

    // We know a^2 + h^2 = r0^2 and b^2 + h^2 = r1^2 which then gives

    // a^2 + r1^2 - b^2 = r0^2 with d = a + b ==> a^2 + r1^2 - (d-a)^2 = r0^2

    // ==> r0^2 + d^2 - r1^2 / 2*d

    a = (r0*r0 + d*d - r1*r1) / (2.0 * d);



    // h is then a^2 + h^2 = r0^2 ==> h = sqrt( r0^2 - a^2 )

    double      arg = r0*r0 - a*a;

    h = (arg > 0.0) ? sqrt(arg) : 0.0;



    // First calculate P2

    p2_x = x0 + a * dx;

    p2_y = y0 + a * dy;



    // and finally the two intersection points

    x =  p2_x - h * dy;

    y =  p2_y + h * dx;

    p1->x = x; p1->y = y;

    x =  p2_x + h * dy;

    y =  p2_y - h * dx;

    p2->x = x; p2->y = y;



    return (arg < 0.0) ? 0 : ((arg == 0.0 ) ? 1 :  2);

}



#if 0

/*! This method returns the size of the intersection area of two circles.

    \param c circle with which intersection should be determined

    \return size of the intersection area. */

double Circle::getIntersectionArea( Circle c )

{

  Vector3 pos1, pos2, pos3;

  double d, h, dArea;

  Angle ang;



  d = (getCenter() - c.getCenter()).mod(); // dist between two centers

  if( d > c.getRadius() + getRadius() )           // larger than sum radii

    return 0.0;                                   // circles do not intersect

  if( d <= fabs(c.getRadius() - getRadius() ) )   // one totally in the other

  {

    double dR = min( c.getRadius(), getRadius() );// return area smallest circle

    return PI*dR*dR;

  }



  int iNrSol = getIntersectionPoints( c, &pos1, &pos2 );

  if( iNrSol != 2 )

    return 0.0;



  // the intersection area of two circles can be divided into two segments:

  // left and right of the line between the two intersection points p1 and p2.

  // The outside area of each segment can be calculated by taking the part

  // of the circle pie excluding the triangle from the center to the

  // two intersection points.

  // The pie equals pi*r^2 * rad(2*ang) / 2*pi = 0.5*rad(2*ang)*r^2 with ang

  // the angle between the center c of the circle and one of the two

  // intersection points. Thus the angle between c and p1 and c and p3 where

  // p3 is the point that lies halfway between p1 and p2.

  // This can be calculated using ang = asin( d / r ) with d the distance

  // between p1 and p3 and r the radius of the circle.

  // The area of the triangle is 2*0.5*h*d.



  pos3 = pos1.getVecPositionOnLineFraction( pos2, 0.5 );

  d = pos1.getDistanceTo( pos3 );

  h = pos3.getDistanceTo( getCenter() );

  ang = asin( d / getRadius() );



  dArea = ang*getRadius()*getRadius();

  dArea = dArea - d*h;



  // and now for the other segment the same story

  h = pos3.getDistanceTo( c.getCenter() );

  ang = asin( d / c.getRadius() );

  dArea = dArea + ang*c.getRadius()*c.getRadius();

  dArea = dArea - d*h;



  return dArea;

}

#endif



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

/***********************  CLASS LINE *******************************************/

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



/*! This constructor creates a line by given the three coefficents of the line.

    A line is specified by the formula ay + bx + c = 0.

    \param dA a coefficients of the line

    \param dB b coefficients of the line

    \param dC c coefficients of the line */

Line::Line( double dA, double dB, double dC )

{

  m_a = dA;

  m_b = dB;

  m_c = dC;

}



/*! This function prints the line to the specified output stream in the

    format y = ax + b.

    \param os output stream to which output is written

    \param l line that is written to output stream

    \return output sream to which output is appended. */

ostream& operator <<(ostream & os, Line l)

{

  double a = l.getACoefficient();

  double b = l.getBCoefficient();

  double c = l.getCCoefficient();



  // ay + bx + c = 0 -> y = -b/a x - c/a

  if( a == 0 )

    os << "x = " << -c/b;

  else

  {

    os << "y = ";

    if( b != 0 )

      os << -b/a << "x ";

    if( c > 0 )

       os << "- " <<  fabs(c/a);

    else if( c < 0 )

       os << "+ " <<  fabs(c/a);

  }

  return os;

}



/*! This method prints the line information to the specified output stream.

    \param os output stream to which output is written. */

void Line::show( ostream& os)

{

  //os << (*this);

}



/*! This method returns the intersection point between the current Line and

    the specified line.

    \param line line with which the intersection should be calculated.

    \return Vector3 position that is the intersection point. */

Vector3 Line::getIntersection( Line line )

{

  Vector3 pos;

  double x, y;



  if( m_b == line.getBCoefficient() ) // lines are parallel, no intersection

  {

    return pos;

  }

  if( m_a == 0 )               // bx + c = 0 and a2*y + b2*x + c2 = 0 ==> x = -c/b

  {                          // calculate x using the current line

    x = -m_c/m_b;                // and calculate the y using the second line

    y = line.getYGivenX(x);

  }

  else if( line.getACoefficient() == 0 )

  {                         // ay + bx + c = 0 and b2*x + c2 = 0 ==> x = -c2/b2

   x = -line.getCCoefficient()/line.getBCoefficient(); // calculate x using

   y = getYGivenX(x);       // 2nd line and calculate y using current line

  }

  // ay + bx + c = 0 and a2y + b2*x + c2 = 0

  // y = (-b2/a2)x - c2/a2

  // bx = -a*y - c =>  bx = -a*(-b2/a2)x -a*(-c2/a2) - c ==>

  // ==> a2*bx = a*b2*x + a*c2 - a2*c ==> x = (a*c2 - a2*c)/(a2*b - a*b2)

  // calculate x using the above formula and the y using the current line

  else

  {

    x = (m_a*line.getCCoefficient() - line.getACoefficient()*m_c)/

                    (line.getACoefficient()*m_b - m_a*line.getBCoefficient());

    y = getYGivenX(x);

  }



  return Vector3( x, y );

}





/*! This method calculates the intersection points between the current line

    and the circle specified with as center 'posCenter' and radius 'dRadius'.

    The number of solutions are returned and the corresponding points are put

    in the third and fourth argument of the method

    \param c circle with which intersection points should be found

    \param posSolution1 first intersection (if any)

    \param posSolution2 second intersection (if any) */

int Line::getCircleIntersectionPoints( Circle circle,

              Vector3 *posSolution1, Vector3 *posSolution2 )

{

  int    iSol;

  double dSol1, dSol2;

  double h = circle.getCenter().x;

  double k = circle.getCenter().y;



  // line:   x = -c/b (if a = 0)

  // circle: (x-h)^2 + (y-k)^2 = r^2, with h = center.x and k = center.y

  // fill in:(-c/b-h)^2 + y^2 -2ky + k^2 - r^2 = 0

  //         y^2 -2ky + (-c/b-h)^2 + k^2 - r^2 = 0

  // and determine solutions for y using abc-formula

  if( fabs(m_a) < EPS )

  {

    iSol = Geometry::abcFormula( 1, -2*k, ((-m_c/m_b) - h)*((-m_c/m_b) - h)

              + k*k - circle.getRadius()*circle.getRadius(), &dSol1, &dSol2);

    posSolution1->x = (-m_c/m_b);

	posSolution1->y = dSol1;

    posSolution2->x = (-m_c/m_b);

	posSolution2->y = dSol2;

    return iSol;

  }



  // ay + bx + c = 0 => y = -b/a x - c/a, with da = -b/a and db = -c/a

  // circle: (x-h)^2 + (y-k)^2 = r^2, with h = center.x and k = center.y

  // fill in:x^2 -2hx + h^2 + (da*x-db)^2 -2k(da*x-db) + k^2 - r^2 = 0

  //         x^2 -2hx + h^2 + da^2*x^2 + 2da*db*x + db^2 -2k*da*x -2k*db

  //                                                         + k^2 - r^2 = 0

  //         (1+da^2)*x^2 + 2(da*db-h-k*da)*x + h2 + db^2  -2k*db + k^2 - r^2 = 0

  // and determine solutions for x using abc-formula

  // fill in x in original line equation to get y coordinate

  double da = -m_b/m_a;

  double db = -m_c/m_a;



  double dA = 1 + da*da;

  double dB = 2*( da*db - h - k*da );

  double dC = h*h + db*db - 2*k*db + k*k - circle.getRadius()*circle.getRadius();



  iSol = Geometry::abcFormula( dA, dB, dC, &dSol1, &dSol2 );



  posSolution1->x = dSol1;

  posSolution1->y = da*dSol1 + db;

  posSolution2->x = dSol2;

  posSolution2->y = da*dSol2 + db;

  return iSol;



}



/*! This method returns the tangent line to a Vector3. This is the line

    between the specified position and the closest point on the line to this

    position.

    \param pos Vector3 point with which tangent line is calculated.

    \return Line line tangent to this position */

Line Line::getTangentLine( Vector3 pos )

{

  // ay + bx + c = 0 -> y = (-b/a)x + (-c/a)

  // tangent: y = (a/b)*x + C1 -> by - ax + C2 = 0 => C2 = ax - by

  // with pos.y = y, pos.x = x

  return Line( m_b, -m_a, m_a*pos.x - m_b*pos.y );

}



/*! This method returns the closest point on a line to a given position.

    \param pos point to which closest point should be determined

    \return Vector3 closest point on line to 'pos'. */

Vector3 Line::getPointOnLineClosestTo( Vector3 pos )

{

  Line l2 = getTangentLine( pos );  // get tangent line

  return getIntersection( l2 );     // and intersection between the two lines

}



/*! This method returns the distance between a specified position and the

    closest point on the given line.

    \param pos position to which distance should be calculated

    \return double indicating the distance to the line. */

double Line::getDistanceWithPoint( Vector3 pos )

{

  return (pos - getPointOnLineClosestTo(pos)).mod();

}



/*! This method determines whether the projection of a point on the current line

    lies between two other points ('point1' and 'point2') that lie on the same

    line.

    \param pos point of which projection is checked.

    \param point1 first point on line

    \param point2 second point on line

    \return true when projection of 'pos' lies between 'point1' and 'point2'.*/

bool Line::isInBetween( Vector3 pos, Vector3 point1, Vector3 point2)

{

  pos          = getPointOnLineClosestTo( pos ); // get closest point

  double dDist = (point1 - point2).mod(); // get distance between 2 pos



  // if the distance from both points to the projection is smaller than this

  // dist, the pos lies in between.

  return (pos - point1).mod() <= dDist &&

         (pos - point2).mod() <= dDist;

}



/*! This method calculates the y coordinate given the x coordinate

    \param x coordinate

    \return y coordinate on this line */

double Line::getYGivenX( double x )

{

 if( m_a == 0 )

 {

   cerr << "(Line::getYGivenX) Cannot calculate Y coordinate: " ;

   show( cerr );

   cerr << endl;

   return 0;

 }

  // ay + bx + c = 0 ==> ay = -(b*x + c)/a

  return -(m_b*x+m_c)/m_a;

}