physics.cpp

RoboCup 3D 仿真组清华大学2005的源代码

       float m, dist;
       if(t_start == t_end) dist = 0.0;
       else
       {
           m = GetBallInitialSpeed(GetBallPosition(t_end)[i] - GetBallPosition(t_start)[i], 
               time[t_start], time[t_end]);
           SimBall(m, time[t_start], time[t_predict], dist);
       }
       v[i] = GetBallPosition(t_start)[i] + dist;
    }
    return v;
}
salt::Vector3f Physics::PredictMyPosition(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 = GetMyInitialSpeed(GetMyPosition(t_end)[i] - GetMyPosition(t_start)[i], 
               time[t_start], time[t_end], i);
           SimMyself(m, time[t_start], time[t_predict], dist, i);
       }
       v[i] = GetMyPosition(t_start)[i] + dist;
    }
    return v;
}

/*
    Predict Speed. called by GetCurrentSpeed()
    note that the absolute speed my be very inaccurate due to big self-locating error :(
    one solution is to consider a longer time interval, 
    but the siutation may change during it if the interval is too large.

    it has three parameters: t_start, t_end, t_predict
*/
salt::Vector3f Physics::PredictPlayerSpeed(int team, int unum, int t_start, int t_end, int t_predict)
{
    salt::Vector3f v(0.0,0.0,0.0);
    for(int i = 0; i < 2; i++)
    {
        float m, dist;
        if(t_start == t_end) v[i] = 0.0f;
        else
        {
            m = GetPlayerInitialSpeed(team, unum, 
                GetPlayerPosition(team, unum, t_end)[i] - GetPlayerPosition(team, unum, t_start)[i], 
                time[t_start], time[t_end]);
            v[i] = SimPlayer(team, unum, m, time[t_start], time[t_predict], dist);
        }
    }    
    return v;
}
salt::Vector3f Physics::PredictBallSpeed(int t_start, int t_end, int t_predict)
{
    salt::Vector3f v(0.0,0.0,0.0);
    for(int i = 0; i < 2; i++)
    {
        float m, dist;
        if(t_start == t_end) v[i] = 0.0f;
        else
        {
            m = GetBallInitialSpeed(GetBallPosition(t_end)[i] - GetBallPosition(t_start)[i], 
                time[t_start], time[t_end]);
            v[i] = SimBall(m, time[t_start], time[t_predict], dist);
        }
    }
    return v;
}
salt::Vector3f Physics::PredictMySpeed(int t_start, int t_end, int t_predict)
{
    salt::Vector3f v(0.0,0.0,0.0);
    for(int i = 0; i < 2; i++)
    {
       float m, dist;
       if(t_start == t_end) v[i] = 0.0f;
       else
       {
           m = GetMyInitialSpeed(GetMyPosition(t_end)[i] - GetMyPosition(t_start)[i], 
               time[t_start], time[t_end], i);
           v[i] = SimMyself(m, time[t_start], time[t_predict], dist, i);
       }
    }
    return v;
}
   
/*
    Other functions.
*/
// given the speed of ball, predict its stop distance
float Physics::GetBallStopDistance(float v)
{
    float k = ball_k;
    float m = mBallMass;
    float s = k / m;
    return v / s;
}

// given current velocity and target velocity with maxforce
// returns the first-cycle force that should be applied, and *estimated* time needed
// when in maxspeed, sudden stop will cause 0.4m's distance
float Physics::GetChangeSpeedDriveForce(float v0, float v1, float maxforce, float& dist, int& t)
{
    float s = my_s;
    float v = v0;
    float dt = 0.01 * agent.GetCycleStep();
    float first_force;
    bool first = true;

    // start iteration    
    dist = 0.0f;
    t = 0;    
    while(fabs(v - v1) > speed_precision)
    {
        // try to change speed as much as possible
        float vm = (v1 - exp(-s*dt)*v) / (1.0 - exp(-s*dt));
        float force = vm / my_maxspeed * 100.0;

        // adjust force and record
        if(fabs(force) > maxforce) force = force > 0 ? maxforce : -maxforce;
        vm = force * my_maxspeed / 100.0;
        if(first)
        {
            first_force = force;
            first = false;
        }

        // apply force
        float ds = (v - vm + vm * s * dt - exp(-s*dt)*(v - vm)) / s;	
        v = vm + exp(-s*dt)*(v - vm);
        dist += ds;
        t++;
    }
    return first_force;
}

/*
   current speed is v0, want to cover `dist', with an ending speed of v1
   The whole process can be viewed as a accelerating->decelerating process, like this:
   
           ^     ________   <----------- climaxspeed, cannot exceed maxspeed. 
   speed   |    /        \               only if climaxspeed = maxspeed, the middle part is not zero
           |  /           \
v1         |/              \
v0         |                \
           |
           +-------------------->  time
 
   assume v1 * dist > 0, that is:
   v0->     v1->  O
   <-v0     v1->  O
   no this situation: v0-> O <-v1, since this can be decomposed into two subtasks 
   (considering obstacle avoiding)
   
   bigger maxspeed will result in SHORTER TIME, but due to inaccurate speed estimation, 
   position stability is not a good idea is to invoke this function from 
   GetDriveForce(float v0, float v1, float dist). it is more stable
*/   

float Physics::GetDriveForce(float v0, float v1, float dist, float maxspeed, int& t)
{
    const float dist_eps = 0.05;
    // well, users should PREVENT calls of this case
    if(v1 * dist < 0)
    {
        if(v1 < 0) return 100.0; // go there, then back
        else return -100.0;
    }
    
    // now, it's true that v1 * dist > 0
    bool flipped = false;
    if(dist < 0)
    {
        //trial.log.Log("Flipped.\n");
        flipped = true;
        dist = -dist;
        v1 = -v1;
        v0 = -v0;
    }
    
    // now it's safe to say that v1 > 0, dist > 0
    // do binary search on the highest speed v so that dist(v0, v) + dist(v, v1) 
    // is as close to (but less than) dist as possible
    float l, r, m, d1, d2;
    int t1, t2;
    l = 0.0f;
    r = maxspeed;
    
    // well, binary search is dangerous, equation d1 + d2 = dist may have two roots.
    // binary search may returns very very small force.
    // current solution: try to prevent calling with v1 > 1.0
    do{
        m = (l + r) * 0.5;
        GetChangeSpeedDriveForce(v0, m, 100.0, d1, t1);
        GetChangeSpeedDriveForce(m, v1, 100.0, d2, t2);
        if(d1 + d2 < dist) l = m; else r = m;
    }while(r - l > speed_precision);
    
    // simulate with climaxspeed = m. note that if 
    float force = GetChangeSpeedDriveForce(v0, m, 100.0, d1, t1);
    float dummy = GetChangeSpeedDriveForce(m, v1, 100.0, d2, t2);
    
    int mid_t = 0;
    if(d1 + d2 < dist - dist_eps && fabs(m) > speed_precision) // attension to near-zero climax speed
        mid_t = (int)((dist - d1 - d2) / m * 100.0); // there is a middle part (constant-velocity-movement)
    t = t1 + t2 + mid_t;
    
    if(flipped) force = -force;    
    return force;
}

// current speed is v0, want to go `dist', ending with a speed of v1
// a good wrapper for kernel GetDriveForce function. The function of maxspeed is proved to work nice.
float Physics::GetDriveForce(float v0, float v1, float dist, int& t) 
{
    // get maxspeed allowed. 
    // when getting closer, the maximal allowed speed is smaller. 
    // Though this takes longer time, but it is stable. And actually not much time is wasted.
    const float brake_dist = 0.45;
    if(fabs(v0) > my_maxspeed) v0 = v0 > 0 ? my_maxspeed : -my_maxspeed; 
    // this is introduced by speed estimation error.
    float maxspeed = (fabs(dist) > brake_dist ? my_maxspeed : my_maxspeed / brake_dist * fabs(dist));

    // invoke original function
    float force = GetDriveForce(v0, v1, dist, maxspeed, t);
    //trial.log.Log("GetDriveForce(%.3f, %.3f, %.3f), returns = %.3f\n", v0, v1, dist, force);
    
    // though we can discretize force, this is not neccesary, since now we often have the following sequence:
    // drive(100), drive(-100), drive(100), drive(-100), ...
    // not drive(0.1), drive(-0.1), ...
    return force;
}

salt::Vector3f Physics::GetBallStopPosition()
{
    salt::Vector3f v(0.0, 0.0, 0.111);
    salt::Vector3f vel = GetBallSpeed();
    for(int i = 0; i < 2; i++)
        v[i] = GetBallPosition()[i] + GetBallStopDistance(vel[i]);
    return v;
}

salt::Vector3f Physics::GetStopDriveForce()
{
    float s = my_s;
    float v1, ds, vm;
    float dt = 0.01 * agent.GetCycleStep();
    
    salt::Vector3f v(0.0, 0.0, 0.0);
    salt::Vector3f vel = GetMySpeed();
    
    for(int i = 0; i < 2; i++)
    {
        float v0, vm;
        v0 = vel[i];
        vm = exp(-s*dt)*v0 / (1.0-exp(-s*dt));
        v[i] = -vm / my_maxspeed * 100.0;
    }
    return v;
}