//Begin page main //Begin page main float othersph[13],othQuat[4],quatZ[4],QuatF[4],quatX[4],QuatI[4]; float otherVel[3],otherPos[3],otherAtt[3],refVec[3]; float myState[3],myPos[3],myVel[3]; float PredRed[3],Inpos[3],Inpos2[3],TugPos[3],save[3]; float dot,Origin[3],DistFromRef,angOrigin,otherRef[3],Ypos[3],DistFromOrigin; int Z,counter,l; #define dist_topoint 0.45 float Champ1[3],Champ[3]; void init(){ quatZ[0]=0; quatZ[1]=0; quatZ[2]=1; quatZ[3]=0; quatX[0]=0.13917310096f; quatX[1]=0; quatX[2]=0; quatX[3]=0.99026806874f; refVec[0]=1; refVec[1]=0; refVec[2]=0; Ypos[0] = 0; Ypos[1] = 1; Ypos[2] = 0; counter=0; Z=0; l=1; Champ1[0]=-0.02; Champ1[1]=-0.168; Champ1[2]=0.02; } void loop(){ //simplified quaternion function(for rotation obviously) api.getOtherSphState(othersph); for(int i=0;i<4;i++){ othQuat[i]=othersph[i+6]; } quatMult(QuatF, othQuat, quatZ); quatMult(QuatI, QuatF, quatX); //generic red and blue info api.getMyZRState(myState); for(int o=0;o<3;o++){ otherPos[o]=othersph[o]; otherVel[o]=othersph[o+3]; myPos[o]=myState[o]; myVel[o]=myState[o+3]; } api.quat2AttVec(refVec, othQuat, otherAtt ); //Calculations for the predicted red position float VecBetRelRed[3]; mathVecSubtract(VecBetRelRed,otherPos,otherRef,3); mathVecNormalize(VecBetRelRed,3); //things I need for Inpos Calc Origin[0] = 0; Origin[1] = -(DistFromRef / tanf(angOrigin)) + otherPos[1]; Origin[2] = 0; DistFromRef = distFun(otherRef, otherPos); otherRef[0] = 0; otherRef[1] = otherPos[1]; otherRef[2] = 0; dot = mathVecInner(Ypos, otherAtt, 3); angOrigin = acosf(dot); DistFromOrigin = distFun(otherPos, Origin); DEBUG(("ORIGIN Y%f angorigin %f otherAtt x%f y%f z%f",Origin[1],angOrigin*180/PI,otherAtt[0],otherAtt[1],otherAtt[2])); //X'=Xcosa-Zsina //Z'=Xsina+Zcosa //a=5 gradi PredRed[0] = 0.9986f*0.18*VecBetRelRed[0]-0.052f*0.18*VecBetRelRed[2]; PredRed[1] =-0.17f; PredRed[2] = 0.052f*0.18*VecBetRelRed[0]+0.9986f*0.18*VecBetRelRed[2]; /* PredRed[0] = 0.18*VecBetRelRed[0]; PredRed[1] =-0.165f; PredRed[2] = 0.18*VecBetRelRed[2]; if( otherPos[0]>0 ) Inpos2[0]=sqrt(0.04/(1+(otherPos[2]/otherPos[0])*(otherPos[2]/otherPos[0]))); else Inpos2[0]=-sqrt(0.04/(1+(otherPos[2]/otherPos[0])*(otherPos[2]/otherPos[0]))); if( otherPos[2]>0 ) Inpos2[2]=sqrt(0.04-(0.04/(1+(otherPos[2]/otherPos[0])*(otherPos[2]/otherPos[0])))); else Inpos2[2]=-sqrt(0.04-(0.04/(1+(otherPos[2]/otherPos[0])*(otherPos[2]/otherPos[0])))); Inpos2[1]=-0.18;//0.16 // (DistFromRef+0.033) DEBUG(("RAGGIO=%f",DistFromRef+0.04)); Inpos2[0] = 0.17*VecBetRelRed[0]; Inpos2[1] =-0.15f; Inpos2[2] = 0.17*VecBetRelRed[2]; Inpos2[0] = PredRed[0] * (DistFromOrigin + 0.34f) / DistFromOrigin; Inpos2[1] = ((PredRed[1] - Origin[1]) * (DistFromOrigin + 0.34f) / DistFromOrigin) + Origin[1]; Inpos2[2] = PredRed[2] * (DistFromOrigin + 0.34f) / DistFromOrigin; */ Inpos[0] = otherPos[0] * (DistFromOrigin + 0.34f) / DistFromOrigin; Inpos[1] = ((otherPos[1] - Origin[1]) * (DistFromOrigin + 0.34f) / DistFromOrigin) + Origin[1]; Inpos[2] = otherPos[2] * (DistFromOrigin + 0.34f) / DistFromOrigin; TugPos[0] = otherPos[0] * (DistFromOrigin + 1.2f) / DistFromOrigin; TugPos[1] = ((otherPos[1] - Origin[1]) * (DistFromOrigin + 1.2f) / DistFromOrigin) + Origin[1]; TugPos[2] = otherPos[2] * (DistFromOrigin + 1.2f) / DistFromOrigin; save[0] = -TugPos[0] * 5; save[1] = 1; save[2] = -TugPos[2] * 4; Champ[0]=0; Champ[1]=otherPos[1]+0.339; Champ[2]=0; // Movement sequence if(l==2 && counter>1 && game.getGamePhase() == 3){ if(distFun(myPos,PredRed)>0.07 && Z==0){ topoint(PredRed); DEBUG(("PRED X%f Y%f Z%f",PredRed[0],PredRed[1],PredRed[2])); } else if(distFun(myPos,PredRed)<0.07 && Z==0){Z++;} if(Z==1 && game.getGamePhase() == 3){ moveInpos(Inpos); DEBUG(("INP X%f Y%f Z%f",Inpos[0],Inpos[1],Inpos[2])); } /*if(distFun(myPos, Inpos2)>0.1) topoint(Inpos2); else moveInpos(Inpos);*/ if(distFun(myPos,Inpos)>0.025) api.setQuatTarget(QuatI); else{ DEBUG(("BACK IN RIGHT QUAT")); api.setQuatTarget(QuatF); } } if(l==1 && counter>1 && game.getGamePhase() == 3){ float QuatXY[4]; float QuatX[4]; float QuatY[4]; QuatX[0]=0.70710678118; QuatX[1]=0; QuatX[2]=0; QuatX[3]=-0.70710678118f; QuatY[0]=0; QuatY[1]=0.70710678118; QuatY[2]=0; QuatY[3]=0.70710678118; quatMult(QuatXY, QuatX, QuatY); api.setQuatTarget(QuatXY); if(counter<33 ) api.setPositionTarget(Champ1); else moveInpos(Champ); } if (otherPos[1] < -0.23f && game.getGamePhase() == 4) { DEBUG(("TugPos x%f y%f z%f", TugPos[0], TugPos[1], TugPos[2])); moveCapVelocity(TugPos); api.setQuatTarget(QuatF); } else if (otherPos[1] > -0.23f) { moveCapVelocity(save); DEBUG(("AAAAAAAH SCAPPAAAA")); } counter++; if(counter<3){ if(mathVecMagnitude(otherVel,3)<0.0014){ l=1; DEBUG(("CHAM")); } else{ l=2; DEBUG(("NORM")); } } } float distFun(float* a, float* b) { float sub[3]; mathVecSubtract(sub, a, b, 3); return mathVecMagnitude(sub, 3); } void multVec(float* vec, float mult, bool pl) { if (pl == 1) { mathVecNormalize(vec, 3); } vec[0] *= mult; vec[1] *= mult; vec[2] *= mult; } void moveCapVelocity(float* whereTo) { float v[3]; mathVecSubtract(v, whereTo, myPos, 3); multVec(v, 0.3f, 1); api.setVelocityTarget(v); } void topoint(float point[3]) { float vecbet[3]; mathVecSubtract(vecbet, point, myPos, 3); multVec(vecbet,0.07f,1); if (distFun(myPos, point) > 0.4) { api.setVelocityTarget(vecbet); DEBUG(("VADO VELOCE")); } else if (distFun(myPos, point) > 0.1){ api.setPositionTarget(point); DEBUG(("MI METTO IN POS")); } else{ float V0[3]; V0[0]=0; V0[1]=0; V0[2]=0; api.setVelocityTarget(V0); } } void moveInpos(float *whereTo){ if (distFun(myPos, whereTo)>0.3f ){ api.setPositionTarget(whereTo); DEBUG(("VADO IN POS")); } else{ float v2[3]; mathVecSubtract(v2, whereTo, myPos, 3); multVec(v2, 0.005f, 1); api.setVelocityTarget(v2); DEBUG(("STRA LENTO")); } } //End page main //End page main