situation.cpp

RoboCup 3D 仿真组清华大学2005的源代码

/***************************************************************************
 *   Copyright (C) 2005 by Rujia Liu                                       *
 *   [e-mail]                                                  *
 *                                                                         *
 *   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; either version 2 of the License, or     *
 *   (at your option) any later version.                                   *
 *                                                                         *
 *   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.,                                       *
 *   59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.             *
 ***************************************************************************/
#include "situation.h"
#include "worldmodel.h"
#include "formation.h"
#include "skill.h"
#include "geometry.h"
extern WorldModel wm;
extern Skill skill;

using namespace salt;
using namespace Geometry;

Situation::Situation()
{
    withBall = false;
    stopped = true;
}


Situation::~Situation()
{
}

void Situation::CycleStart()
{
    Vector3f ballpos = wm.GetBallPosition();
    Vector3f mypos = wm.GetMyPosition();
    
    // withball information
    if((mypos-ballpos).Length() < 0.5)
    {
        if(!withBall)
        {
            withBall = true;
            lastWithBallTime = wm.GetTime();
        }
    }
    else
        withBall = false;

    if((mypos-wm.GetMyPosition(1)).Length() < 0.12)
    {
        if(!stopped)
        {
            stopped = true;
            lastStoppedTime = wm.GetTime();
        }
    }
    else
        stopped = false; 
}

float Situation::GetStoppedTime()
{
    return wm.GetTime() - lastStoppedTime;
}

bool Situation::OkToShoot(int team, int unum, float& shoottarget)
{
    Vector3f ballpos = wm.GetBallPosition();
    Vector3f pos = wm.GetPlayerPosition(team, unum);
    
    if(team == 0)
    {
        // consider our shoot
        if((ballpos-pos).Length() < 1.0)
        {
            if(ballpos[0] - pos[0] < 1e-3) return false; // direction incorrect
            else
            {
                // get intersection
                float x1 = ballpos[0] - pos[0];
                float x2 = wm.GetFieldLength() / 2.0 - pos[0];
                float y1 = ballpos[1] - pos[1];
                float y2 = x2*y1/x1;
                shoottarget = pos[1] + y2;

                if(fabs(shoottarget) > wm.GetGoalWidth() / 2.0) return false;
            }
            return true;    
        }

    }
    else
    {
        // their shoot
        if((ballpos-pos).Length() < 1.0)
        {
            if(pos[0] - ballpos[0] < 1e-3) return false; // direction incorrect
            else
            {
                // get intersection
                float x1 = ballpos[0] - pos[0];
                float x2 = -wm.GetFieldLength() / 2.0 - pos[0];
                float y1 = ballpos[1] - pos[1];
                float y2 = x2*y1/x1;
                shoottarget = pos[1] + y2;

                if(fabs(shoottarget) > wm.GetGoalWidth() / 2.0) return false;
            }
            return true;    
        }
    }
}

// sombody is very close (<0.7m) to the ball
bool Situation::Dangerous()
{
    Point ballp = MakePoint(wm.GetBallPosition());
    Point myp = MakePoint(wm.GetMyPosition());
    for(int enemy = 1; enemy <= 11; enemy++)
    {
        Vector3f epos = wm.GetPlayerPosition(1, enemy);
        Point ep = MakePoint(epos);
        float dist = GetProjectionDistance(myp, ballp, ep);
        if(dist > 0 && dis(ballp, ep) < 0.7) return true; // in front, and close
    }
    return false;
}

int Situation::JudgeSituation()
{
    int situation = SBP_Attack;
    float time = skill.InterceptTime(0, wm.GetTeamUnum());
    for(int i = 1; i <= 11; i++)
        if (skill.InterceptTime(0, i) < time)
            time = skill.InterceptTime(0, i);
    
    for(int i = 0; i <= 11; i++)
        if (skill.InterceptTime(1, i) < time)
        {
            time = skill.InterceptTime(1, i);
            situation = SBP_Defense;
        }
    return situation;
}

float Situation::GetTeamClosestDistance(int team, Vector3f pos)
{
     float min = 1000.0;
     for(int i = 1; i <= 11; i++)
     {
         float dist = (wm.GetPlayerPosition(team, i) - pos).Length();
         if(dist < min) min = dist;
     }
     return min;
}

float Situation::GetTeamClosestDistanceExcept(int team, int unum, Vector3f pos)
{
     float min = 1000.0;
     for(int i = 1; i <= 11; i++)
         if(i != unum)
         {
             float dist = (wm.GetPlayerPosition(team, i) - pos).Length();
             if(dist < min) min = dist;
         }
     return min;
}

// for dribble and pass to consider
float Situation::Emptiness(Vector3f pos)
{

    return 0.0f;
}

// for dribble, pass and mark to consider
float Situation::Attackness(Vector3f pos)
{
    Vector3f ourgoal(-wm.GetFieldLength()/2.0, 0.0, 0.0);
    return 200.0f - (pos-ourgoal).Length();
}

bool Situation::InField(Vector3f pos)
{
    float left = -wm.GetFieldLength() / 2.0;
    float right = -left;
    float bottom = -wm.GetFieldWidth() / 2.0;
    float top = -bottom; 
    return (pos[0] > left && pos[0] < right && pos[1] > bottom && pos[1] < top);
}

bool Situation::InPenaltyArea(int team, Vector3f pos)
{
    float left = -wm.GetFieldLength() / 2.0;
    float right = left + PENALTY_AREA_WIDTH;
    float bottom = -PENALTY_AREA_LENGTH / 2.0;
    float top = -bottom;
    if(team == 1)
    {
        right = wm.GetFieldLength() / 2.0;
        left = right - PENALTY_AREA_WIDTH;
    } 
    return (pos[0] > left && pos[0] < right && pos[1] > bottom && pos[1] < top);
}