Esempio n. 1
0
void Movimiento(){
    
    float termino1 = 0;
    float termino2 = 0;
    termino1 = mathSquare(vector[1]/vector[0]);
    termino2 = mathSquare(vector[2]/vector[0]);
    float A = 1+termino1+termino2;
    
    float B = ((2/vector[0])*(vector[1]*(posicionInicial[1]-(vector[1]*posicionInicial[0]/vector[0]))
              +vector[2]*(posicionInicial[2]-(vector[2]*posicionInicial[0]/vector[0]))));
              
    float termino3 = 0;
    float termino4 = 0;
    termino3 = mathSquare(posicionInicial[1]-(vector[1]*posicionInicial[0]/vector[0]));
    termino4 = mathSquare(posicionInicial[2]-(vector[2]*posicionInicial[0]/vector[0]));
    float C = termino3+termino4 - 0.22*0.22;
    
    DEBUG(("A es: %f , B es: %f , C es: %f \n", A, B, C));
    
    if((B*B)<(4*A*C)){
        MovimientoRecto();
        DEBUG(("Movimiento recto \n"));
    }
    else {
        MovimientoEsferico();
        DEBUG(("Movimiento esferico \n"));
    }
    return;
}
Esempio n. 2
0
	void goFast(float target[3], float speed) {
		if(distance(self,target) > mathSquare(speed)*55.0f) {
			drift(target,speed);
		}else
		{
			api.setPositionTarget(target);
		}
	}
//User01: stuynaught Team: Stuy-Naught Project: squares
void ZRUser01(float *myState, float *otherState, float time)
{
float mypos[3];
int i = 0;
float dist;
for (i = 0; i < 3; i++) {
    mypos[i] = myState[i];}
dist = mathSquare(mathVecMagnitude(mypos, 3)) + mathSquare(mathVecMagnitude(pos, 3)) - 2 * mathVecInner(mypos, pos, 3);
dist = sqrt(dist);
if (dist < .05) {
if (state % 4 == 0) {
    pos[0] = .5;}
else if (state % 4 == 1) {
    pos[1] = .5;}
else if (state % 4 == 2) {
    pos[0] = -.5;}
else {
    pos[1] = -.5;}
state++;}
ZRSetPositionTarget(pos);
}
static void CoastToTarget(float* myPos, float* coastTarget, float magnitude)
{
 float temp[3];
 if (magnitude > 0.04) magnitude = 0.04;
 Vfunc(2, (coastTarget), (myPos), (temp), 0);
 if (mathVecMagnitude((temp), 3) < (mathSquare(mathVecMagnitude((&myPos[3]), 3)) * 50.0 + 0.08))
 {
  ZRSetPositionTarget(coastTarget);
 }
 else
 {
  mathVecNormalize((temp), 3);
  Vfunc(4, (temp), NULL, (temp), (magnitude));
  if (Vfunc(6, (temp), (&myPos[3]), NULL, 0) > 0.02)
   ZRSetVelocityTarget(temp);
 }
}
Esempio n. 5
0
void ZRUser(float* myState, float* otherState, float time)
{
 float target[3];
 float station[4];

 float baseAngle;
 float angleDiff;
 float baseRadius;

 float to_opponent[3];
 float sun[3] = {0,0,0};

 float tolerance=.02;

 float a1, a2;
 float s1 = 0;
 float s2 = 0; //s1 = score 1 second ago. s2 = current score

 DEBUG(("time: %4.0f, state: %d\n", time, state));
 switch (state)
 {
  case 0:
//Code to initialize the sphere, then search for the panel...

state = 1;
   break;
  case 1:
//Code to move towards panel initialization circle...



if (fabs(myState[0] - (getPanelSide() * 0.7)) < 0.1)
 state = 2;

baseAngle = atan2f(myState[2], myState[1]);

target_pos[0] = (getPanelSide() * 0.7);
target_pos[1] = cosf(baseAngle) * 0.7;
target_pos[2] = sinf(baseAngle) * 0.7;

ZRSetPositionTarget(target_pos);

Vfunc(9, (sun), (myState), (target_att), 0);

ZRSetAttitudeTarget(target_att);


//Code to find tangent lines...

baseAngle = atan2f(-target_pos[2], -target_pos[1]);
angleDiff = asinf(0.5 / sqrtf(mathSquare(target_pos[1]) + mathSquare(target_pos[2])));

tangentPoints[0] = baseAngle - angleDiff;
tangentPoints[1] = baseAngle + angleDiff;



tangentPoints[2] = tangentPoints[0];
   break;
  case 2:
if (getPercentChargeRemaining() >= 95) {
    state = 3;
    break;
} else {
    // calc vector from current position back toward sun
    Vfunc(9, (sun), (myState), (target_att), 0);
    ZRSetPositionTarget(target_pos);
    ZRSetAttitudeTarget(target_att);
}
   break;
  case 3:
//Code to wait for the opponent to get into their panel's plane



Vfunc(9, (myState), (otherState), (to_opponent), 0);



if(fabs(otherState[0] - (getPanelSide() * -0.7) > .10)){
 state = 5;
 break;
}
else {
 ZRSetAttitudeTarget(to_opponent);
 if(fabs(otherState[0] - (getPanelSide() * -0.7)) < .005){
  state = 4;
  break;
 }
}
   break;
 case 4:
ZRSetPositionTarget(target_pos);
Vfunc(9, (myState), (otherState), (to_opponent), 0);
if (Vfunc(8, (to_opponent), (myState+6), NULL, 0) < 5 && getPercentChargeRemaining() > 0 && fabs(otherState[0]) > .68 && fabs(otherState[0]) < .81) {
    DEBUG(("time: %4.0f, (BLUE): ZAPPING ++++++++++++++++++++\n",time));
    ZRRepel();
}
else {
    if (getPercentChargeRemaining() < 1) {
        state = 5;
        break;
    }
    Vfunc(9, (myState), (otherState), (to_opponent), 0);
    DEBUG(("time: %4.0f, (BLUE): angle to opponent: %f\n",time,Vfunc(8, (to_opponent), (myState+3), NULL, 0)));
    ZRSetAttitudeTarget(to_opponent);
}
   break;
  case 5:
//Code to search for panel...

//Check if panel was found.

if (isPanelFound())
{
 getPanelState(panelState);


 panelState[4] = panelState[0];

 baseAngle = atan2f(panelState[2], panelState[1]);
 baseRadius = sqrtf(mathSquare(panelState[1]) + mathSquare(panelState[2])) - 0.03;

 panelState[5] = cosf(baseAngle) * baseRadius;
 panelState[6] = sinf(baseAngle) * baseRadius;

 state = 6;
 break;
}


//Code to find tangent lines...

baseAngle = atan2f(-myState[2], -myState[1]);
angleDiff = asinf(0.5 / sqrtf(mathSquare(myState[1]) + mathSquare(myState[2])));

tangentPoints[0] = baseAngle - angleDiff;
tangentPoints[1] = baseAngle + angleDiff;


//Point in the direction of tangentPoints[2]

target_att[0] = 0;
target_att[1] = cosf(tangentPoints[2]);
target_att[2] = sinf(tangentPoints[2]);

ZRSetAttitudeTarget(target_att);

if(fabs(otherState[0] - (getPanelSide() * -0.7) < .05 && getPercentChargeRemaining() > 0)){
 state = 3;
 break;
}

tangentPoints[2] += scanTarget * 0.1;

if ((scanTarget == 1) && (tangentPoints[2] >= tangentPoints[1]))
 scanTarget = -1;
else if ((scanTarget == -1) && (tangentPoints[2] <= tangentPoints[0]))
 scanTarget = 1;

target_pos[0] = getPanelSide() * 0.7;
target_pos[1] = myState[1];
target_pos[2] = myState[2];

ZRSetPositionTarget(target_pos);
   break;
  case 6:
//Code to move to panel...

if (iHavePanel())
{
 state = 7;
 scanTarget = 0;
 break;
}

ZRSetPositionTarget(&panelState[4]);

Vfunc(7, (panelState), NULL, (target_pos), 0);
target_pos[0] = 0;
Vfunc(3, (target_pos), NULL, (target_pos), 0);

ZRSetAttitudeTarget(target_pos);
   break;
  case 7:
//Code to beeline towards the station...

if (s2 - s1 == 0 && s2 > 0){
state = 8;
break;
}
else {
s1 = s2;
s2 = getOtherCurrentScore();
getStationState(station);

Vfunc(7, (station), NULL, (target_att), 0);

target_att[1] += cosf(station[3]) * 0.03;
target_att[2] += sinf(station[3]) * 0.03;

CoastToTarget(myState, target_att);

target_att[0] = 0;
target_att[1] = cosf(station[3]);
target_att[2] = sinf(station[3]);

ZRSetAttitudeTarget(target_att);
}
   break;
  case 8:
//Code to beeline towards the station...



target[0] = (getPanelSide() * 0.7);
target[1] = 0;
target[2] = 0;

CoastToTarget(myState, target);
   break;
 }
}
float getDist(float pt1[3], float pt2[3]) {
    return sqrtf(mathSquare(pt2[2] - pt1[2]) + mathSquare(pt2[1] - pt1[1]) + mathSquare(pt2[0] - pt1[0]));
}