skill.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 "skill.h"
#include "agent.h"
#include "worldmodel.h"
#include "geometry.h"
#include "decision.h"
#include "situation.h"

extern Agent agent;
extern WorldModel wm;
extern Decision decision;
extern Situation situation;

using namespace Geometry;

Skill::Skill()
{
}

Skill::~Skill()
{
}

bool Skill::Kickable()
{
    Vector3f pos = wm.GetMyPosition();
    Vector3f ballpos = wm.GetBallPosition();
    return((pos-ballpos).Length() < 0.5);
}    

// if running very fast, just accerate with negative force, otherwise, stop driving, let friction stops it
void Skill::StopMoving()
{
    Vector3f myvel = wm.GetMySpeed();
    Vector3f DriveLine = myvel;

    DriveLine[2] = 0.0f;
    DriveLine.Normalize();
    DriveLine *= -100.0;

    if ( myvel.Length() < 0.2f ) agent.Drive(Vector3f(0.0f,0.0f,0.0f));
    else agent.Drive (DriveLine);
}

// for goalie
void Skill::RunToKick()
{
    Intercept(-60.0, 60.0);
    agent.Kick(50, 100.0);
}

// algorithm: decomposition of velocity.
// still need extensive tests :(
// version 1.0, still usable
void Skill::Goto(Vector3f pos, bool avoid)
{
    Vector3f mypos, myvel, driveline;
    Point p, myp, myvelp, drivelinep, o(0.0, 0.0), ptmp;
    int itmp;
    float ftmp, dist;
    float vel;
    float devvel;
    float indvel;
    float force_velcorr;
    float force_indepth_max;
    float force_indepth;
    
    if(avoid)
    {
        log.Log("(Goto) old pos = (%.3f %.3f)\n", pos[0], pos[1]);
        pos = wm.TransformTarget(pos);
        log.Log("(Goto) new pos = (%.3f %.3f)\n", pos[0], pos[1]);
    }

    mypos = wm.GetMyPosition();
    myvel = wm.GetMySpeed();
    driveline = pos-mypos;
    driveline[2] = 0.0;
    driveline.Normalize();
       
    p = MakePoint(pos);
    myp = MakePoint(mypos);
    myvelp = MakePoint(myvel);
    drivelinep = MakePoint(driveline);
    dist = dis(p, myp);
    
    // needs further improvement!
    vel = myvel.Length();    
    devvel = DistanceToLine(myvelp, o, drivelinep, ptmp);    
    indvel = GetProjectionDistance(o, drivelinep, myvelp);
    force_velcorr = wm.physics.GetChangeSpeedDriveForce(devvel, 0.0, 100.0, ftmp, itmp);    
    force_indepth_max = sqrt(10000.0 - force_velcorr*force_velcorr);         
    force_indepth = wm.physics.GetDriveForce(indvel, 0.0, dist, itmp);    
    if(fabs(force_indepth) > force_indepth_max)
        force_indepth = force_indepth > 0 ? force_indepth_max : -force_indepth_max;          

    Point force = VectorCombine(drivelinep, force_velcorr, force_indepth);    
    agent.Drive(Vector3f(force[0], force[1], 0.0));
    
    int t, min_time, max_time;
    t = wm.GetMyGotoTime(pos, min_time, max_time);
}

// parameters: for player(team, unum) who wants to intercept the ball at desired kickangle [low, high]
// what is the estimation time, minimal time and maximal time? 
// it returns the intercept point
// large pen for more accuracy, but more time!
Vector3f Skill::Intercept
(int team, int unum, float low, float high, int& est_time, int& min_time, int& max_time, float pen)
{
    Vector3f ballvel = wm.GetBallSpeed();
    ballvel[2] = 0.0f;
    float ballv = ballvel.Length();
    
    Vector3f driveline_low = Vector3f(gCos(gDegToRad(low)), gSin(gDegToRad(low)), 0.0);
    Vector3f driveline_high = Vector3f(gCos(gDegToRad(high)), gSin(gDegToRad(high)), 0.0);
    Vector3f driveline_mid = Vector3f(gCos(gDegToRad((low+high)/2.0)), gSin(gDegToRad((low+high)/2.0)), 0.0);

    // important triangle: mp-bp-bsp
    Point mp = MakePoint(wm.GetPlayerPosition(team, unum));
    Point bp = MakePoint(wm.GetBallPosition());
    Point bsp = MakePoint(wm.GetBallStopPosition());
    Point dummy;
    Vector3f target, tar;
    
    // get projection
    Point p = GetProjectionPoint(bp, bsp, mp);
    float dist = GetProjectionDistance(bp, bsp, mp);
    float roll_dist = dis(bsp, bp);

    // for binary search    
    float l, r, m, s;
    
    // penalty = 1.0: i'm there when the ball needs to roll 1.0m more! blocking
    // penalty = -1.0: i'm there when the ball already passed. following.
    // penalty < 0 is dangerous since we may stop before reaching the ball. 
    // this is adjusted by the condition when you are running towards the ball.
    float penalty = 0.0; 
                             
    int t, player_time;
    Point pp;
    
    // already on the line, run direct to it. 
    // this is added because of vision error may cause incorrect GoTo time, which may cause failure
    // the biggest problem is the wrong direction (block? follow?), so the patch is added below
    Vector3f ballpos = wm.GetBallPosition();
    Vector3f ballstoppos = wm.GetBallStopPosition();
    if(fabs(DistanceToLine(mp, bp, bsp, dummy)) < 0.15)
    {
        log.Log("(Intercept) Direction correct, approaching.\n");
        if(dist > 0) // the ball is approaching
            target = Vector3f(ballpos[0], ballpos[1], 0.0) - driveline_mid * 0.32;
        else
            target = Vector3f(ballstoppos[0], ballstoppos[1], 0.0) - driveline_mid * 0.32;
    }
    else // find the intercept point
    {    
        if(dist < 0) 
        {
            log.Log("(Intercept) Case 1: Ball runs too fast, must follow it.");
            l = 0; r = roll_dist;
        }
        else if(dist > roll_dist) 
        {
            log.Log("(Intercept) Case 2: Ball stops before arriving p. Go towards it.");
            l = 0; r = roll_dist;
        }
        else 
        {
            log.Log("(Intercept) Case 3: Ball passes p. Must decide which side of p to go to. \
                    It also depends on the desired kick direction");

            // time to go to p
            t = wm.GetMyGotoTime(Vector3f(p[0], p[1], 0.0), min_time, max_time);
            player_time = t;
            wm.physics.SimBall(ballv, 0, player_time, s);

            // ball is too fast, binary search on [dist, roll_dist]
            if(s > dist){ l = dist; r = roll_dist; }
            // ball is too slow, binary search on [0, dist]
            else { l = 0; r = dist; }
        }
    
        if(ballvel[0] > 0) penalty = -pen; // follow
        else penalty = pen; // block
        l -= pen; r += pen; // consider penalty, bigger interval

        log.Log("(Intercept) l = %.3f, r = %.3f, penalty = %.3f\n", l, r, penalty);
    
        // binary search part.
        // l, r are already set
        do{
            // point to check
            m = (l + r) * 0.5;
            pp = GetPointOnRay(bp, bsp, m); 

            // check low
            tar = Vector3f(pp[0], pp[1], 0.0);
            tar = tar - driveline_low * 0.32;
            target = tar;
            t = wm.GetMyGotoTime(tar, min_time, max_time); // player's time and ball's distance
            player_time = t;
            wm.physics.SimBall(ballv, 0, player_time, s);
    
            // check high 
            tar = Vector3f(pp[0], pp[1], 0.0);
            tar = tar - driveline_high * 0.32;
            t = wm.GetMyGotoTime(tar, min_time, max_time); // player's time and ball's distance
            if(t < player_time)
            {
                target = tar;
                player_time = t;
                wm.physics.SimBall(ballv, 0, player_time, s);