decision.cpp

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

void Decision::BehaveMyFreeKick()
{
    BehavePlayOn();
}

void Decision::BehaveTheirFreeKick()
{
    BehavePlayOn();
}

float Decision::GetPassAngle()
{
    int unum = GetBestPassTeammate();
    Vector3f pos;
    if(unum == 0)
        pos = Vector3f(wm.GetFieldLength()/2.0 - 2.5, 0.0, 0.0);  // towards goal
    else
        pos = wm.GetPlayerPosition(0, unum); // pass to a teammate

    Vector3f intpos = skill.GetInterceptPoint(-5.0, 5.0); // do NOT read current position
    float dx = pos[0] - intpos[0];
    float dy = pos[1] - intpos[1];
    return gRadToDeg(gArcTan2(dy, dx));
}

// return 0 if nobody is preferred
int Decision::GetBestPassTeammate()
{
    // pass to a player at least 2m ahead
    // only one exception: pass from wing to center
    int best = 0;
    float rightmost = -1000.0;
    Vector3f intpos = skill.GetInterceptPoint(-5.0, 5.0); // do NOT read current position
    Vector3f pos;
    
    switch(wm.GetTeamUnum())
    {
        // pass to 5 or directly to 6, if possible
        case 3:
            pos = wm.GetPlayerPosition(0, 5);
            if(pos[0] > intpos[0] + 2.0 && (pos-intpos).Length() < 20.0 && pos[0] > rightmost) 
            { rightmost = pos[0]; best = 5; }
            pos = wm.GetPlayerPosition(0, 6);
            if(pos[0] > intpos[0] + 2.0 && (pos-intpos).Length() < 20.0 && pos[0] > rightmost) 
            { rightmost = pos[0]; best = 6; }
            break;
        // pass to 6 or 7, 8, if possible
        case 2:
        case 4:
        case 5:
            pos = wm.GetPlayerPosition(0, 6);
            if(pos[0] > intpos[0] + 2.0 && (pos-intpos).Length() < 20.0 && pos[0] > rightmost) 
            { rightmost = pos[0]; best = 6; }
            pos = wm.GetPlayerPosition(0, 7);
            if(pos[0] > intpos[0] + 2.0 && (pos-intpos).Length() < 20.0 && pos[0] > rightmost) 
            { rightmost = pos[0]; best = 7; }
            pos = wm.GetPlayerPosition(0, 8);
            if(pos[0] > intpos[0] + 2.0 && (pos-intpos).Length() < 20.0 && pos[0] > rightmost) 
            { rightmost = pos[0]; best = 8; }
            break;
        // pass to 7~11, as long as 
        case 6:
        case 7:
        case 8:
        case 9:
        case 10:
        case 11:
            pos = wm.GetPlayerPosition(0, 7);
            if(pos[0] > intpos[0] + 2.0 && (pos-intpos).Length() < 20.0 && pos[0] > rightmost)
            { rightmost = pos[0]; best = 7; }
            pos = wm.GetPlayerPosition(0, 8);
            if(pos[0] > intpos[0] + 2.0 && (pos-intpos).Length() < 20.0 && pos[0] > rightmost) 
            { rightmost = pos[0]; best = 8; }
            pos = wm.GetPlayerPosition(0, 9);
            if(pos[0] > intpos[0] + 2.0 && (pos-intpos).Length() < 20.0 && pos[0] > rightmost) 
            { rightmost = pos[0]; best = 9; }
            pos = wm.GetPlayerPosition(0, 10);
            if(pos[0] > intpos[0] + 2.0 && (pos-intpos).Length() < 20.0 && pos[0] > rightmost) 
            { rightmost = pos[0]; best = 10; }
            pos = wm.GetPlayerPosition(0, 11);
            if(pos[0] > intpos[0] + 2.0 && (pos-intpos).Length() < 20.0 && pos[0] > rightmost) 
            { rightmost = pos[0]; best = 11; }
            break;
    }
    return best;
}

void Decision::Position()
{
    float farthest;
    Vector3f ballpos = wm.GetBallPosition();
    Vector3f target = formation.GetStrategicPosition(situation.JudgeSituation(), ballpos, wm.GetTeamUnum());  
    
    // well, only three people can actually mark! that is 2, 4 and 5
    switch(wm.GetTeamUnum())
    {
       // back defender, don't mark, just keep the goal
       case 3:
           // do NOT go too far!
           farthest = -wm.GetFieldLength()/2.0 + PENALTY_AREA_WIDTH + 5.0f; 
           if(target[0] > farthest)
           {
               target[0] = farthest;
               skill.Goto(target);
           }
           else
           {
               Vector3f ballpos = wm.GetBallPosition();   
               Vector3f ourgoal(-wm.GetFieldLength()/2.0, 0.0, 0.0);
               Vector3f driveline = ourgoal - ballpos;
               driveline.Normalize();
    
               // in front and very close
               Vector3f gotopos = ballpos + driveline * 0.44f;
               skill.Goto(gotopos);
           }
           break;
           // wing defender, mark
       case 2:
       case 4:
           farthest = -wm.GetFieldLength()/2.0 + PENALTY_AREA_WIDTH + 10.0f; 
           if(target[0] > farthest) target[0] = farthest;
           if(ballpos[0] > 0.0)
               skill.Goto(target);
           else
               Mark();
           break;
       case 5:
           if(ballpos[0] > -10.0) // orignially 0.0
               skill.Goto(target);
           else
               Mark();
           break;
       case 6:
       case 7:
       case 8:
       case 9:
       case 10:
       case 11:
           skill.Goto(target);
           break;
    }
}

void Decision::Mark()
{
    int unum = wm.GetTeamUnum();
    if(unum != 2 && unum != 4 && unum != 5) return;
    
    // TODO: best match
    int i,j;
    
    // calculate attackness
    float attackness[12];
    for(i = 1; i <= 11; i++)
        attackness[i] = situation.Attackness(wm.GetPlayerPosition(1, i));
    
    // select top 3
    int rank[3];
    for(i = 0; i < 3; i++)
    {
        rank[i] = 1;
        for(j = 1; j <= 11; j++)
            if(attackness[j] > attackness[rank[i]])
                rank[i] = j;
        attackness[rank[i]] = -attackness[rank[i]]; // mark this entry as deleted
    }
    
    // only consider top three vs our 2, 4, 5
    int task[12];
    Vector3f p2 = wm.GetPlayerPosition(0, 2);
    Vector3f p4 = wm.GetPlayerPosition(0, 4);
    Vector3f p5 = wm.GetPlayerPosition(0, 5);
    
    // enumerate 6 possible matches
    float min = 1000.0;
    int t;
    for(i = 0; i < 6; i++)
    {
        switch(i)
        {
            case 0:
                task[2] = rank[0]; task[4] = rank[1]; task[5] = rank[2];
                break;
            case 1:
                task[2] = rank[0]; task[4] = rank[2]; task[5] = rank[1];
                break;
            case 2:
                task[2] = rank[1]; task[4] = rank[0]; task[5] = rank[2];
                break;
            case 3:
                task[2] = rank[1]; task[4] = rank[2]; task[5] = rank[0];
                break;
            case 4:
                task[2] = rank[2]; task[4] = rank[0]; task[5] = rank[1];
                break;
            case 5:
                task[2] = rank[2]; task[4] = rank[1]; task[5] = rank[0];
                break;
        }
        float d2 = (p2 - wm.GetPlayerPosition(1, task[2])).Length();
        float d4 = (p4 - wm.GetPlayerPosition(1, task[4])).Length();
        float d5 = (p5 - wm.GetPlayerPosition(1, task[5])).Length();
        float tot = d2+d4+d5;
        if(tot < min)
        {
            t = task[unum];
            min = tot;
        }
    }
    
    // now, i know that I should mark t
    Vector3f ourgoal(-wm.GetFieldLength()/2.0, 0.0, 0.0);
    Vector3f epos = wm.GetPlayerPosition(1, t);
    Vector3f driveline = ourgoal - epos;
    driveline.Normalize();
    
    // in front and very close
    Vector3f markpos = epos + driveline * 0.44f;
    if(markpos[0] < -wm.GetFieldLength()/2.0 + 20.0f)
        skill.Goto(markpos);
    else
        skill.Goto(ourgoal - driveline * 20.0f); // newly added
}

// find the biggest gap and goes through it
float Decision::GetDribblePriority(float& angle)
{
    // at least 0.2, since if passing and shooting are all impossible, i must dribble
    int i;
    Vector3f mypos = wm.GetMyPosition();
    Vector3f ballpos = wm.GetBallPosition();
    float angles[12];
    int oppcount = 0;
    for(i = 1; i <= 11; i++)
    {
        Vector3f pos = wm.GetPlayerPosition(1, i);
        if((pos-ballpos).Length() > 20.0) continue; // too far, ignore him.

        float dx = pos[0] - mypos[0];
        float dy = pos[1] - mypos[1];
        float ang = gArcTan2(dy, dx);
        if(ang > -90.0 && ang < 90.0) angles[oppcount++] = ang; // must dribble forward
    }
    
    if(oppcount == 0)
        angle = 0.0;
    else
    {
        std::sort(angles, angles+oppcount);
        int max_gap_start = 0;
        float max_gap_len = angles[1] - angles[0];
        for(i = 1; i < oppcount; i++)
        {
            float gap = angles[i+1] - angles[i];
            if(gap > max_gap_len)
            {
                max_gap_len = gap;
                max_gap_start = i;
            }
        }
        angle = (angles[max_gap_start] + angles[max_gap_start+1]) / 2.0;
    }
    
    // well, well, i decided to directly dribble to goal, since...
    float dx = wm.GetFieldLength()/2.0 - mypos[0];
    float dy = -mypos[1];
    if(mypos[1] > 3.0) dy += -1.2;
    else if(mypos[1] < -3.0) dy -= 1.2; 
    
    angle = gRadToDeg(gArcTan2(dy, dx));
    
    return 0.2f; // now fixed
}

float Decision::GetPassPriority(float& dist)
{
    // if no teammate in front, returns 0.0
    // consider every 0.5m, from 0 to 25m
    int i;
    float min = 1000.0;
    Vector3f mypos = wm.GetMyPosition();
    Vector3f ballpos = wm.GetBallPosition();
    
    Vector3f driveline = ballpos - mypos;
    driveline.Normalize();
    
    int maxdist = 50;
    if(mypos[0] > ballpos[0] - 0.2) return 0.0; // must pass forward
    if(mypos[0] > ballpos[0])
    {
        if(situation.InPenaltyArea(0, ballpos)) return 0.0; // no back-pass in my penalty area
        maxdist = 6; // restricted backpass

        // FORBIDDEN BACKPASS!
        return 0.0;
    }
    for(i = 0; i <= maxdist; i++)
    {
        float d = 0.5 * (float)i;
        Vector3f target = ballpos+driveline * d;
        if(!situation.InField(target)) continue;
        if(target[0] > wm.GetFieldLength()/2.0 - PENALTY_AREA_WIDTH && 
            fabs(target[1]) > PENALTY_AREA_LENGTH/2.0) continue; // well, i don't like that area
        if(target[0] > wm.GetFieldLength()/2.0 - 1.0) continue; // no angle :(
        if(situation.InPenaltyArea(0, target)) continue;

        Vector3f centerpos(wm.GetFieldLength()/2.0, 0.0, 0.0);
        float dd = (centerpos-target).Length();    // TODO :: attackness

        // TODO: near enemy's penalty, only pass into the penalty is considered
        float m1 = situation.GetTeamClosestDistanceExcept(0, wm.GetTeamUnum(), target);
        float m2 = situation.GetTeamClosestDistance(1, target);
        float w = m1 - m2;
        if(w > -1.0f) continue;

        // pass to a position very close to ball
        if(dd < min)
        {
            min = dd;
            dist = d;
        }
    }
    
    // is it really a pass?
    if(min > 999.0 || dist < 1.0)
        return 0.0f;
    else
    {
        Vector3f target = ballpos+driveline * dist;
        log.Log("(GetPassPriority) mypos = (%.3f, %.3f), ballpos = (%.3f, %.3f)", mypos[0], mypos[1],
        ballpos[0], ballpos[1]);
        log.Log("(GetPassPriority) dist = %.3f, Target = (%.3f, %.3f\n)", dist, target[0], target[1]);
        float m1 = situation.GetTeamClosestDistance(0, target);
        float m2 = situation.GetTeamClosestDistance(1, target);
        float w = m1 - m2;
        log.Log("(GetPassPiorirty) m1 = %.3f, m2 = %.3f, w = %.3f", m1, m2, w);
        return 1.0f;
    }
}

float Decision::GetShootPriority()
{
    Vector3f mypos = wm.GetMyPosition();
    Vector3f ballpos = wm.GetBallPosition();
    Vector3f centerpos(wm.GetFieldLength()/2.0, 0.0, 0.0);
    
    // too far away
    if((ballpos - centerpos).Length() > 18.0) return 0.0;

    // midfield, good chance    
    if(fabs(ballpos[1]) < 7.5)
    {
        return 1.0;
    }
    else
    {
        if(ballpos[1] > 0.0)
        {
            if(mypos[1] > ballpos[1] + 0.15) return 1.0;
            else return 0.0;
        }
        else
        {
            if(mypos[1] < ballpos[1] - 0.15) return 1.0;
            else return 0.0;
        }
    }
}