worldmodelhighlevel.c

机器足球2D比赛程序 对trlen_base_2002的改进

/*
   Copyright (c) 2000-2002, 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.C
  <pre>
  <b>File:</b>          WorldModelHighLevel.C
  <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 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, Rectangle * 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 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 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 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 = 1000.0;
  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);
  if (dDist != NULL)
    *dDist = dMinMag;
  return closestObject;
}

/*! This method returns the object type of the second closest object to the
  object type that is supplied as the second argument. Only objects are taken
  into account within set 'set' and 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 obj ObjectT that represent the type of the object to check
  \param dDist will be filled with the distance to this player.
  \param dConfThr minimum confidence threshold for the objects in 'set'
  \return ObjectType that is second closest to obj */
ObjectT
WorldModel::getSecondClosestInSetTo (ObjectSetT set, ObjectT obj,
				     double *dDist, double dConfThr)
{
  VecPosition v;
  ObjectT closestObject = OBJECT_ILLEGAL;
  ObjectT secondClosestObject = OBJECT_ILLEGAL;
  double dMinMag = 1000.0;
  double dSecondMinMag = 1000.0;
  int iIndex;

  if (dConfThr == -1.0)
    dConfThr = PS->getPlayerConfThr ();

  for (ObjectT o = iterateObjectStart (iIndex, set, dConfThr);
       o != OBJECT_ILLEGAL; o = iterateObjectNext (iIndex, set, dConfThr))
    {
      if (o != obj)
	{
	  v = getGlobalPosition (obj) - getGlobalPosition (o);
	  if (v.getMagnitude () < dMinMag)	// closer then first
	    {
	      dSecondMinMag = dMinMag;	// put first to second
	      secondClosestObject = closestObject;
	      dMinMag = v.getMagnitude ();	// and this to first
	      closestObject = o;
	    }
	  else if (v.getMagnitude () < dSecondMinMag)	// between first and 2nd
	    {
	      dSecondMinMag = v.getMagnitude ();	// put this to second
	      secondClosestObject = o;
	    }
	}
    }
  iterateObjectDone (iIndex);
  if (dDist != NULL)
    *dDist = dSecondMinMag;
  return secondClosestObject;
}

/*! This method returns the object type of the second closest object relative
  to the agent. Only objects are taken into account within set 'set' and
  which where seen in the last see message.
  \param set ObjectSetT which denotes objects taken into consideration
  \param dDist will be filled with the distance to this this object
  \return ObjectType that is second closest to the agent */
ObjectT
WorldModel::getSecondClosestRelativeInSet (ObjectSetT set, double *dDist)
{
  ObjectT closestObject = OBJECT_ILLEGAL;
  ObjectT secondClosestObject = OBJECT_ILLEGAL;
  double dMinMag = 1000.0;
  double dSecondMinMag = 1000.0;
  double d;
  int iIndex;

  for (ObjectT o = iterateObjectStart (iIndex, set, 1.0);
       o != OBJECT_ILLEGAL; o = iterateObjectNext (iIndex, set, 1.0))
    {
      d = getRelativeDistance (o);
      if (d < dMinMag)		// closer then first
	{
	  dSecondMinMag = dMinMag;	// put first to second
	  secondClosestObject = closestObject;
	  dMinMag = d;		// and this to first
	  closestObject = o;
	}
      else if (d < dSecondMinMag)	// between first and 2nd
	{
	  dSecondMinMag = d;	// put this to second
	  secondClosestObject = o;
	}
    }
  iterateObjectDone (iIndex);
  if (dDist != NULL)
    *dDist = dSecondMinMag;
  return secondClosestObject;
}


/*! This method returns the object type of the furthest 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