worldmodelupdate.cpp

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

              *strMsg += 2;
              jnt.setRate1( Parse::parseFirstDouble( strMsg ) );
            }
            else
            {
              *strMsg += 2;
              jnt.setRate2( Parse::parseFirstDouble( strMsg ) );
            }
          }
          else
          {
            cerr << "WorldModel::updateUniversalJoint: Server message truncated near rate predicate" << endl;
            return false;
          }
          break;

        default:
          cerr << "WorldModel::updateUniversalJoint: Found unknown predicate" << endl;
          break;
        }

      Parse::gotoFirstOccurenceOf( ')', strMsg );
      ++*strMsg;
      Parse::gotoFirstNonSpace(strMsg);
    }
    while( **strMsg && **strMsg != ')' );
  }
  else
  {
    cerr << "WorldModel::updateUniversalJoint: This is not a universal joint message" << endl;
    return false;
  }

  if( **strMsg != ')' )
  {
    cerr << "WorldModel::updateUniversalJoint: Server message truncated at end of data" << endl;
    return false;
  }

  ++*strMsg;
  setJoint( jnt.getID(), jnt );

  return true;
}

/*! This method updates a hinge joint (A joint which can
    only turn in 1 direction) based on the  sent message 
    from server, a hinge message looks like this:
    "(HJ (n laj3) (ax -0.00))"
    \param strMsg Hinge message to update from
    \return bool indicating weather update was successful
*/
bool WorldModel::updateHingeJoint ( char **strMsg )
{
  char token_name[128] = "";
  int i = 0;
  Joint jnt;

  Parse::gotoFirstOccurenceOf( '(', strMsg );
  ++*strMsg;

  if(**strMsg == 'H') // Hinge Joint
  {
    do
    {
      Parse::gotoFirstOccurenceOf( '(', strMsg );
      ++*strMsg;

      if(strlen(*strMsg) > 1)
        switch( **strMsg )
        {
        case 'n': // name
          if(strlen(*strMsg) > 2) // n
          {
            *strMsg += 2;
            while( **strMsg && **strMsg != ' ' && (isalnum(**strMsg) || **strMsg == '_') )
            {
              token_name[i++] = **strMsg;
              ++*strMsg;
            }

            jnt.setID( SoccerTypes::getJointFromStr( token_name ) );
            if( jnt.getID() == JID_ILLEGAL )
            {
              cerr << "WorldModel::updateHingeJoint: Joint is not a registered joint: '" << token_name << "'" << endl;
              return false;
            }
          }
          else
          {
            cerr << "WorldModel::updateHingeJoint: Server message truncated near name predicate" << endl;
            return false;
          }
          break;

        case 'a': // axis
          if(strlen(*strMsg) > 3) // axis
          {
            *strMsg += 3;
            jnt.setAxis( Parse::parseFirstDouble( strMsg ) );
          }
          else
          {
            cerr << "WorldModel::updateHingeJoint: Server message truncated near axis predicate" << endl;
            return false;
          }
          break;

        case 'r' : //rate
          if(strlen(*strMsg) > 5) // rate
          {
            *strMsg += 5;
            jnt.setRate( Parse::parseFirstDouble( strMsg ) );
          }
          else
          {
            cerr << "WorldModel::updateHingeJoint: Server message truncated near rate predicate" << endl;
            return false;
          }
          break;

        default:
          break;
        }

      Parse::gotoFirstOccurenceOf( ')', strMsg );
      ++*strMsg;
      Parse::gotoFirstNonSpace(strMsg);
    }
    while( **strMsg && **strMsg != ')' );
  }
  else
  {
    cerr << "WorldModel::updateHingeJoint: This is not a Hinge message" << endl;
    return false;
  }

  if( **strMsg != ')' )
  {
    cerr << "WorldModel::updateHingeJoint: Server message truncated at the end of data" << endl;
    return false;
  }

  ++*strMsg;
  setJoint( jnt.getID(), jnt );

  return true;
}

/*! This method updates ball position from server message
    Like this: "(Ball (pol 5.13019 -94.7171 77.8783))"
    \param strMsg message from server tha contains ball information
    \return bool indicating weather update was succesful */
bool WorldModel::updateBallPosition( char **strMsg )
{
  double dLength, dXY, dZ;

  Parse::gotoFirstOccurenceOf( '(', strMsg );
  ++*strMsg;

  if(**strMsg == 'B') // Ball Message
  {
    Parse::gotoFirstOccurenceOf( '(', strMsg );
    ++*strMsg;

    if(strlen(*strMsg) > 1 && **strMsg == 'p')
    {
      // polar position
      if(strlen(*strMsg) > 5) // ball coordinates
      {
        *strMsg += 4;

        dLength = Parse::parseFirstDouble( strMsg );
        dXY = Parse::parseFirstDouble( strMsg );
        dZ = Parse::parseFirstDouble( strMsg );

        Parse::gotoFirstOccurenceOf( ')', strMsg );
        ++*strMsg;
      }
      else
      {
        cerr << "WorldModel::updateBallPosition: Unsupplied position for ball - message truncated" << endl;
        return false;
      }
    }
    else
    {
      cerr << "WorldModel::updateBallPosition: Unsatisfied message for ball - message truncated" << endl;
      return false;
    }

    Parse::gotoFirstOccurenceOf( ')', strMsg );
    ++*strMsg;
    Parse::gotoFirstNonSpace(strMsg);
  }
  else
  {
    cerr << "WorldModel::updateBallPosition: Supplied message doesn't look like a ball vision message" << endl;
    return false;
  }

  VecPosition pos = VecPosition( dLength, dXY, dZ, POLAR );
  m_Ball.setPosition( pos );
  m_Ball.setTimeLastSeen( getSimulatorTime() );

  return true;
}

/*! This method updates corners and goal pole positions from server
    message that looks like this: "(F1l (pol 67.6468 61.728 93.31))"
    \param strMsg message from server tha contains ball information
    \return bool indicating weather update was succesful */
bool WorldModel::updateFlagPosition( char **strMsg )
{
  double dLength = UnknownDoubleValue, dXY = UnknownDoubleValue, dZ = UnknownDoubleValue;
  char token_name[128] = "";
  int i = 0;
  ObjectT objFlag;

  Parse::gotoFirstOccurenceOf( '(', strMsg );
  ++*strMsg;

  if(**strMsg == 'F' || **strMsg == 'G') // Flag Message
  {
    while( **strMsg && **strMsg != ' ' && (isalnum(**strMsg) || **strMsg == '_') )
    {
      token_name[i++] = **strMsg;
      ++*strMsg;
    }

    objFlag = SoccerTypes::getObjectFromStr( token_name );
    if( objFlag == OBJECT_ILLEGAL )
    {
      cerr << "WorldModel::updateFlagPosition: Unknown flag recieved: '" << token_name << "'" << endl;
      return false;
    }

    do
    {
      Parse::gotoFirstOccurenceOf( '(', strMsg );
      ++*strMsg;

      if(strlen(*strMsg) > 1 && **strMsg == 'p')
      {
        if(strlen(*strMsg) > 5) // ball coordinates
        {
          *strMsg += 4;

          dLength = Parse::parseFirstDouble( strMsg );
          dXY = Parse::parseFirstDouble( strMsg );
          dZ = Parse::parseFirstDouble( strMsg );
        }
        else
        {
          cerr << "WorldModel::updateFlagPosition: Unsupplied position for flag - message truncated" << endl;
          return false;
        }
      }
      else
      {
        cerr << "WorldModel::updateFlagPosition: Unsupplied position for flag - message truncated" << endl;
        return false;
      }

      Parse::gotoFirstOccurenceOf( ')', strMsg );
      ++*strMsg;
      Parse::gotoFirstNonSpace(strMsg);
    }
    while( **strMsg && **strMsg != ')' );
  }
  else
  {
    cerr << "WorldModel::updateFlagPosition: Message is not a flag message" << endl;
    return false;
  }

  Parse::gotoFirstNonSpace( strMsg );
  if( **strMsg != ')' )
  {
    cerr << "WorldModel::updateFlagPosition: Message truncated" << endl;
    return false;
  }
  else
    ++*strMsg;

  VecPosition pos = VecPosition( dLength, dXY, dZ, POLAR );
  if( SoccerTypes::isFlag(objFlag) )
  {
    m_Flags[ SoccerTypes::getIndex( objFlag ) ].setPosition( pos );
    m_Flags[ SoccerTypes::getIndex( objFlag ) ].setTimeLastSeen( getSimulatorTime() );
  }
  else if( SoccerTypes::isGoal(objFlag) )
  {
    m_Goals[ SoccerTypes::getIndex( objFlag ) ].setPosition( pos );
    m_Goals[ SoccerTypes::getIndex( objFlag ) ].setTimeLastSeen( getSimulatorTime() );
  }

  return true;
}

/*! This method updates positions of ball and flags & goals from a server
    message
    \param strMsg message from server that contains update information
    \return bool indicating weather update was successfull */
bool WorldModel::updatePositions( char **strMsg )
{
  bool bResult = true;

  Parse::gotoFirstOccurenceOf( '(', strMsg );
  ++*strMsg;

  if(**strMsg == 'S') // See
  {
    ++*strMsg;

    while(**strMsg != ')')
    {
      Parse::gotoFirstOccurenceOf( '(', strMsg );
      if( strlen(*strMsg) < 2)
        break;

      switch( *(*strMsg + 1) )
      {
      case 'B':  // Ball Messages
        bResult &= updateBallPosition( strMsg );
        break;

      case 'G':  // Goal Position
      case 'F':  // Flag messages
        bResult &= updateFlagPosition( strMsg );
        break;

      default:   // Unknown parameter.
        bResult &= wasteMessage( strMsg );
        break;
      }
    }

    Parse::gotoFirstOccurenceOf( ')', strMsg );
    ++*strMsg;

    return bResult;
  }
  else
  {
    cerr << "WorldModel::updatePositions: Supplied message doesn't look like a vision message" << endl;
    return false;
  }

}

/*! This method returns position of flag relative to agent before
    it has been parsed
    \param objFlag Flag to return its sensed position
    \return VecPosition Position of flag */
VecPosition WorldModel::getRawPosition( ObjectT objFlag ) const
{
  if( SoccerTypes::isFlag( objFlag ) )
    return m_Flags[ SoccerTypes::getIndex( objFlag ) ].getPosition();
  else if( SoccerTypes::isGoal( objFlag ) )
    return m_Goals[ SoccerTypes::getIndex( objFlag ) ].getPosition();
  else
    return getPosition( objFlag );
}

/*! This method updates field length and width based on percepted
    message from server */
bool WorldModel::updateCoordinates()
{
  ObjectT objTop    = getTimeLastSeen( OBJECT_CORNER_L_T ) > getTimeLastSeen( OBJECT_CORNER_R_T ) ? OBJECT_CORNER_L_T : OBJECT_CORNER_R_T;
  ObjectT objBottom = getTimeLastSeen( OBJECT_CORNER_L_B ) > getTimeLastSeen( OBJECT_CORNER_R_B ) ? OBJECT_CORNER_L_B : OBJECT_CORNER_R_B;
  ObjectT objLeft   = getTimeLastSeen( OBJECT_CORNER_L_T ) > getTimeLastSeen( OBJECT_CORNER_L_B ) ? OBJECT_CORNER_L_T : OBJECT_CORNER_L_B;
  ObjectT objRight  = getTimeLastSeen( OBJECT_CORNER_R_T ) > getTimeLastSeen( OBJECT_CORNER_R_B ) ? OBJECT_CORNER_R_T : OBJECT_CORNER_R_B;

  double length = getRawPosition( objLeft ).getDistanceTo( getRawPosition( objRight ) );
  double width = getRawPosition( objTop ).getDistanceTo( getRawPosition( objBottom ) );