soccerruleaspect.cpp

rcssserver3d Robocup 3D比赛官方指定平台

/* -*- mode: c++; c-basic-offset: 4; indent-tabs-mode: nil -*-

   this file is part of rcssserver3D
   Fri May 9 2003
   Copyright (C) 2002,2003 Koblenz University
   Copyright (C) 2003 RoboCup Soccer Server 3D Maintenance Group
   $Id: soccerruleaspect.cpp,v 1.41 2008/06/22 12:12:58 sgvandijk Exp $

   This program is free software; you can redistribute it and/or modify
   it under the terms of the GNU General Public License as published by
   the Free Software Foundation; version 2 of the License.

   This program is distributed in the hope that it will be useful,
   but WITHOUT ANY WARRANTY; without even the implied warranty of
   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
   GNU General Public License for more details.

   You should have received a copy of the GNU General Public License
   along with this program; if not, write to the Free Software
   Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
*/
#include "soccerruleaspect.h"
#include <salt/random.h>
#include <zeitgeist/logserver/logserver.h>
#include <oxygen/agentaspect/agentaspect.h>
#include <oxygen/physicsserver/body.h>
#include <oxygen/sceneserver/scene.h>
#include <oxygen/gamecontrolserver/gamecontrolserver.h>
#include <soccer/soccerbase/soccerbase.h>
#include <soccer/gamestateaspect/gamestateaspect.h>
#include <soccer/ballstateaspect/ballstateaspect.h>
#include <soccer/agentstate/agentstate.h>

using namespace oxygen;
using namespace boost;
using namespace std;
using namespace salt;

SoccerRuleAspect::SoccerRuleAspect() :
    SoccerControlAspect(),
    mBallRadius(0.111),
    mGoalPauseTime(3),
    mKickInPauseTime(1),
    mHalfTime(2.25 * 60),
    mFreeKickDist(9.15),
    mFreeKickMoveDist(15.15),
    mGoalKickDist(1.0),
    mAutomaticKickOff(false),
    mWaitBeforeKickOff(1.0),
    mSingleHalfTime(false),
    mSayMsgSize(20),
    mAudioCutDist(50.0),
    mUseOffside(true),
    mFirstCollidingAgent(true),
    mNotOffside(false),
    mLastModeWasPlayOn(false)
{
    mFreeKickPos = Vector3f(0.0,0.0,mBallRadius);

}

SoccerRuleAspect::~SoccerRuleAspect()
{
}

void
SoccerRuleAspect::MoveBall(const Vector3f& pos)
{
    mBallBody->SetPosition(pos);
    mBallBody->SetVelocity(Vector3f(0,0,0));
    mBallBody->SetAngularVelocity(Vector3f(0,0,0));
    mBallBody->Enable();
}

void
SoccerRuleAspect::ClearPlayers(const salt::Vector3f& pos, float radius,
                               float min_dist, TTeamIndex idx)
{
    if (idx == TI_NONE || mBallState.get() == 0) return;

    std::list<boost::shared_ptr<AgentState> > agent_states;
    if (! SoccerBase::GetAgentStates(*mBallState.get(), agent_states, idx))
        return;

    salt::BoundingSphere sphere(pos, radius);
    boost::shared_ptr<oxygen::Transform> agent_aspect;
    std::list<boost::shared_ptr<AgentState> >::const_iterator i;
    for (i = agent_states.begin(); i != agent_states.end(); ++i)
    {
        SoccerBase::GetTransformParent(**i, agent_aspect);

        // if agent is too close, move it away
        Vector3f new_pos = agent_aspect->GetWorldTransform().Pos();
        AABB3 agentAABB = SoccerBase::GetAgentBoundingBox(*agent_aspect);

        if (sphere.Intersects(agentAABB))
        {
            float dist = salt::UniformRNG<>(min_dist, min_dist + 2.0)();

            if (idx == TI_LEFT)
            {
                if (pos[0] - dist < -mFieldLength/2.0)
                {
                    new_pos[1] = pos[1] < 0 ? pos[1] + dist : pos[1] - dist;
                } else {
                    new_pos[0] = pos[0] - dist;
                }
            } else {
                if (pos[0] + dist > mFieldLength/2.0)
                {
                    new_pos[1] = pos[1] < 0 ? pos[1] + dist : pos[1] - dist;
                } else {
                    new_pos[0] = pos[0] + dist;
                }
            }
            SoccerBase::MoveAgent(agent_aspect, new_pos);
        }
    }
}

void
SoccerRuleAspect::ClearPlayers(const salt::AABB2& box,
                               float min_dist, TTeamIndex idx)
{
    if (idx == TI_NONE || mBallState.get() == 0) return;
    std::list<boost::shared_ptr<AgentState> > agent_states;
    if (! SoccerBase::GetAgentStates(*mBallState.get(), agent_states, idx))
        return;

    boost::shared_ptr<oxygen::Transform> agent_aspect;
    std::list<boost::shared_ptr<AgentState> >::const_iterator i;
    for (i = agent_states.begin(); i != agent_states.end(); ++i)
    {
        SoccerBase::GetTransformParent(**i, agent_aspect);

        // if agent is too close, move it away
        Vector3f new_pos = agent_aspect->GetWorldTransform().Pos();
        AABB2 agentAABB2 = SoccerBase::GetAgentBoundingRect(*agent_aspect);
        
        if (box.Intersects(agentAABB2))
        {
            if (idx == TI_LEFT)
            {
                new_pos[0] = box.minVec[0] -
                    salt::UniformRNG<>(min_dist, min_dist * 2.0)();
            } else {
                new_pos[0] = box.maxVec[0] +
                    salt::UniformRNG<>(min_dist, min_dist * 2.0)();
            }
            SoccerBase::MoveAgent(agent_aspect, new_pos);
        }
    }
}

void SoccerRuleAspect::ClearPlayersBeforeKickOff(TTeamIndex idx)
{
    if (idx == TI_NONE || mBallState.get() == 0) return;
    
    // move the non-kick off team to own half field except the center
    // circle
    TTeamIndex opp = SoccerBase::OpponentTeam(idx);
    if ( TI_RIGHT == opp ){
        ClearPlayers(mLeftHalf, mFreeKickMoveDist, opp);
    }
    else {
        ClearPlayers(mRightHalf, mFreeKickMoveDist, opp);
    }
    ClearPlayers(Vector3f(0,0,0), mFreeKickDist, mFreeKickMoveDist,opp);

    // move the kick off team to own half field and the center circle
    std::list<boost::shared_ptr<AgentState> > agent_states;
    if (! SoccerBase::GetAgentStates(*mBallState.get(), agent_states, idx))
        return;
    
    salt::AABB2 box;
    if ( TI_RIGHT == idx ){
        box = mLeftHalf;
    }
    else{
        box = mRightHalf;
    }
        
    boost::shared_ptr<oxygen::Transform> agent_aspect;
    std::list<boost::shared_ptr<AgentState> >::const_iterator i;
    float freeKickDist2 = mFreeKickDist*mFreeKickDist;
    for (i = agent_states.begin(); i != agent_states.end(); ++i)
    {
        SoccerBase::GetTransformParent(**i, agent_aspect);
        // if agent is on opponent half field, move it away
        AABB2 agentAABB2 = SoccerBase::GetAgentBoundingRect(*agent_aspect);
        if (box.Intersects(agentAABB2))
        {
            Vector3f new_pos = agent_aspect->GetWorldTransform().Pos();
            // if agent is in the center circle, do not move it
            if ( agentAABB2.minVec.SquareLength() < freeKickDist2
                 && agentAABB2.maxVec.SquareLength() < freeKickDist2
                 && Vector2f(agentAABB2.minVec.x(),agentAABB2.maxVec.y()).SquareLength() < freeKickDist2
                 && Vector2f(agentAABB2.maxVec.x(),agentAABB2.minVec.y()).SquareLength() < freeKickDist2)
                continue;
            
            if (idx == TI_LEFT)
            {
                new_pos[0] = box.minVec[0] -
                    salt::UniformRNG<>(mFreeKickMoveDist, mFreeKickMoveDist * 2.0)();
            } else {
                new_pos[0] = box.maxVec[0] +
                    salt::UniformRNG<>(mFreeKickMoveDist, mFreeKickMoveDist * 2.0)();
            }
            SoccerBase::MoveAgent(agent_aspect, new_pos);
        }
    }
}

void
SoccerRuleAspect::DropBall()
{
    DropBall(mBallBody->GetPosition());
}

void
SoccerRuleAspect::DropBall(Vector3f pos)
{
    salt::Vector2f ball_pos(pos.x(), pos.y());

    // we do not drop the ball within the penalty area
    if (mLeftPenaltyArea.Contains(ball_pos))
    {
        pos[0] = mLeftPenaltyArea.maxVec[0];
        pos[1] = pos.y() < 0 ?
            mLeftPenaltyArea.minVec[1] : mLeftPenaltyArea.maxVec[1];
    }
    else if (mRightPenaltyArea.Contains(ball_pos))
    {
        pos[0] = mRightPenaltyArea.minVec[0];
        pos[1] = pos.y() < 0 ?
            mRightPenaltyArea.minVec[1] : mRightPenaltyArea.maxVec[1];
    }

    MoveBall(pos);

    ClearPlayers(pos, mFreeKickDist, mFreeKickMoveDist, TI_LEFT);
    ClearPlayers(pos, mFreeKickDist, mFreeKickMoveDist, TI_RIGHT);

    mGameState->SetPlayMode(PM_PlayOn);
}

void
SoccerRuleAspect::UpdateBeforeKickOff()
{
    // get game control server to check agent count
    static shared_ptr<GameControlServer> game_control;

    if  (game_control.get() == 0)
    {
        game_control = shared_dynamic_cast<GameControlServer>
            (GetCore()->Get("/sys/server/gamecontrol"));

        if (game_control.get() == 0)
        {
            GetLog()->Error() << "(SoccerRuleAspect) Error: can't get GameControlServer.\n";
            return;
        }
    }

    // if no players are connected, just return
    if (! game_control->GetAgentCount()) return;


    // before the game starts the ball should stay in the middle of
    // the playing field
    Vector3f pos(0,0,mBallRadius);
    MoveBall(pos);

    ClearPlayers(mRightHalf, mFreeKickMoveDist, TI_LEFT);
    ClearPlayers(mLeftHalf, mFreeKickMoveDist, TI_RIGHT);

#if 0
    //
    // TODO: this has to be tested (compiles and no crashes at least)
    mInOffsideLeftPlayers.clear();
    mInOffsideRightPlayers.clear();
#endif

    if (mAutomaticKickOff && mGameState->GetModeTime() > mWaitBeforeKickOff)
    {
        mGameState->KickOff();
    }
}

void
SoccerRuleAspect::UpdateKickOff(TTeamIndex idx)
{
    ClearPlayersBeforeKickOff(idx);

    // if no player touched the ball for mDropBallTime, we move away
    // all players and set the play mode to play on
    if (mDropBallTime > 0 &&
        mGameState->GetModeTime() > mDropBallTime)
    {
        // Drop the ball at its current position.
        // This should always be (0,0) during kickoff.
        DropBall(mBallBody->GetPosition());
        return;
    }

    // after the first agent touches the ball move to PM_PLAYON
    shared_ptr<AgentAspect> agent;
    TTime time;
    if (! mBallState->GetLastCollidingAgent(agent,time))
    {
        return;
    }
    if (time > mGameState->GetLastModeChange())
    {
        mGameState->SetPlayMode(PM_PlayOn);
    }
}

void
SoccerRuleAspect::UpdateKickIn(TTeamIndex idx)
{
    // do nothing for the duration of mKickInPauseTime
    if (mGameState->GetModeTime() < mKickInPauseTime)
    {
        return;
    }
    // move away opponent team
    ClearPlayers(mFreeKickPos, mFreeKickDist, mFreeKickMoveDist,
                 SoccerBase::OpponentTeam(idx));

    // if no player touched the ball for mDropBallTime, we move away
    // all players and set the play mode to play on
    if (mDropBallTime > 0 &&
        mGameState->GetModeTime() > mDropBallTime)
    {
        DropBall(mFreeKickPos);
        return;
    }

    // after the first agent touches the ball move to PM_PLAY_ON. the
    // time when the agent last touches the ball must be after the
    // change to the KickIn mode
    shared_ptr<AgentAspect> agent;
    TTime time;
    if (! mBallState->GetLastCollidingAgent(agent,time))
    {
        GetLog()->Error() << "ERROR: (SoccerRuleAspect) " << "no agent collided yet\n";
        return;
    }

    TTime lastChange = mGameState->GetLastModeChange();
    if (time > lastChange)
    {
        mGameState->SetPlayMode(PM_PlayOn);
        GetLog()->Error() << "ERROR: (SoccerRuleAspect) " << "Set Playmode to playon\n";
    } else
    {
        // move the ball back on the ground where it left the playing
        // field
        MoveBall(mFreeKickPos);
    }
}


//-----------------------------------


void
SoccerRuleAspect::UpdateFreeKick(TTeamIndex idx)
{
    // do nothing for the duration of mKickInPauseTime
    if (mGameState->GetModeTime() < mKickInPauseTime)
    {
        return;
    }
//---------------

    salt::Vector2f ball_pos(mFreeKickPos.x(), mFreeKickPos.y());
    // we do not drop the ball within the penalty area
    if (mLeftPenaltyArea.Contains(ball_pos))
    {
        mFreeKickPos[0] = mLeftPenaltyArea.maxVec[0];
        mFreeKickPos[1] = mFreeKickPos.y() < 0 ?
            mLeftPenaltyArea.minVec[1] : mLeftPenaltyArea.maxVec[1];
    }
    else if (mRightPenaltyArea.Contains(ball_pos))
    {
        mFreeKickPos[0] = mRightPenaltyArea.minVec[0];
        mFreeKickPos[1] = mFreeKickPos.y() < 0 ?
            mRightPenaltyArea.minVec[1] : mRightPenaltyArea.maxVec[1];
    }

    MoveBall(mFreeKickPos);
//--------------------------

    // move away opponent team

    ClearPlayers(mFreeKickPos, mFreeKickDist, mFreeKickMoveDist,
                 SoccerBase::OpponentTeam(idx));

    // if no player touched the ball for mDropBallTime, we move away
    // all players and set the play mode to play on
    if (mDropBallTime > 0 &&
        mGameState->GetModeTime() > mDropBallTime)
    {
        DropBall(mFreeKickPos);
        return;
    }

    // after the first agent touches the ball move to PM_PLAY_ON. the
    // time when the agent last touches the ball must be after the
    // change to the KickIn mode
    shared_ptr<AgentAspect> agent;
    TTime time;
    if (! mBallState->GetLastCollidingAgent(agent,time))
    {
        GetLog()->Error() << "ERROR: (SoccerRuleAspect) " << "no agent collided yet\n";
        return;
    }

    TTime lastChange = mGameState->GetLastModeChange();
    if (time > lastChange)
    {
        mGameState->SetPlayMode(PM_PlayOn);
        GetLog()->Error() << "ERROR: (SoccerRuleAspect) " << "Set Playmode to playon\n";
    } else
    {
        // move the ball back on the ground where it left the playing
        // field
        MoveBall(mFreeKickPos);
    }
}

//-----------------------------------



void
SoccerRuleAspect::UpdateGoalKick(TTeamIndex idx)
{
    // do nothing for the duration of mKickInPauseTime
    if (mGameState->GetModeTime() < mKickInPauseTime)
    {
        return;
    }
    // move away opponent team
    ClearPlayers(idx == TI_LEFT ? mLeftPenaltyArea : mRightPenaltyArea,
                 mFreeKickMoveDist, SoccerBase::OpponentTeam(idx));