Fast bot

// 0, 1, 2, 3 - going past debri and doing Rendezvous

float mypos[12],hp[3],vb[3],debris[41][4],base[3],origine[3],debrisx[3],bordo[3],distanza[4],p1[3],p2[3],p3[3],st[3],e[12];
int i,k;
float bestPoint[3];
float preBest[3];
float myState[12];
float myPos[3];
float dest[3];
float initPos[3]; //initPos of other bot
float target[12];

float distanze(float x[], float y[]){
    float z,vbr[3];
    mathVecSubtract(vbr,x,y,3);
    z=mathVecMagnitude(vbr,3);
    return z;
    }

void init(){
    api.getOtherZRState(target);
    initPos[0] = target[0];
    initPos[1] = target[1];
    initPos[2] = target[2];

    i=0;
    k=0;
    //api.setPosGains(0.85,3,4);
    api.setPosGains(0.65,3,4);
    base[0]=0.197;
    base[1]=-0.135;
    base[2]=0.033;
    origine[0]=0;
    origine[1]=0.4;
    origine[2]=0;
    st[0]=180*PI/180;
    st[1]=0;
    st[2]=-PI/2;
    api.getMyZRState(mypos);
    game.getDebris(debris);

    debrisx[0]=debris[20][0];
    debrisx[1]=debris[20][1];
    debrisx[2]=debris[20][2];
    bordo[0]=-0.64;
    bordo[1]=debrisx[1];
    bordo[2]=0;
    distanza[0]=distanze(debrisx,bordo);


    debrisx[0]=debris[23][0];
    debrisx[1]=debris[23][1];
    debrisx[2]=debris[23][2];
    bordo[0]=0.64;
    bordo[1]=debrisx[1];
    bordo[2]=0;
    distanza[1]=distanze(debrisx,bordo);

     debrisx[0]=debris[35][0];
    debrisx[1]=debris[35][1];
    debrisx[2]=debris[35][2];
    bordo[0]=-0.64;
    bordo[1]=debrisx[1];
    bordo[2]=0;
    distanza[2]=distanze(debrisx,bordo);

     debrisx[0]=debris[37][0];
    debrisx[1]=debris[37][1];
    debrisx[2]=debris[37][2];
    bordo[0]=-0.64;
    bordo[1]=debrisx[1];
    bordo[2]=0;
    distanza[3]=distanze(debrisx,bordo);

     if(distanza[2]>distanza[3] and distanza[2]>0.22){
         p1[0]=-0.55;
         p1[1]=0.55;
         p1[2]=0;
         p2[0]=-0.55;
         p2[1]=-0.11;
         p2[2]=0;
         p3[0]=-0.26;
         p3[1]=-0.18;
         p3[2]=0;
     }

    else if(distanza[3]>distanza[2] and distanza[3]>0.22){
             p1[0]=0.55;
             p1[1]=0.55;
             p1[2]=0;
             p2[0]=0.55;
             p2[1]=-0.11;
             p2[2]=0;
             p3[0]=0.26;
             p3[1]=-0.18;
             p3[2]=0;
    }

    else{
        if(distanza[0]>distanza[1]){
             p1[0]=-0.55;
             p1[1]=0.55;
             p1[2]=0;
             p2[0]=-0.55;
             p2[1]=-0.11;
             p2[2]=0;
             p3[0]=-0.26;
             p3[1]=-0.18;
             p3[2]=0;
        }

        else{
             p1[0]=0.55;
             p1[1]=0.55;
             p1[2]=0;
             p2[0]=0.55;
             p2[1]=-0.11;
             p2[2]=0;
             p3[0]=0.26;
             p3[1]=-0.18;
             p3[2]=0;
        }
    }
    api.getMyZRState(myState);
    getBestRamPoint(bestPoint);

    preBest[0] = bestPoint[0];
    preBest[1] = myState[1] - .5f;
    preBest[2] = bestPoint[2];

}

void loop(){
    api.getMyZRState(myState);

    for(int inc = 0; inc < 3; inc++){
        myPos[inc] = myState[inc];
    }
    game.getOtherEulerState(e);
    DEBUG(("%d",i));
    game.getMyEulerState(mypos);
    hp[0]=0.2;
    hp[1]=1;
    hp[2]=0;
    e[6]=e[6];
    e[7]=-e[7];
    e[8]=-e[8];
    if(i<=3){
    game.setEulerTarget(st);
    }
    if(i==0){
        api.setPositionTarget(preBest);
        //moveTo(preBest,0);
        DEBUG(("Distance:%f",distanze(myPos,preBest)));
        if(distanze(myPos,preBest) < .25f){
            i = 1;
        }
    }
    if(i==1){
    api.setPositionTarget(bestPoint);
    //moveTo(bestPoint,1);
    DEBUG(("Distance:%f",distanze(myPos,bestPoint)));
    if(distanze(myPos,bestPoint) < .2f) {
        i = 3;
    }
    }
    else if(i==3){
        predict(15.3);
        moveTo(dest,1);
        //api.setPositionTarget(dest);

    }
     else if(i==4){
         base[0]=e[0]-0.002;
         base[2]=e[2]+0.005;
         api.setPositionTarget(base);
         e[6]=e[6];
         e[8]=e[8];
         game.setEulerTarget(e+6);
         if(distanze(mypos,base)<0.01)
         i++;
     }

     else if(i==5){
         base[0]=e[0]+0.04;
         base[1]=e[1]+0.166+0.1705;
         base[2]=e[2];
         api.setPositionTarget(base);
         game.setEulerTarget(e+6);
         if(distanze(mypos,base)<0.01){
         i++;
        }
     }

     else if(i==6){
         api.setPosGains(0.5,1,5);
         base[0]=e[0]+0.04;
         base[1]=e[1]+0.166+0.1705;
         base[2]=e[2];
         api.setPositionTarget(base);
        e[8]=e[8]-0.20;
         game.setEulerTarget(e+6);
     }

     else if(i==8){
         game.setEulerTarget(e+6);
         api.setPositionTarget(origine);
     }

    if(game.checkRendezvous()==true and i==3){
        game.completeRendezvous();
        i++;
    }
}

void getBestRamPoint(float bestpoint[3]){
    float debris[41][4];

    game.getDebris(debris);

    float min = 100000;
    float minx = -0.64f;
    float minz = -0.64f;

    for(float x = -0.3f; x <= 0.5f; x += 0.01f){
        for(float z = -0.4f; z <= 0.4f; z += 0.01f){
           float s = (x - z)*(x - z)*0.01;
           for(int i = 0; i < 41; i++){
               if(inrange(x, z, debris[i][0], debris[i][2], debris[i][3], 0.155f)){
                   s += debris[i][3];
               }
           }
           if(s < min){
               min = s;
               minx = x;
               minz = z;
           }
        }
    }

    bestpoint[0] = minx;
    bestpoint[1] = 0;
    bestpoint[2] = minz;

}
bool inrange(float x1, float z1, float x2, float z2, float r1, float r2){
    return
        (((x1 - x2) * (x1 - x2)) + ((z1 - z2) * (z1 - z2)))
        <=
        ((r1 + r2) * (r1 + r2));

}
void moveTo(float target[3], int stop) {
    /*if(distance(target,myPos) < .17f){
        api.setPositionTarget(target);
    }*/

    if(stop == 1){
    float moveTemp[3];
    mathVecSubtract(moveTemp, target, myState, 3);



    float d = mathVecMagnitude(moveTemp, 3);
    d = sqrt(sqrtf(d * d * d ));

    scale(moveTemp, d / mathVecMagnitude(moveTemp, 3));

    mathVecAdd(moveTemp, myState, moveTemp, 3);

    //DEBUG(("%f, %f, %f", moveTemp[0], moveTemp[1], moveTemp[2]));


    api.setPositionTarget(moveTemp);
    }

    else{
        float moveTemp[3];
    mathVecSubtract(moveTemp, target, myState, 3);

    float d = mathVecMagnitude(moveTemp, 3);
    d = sqrt(sqrtf(d * d ));

    scale(moveTemp, d / mathVecMagnitude(moveTemp, 3));

    mathVecAdd(moveTemp, myState, moveTemp, 3);

    //DEBUG(("%f, %f, %f", moveTemp[0], moveTemp[1], moveTemp[2]));


    api.setPositionTarget(moveTemp);
    }


}
void scale(float output[3], float scale) {
    output[0] *= scale;
    output[1] *= scale;
    output[2] *= scale;
}


//Rendezvous stuff
//Angle after certain time
float getTheta(int time){
    return (.0667f * time);
}

void predict(int time){
    //dest[0]=0.2*cosf(atanf(other[2]/other[0])+2*PI/3)+0.04;dest[1]=other[1]+0.25;dest[2]=0.2*sinf(atanf(other[2]/other[0])+2*PI/3) - .05f;
    dest[2] = (1 * initPos[2] * cosf(getTheta(time)) - initPos[0] * sinf(getTheta(time)) ) + .04f; //slight adjustment with +.05
    dest[0] = -1 * initPos[2] * sinf(getTheta(time)) + initPos[0] * cosf(getTheta(time)) + .03f;
    dest[1] = initPos[1] + .34f;
    DEBUG(("dest:%f,%f,%f",dest[0],dest[1],dest[2]));
}