mathfun.c

机器人足球,是用c语言实现的。大概有两千行

#define PI 3.1415926
union REGS regs;
float ReflectAngle(float a,float x1,float y1,float x2,float y2);
float SynthesisSpeed(float a1,float a2,float x1,float x2);
float Distance(float x0,float y0,float x1,float y1,float x2,float y2);
float SynthesisAngle(float a1,float a2,float x1,float x2);
void cursor(int x,int y);
int init(int x,int x1,int y,int y1);
int read(int *x,int *y,int *p);
void newxy(int *mx,int *my,int *mbutt);

/*计算球碰到边界后的反弹角*/
float ReflectAngle(float a,float x1,float y1,float x2,float y2)
{
    float p1,p2,result;

    if(x1==x2)
    {
        p1=PI/2;
        p2=PI + p1;
        if(fabs(p1 - a) <= PI / 2)
            result = 2 * p1 - a;
        else    result = 2 * p2 - a;
    }
    else
    {
        if(atan((y2 - y1) / (x1 - x2)) >= 0)
        {
            p1=atan((y2 - y1) / (x1 - x2));
            p2=PI + atan((y2 - y1) / (x1 - x2));
            if(a > 3 * PI /2 && a <= 2 * PI)
                p1 += 2 * PI;
            if(fabs(p1 - a) <= PI / 2)
                result = p1 + atan((tan(p1) - tan(a)) / (1 + tan(p1) * tan(a)));
            else    result = p2 + atan((tan(p2) - tan(a)) / (1 + tan(p2) * tan(a)));
        }
        else
        {
            p1=PI + atan((y2 - y1) / (x1 - x2));
            p2=2 * PI + atan((y2 - y1) / (x1 - x2));
            if(a >=0 && a < PI / 2)
                a += 2 * PI;
            if(fabs(p1-a) <= PI/2)
                result = p1 + atan((tan(p1) - tan(a)) / (1 + tan(p1) * tan(a)));
            else    result = p2 + atan((tan(p2) - tan(a)) / (1 + tan(p2) * tan(a)));
        }
    }
    if(result > 2 * PI)
        result -= 2 * PI;

    return result;
}

/*计算球与障碍物的距离*/
float Distance(float x0,float y0,float x1,float y1,float x2,float y2)
{
    float a,b,c,result;

    if(x1==x2)
    {
        a = 1;
        b = 0;
        c = -x1;
    }
    else
    {
        a = y2 - y1;
        b = x1 - x2;
        c = -x1 * y2 + x2 * y1;
    }
    result = fabs(a * x0 + b * y0 + c) / sqrt(a * a + b * b);

    return result;
}

/*计算机器人与球撞后球的速度*/
float SynthesisSpeed(float a1,float a2,float x1,float x2)
{
    float result;

    result = sqrt(a1 * a1 + a2 * a2 + 2 * a1 * a2 * cos(x1 - x2));

    return result;
}

/*计算机器人与球撞后反弹角*/
float SynthesisAngle(float a1,float a2,float x1,float x2)
{
    float re,result,p1,p2;

    re = atan((a1 * sin(x1) + a2 * sin(x2)) / (a1 * cos(x1) + a2 * cos(x2)));
    if(re < 0)
    {
        p2 = re + PI;
        p1 = p2 + PI;
    }
    else
    {
        p1 = re;
        p2 = p1 + PI;
    }
    if((a1 * cos(x1) + a2 * cos(x2)) > 0)
        result = p1;
    else if(((a1 * cos(x1) + a2 * cos(x2)) == 0) && ((a1 * sin(x1) + a2 * sin(x2)) >= 0))
        result = 3 * PI / 2;
    else if(((a1 * cos(x1) + a2 * cos(x2)) == 0) && ((a1 * sin(x1) + a2 * sin(x2)) < 0))
        result = PI / 2;
    else    result = p2;

    return result;
}

/*以下是控制鼠标的函数*/
void cursor(int x,int y)
{
    int x1,x2,y1,y2;

    x1=x-4;
    x2=x+4;
    y1=y-3;
    y2=y+3;
    line(x1,y,x2,y);
    line(x,y1,x,y2);
}

int init(int x,int x1,int y,int y1)
{
    int retcode;
    regs.x.ax=0;
    int86(51,&regs,&regs);
    retcode=regs.x.ax;
    if(retcode==0)
        return 0;
    regs.x.ax=7;
    regs.x.cx=x;
    regs.x.dx=x1;
    int86(51,&regs,&regs);
    regs.x.ax=8;
    regs.x.cx=y;
    regs.x.dx=y1;
    int86(51,&regs,&regs);

    return retcode;
}

int read(int *x,int *y,int *p)
{
    int xx0=*x,yy0=*y,buto=0;
    int xnew,ynew;
    do{
        regs.x.ax=3;
        int86(51,&regs,&regs);
        xnew=regs.x.cx;
        ynew=regs.x.dx;
        *p=regs.x.bx;
      }while(xnew==xx0&&ynew==yy0&&*p==buto);
      *x=xnew;
      *y=ynew;
      if(*p)
      {
        *x=xnew;
        *y=ynew;
        return -1;
      }
      else
      {
        *x=xnew;
        *y=ynew;
        return 1;
      }
}

void newxy(int *mx,int *my,int *mbutt)
{
    int xx0=*mx,yy0=*my,x,y,size;
    int xm,ym;
    void *buffer;

    read(&xm,&ym,mbutt);
    cursor(xx0,yy0);
    cursor(xm,ym);
    *mx=xm;
    *my=ym;
}