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;
}
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);
}
}
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]));
}