worldmodel.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 <math.h>
#include <zeitgeist/leaf.h>
#include "worldmodel.h"
#include "geometry.h"
#include "skill.h"
using namespace salt;
using namespace Geometry;
extern Skill skill;

WorldModel::WorldModel()
{
  mFieldLength = -105.0;
  mFieldWidth = -68.0;
  mFieldHeight = -20.0;
  mGoalWidth = -7.32;
  mGoalDepth = -2.0f;
  mGoalHeight = -2.44;
  mBorderSize = -4.0;  
  
  // not belonging to any team
  mTeamIndex = TI_NONE;
  mTeamUnum = 0;  
}

WorldModel::~WorldModel()
{
}

void WorldModel::LogWM()
{
    int index = GetTeamIndex() == TI_LEFT ? 1 : 2;
    int num = GetTeamUnum();
  
    // basic
#define LOG_AGENT_STATE  
#ifdef LOG_AGENT_STATE  
    log.Log("Battery = %.2f, Temperature = %.2f\n", GetBattery(), GetTemperature());
#endif 
 
#define LOG_BASIC
#ifdef LOG_BASIC
    log.Log(mFieldLength, "FieldLength");
    log.Log(mFieldWidth, "FieldWidth");
    log.Log(mFieldHeight, "FieldHeight");
    log.Log(mGoalWidth, "GoalWidth");
    log.Log(mGoalDepth, "GoalDepth");
    log.Log(mGoalHeight, "GoalHeight");
    log.Log(mBorderSize, "BorderSize");
    log.Log(physics.mAgentMass, "AgentMass");
    log.Log(physics.mAgentRadius, "AgentRadius");
    log.Log(physics.mAgentMaxSpeed, "AgentMaxSpeed");
    log.Log(physics.mBallRadius, "BallRadius");
    log.Log(physics.mBallMass, "BallMass");
#endif

#define LOG_LAST_RELIABLE_VISION
#ifdef LOG_LAST_RELIABLE_VISION
    log.Log("mylrv = %d\n", physics.GetMyLRV());
    log.Log("balllrv = %d\n", physics.GetBallLRV());
#endif

#define LOG_POS  
#ifdef LOG_POS  
    // my position & ball position
    for(int i = 0; i < max_vision_history; i++)
        log.Log("T%d(%.2f): mypos = (%.3f, %.3f, %.3f)\n", 
        GetSimTime(i), GetTime(i), GetMyPosition(i)[0], GetMyPosition(i)[1], GetMyPosition(i)[2]);
#endif

#define LOG_BALL_POS    
#ifdef LOG_BALL_POS    
    for(int i = 0; i < max_vision_history; i++)
        log.Log("T%d(%.2f): ballpos = (%.3f, %.3f, %.3f)\n", 
        GetSimTime(i), GetTime(i), GetBallPosition(i)[0], GetBallPosition(i)[1], GetBallPosition(i)[2]);
#endif

#define LOG_OTHER_POS  
#ifdef LOG_OTHER_POS  
    // player positions  
    salt::Vector3f v;
    log.Log("our team players:\n");
    for(int i = 1; i <= 11; i++){
        v = GetPlayerPosition(0, i);
        log.Log("id = %d, pos = (%.3f, %.3f, %.3f)\n", i, v[0], v[1], v[2]);
    }
    log.Log("other team players:\n");
    for(int i = 1; i <= 11; i++){
        v = GetPlayerPosition(1, i);
        log.Log("id = %d, pos = (%.3f, %.3f, %.3f)\n", i, v[0], v[1], v[2]);
    }
#endif  
}

int
WorldModel::GetNearestPlayer(salt::Vector3f pos, int team)    
{
    int best = 1;
    // delete the 3rd dimension!
    float min = (pos - GetPlayerPosition(team, 1)).Length();
    for(int i = 2; i <= 11; i++)
    {
        float dist = (pos - GetPlayerPosition(team, i)).Length();
        if(dist < min)
        {
            min = dist;
            best = i;
        }
    }
    return best;
}

/*
              velp
            /
          /
        mp --------> pos
*/
int
WorldModel::GetPlayerGotoTime(salt::Vector3f pos, int team, int unum, int& min_time, int& max_time)
{
    int t;
    float indvel, dist, force;
    Vector3f mpos = GetPlayerPosition(team, unum);
    Vector3f vel = GetPlayerSpeed(team, unum);
    Vector3f driveline = pos - mpos;
    driveline[2] = 0.0;
    dist = driveline.Length();                           // dist to cover
    if(fabs(dist) < 0.4) { min_time = max_time = t = int(dist / my_maxspeed * 100.0); }
    else
    {    
       Point velp = MakePoint(vel);
       Point o(0.0, 0.0);
       Point drivelinep = MakePoint(driveline);
    
       indvel = GetProjectionDistance(o, drivelinep, velp); // current in-depth velocity
       force = physics.GetDriveForce(indvel, 0.0, dist, t); // time needed
    
       force = physics.GetDriveForce(my_maxspeed, 0.0, dist, min_time); // best case
       force = physics.GetDriveForce(-my_maxspeed, 0.0, dist, max_time); // worse case
    }
    return t;
}

int
WorldModel::GetMyGotoTime(salt::Vector3f pos, int& min_time, int& max_time)
{
    return GetPlayerGotoTime(pos, 0, GetTeamUnum(), min_time, max_time);
}

/*
        -------- ballp ----------
                  tar
    myp ---------proj------------ p

*/

salt::Vector3f
WorldModel::TransformTarget(salt::Vector3f pos)
{
    Vector3f mypos = GetMyPosition();
    Vector3f ballpos = GetBallPosition();
    
    // consider ball
    Point myp = MakePoint(mypos);
    Point p = MakePoint(pos);
    Point ballp = MakePoint(ballpos);
    Point dummy;
    
    float dist = dis(p, myp);
    float distp = GetProjectionDistance(myp, p, ballp);
    
    skill.log.Log("(TransformTarget) dist = %.3f, distp = %.3f\n", dist, distp);
    
    // possibly blocking the path
    if(distp < dist)
    {
        float disttoline = DistanceToLine(ballp, myp, p, dummy);
        skill.log.Log("(TransformTarget) disttoline = %.3f\n", disttoline);
        if(fabs(disttoline) < 0.33)
        {
            // blocked! calculate the intermediate target point
            if(disttoline > 0) disttoline -= 0.33;
            else disttoline += 0.33;
    
            skill.log.Log("(TransformTarget) new disttoline = %.3f\n", disttoline);
    
            Point proj = GetProjectionPoint(myp, p, ballp);
            Point direction;
            direction[0] = p[0] - myp[0];
            direction[1] = p[1] - myp[1];
            Point newdirection = VectorCombine(direction, disttoline, distp);
            Point tar;
            tar[0] = myp[0] + newdirection[0];
            tar[1] = myp[1] + newdirection[1];
    
            skill.log.Log("(TransformTarget) new destination = (%.3f, %.3f)\n", tar[0], tar[1]);
    
            return Vector3f(tar[0], tar[1], 0.0);
        }
    }
    return pos; // unchanged    
}