physics.h

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.             *
 ***************************************************************************/
#ifndef PHYSICS_H
#define PHYSICS_H
#include "logserver.h"

// number of time pieces recorded. at least 3. time[0]: current, time[1]: last vision
const int max_vision_history = 20;
const int num_side = 2;
const int num_players = 11;

// set to constant
const float my_maxspeed = 1.62;
const float my_s = 2.51038;
const float ball_k = 0.2148156;

class WorldModel;
class WMBuilder;

class Physics{
public:
    Physics();
    ~Physics();

public:    
    // predicting. called by WMBuilder
    void TimeShift();
    void InitPrediction();
    void GetCurrentPosition();
    void GetCurrentSpeed();
    
    // predict position. called inside
    salt::Vector3f PredictPlayerPosition(int team, int unum, int t_start, int t_end, int t_predict);
    salt::Vector3f PredictBallPosition(int t_start, int t_end, int t_predict);
    salt::Vector3f PredictMyPosition(int t_start, int t_end, int t_predict);

    // predict speed
    salt::Vector3f PredictPlayerSpeed(int team, int unum, int t_start, int t_end, int t_predict);
    salt::Vector3f PredictBallSpeed(int t_start, int t_end, int t_predict);
    salt::Vector3f PredictMySpeed(int t_start, int t_end, int t_predict);
    
    // physics to get initial speed
    float GetPlayerInitialSpeed(int team, int unum, float dist, int t0, int t1);
    float GetBallInitialSpeed(float dist, int t0, int t1);
    float GetMyInitialSpeed(float dist, int t0, int t1, int axis);
    
    // physics to predict distance and last speed
    float SimPlayer(int team, int unum, float s0, float t0, float t1, float& S);
    float SimBall(float v0, int t0, int t1, float& S);
    float SimMyself(float v0, int t0, int t1, float& S, int axis);    
    
    // basic 1D function
    float GetBallStopDistance(float v);
    float GetChangeSpeedDriveForce(float v0, float v1, float maxforce, float& dist, int& t);
    float GetDriveForce(float v0, float v1, float dist, float maxspeed, int& t); // kernel function
    float GetDriveForce(float v0, float v1, float dist, int& t); 
    // wrapper maxspeed is calculated carefully to invoke kernel function
    
    // high-level functions
    salt::Vector3f GetBallStopPosition();
    salt::Vector3f GetStopDriveForce();    
    
    // prediction functions. called by WMBuilder once for each vision sense
    salt::Vector3f GetPlayerPosition(int team, int unum, int t = 0) 
    { return mMyPos[t] + mPlayerRelPosition[team][unum][t]; }
    salt::Vector3f GetMyPosition(int t = 0) { return mMyPos[t]; }
    salt::Vector3f GetBallPosition(int t = 0) { return mMyPos[t] + mBallRelPos[t]; }
    
    salt::Vector3f GetPlayerSpeed(int team, int unum, int t = 0) { return mPlayerSpeed[team][unum][t]; }
    salt::Vector3f GetBallSpeed(int t = 0) { return mBallSpeed[t]; }
    salt::Vector3f GetMySpeed(int t = 0) { return mMySpeed[t]; }
    
    // last reliable vision related
    int GetMaxLRV(){ return max_lrv; }
    void SetMaxLRV(int lrv) { max_lrv = lrv; }
    
    int GetMyLRV(){ return mMy_lrv; }
    int GetBallLRV(){ return mBall_lrv; }
    int GetPlayerLRV(int team, int unum){ return mPlayer_lrv[team][unum]; }
    
    void SetSpeedPrecision(float prec) { speed_precision = prec; }

protected:        
    // agent parameters
    float mAgentMass;
    float mAgentRadius;
    float mAgentMaxSpeed;
    float mKickRange;
    
    // ball parameters
    float mBallRadius;
    float mBallMass;   

    // history. time[0] is now, time[1] is last vision's arrival time, time[2] is of the second last vision...
    // each simulation time is 0.01 second
    int time[max_vision_history];

    // raw data calculated from vision sense, except index = 0
    salt::Vector3f mMyPos[max_vision_history];
    salt::Vector3f mBallRelPos[max_vision_history];
    salt::Vector3f mPlayerRelPosition[num_side][num_players + 1][max_vision_history];

    // all predicted           
    salt::Vector3f mMySpeed[max_vision_history];
    salt::Vector3f mBallSpeed[max_vision_history];
    salt::Vector3f mPlayerSpeed[num_side][num_players + 1][max_vision_history];
    
    friend class WorldModel;    
    friend class WMBuilder;
    
    // last reliable vision
    int max_lrv;
    int mMy_lrv;
    int mBall_lrv;
    int mPlayer_lrv[num_side][num_players + 1];
    
    float speed_precision;
};

#endif