worldmodelhighlevel.cpp

自己写的robocup-2d程序

/*
Copyright (c) 2000-2003, Jelle Kok, University of Amsterdam
All rights reserved.

Redistribution and use in source and binary forms, with or without
modification, are permitted provided that the following conditions are met:

1. Redistributions of source code must retain the above copyright notice, this
list of conditions and the following disclaimer.

2. Redistributions in binary form must reproduce the above copyright notice,
this list of conditions and the following disclaimer in the documentation
and/or other materials provided with the distribution.

3. Neither the name of the University of Amsterdam nor the names of its
contributors may be used to endorse or promote products derived from this
software without specific prior written permission.

THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS"
AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE
IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLE
FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL
DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR
SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER
CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE
OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
*/

/*! \file WorldModelHighLevel.cpp
<pre>
<b>File:</b>          WorldModelHighLevel.cpp
<b>Project:</b>       Robocup Soccer Simulation Team: UvA Trilearn
<b>Authors:</b>       Jelle Kok
<b>Created:</b>       12/02/2001
<b>Last Revision:</b> $ID$
<b>Contents:</b>      class definitions of WorldModel. This class contains
               methods that reason about the world model on a higher
               level and return more abstract information about the current
               world state.
<hr size=2>
<h2><b>Changes</b></h2>
<b>Date</b>             <b>Author</b>          <b>Comment</b>
12/02/2001       Jelle Kok       Initial version created
</pre>
*/

#include<list>            // needed for list<double>
#include<stdio.h>         // needed for printf
#include "WorldModel.h"

/*! This method returns the number of objects that are within the circle 'c'
    Only objects are taken into account that are part of the set 'set' and
    have a confidence higher than the threshold defined in PlayerSettings.
    \param c circle in which objects should be located to be counted
    \return number of objects from 'set' in circle 'c'*/
int WorldModel::getNrInSetInCircle( ObjectSetT set, Circle c )
{
  double dConfThr = PS->getPlayerConfThr();
  int    iNr      = 0;
  int    iIndex;

  for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, dConfThr ) )
  {
    if( c.isInside( getGlobalPosition( o ) ) )
      iNr++;
  }
  iterateObjectDone( iIndex );

  return iNr;
}

/*! This method returns the number of visible objects that are part of
    the object set 'set' and located in the rectangle 'rect'. When no
    rectangle is defined (rect=NULL) the whole field is taken into
    account. Only objects with a confidence value higher than the
    threshold defined in PlayerSettings are taken into consideration.

    \param set ObjectSetT from which objects are taken into consideration
    \param rect Rectangle in which objects are counted (default NULL)
    \return number of objects in Rectangle 'rect'.*/
int WorldModel::getNrInSetInRectangle( ObjectSetT set, Rect *rect  )
{
  double dConfThr = PS->getPlayerConfThr();
  int    iNr      = 0;
  int    iIndex;

  for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, dConfThr ) )
  {
    if( rect == NULL || rect->isInside( getGlobalPosition( o ) ) )
      iNr++;
  }
  iterateObjectDone( iIndex );
  return iNr;
}

/*! This method returns the number of objects in the specified cone.
    A cone is like a piece of a pie, in which 'start' is
    the center of the pie, 'end' is the edge of the pie and 'dWidth' is the
    half width of the piece after distance 1. Only objects are taken into
    consideration that are within the set 'set' and have a confidence higher
    than the threshold defined in PlayerSettings.
    \param set ObjectSetT of which objects are taken into consideration
    \param dWidth half width of the cone after distance 1.0
    \param start center of the cone
    \param end position that is the end of the cone.
    \return number of objects part of 'set' and located in this cone. */
int WorldModel::getNrInSetInCone( ObjectSetT set, double dWidth,
                                      VecPosition start , VecPosition end )
{
  double      dConfThr   = PS->getPlayerConfThr();
  int         iNr        = 0;
  int         iIndex;
  Line        line       = Line::makeLineFromTwoPoints( start, end );
  VecPosition posOnLine;
  VecPosition posObj;

  for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, dConfThr ) )
  {
    posObj    = getGlobalPosition( o );
    posOnLine = line.getPointOnLineClosestTo( posObj );
    // whether posOnLine lies in cone is checked by three constraints
    // - does it lie in triangle (to infinity)
    // - lies between start and end (and thus not behind me)
    // - does it lie in circle
    if(posOnLine.getDistanceTo(posObj) < dWidth*posOnLine.getDistanceTo(start)
       && line.isInBetween( posOnLine, start, end )
       && start.getDistanceTo( posObj ) < start.getDistanceTo( end ) )
        iNr++;
  }
  iterateObjectDone( iIndex );
  return iNr;
}


/*! This method returns whether the space in direction 'ang'' of
    object 'obj' is occupied by any opponents. */
bool WorldModel::isEmptySpace( ObjectT obj, AngDeg ang, double dDist )
{
  if( obj == OBJECT_ILLEGAL )
    return false;

  VecPosition pos = getGlobalPosition( obj );
   pos += VecPosition( dDist, ang, POLAR );

  if( getNrInSetInCircle( OBJECT_SET_OPPONENTS, Circle( pos, dDist ) ) == 0 )
    return true;

  return false;
}


bool WorldModel::coordinateWith( ObjectT obj )
{
  VecPosition pos = getGlobalPosition( obj );
  if( pos.getDistanceTo( getBallPos() ) < 30.0 &&
      pos.getX() > getBallPos().getX() - 5.0 )
  {
    
    if( getFastestInSetTo( OBJECT_SET_TEAMMATES, OBJECT_BALL ) ==
        getAgentObjectType() )
    {
      logCircle( 700, pos, 2.5 );
    }
    Log.log( 700, "coordinate with %d %f %f (%f %f)", 
             obj, pos.getDistanceTo( getBallPos() ),
             pos.getX(), getBallPos().getX(), getBallPos().getY() );
    return true;
  }
  
  return false;
}

/*! This method returns the object type of the closest object to the
    ObjectT that is supplied as the second argument. Only objects are
    taken into account that are part of the set 'set' and have a
    confidence higher than the supplied threshold. If no threshold is
    supplied, the threshold defined in PlayerSettings is used.

    \param set ObjectSetT which denotes objects taken into consideration

    \param objTarget ObjectT that represent the type of the object to
    compare to

    \param dDist will be filled with the closest distance

    \param dConfThr minimum confidence threshold for the objects in 'set'
    \return ObjectType that is closest to o */
ObjectT WorldModel::getClosestInSetTo( ObjectSetT set, ObjectT objTarget,
                                        double *dDist, double dConfThr )
{
  if( dConfThr == -1.0 ) dConfThr      = PS->getPlayerConfThr();
  ObjectT     closestObject = OBJECT_ILLEGAL;
  double      dMinMag       = 1000.0;
  VecPosition v;
  int         iIndex;

  for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, dConfThr ) )
  {
    if( o != objTarget )               // do not include target object
    {
      v = getGlobalPosition( objTarget ) - getGlobalPosition( o );
      if( v.getMagnitude() < dMinMag )
      {
        dMinMag       = v.getMagnitude();
        closestObject = o;
      }
    }
  }

  iterateObjectDone( iIndex );
  if( dDist != NULL )
    *dDist = dMinMag;
  return closestObject;
}

/*! This method returns the ojbect type of the closest object to the specified
    position and that is part of the object set 'set' with a confidence
    higher than the
    supplied threshold. If no threshold is supplied, the threshold defined in
    PlayerSettings is used.
    \param set ObjectSetT which denotes objects taken into consideration

    \param pos position to which player should be compared

    \param dDist will be filled with the distance between pos and
    closest object

    \param dConfThr minimum confidence threshold for the objects in 'set'
    \return ObjectT representing object that is closest to pos */
ObjectT WorldModel::getClosestInSetTo( ObjectSetT set, VecPosition pos,
                                       double *dDist,  double dConfThr )
{
  ObjectT     closestObject   = OBJECT_ILLEGAL;
  double      dMinMag         = 1000.0;
  VecPosition v;
  int         iIndex;

  if( dConfThr == -1.0 ) dConfThr      = PS->getPlayerConfThr();
  for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, dConfThr ) )
  {
    v = pos - getGlobalPosition( o );
    if( v.getMagnitude() < dMinMag )
    {
      dMinMag         = v.getMagnitude();
      closestObject   = o;
    }
  }
  iterateObjectDone( iIndex );
  if( dDist != NULL )
    *dDist = dMinMag;
  return closestObject;
}

/*! This method returns the closest object in 'set' to the line
    'l'. The projection p of the global position of this object on the
    line 'l' should lie between pos1 and pos2. After the method is
    finished, it returns this object and the last two arguments of
    this method are set to the the distance between the object and p
    and the distance from pos1 to p respectively.

    \param set ObjectSetT which denotes objects taken into consideration
    \param l line to which opponents should be projected
    \param pos1 minimum allowed projection point
    \param pos2 maximum allowed projection point
    \param dDistObjToLine will contain distance from opponent to line l
    \param dDistPos1ToPoint will contain distance from pos1 to projection point
    opponent on line l
    \return object type of closest object to line l */
ObjectT WorldModel::getClosestInSetTo( ObjectSetT set, Line l,
                              VecPosition pos1, VecPosition pos2,
                              double *dDistObjToLine, double *dDistPos1ToPoint)
{
  VecPosition posObj;
  VecPosition posOnLine;
  double      dConfThr  = PS->getPlayerConfThr();
  ObjectT     obj       = OBJECT_ILLEGAL;
  double      dDist     ;
  double      dMinDist  = 1000.0;
  double      dDistPos1 = 1000.0;
  int         iIndex;

  for( ObjectT o = iterateObjectStart( iIndex, set, dConfThr );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, dConfThr ) )
  {
    posObj    = getGlobalPosition( o );
    posOnLine = l.getPointOnLineClosestTo( posObj );
    dDist     = posObj.getDistanceTo( posOnLine );
    if( l.isInBetween( posOnLine, pos1, pos2 ) && dDist < dMinDist )
    {
      dMinDist  = dDist;
      obj       = o;
      dDistPos1 = pos1.getDistanceTo( posOnLine );
    }
  }
  iterateObjectDone( iIndex );
  if( dDistObjToLine != NULL )
    *dDistObjToLine = dMinDist;
  if( dDistPos1ToPoint != NULL )
    *dDistPos1ToPoint = dDistPos1;

  return obj;
}

/*! This method returns the object type of the closest object relative to
    the agent. Only objects are taken into account that are part of the set
    'set'.
    \param set ObjectSetT which denotes objects taken into consideration
    \param dDist will be filled with the closest relative distance
    \return ObjectType that is closest to the agent*/
ObjectT WorldModel::getClosestRelativeInSet( ObjectSetT set, double *dDist )
{
  ObjectT     closestObject = OBJECT_ILLEGAL;
  double      dMinMag       = 1000.0;
  int         iIndex;

  for( ObjectT o = iterateObjectStart( iIndex, set, 1.0 );
       o != OBJECT_ILLEGAL;
       o = iterateObjectNext ( iIndex, set, 1.0 ) )
  {
    if( getRelativeDistance( o ) < dMinMag )
    {