physics.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 "physics.h"
#include "worldmodel.h"
#include "agent.h"
using namespace salt;

extern WorldModel wm;
extern Agent agent;

Physics::Physics()
{
    int i, j;
    
    // will be overwritten when a gamestate sensation was parsed
    mAgentMass = -75;
    mAgentRadius = -0.22;
    mAgentMaxSpeed = -10;  
    mBallRadius = -0.111;
    mBallMass = -0.4;
    
    // constant
    mKickRange = 0.04f;
    
    // initialize lrv;
    for(i = max_lrv - 1; i >= 0; i--)
        time[i] = -1;
    mMy_lrv = mBall_lrv = 1;
    for(i = 0; i <= 1; i++)
        for(j = 1; j <= 11; j++)
            mPlayer_lrv[i][j] = 1;
}

Physics::~Physics()
{
}

/* 
  These functions are called from WMBuilder
  
  TimeShift()          - shift time[] array
  GetCurrentPosition() - calculate current positions and put into mMyPos[0], mBallRelPos[0],
                         mPlayerRelPosition[i][j][0]
  GetCurrentSpeed()    - calculate current speed and put into mMySpeed[0], mBallSpeed[0] 
                         and mPlayerSpeed[i][j][0]
*/

#define SHIFT(x)    if(x < max_lrv - 1) x++

void Physics::TimeShift()
{
    int i, j, k;
    
    // shift lrv (lst reliable vision)
    SHIFT(mMy_lrv);
    SHIFT(mBall_lrv);
    for(i = 0; i <= 1; i++)
        for(j = 1; j <= 11; j++)
            SHIFT(mPlayer_lrv[i][j]);
    
    // shift raw visions
    for(k = max_lrv - 1; k > 1; k--)
    {
        time[k] = time[k-1];
        mMyPos[k] = mMyPos[k-1];
        mBallRelPos[k] = mBallRelPos[k-1];
        for(i = 0; i <= 1; i++)
            for(j = 1; j <= 11; j++)
                mPlayerRelPosition[i][j][k] = mPlayerRelPosition[i][j][k-1];
    }
}

// lrv set to 1: the object must be reset (offside, trainer etc), so NO OLD INFORMATION should be used
// lrv set to 2: a new episode starts(kickball, agent collision etc), so only last vision should be used
void Physics::InitPrediction()
{
    int i, j;
    
    // maximal error can be achieved WITHIN an episode. if error is more, another episode begins.
    // this should NOT be too small, especially predicting ball's speed. 
    // small episode is very dangerous in predicting speed
    float in_episode_error = 0.5; 
    
    // maximal distance that can be covered between two vision senses. if more is covered, it MUST BE RESET.
    float max_ball_distance = 5.0;
    float max_player_distance = 0.5;
    
    // a ball can go 2.0m away	
    if(((mMyPos[2]+mBallRelPos[2]) - (mMyPos[1]+mBallRelPos[1])).Length() > max_ball_distance) mBall_lrv = 1;
    // an agent can be at most 0.33m away 
    if((mMyPos[2] - mMyPos[1]).Length() > max_player_distance) mMy_lrv = 1;    
    // the same for other agents	
    for(i = 0; i <= 1; i++)
        for(j = 1; j <= 11; j++)
            if(((mMyPos[2]+mPlayerRelPosition[i][j][2]) - (mMyPos[1]+mPlayerRelPosition[i][j][1])).Length() 
                > max_player_distance)
                mPlayer_lrv[i][j] = 1;

    // try to use time[lrv..2] to predict time[1] to see if time[1] is from another episode
    Vector3f v;    
    if(mMy_lrv > 2)
    {
        v = PredictMyPosition(mMy_lrv, 2, 1);
        if((v - mMyPos[1]).Length() > in_episode_error)
            mMy_lrv = 2;
    }
    if(mBall_lrv > 2)
    {
        v = PredictBallPosition(mBall_lrv, 2, 1);
        if((v - (mMyPos[1] + mBallRelPos[1])).Length() > in_episode_error)
            mBall_lrv = 2;
    }    
    for(i = 0; i <= 1; i++)
        for(j = 1; j <= 11; j++)
            if(mPlayer_lrv[i][j] > 2)
            {
                v = PredictPlayerPosition(i, j, mPlayer_lrv[i][j], 2, 1);
                if((v - (mMyPos[1]+mPlayerRelPosition[i][j][1])).Length() > in_episode_error)
                    mPlayer_lrv[i][j] = 2;
            }
}

void Physics::GetCurrentPosition()
{
    int i, j;
    mMyPos[0] = PredictMyPosition(mMy_lrv, 1, 0); 
    mBallRelPos[0] = PredictBallPosition(mBall_lrv, 1, 0) - mMyPos[0];
    for(i = 0; i < 2; i++)
        for(j = 1; j <= 11; j++)
            mPlayerRelPosition[i][j][0] = PredictPlayerPosition(i, j, mPlayer_lrv[i][j], 1, 0) - mMyPos[0];
    mPlayerRelPosition[0][wm.GetTeamUnum()][0] = Vector3f(0.0, 0.0, 0.0);  
}

void Physics::GetCurrentSpeed()
{
    int i, j;
    mMySpeed[0] = PredictMySpeed(mMy_lrv, 1, 0);    
    mBallSpeed[0] = PredictBallSpeed(mBall_lrv, 1, 0);
        for(i = 0; i < 2; i++)
           for(j = 1; j <= 11; j++)
               mPlayerSpeed[i][j][0] = PredictPlayerSpeed(i, j, mPlayer_lrv[i][j], 1, 0);	       
}

/*
    Auxiliary functions: GetXXXInitialSpeed()
    Given the distance covered during [t0, t1], calculate its initial speed at time t0
    
    players: assume the speed is constant
    players: the ball, assume that the friction is F=-kv
    players: myself, drive forces in [t0, t1] should be read
*/
float Physics::GetPlayerInitialSpeed(int team, int unum, float dist, int t0, int t1)
{
    if(t0 == t1) return 0.0;
    else return dist / (t1 - t0);
}
float Physics::GetBallInitialSpeed(float dist, int t0, int t1)
{
    float k = ball_k;
    float m = mBallMass;
    float s = k / m;
    return dist * s / (1.0-exp(-s*(t1-t0)*0.01));
}
float Physics::GetMyInitialSpeed(float dist, int t0, int t1, int axis)
{
    // strange speed, cannot be achieved. not set of 1.616 since it could be a little bigger
    if((t1 - t0) * 2.5 < dist) return 0.0f;
    
    // do a binary search to find out the initial speed
    float l, r, m, d;
    l = -2.5; r = 2.5;
    while(r - l > speed_precision)
    {
        m = (l + r) * 0.5;
        SimMyself(m, t0, t1, d, axis);
        if(d < dist) l = m; else r = m;
    }
    return m;
}

/*
    Auxiliary functions: SimXXX()
    Given the initial speed v0 at time t0, how long will it traval during time [t0, t1]?
    what is the last speed?
    due to possible essential think-time latancy, SimMyself() function may be not accurate
*/
float Physics::SimPlayer(int team, int unum, float v0, float t0, float t1, float& S)
{
    S = v0 * (t1 - t0);
    return v0;
}
float Physics::SimBall(float v0, int t0, int t1, float& S)
{
    float k = ball_k;
    float m = mBallMass;
    float s = k / m;
    S = v0 * (1.0-exp(-s*(t1-t0)*0.01)) / s;
    return v0 * exp(-s*(t1-t0)*0.01);
}    
float Physics::SimMyself(float v0, int t0, int t1, float& S, int axis)
{
    float s = my_s;
    float v1, ds, vm;
    float dt = 0.01;

    // do simulation
    S = 0.0;
    for(int t = t0; t <= t1; t++){
      vm = agent.GetDriveVec(t)[axis] / 100.0 * my_maxspeed;
      v1 = vm + exp(-s*dt)*(v0-vm);
      ds = (v0-v1)/s + vm * dt;
      v0 = v1;
      S += ds;
    }
       
    return v0;
}    

/*
    Predict Positions. called by GetCurrentPosition()
    This set of function is extremly useful, since it predicts positions when the last vision is too out-dated
    it has three parameters: t_start, t_end, t_predict
*/    
salt::Vector3f Physics::PredictPlayerPosition(int team, int unum, int t_start, int t_end, int t_predict)
{
    salt::Vector3f v(0.0,0.0,0.22);
    for(int i = 0; i < 2; i++)
    {
       float m, dist;
       if(t_start == t_end) dist = 0.0;
       else
       {
           m = GetPlayerInitialSpeed(team, unum, 
               GetPlayerPosition(team, unum, t_end)[i] - GetPlayerPosition(team, unum, t_start)[i], 
               time[t_start], time[t_end]);
           SimPlayer(team, unum, m, time[t_start], time[t_predict], dist);
       }
       v[i] = GetPlayerPosition(team, unum, t_start)[i] + dist;
    }
    return v;
}
salt::Vector3f Physics::PredictBallPosition(int t_start, int t_end, int t_predict)
{ 
    salt::Vector3f v(0.0,0.0,0.111);
    for(int i = 0; i < 2; i++)
    {