worldmodel.cpp

robocup 3d, a 3d base team similar to UvA 2d

      posReturn.setVecPosition(  m_FieldLength / 2.0,  m_FieldWidth / 2.0 );
      break;

    case OBJECT_GOAL_L_B:
      posReturn.setVecPosition( -m_FieldLength / 2.0, -m_GoalWidth / 2.0 );
      break;

    case OBJECT_GOAL_L_T:
      posReturn.setVecPosition( -m_FieldLength / 2.0,  m_GoalWidth / 2.0 );
      break;

    case OBJECT_GOAL_R_B:
      posReturn.setVecPosition(  m_FieldLength / 2.0, -m_GoalWidth / 2.0 );
      break;

    case OBJECT_GOAL_R_T:
      posReturn.setVecPosition(  m_FieldLength / 2.0,  m_GoalWidth / 2.0 );
      break;

    default:
      posReturn.setVecPosition( UnknownDoubleValue, UnknownDoubleValue, UnknownDoubleValue );
  }

  return posReturn;
}

/*! This method checks for time which object specified was last seen
    and determines object is reliable to decide on or not
    \param obj Object to check for its confidence
    \return bool Indicates that object is reliable */
bool WorldModel::isConfidenceGood( ObjectT obj ) const
{
  return ( getTimeLastSeen( obj ) - getSimulatorTime() ) <= getSimulatorStep();
}

/*! This method returns last time which object requested was seen.
    time is the simulator time, not game time
    \param obj Object to check for time which was last seen
    \return Time Time object was last seen */
Time WorldModel::getTimeLastSeen( ObjectT obj ) const
{
  if( SoccerTypes::isFlag( obj ) )
    return m_Flags[SoccerTypes::getIndex( obj )].getTimeLastSeen();

  if( SoccerTypes::isGoal( obj ) )
    return m_Goals[SoccerTypes::getIndex( obj )].getTimeLastSeen();

  if( SoccerTypes::isTeammate( obj ) )
      return m_Teammates[SoccerTypes::getIndex( obj )].getTimeLastSeen();

  if( SoccerTypes::isOpponent( obj ) )
      return m_Opponents[SoccerTypes::getIndex( obj )].getTimeLastSeen();

  if( obj == OBJECT_BALL )
    return m_Ball.getTimeLastSeen();

  return UnknownTime;
}

/*! This method returns position of object specified. To check for position
    reliability check the time object was last seen and compare it with
    simulator time, or check it using isConfidenceHigh
    \param obj Object To check for its position
    \return VecPosition Position of object requested */
VecPosition WorldModel::getPosition( ObjectT obj ) const
{
  if( SoccerTypes::isFlag( obj ) || SoccerTypes::isGoal( obj ) )
    return getGlobalPositionFlag( obj );

  if( SoccerTypes::isTeammate( obj ) )
      return m_Teammates[SoccerTypes::getIndex( obj )].getPosition();

  if( SoccerTypes::isOpponent( obj ) )
      return m_Opponents[SoccerTypes::getIndex( obj )].getPosition();

  if( obj == OBJECT_BALL )
    return m_Ball.getPosition();

  return UnknownPosition;
}

/*! This methods return one of the joints of the agent.
    \param ID Joint ID to send the information
    \return Joint The joint requested */
Joint WorldModel::getJoint( JointT ID ) const
{
  return m_Joints[(int) ID];
}

/*! This methods sets one of the joints information of the agent.
    \param ID Joint ID to set the information
    \param jnt Information to be set in the joint
    \return bool indicating weather update was successful */
bool WorldModel::setJoint(JointT ID, const Joint & jnt)
{
  m_Joints[(int) ID] = jnt;
  
  return true;
}

/*! This methods sets one of the joints information of the agent.
    \param ID Joint ID to set the information
    \param axis1 First Axis Information to be set in the joint
    \param axis2 Second Axis Information to be set in the joint
    \param rate1 First Rate Information to be set in the joint
    \param rate2 Second Rate Information to be set in the joint
    \return bool indicating weather update was successful */
bool WorldModel::setJoint(JointT ID, double axis1, double rate1, double axis2, double rate2)
{
  m_Joints[(int) ID].setAxis1(axis1);
  m_Joints[(int) ID].setAxis2(axis2);
  m_Joints[(int) ID].setRate1(rate1);
  m_Joints[(int) ID].setRate2(rate2);
  
  return true;
}

/*! This method returns the value of the sensor
    \param sensor The sensor to get the value
    \return double Sensor value */
double WorldModel::getSensor( SensorT sensor ) const
{
  return m_Sensors[ sensor ].getValue();
}

/*! This method returns the gyroscope parameters - currently
    x, y & z rate of the torso
    \return VecPosition Torso's gyroscope parameters */
VecPosition WorldModel::getGyroscope() const
{
  return m_Gyroscope;
}

/*! This method derives Phi direction of agent from all the angles
    he gets from server
    \return AngDeg Phi angle of agent*/
AngDeg WorldModel::getAgentPhi() const
{
  VecPosition pos, posFar;

  for( int i = 0; i < 4; i++)
  {
    pos = getGlobalPositionFlag( (ObjectT)( i + (int)OBJECT_CORNER_L_T ) ) - getAgentPosition();

    if(pos.getMagnitude() > posFar.getMagnitude())
      posFar = pos;
  }

  return posFar.getPhi();
}

/*! This method returns the agent global direction in field,
    ie. the direction he is facing to
    \return AngDeg Current Agent direction in field */
AngDeg WorldModel::getAgentDirection() const
{
  return m_Agent.getDirection();
}

/*! This method returns agents position on field
    \return VecPosition Agent position on field */
VecPosition WorldModel::getAgentPosition() const
{
  return m_Agent.getPosition();
}

/*! This method returns agents mass
    \return VecPosition Agents mass */
double WorldModel::getAgentMass() const
{
  return m_Agent.getMass();
}

/*! This method returns agents battery
    \return VecPosition Agents batery */
double WorldModel::getAgentBattery() const
{
  return m_Agent.getBattery();
}

/*! This method returns agents temp
    \return VecPosition Agents temp */
double WorldModel::getAgentTemprature() const
{
  return m_Agent.getTemprature();
}

/*! This method returns agents number on field
    \return int Agent number on field */
int WorldModel::getAgentNumber() const
{
  return m_AgentNumber;
}

/*! This method sets the agent number in game, if previously set then
    its not going to be set again, siumlator sends agent a number when
    agent requests a number, but that if a special number is desired,
    its better to set it before main loop via this function
    \param number New number of agent on field
    \return bool Indicating wheather update was successfull */
bool WorldModel::setAgentNumber( const int & number )
{
  if( m_AgentNumber != 0 )
    return false;

  m_AgentNumber = number;
  return true;
}

/*! This method returns agents team name
    \return string Team name of agent */
string WorldModel::getTeamName() const
{
  return m_TeamName;
}

/*! This method sets team name of agent
    \param name team name of agent
    \return bool Indicates that team name is set to desired one */
bool WorldModel::setTeamName( const string & name )
{
  if( m_TeamName != "" )
    return false;

  m_TeamName = name;
  return true;
}

/*! This method returns the current formation in use
    \return FormationT Current formation */
FormationT WorldModel::getCurrentFormation() const
{
  return m_Formations->getFormation();
}

/*! This method sets the current formation type
    \param formation Current formation type
    \return bool Indicating update was successfull */
bool WorldModel::setCurrentFormation( FormationT formation )
{
  m_Formations->setFormation( formation );
  return true;
}

/*! This method returns the strategic position for player number
    and formation specified
    \param number Player number to check for stratgic position
    \param formation Formation type to check
    \return VecPosition Strategic position */
VecPosition WorldModel::getStrategicPosition( int number, FormationT formation ) const
{
  if( number == -1 )
    number = getAgentNumber();

  FormationT form = formation == FT_ILLEGAL ? getCurrentFormation() : formation;
  return m_Formations->getStrategicPosition( number - 1, getBallPosition(), m_FieldWidth, 100, false, 0.8, form );
}