gfc.cpp

机器人足球的一个自学的源代码

double robotAngel(OpponentRobot *robot,int rID){
        //
        double alpha;

        alpha = robot->rotation;
        if(alpha > 0)
                alpha = alpha;
        else if(alpha < 0)
                alpha = 360.0 + alpha;
        return alpha;
}

void vDirection(){
        //
        int lt = 0;

        for(lt = 0; lt < 5; lt++){
                if(HOMEEV[lt] == 0)
                        HOMEVDIRECTION[lt] = -1;
                else
                        HOMEVDIRECTION[lt] = lineAngel(
                                HOMETRACE[lt][0][0],HOMETRACE[lt][0][1],
                                HOMETRACE[lt][1][0],HOMETRACE[lt][1][1]
                        );
        }
        for(lt = 0; lt < 5; lt++){
                if(ENEMYEV[lt] == 0)
                        ENEMYVDIRECTION[lt] = -1;
                else
                        ENEMYVDIRECTION[lt] = lineAngel(
                                ENEMYTRACE[lt][0][0],ENEMYTRACE[lt][0][1],
                                ENEMYTRACE[lt][1][0],ENEMYTRACE[lt][1][1]
                        );
        }
        if(BALLEV[0] == 0)
                        BALLVDIRECTION[0] = -1;
        else
                BALLVDIRECTION[0] = lineAngel(
                        BALLTRACE[0][0][0],BALLTRACE[0][0][1],
                        BALLTRACE[0][1][0],BALLTRACE[0][1][1]
                );
}

double f(double k, double x0, double y0, double x){
        //
        return k*x - k*x0 + y0;
}

double fn(double k,double x0,double y0,double y){
        //
        return x0 + (y - y0)/k;
}

double l(double x1, double y1, double x2, double y2){
        //
        double dx,dy;
        
        dx = x1 - x2;
        dy = y1 - y2;
        return sqrt(dx*dx + dy*dy);
}

void predictBall(double xs){
        //xs 秒后球的位置
        double dx, dy;

        dx = BALLTRACE[5][1][0] - BALLTRACE[5][0][0];
        dy = BALLTRACE[5][1][1] - BALLTRACE[5][0][1];
        PBP[0] = ENV->currentBall.pos.x + dx*xs*30;
        PBP[1] = ENV->currentBall.pos.y + dy*xs*30;
}

void rotation(){
        //
        int lt = 0;

        if(HOMEROTATION[0][0] == -1){
                for(lt= 0; lt <5; lt++){
                        HOMEROTATION[lt][0] = robotAngel(&ENV->home[lt],lt);
                        HOMEROTATION[lt][1] = robotAngel(&ENV->home[lt],lt);
                }
                for(lt= 0; lt <5; lt++){
                        ENEMYROTATION[lt][0] = robotAngel(&ENV->home[lt],lt);
                        ENEMYROTATION[lt][1] = robotAngel(&ENV->home[lt],lt);
                }
        }
        else{
                for(lt = 0; lt< 5; lt++){
                HOMEROTATION[lt][0] = HOMEROTATION[lt][1];
                HOMEROTATION[lt][1] = robotAngel(&ENV->home[lt],lt);
                ENEMYROTATION[lt][0] = ENEMYROTATION[lt][1];
                ENEMYROTATION[lt][1] = robotAngel(&ENV->home[lt],lt);
                }
        }
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%f\n",HOMEROTATION[3][1] - HOMEROTATION[3][0]);
        //fclose(DEBUGFILE);
}

bool isStill(Robot *robot,int rID){
        //
        if(HOMEEV[rID] < 1 && fabs(HOMEROTATION[rID][1] - HOMEROTATION[rID][0])<1)
                return true;
        else
                return false;
}

int direction(double ra,double la){
        //
        int dire;
        double beta;

        beta = fabs(ra - la); //计算夹角
        if(ra - la >0 && beta < 180)
                dire = 1;
        else if(ra - la >0 && beta > 180)
                dire = -1;
        else if(ra - la <0 && beta < 180)
                dire = -1; 
        else if(ra - la <0 && beta > 180)
                dire = 1;
        return dire;
}

bool canKShoot(Robot *robot, int rID){
        //是否可以用头撞球进 can kick shoot
        double rx, ry, bx, by; //ball x,y and robot x,y
        double k, y; //y 是直线与 x=6.8118,93.4259的交点
        double length;

        rx = robot->pos.x;
        ry = robot->pos.y;
        bx = ENV->currentBall.pos.x;
        by = ENV->currentBall.pos.y;
        length = sqrt((rx - bx)*(rx - bx) + (ry - by)*(ry - by));
        if(fabs(bx - rx) < 0.1)
                return false;
        k = (by - ry)/(bx - rx);
        if(WHO ==1){
                if(length > 3 && rx - bx > 2.517){ //length 的作用 ？
                        y = f(k, bx, by, 6.8118);
                        if(y > 33.9 && y < 49.6)
                                return true;
                        else
                                return false;
                }
                else
                        return false;
        }
        else {
                if(length > 3 && bx - rx > 2.517){ //length 的作用 ？
                        y = f(k, bx, by, 93.4259);
                        if(y > 33.9 && y < 49.6)
                                return true;
                        else
                                return false;
                }
                else
                        return false;
        }
}

bool canTShoot(Robot *robot,int rID){
        //
        double rx,ry,bx,by;
        double dx,dy;
        double X; //界限
        double cy; //与门线交点的 y 坐标
        double  k; //斜率
        int dire; //旋转方向

        cy = 0;
        k = 0;
        rx = robot->pos.x;
        ry = robot->pos.y;
        bx = ENV->currentBall.pos.x;
        by = ENV->currentBall.pos.y;
        dx = rx - bx;
        dy = ry - by;
        if(WHO == 1){
                X = 27.81;
                if(rx < X){
                        if(dx>0 && dx<(BALLD/2+sqrt(2)*ROBOTWIDTH/2) && fabs(dy)<ROBOTWIDTH/2){ //球在左边
                                if(ry > (GTOPY + GBOTY)/2){
                                        k = sqrt(3)/3;
                                        dire = -1;
                                }
                                else{
                                        k = -1*sqrt(3)/3;
                                        dire = 1;
                                }
                                cy = f(k,bx,by,GLEFT);
                        }
                        else if(dy<0 && dy>-1*(BALLD/2+sqrt(2)*ROBOTWIDTH/2) && fabs(dx)<ROBOTWIDTH/2){ //球在上边
                                k = -1*sqrt(3)/3;
                                dire = -1;
                                cy = f(k,bx,by,GLEFT);
                        }
                        else if(dy>0 && dy<(BALLD/2+sqrt(2)*ROBOTWIDTH/2) && fabs(dx)<ROBOTWIDTH/2){ //球在下边
                                k = sqrt(3)/3;
                                dire = 1;
                                cy = f(k,bx,by,GLEFT);
                        }
                        turn(robot,rID,dire);
                        if(cy > GBOTY && cy < GTOPY)
                                return true;
                        else
                                return false;
                }
                else
                        return false;
        }
        else{
                X = 70.12;
                if(rx > X){
                        if(dx<0 && dx>-1*(BALLD/2+sqrt(2)*ROBOTWIDTH/2) && fabs(dy)<ROBOTWIDTH/2){ //球在右边
                                if(ry > (GTOPY + GBOTY)/2){
                                        dire = 1;
                                        k = -1*sqrt(3)/3;
                                }
                                else{
                                        k = sqrt(3)/3;
                                        dire = -1;
                                }
                                cy = f(k,bx,by,GRIGHT);
                        }
                        else if(dy<0 && dy>-1*(BALLD/2+sqrt(2)*ROBOTWIDTH/2) && fabs(dx)<ROBOTWIDTH/2){ //球在上边
                                k = sqrt(3)/3;
                                dire = 1;
                                cy = f(k,bx,by,GRIGHT);
                        }
                        else if(dy>0 && dy<(BALLD/2+sqrt(2)*ROBOTWIDTH/2) && fabs(dx)<ROBOTWIDTH/2){ //球在下边
                                k = -1*sqrt(3)/3;
                                dire = -1;
                                cy = f(k,bx,by,GRIGHT);
                        }
                        turn(robot,rID,dire);
                        if(cy > GBOTY && cy < GTOPY)
                                return true;
                        else
                                return false;
                }
                else
                        return false;
        }
}

void turn(Robot *robot,int rID,int dire){
        //
        if(dire ==1)
                run(robot,rID,125,-125);
        else if(dire == -1)
                run(robot,rID,-125,125);
        else
                run(robot,rID,0,0); //error
}

void goBackTo(Robot *robot, int rID, double x,double y){
        //
        double rx,ry;
        double alpha,ra,beta; //直线、机器人角度,夹角
        int dire; //1 正 -1逆 时针
        int vl,vr;
        double length;

        rx = robot->pos.x;
        ry = robot->pos.y;
        alpha = lineAngel(rx,ry,x,y);
        ra = robotAngel(robot, rID);
        ra +=180.0;
        if(ra >= 360.0)
                ra -=360.0;
        beta = fabs(ra - alpha); //计算夹角
        dire = direction(ra, alpha);
        if(beta > 180)
                beta = 360 - beta;
        else 
                beta = beta;
        if(dire == 1){
                vr = -1*125;
                vl = int(-125 + beta*25/18); //假定在 3 个周期内完成转动
                if(vl >= 0)
                        vl = -1;
                if(beta < 1)
                        vl = -125;
        }
        if(dire == -1){
                vl = -1*125;
                vr = int(-125 + beta*25/18); //假定在 3 个周期内完成转动
                if(vr >= 0)
                        vr = -1;
                if(beta < 1)
                        vr = -125;
        }
        run(robot, rID,vl,vr);
        length = l(x,y,rx,ry);
        if(beta > 90.0)
                beta = 180.0 - beta;
        if(length < 3 && beta > 30.0)
                sRotate(robot,rID,alpha);
        beta = fabs(HOMEVDIRECTION[rID] - ra);
        if(HOMEVDIRECTION[rID] != -1)
                if((beta>180.0?360-beta:beta) > 90.0)
                        run(robot,rID,0,0);
        //DEBUGFILE = fopen("debug.txt", "a");
        //fprintf(DEBUGFILE, "%d %d %d %f %d\n",rID,vl,vr,beta,HOMEEV[rID]);
        //fclose(DEBUGFILE);
}

void goTo(Robot *robot, int rID, double x, double y){
        //
        double rx,ry;
        double alpha,ra,beta; //直线、机器人角度,夹角
        int dire; //1 正 -1逆 时针
        int vl,vr;
        double length;

        rx = robot->pos.x;
        ry = robot->pos.y;
        alpha = lineAngel(rx,ry,x,y);
        ra = robotAngel(robot, rID);
        beta = fabs(ra - alpha); //计算夹角
        dire = direction(ra,alpha);
        if(beta > 180)
                beta = 360 - beta;
        else 
                beta = beta;
        if(dire == 1){
                vl = 125;
                vr = int(125 - beta*25/18); //假定在 3 个周期内完成转动
                if(vr <= 0)
                        vr = 1;
                if(beta < 1)
                        vr = 125;
        }
        else if(dire == -1){
                vr = 125;
                vl = int(125 - beta*25/18); //假定在 3 个周期内完成转动