/*Copyright (c) 2000-2003, Jelle Kok, University of AmsterdamAll rights reserved.Redistribution and use in source and binary forms, with or withoutmodification, are permitted provided that the following conditions are met:1. Redistributions of source code must retain the above copyright notice, thislist 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 documentationand/or other materials provided with the distribution.3. Neither the name of the University of Amsterdam nor the names of itscontributors may be used to endorse or promote products derived from thissoftware 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, THEIMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE AREDISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT OWNER OR CONTRIBUTORS BE LIABLEFOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIALDAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS ORSERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVERCAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USEOF 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 )    {      dMinMag       = getRelativeDistance( o );      closestObject = o;    }  }  iterateObjectDone( iIndex );