formation.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 <fstream>
#include "formation.h"
#include "worldmodel.h"
#include "skill.h"
#include "decision.h"
extern WorldModel wm;
extern Skill skill;
extern Decision decision;

using namespace std;
using namespace salt;

// basic points for interpolation
float SBP_DX[]={-52, -44, -36, -24, -12, 0, 12, 24, 36, 44, 52};
float SBP_DY[]={-36, -30, -20,  -9,   0, 9, 20, 30, 36};

float init_x_pos[] = {-52.0, -27.6, -28.7, -27.6, -28.7, -15.0,  -6.0, -6.0, -0.6,  -2.0, -2.0}; // bug fixed
float init_y_pos[] = {0.0, -14.2,  -6.7,  14.2,   6.7,   0.0, -14.1, 14.1,  0.0, -20.4, 20.4};

Formation::Formation()
{
}

Formation::~Formation()
{
}

//  For a player type, get his strategic position according to the ball and situation.
//  This relies on SBPosition[][][], which is read from file playertypes.sbp
salt::Vector2f 
Formation::tPlayerType::GetStrategicPosition
(salt::Vector2f ballposition, salt::Vector2f playerbsp, int situation)
{
    const float eps = 1e-5;
    int i, j, x, y;
    float rx, ry, d;
    salt::Vector2f p[2][2];
    salt::Vector2f p1, p2, add;    
    salt::Vector2f pp(1.0f,1.0f);

    // find position of ball, initialze interpolation
    for (i = 1; i <= 10; i++) 
        if (ballposition.x() < SBP_DX[i])
            break;
    for (j = 1; j <= 8; j++) 
        if (ballposition.y() < SBP_DY[j]) 
            break;    
    rx = (ballposition.x() - SBP_DX[i-1]) / (SBP_DX[i] - SBP_DX[i-1]);
    ry = (ballposition.y() - SBP_DY[j-1]) / (SBP_DY[j] - SBP_DY[j-1]);
    x = i - 1;
    y = j - 1;

    // consider four corners of the grid square containing the ball and get the basic point pp
    for (i = 0; i <= 1; i++)
        for (j = 0; j <= 1; j++)
            p[i][j] = SBPosition[situation][x+i][y+j];	
    p1 = p[0][0] + (p[1][0] - p[0][0]) * rx;
    p2 = p[0][1] + (p[1][1] - p[0][1]) * rx;
    pp = p1 +(p2 - p1) * ry;
    
    // adjust pp by a incremental value add
    d = (ballposition - pp).Length();
    if (d < 2.0) d = 2.0;
    if (d > 10.0) d = 10.0;
    add = playerbsp - BasePosition;

    if (fabs(add.x()) > eps)
        add.x() = (add.x()/(float)fabs(add.x())) * (float)sqrt((float)fabs(add.x())/d)*d;
    if (fabs(add.y()) > eps)
        add.y() = (add.y()/(float)fabs(add.y())) * (float)sqrt((float)fabs(add.y())/d)*d;

    if ((add.x() > 0) && (situation == SBP_Defense))
        add.x() /= 2.0;
    if ((add.x() < 0) && (situation == SBP_Attack))
        add.x() /= 4.0;

    // apply the adjustment
    pp += add;
    return pp;
}

int Formation::GetIndex(const char* s)
{
    int i;
    for (i = 0; i < Num_playertypes; i++)
        if (strcmp(ptype[i].Name, s) == 0) return i;
    return -1;
}

// read SBPosition[][][] from *.sbp file.
int Formation::SetPlayerTypes(const char *filename)
{
    const int len = 256;
    int i, j, k;
    float t1, t2;

    // open file
    ifstream file;
    file.open(filename);    
    if (!file.is_open())
    {
       log.Log("Cannot open: %s\n", filename);
       return 0;
    }

    // read file
    char nouse[len];    
    for (i = 0; i <= 18; i++)
    {
        // basic information
        log.Log("ptype[%d]:\n", i);    
        ptype[i].id = i;
        file.getline(ptype[i].Name, len);	
        log.Log("Name = %s\n", ptype[i].Name);
        file >> ptype[i].BasePosition.x() >> ptype[i].BasePosition.y();
        log.Log("BasePosition = (%.3f, %.3f)\n", ptype[i].BasePosition[0], ptype[i].BasePosition[1]);

        // SBP_Attack
        file.getline(nouse, len);
        file.getline(nouse, len);
        for (j=1;j<=9;j++)
            for (k=1;k<=7;k++)
                file >> t1 >> t2 >> ptype[i].SBPosition[SBP_Attack][j][k].x() 
                     >> ptype[i].SBPosition[SBP_Attack][j][k].y();
        for (j=1;j<=9;j++)
        {
            ptype[i].SBPosition[SBP_Attack][j][0].x() = ptype[i].SBPosition[SBP_Attack][j][1].x();
            ptype[i].SBPosition[SBP_Attack][j][0].y() = ptype[i].SBPosition[SBP_Attack][j][1].y();		
            ptype[i].SBPosition[SBP_Attack][j][8].x() = ptype[i].SBPosition[SBP_Attack][j][7].x();
            ptype[i].SBPosition[SBP_Attack][j][8].y() = ptype[i].SBPosition[SBP_Attack][j][7].y();
        }
        for (k=0;k<=8;k++)
        {
            ptype[i].SBPosition[SBP_Attack][0][k].x() =
            ptype[i].SBPosition[SBP_Attack][1][k].x()*2-ptype[i].SBPosition[SBP_Attack][2][k].x();
            ptype[i].SBPosition[SBP_Attack][0][k].y() =
            ptype[i].SBPosition[SBP_Attack][1][k].y()*2-ptype[i].SBPosition[SBP_Attack][2][k].y();
            ptype[i].SBPosition[SBP_Attack][10][k].x() =
            ptype[i].SBPosition[SBP_Attack][9][k].x()*2-ptype[i].SBPosition[SBP_Attack][8][k].x();
            ptype[i].SBPosition[SBP_Attack][10][k].y() =
            ptype[i].SBPosition[SBP_Attack][9][k].y()*2-ptype[i].SBPosition[SBP_Attack][8][k].y();
            if ((ptype[i].Name[0] != 'S') && (ptype[i].Name[0] != 'F'))
            {
                ptype[i].SBPosition[SBP_Attack][10][k].x() =
                ptype[i].SBPosition[SBP_Attack][9][k].x()*5.0f/3.0f;
                ptype[i].SBPosition[SBP_Attack][10][k].x() -=
                ptype[i].SBPosition[SBP_Attack][8][k].x()*2.0f/3.0f;
            }
            ptype[i].SBPosition[SBP_Attack][0][k].x() = ptype[i].SBPosition[SBP_Attack][1][k].x();
        }

        // SBP_Defense
        file.getline(nouse,256);
        file.getline(nouse,256);
        for (j=1;j<=9;j++)
            for (k=1;k<=7;k++)
                file>>t1>>t2>>ptype[i].SBPosition[SBP_Defense][j][k].x()
                    >>ptype[i].SBPosition[SBP_Defense][j][k].y();
        for (j=1;j<=9;j++)
        {
            ptype[i].SBPosition[SBP_Defense][j][0].x() = ptype[i].SBPosition[SBP_Defense][j][1].x();
            ptype[i].SBPosition[SBP_Defense][j][0].y() = ptype[i].SBPosition[SBP_Defense][j][1].y();
            ptype[i].SBPosition[SBP_Defense][j][8].x() = ptype[i].SBPosition[SBP_Defense][j][7].x();
            ptype[i].SBPosition[SBP_Defense][j][8].y() = ptype[i].SBPosition[SBP_Defense][j][7].y();
        }
        for (k=0;k<=8;k++)
        {
            ptype[i].SBPosition[SBP_Defense][0][k].x()=
            ptype[i].SBPosition[SBP_Defense][1][k].x()*2-ptype[i].SBPosition[SBP_Defense][2][k].x();
            ptype[i].SBPosition[SBP_Defense][0][k].y()=
            ptype[i].SBPosition[SBP_Defense][1][k].y()*2-ptype[i].SBPosition[SBP_Defense][2][k].y();
            ptype[i].SBPosition[SBP_Defense][10][k].y()=
            ptype[i].SBPosition[SBP_Defense][9][k].y()*2-ptype[i].SBPosition[SBP_Defense][8][k].y();
            ptype[i].SBPosition[SBP_Defense][0][k].x()=
            ptype[i].SBPosition[SBP_Defense][1][k].x();
            ptype[i].SBPosition[SBP_Defense][10][k].x()=
            ptype[i].SBPosition[SBP_Defense][9][k].x();
        }

        // attack rate and defense rate
        file >> ptype[i].AttackRate >> ptype[i].DefenseRate;
        log.Log("AttackRate = %.3f, DefenseRate = %.3f\n", ptype[i].AttackRate, ptype[i].DefenseRate);
        file.getline(nouse,256);
    }

    // goalie type is default
    ptype[19].id = 19;
    strcpy(ptype[19].Name, "GK");
    ptype[19].AttackRate = 0.0f;
    ptype[19].DefenseRate = 1.0f;

    file.close();
    return 1;
}

// read formation file *.fmt
int Formation::SetFormation(char* filename)
{
    const int len = 256;
    
    // open file
    ifstream file;  
    file.open(filename);
    if(!file.is_open())
    {
        log.Log("Cannot open file: %s\n", filename);
        return 0;
    }
    
#define skip()  file.getline(nouse, len)

    // basic information
    int i;      
    char nouse[len];    
    file.getline(Name, len); 
    log.Log("Formation name: %s", Name);
    
    file >> AttackRate; skip();
    file >> DefenseRate; skip();
    file >> AttackDirection; skip();
    file >> FormationMode; skip();
    file >> DefenseMode; skip();
    file >> AttackMode; skip();
    
    log.Log("Attack Rate = %.3f, Defense Rate = %.3f\n", AttackRate, DefenseRate);
    log.Log("AttackDirection, FormationMode, DefenseMode, AttackMode = %d, %d, %d, %d", 
             AttackDirection, FormationMode, DefenseMode, AttackMode);
    
    // each player in the formation
    for (i = 2; i <= SP_team_size; i++)
    {
        log.Log("Player %d:\n", i);
        skip(); skip();    
        log.Log("Type Name = %s\n", nouse);
        iPlayerType[i] = GetIndex(nouse);

        // information
        file >> BaseStrategicPosition[i].x(); skip();
        file >> BaseStrategicPosition[i].y(); skip();
        file >> Activity[i].RunWithBall; skip();
        file >> Activity[i].ControlBall; skip();
        file >> Activity[i].Dribble; skip();
        file >> Activity[i].PreferPassRoute; skip();
        file >> Activity[i].PreferDefense; skip();
        file >> Activity[i].FormationMode; skip();
        file >> Activity[i].AttackRate; skip();
        file >> Activity[i].DefenseRate; skip();
        skip();

        // logging
        log.Log("BaseStrategicPosition: (%.3f %.3f)", BaseStrategicPosition[i][0], BaseStrategicPosition[i][1]);
        log.Log("Activity[i].RunWithBall = %d\n", Activity[i].RunWithBall);
        log.Log("Activity[i].ControlBall = %d\n", Activity[i].ControlBall);
        log.Log("Activity[i].Dribble = %d\n", Activity[i].Dribble);
        log.Log("Activity[i].PreferPassRoute = %d\n", Activity[i].PreferPassRoute);
        log.Log("Activity[i].PreferDefense = %d\n", Activity[i].PreferDefense);
        log.Log("Activity[i].FormationMode = %d\n", Activity[i].FormationMode);
        log.Log("Activity[i].AttackRate = %.3f\n", Activity[i].AttackRate);
        log.Log("Activity[i].DefenseRate = %.3f\n", Activity[i].DefenseRate);	
    }
    
    // goalie
    iPlayerType[1] = GetIndex("GK");
    BaseStrategicPosition[1].x() = -50.0f;
    BaseStrategicPosition[1].y() = 0.0f;
    
    file.close();
    return 1;
}

salt::Vector2f Formation::GetStrategicPosition(int situation, salt::Vector2f& ballpos, SNum number)
{
    if(number < 1 || number > 11)
    {
        log.Log("Wrong player number %d\n");
        return salt::Vector2f(0.0f, 0.0f);
    }
    if(IsGoalie(number))
    {
        Vector3f pos = GetInitialFormation(number);
        return Vector2f(pos[0], pos[1]);
    }
  
    // get strategic position according to player type
    salt::Vector2f pp = ptype[iPlayerType[number]].GetStrategicPosition(
                        ballpos, BaseStrategicPosition[number], situation);
    log.Log("Time: %.2f, Situaton: %s\n", wm.GetTime(), (situation==SBP_Attack ? "Attack" : "Defense"));
    log.Log("ballpos = (%.3f, %.3f), BSP: (%.3f, %.3f)\n", 
           ballpos[0], ballpos[1], BaseStrategicPosition[number][0], BaseStrategicPosition[number][1]);
    log.Log("Position 1: (%.3f, %.3f)\n", pp[0], pp[1]);

    // adjust according to formation_mode
    if ((Activity[number].FormationMode == FS_Open) || 
       ((Activity[number].FormationMode == FS_SameWithTeam) && (FormationMode == FS_Open)))
    {
        float dist = -1.0f;
        pp.x() = (pp + (ballpos - pp).Normalized() * dist).x();
        pp.y() = (pp + (ballpos - pp).Normalized() * dist).y();
    }  
    if ((Activity[number].FormationMode==FS_Close) ||
       ((Activity[number].FormationMode==FS_SameWithTeam)&&(FormationMode==FS_Close)))
    {
        float len = (pp-ballpos).Length();
        float dist = len > 1.0f ? 1.0f : len;
        pp.x() = (pp + (ballpos - pp).Normalized() * dist).x();
        pp.y() = (pp + (ballpos - pp).Normalized() * dist).y();
    }
  
    // adjust according to field size
    if (pp.x()<-wm.GetFieldLength()/2) pp.x() = -wm.GetFieldLength()/2;
    if (pp.x()>wm.GetFieldLength()/2) pp.x() = wm.GetFieldLength()/2;
    if (pp.y()<-wm.GetFieldWidth()/2) pp.y() = -wm.GetFieldWidth()/2;
    if (pp.y()>wm.GetFieldWidth()/2) pp.y() = wm.GetFieldWidth()/2;
    if ((pp.x() < -wm.GetFieldLength()/2 + wm.GetGoalWidth()) && (fabs(pp.y()) < PENALTY_AREA_WIDTH / 2.0))
        pp.x() = -wm.GetFieldLength()/2 + wm.GetGoalWidth();
    if ((pp.x() > wm.GetFieldLength()/2 - wm.GetGoalWidth()) && (fabs(pp.y()) < wm.GetGoalWidth()/ 2.0))
        pp.x() = wm.GetFieldLength()/2 - wm.GetGoalWidth();

    log.Log("Position 2: (%.3f, %.3f)\n", pp[0], pp[1]);
    return pp;
}

int Formation::Init()
{
    Name[0] = '\0';
    init_x_pos[0] = -wm.GetFieldLength() / 2.0;

    for(int i = 0; i< Num_playertypes; i++)
        ptype[i].Name[0] = '\0';

    log.Log("Opening %s...\n", SBPFile);
    if(!SetPlayerTypes(SBPFile)) return 0;
    
    log.Log("Opening %s...\n", FMTFile);
    if(!SetFormation(FMTFile)) return 0;
    
    return 1;
}