bool pathToTarget(float target[],float waypoint[]){ //returns true if there is something in the way float V1[3], V2[3], V3[3], V1p[3]; api.getMyZRState(me); mathVecSubtract(V1, target, me, 3); mathVecSubtract(V2, me, origin, 3); for(int i = 0; i < 3; i++){ V1p[i] = V1[i] * mathVecInner(V2,V1,3) / mathVecInner(V1,V1,3); } mathVecSubtract(V3, V2, V1p, 3); if(mathVecMagnitude(V3,3) < 0.31){ DEBUG(("setting waypoint")); for(int i = 0; i < 3; i++){ if(V3[i] >= 0){ waypoint[i] = V3[i] * (0.31 / mathVecMagnitude(V3,3)) + 0.05; } else{ waypoint[i] = V3[i] * (0.31 / mathVecMagnitude(V3,3)) - 0.05; } } return true; } else{ return false; } }
bool pathToTarget(float pos[], float target[],float waypoint[]){ //returns true if there is something in the way float V1[3], V2[3], V3[3], V1p[3]; mathVecSubtract(V1, target, pos, 3); for(int i = 0; i < 3; i++){ V2[i] = pos[i]; } for(int i = 0; i < 3; i++){ V1p[i] = V1[i] * mathVecInner(V2,V1,3) / mathVecInner(V1,V1,3); } mathVecSubtract(V3, V2, V1p, 3); if(mathVecMagnitude(V3,3) < 0.31){ for(int i = 0; i < 3; i++){ if(V3[i] >= 0){ waypoint[i] = V3[i] * (0.31 / mathVecMagnitude(V3,3)) + 0.05; } else{ waypoint[i] = V3[i] * (0.31 / mathVecMagnitude(V3,3)) - 0.05; } } return true; } else{ return false; } }
bool insidePoiZone(int poi){ if(poi==0){ mathVecSubtract(tempVec,myState,tP1,3); if(mathVecMagnitude(myState,3)>.31&&mathVecMagnitude(myState,3)<.42&&mathVecInner(tempVec,tP1,3)<.8){ return true; } }else if(poi==1){ mathVecSubtract(tempVec,myState,tP1,3); if(mathVecMagnitude(myState,3)>.42&&mathVecMagnitude(myState,3)<.53&&mathVecInner(tempVec,tP1,3)<.4){ return true; } }else return false; }
int which_he_lookin_at( ZRState someone, float locs[][3], int num_locs ) { // Returns the index of the location such that the vector from someone to that // location makes the smallest possible angle with someone's attitude. float min_angle = 10; // any number bigger than PI int ans = num_locs; float angle; float to_loc[3]; // Get the attitude vector. float attitude[3]; memcpy(attitude, &someone[6], 3*sizeof(float)); while (num_locs > 0) { // Construct the vector from someone to loc[i]. to_loc[0] = locs[num_locs][0] - someone[0]; to_loc[1] = locs[num_locs][1] - someone[1]; to_loc[2] = locs[num_locs][2] - someone[2]; mathVecNormalize(to_loc, 3); // Find the angle between attitute and to_loc. angle = acosf(mathVecInner(attitude, to_loc, 3)); // If this is the smallest angle so far, update ans and min_angle. if (angle < min_angle) { ans = num_locs; min_angle = angle; } num_locs--; } // Return the index of the location in locs that produced the smallest angle. return ans; }
float angle(float temp[3], float lol[3]){ api.getMyZRState(me); float a, b, c; a = mathVecInner(temp, lol, 3); b = mathVecMagnitude(temp, 3); c = mathVecMagnitude(lol, 3); return acosf(a/(b*c)); }
void mathVecProject(float c[], float a[], float b[], int n) { // finds the projection of a onto b, puts the result in c if (mathVecMagnitude(b,3) * mathVecMagnitude(b,3) / 10 == 0) { DEBUG(("DIVISION BY ZERO WHILE PROJECTING!")); } for (int i = 0; i < n; i++) { c[i] = (mathVecInner(a,b,3) * b[i]) / (mathVecMagnitude(b,3) * mathVecMagnitude(b,3)); } }
float angleBetween(float pt1[3], float pt2[3]){ float dot; float vectorBetweenS1[3], vectorBetweenS2[3]; mathVecSubtract(vectorBetweenS1,pt1, origin ,3); mathVecNormalize(vectorBetweenS1,3); mathVecSubtract(vectorBetweenS2,pt2,origin,3); mathVecNormalize(vectorBetweenS2,3); dot = mathVecInner(vectorBetweenS1, vectorBetweenS2, 3); return acosf(dot); }
float angleBetween(float A[3], float B[3]) { float dotProduct = mathVecInner(A, B, 3); float magnitudeA = mathVecMagnitude(A, 3); float magnitudeB = mathVecMagnitude(B, 3); float angle = acos(dotProduct / (magnitudeA * magnitudeB)); return angle; }
/* getETA: get estimated time of arrival upon item based on the sphere, * finds the angle between th other's velocity vector and the vector from the * other that points towards the location. If that is more than * CONE_ANGLE_COSINE as defined in INIT, the function will return MAX_INT, * otherwise the function returns the distance divided by the other's speed. */ float getETA (float sphere[], float location[]){ mathVecSubtract(temp, location, sphere, 3); float distance = mathVecMagnitude(temp, 3); float speed = mathVecMagnitude(sphere + 3, 3); float cosine = mathVecInner(temp, sphere + 3, 3) / (distance * speed); if (cosine > CONE_ANGLE_COSINE) { // means that the angle is within the boundary return distance / speed; } return MAX_INT; }
bool isAligned(float myPos[3], float myAtt[3], float poiLoc[3]) { if (mathVecInner(poiLoc, myAtt) < -0.99) //dot product should be -1 if the two vectors are indeed pointing in opposite directions { return true; //untested, can someone see if this works? } else { return false; } }
//User-defined procedures static void shoot (float myState[12], float target[3], unsigned int fire) { //BEGIN::PROC::shoot float direction[3]; mathVecSubtract(direction, target, myState, 3); mathVecNormalize(direction, 3); ZRSetAttitudeTarget(direction); if (fire && (acos(mathVecInner(&myState[6], direction, 3) / mathVecMagnitude(&myState[6], 3)) < (0.1))) Plaser(); //END::PROC::shoot }
void loop(){ //This function is called once per second. Use it to control the satellite. updateVariables(); facePos(center); //Always face the center if((timeToFlare>2||timeToFlare==-1)&&!on){ //Turn sphere back on game.turnOn(); on=true; } else if(timeToFlare!=-1&&timeToFlare<=2&&on){ //Auto Shutdown, doesn't let sphere be on during solar flare game.turnOff(); on=false; } if(game.getMemoryFilled()<2&&poiZone<2&&(timeToFlare>12||timeToFlare==-1)&&time<230){ //If we have memory space and no incoming solar flare switch(poiZone){ case 0: //Outer Ring calcPoiEntry(spherePoi,tP1,.4425f);//.430f safeSetPosition(tP1,.2f);//.15 break; case 1: //Inner Ring calcPoiEntry(spherePoi,tP1,.3725f);//.370f safeSetPosition(tP1,.2f);//.15 break; } mathVecSubtract(tempVec,center,myState,3); if(distance(myState,tP1)<.025f&&mathVecInner(tP1,tempVec,3)<.05f){ //If in range, take picture game.takePic(spherePoi); picTries++; if(game.getMemoryFilled()>lastMem||picTries>7){ //Successfully taken picture or stuck taking pic, go to next poi poiZone++; } } }else{ //Solar Flare incoming if(game.getMemoryFilled()>0){ //Upload pictures in memory //float memPack[] = {-.5,sphere*.6,0}; calcPoiEntry(spherePoi,tP1,.7); safeSetPosition(tP1,.15f); game.uploadPic(); if(distance(myState,center)>.5){ game.uploadPic(); picTries=0; poiZone=0; } }else{ //Stop api.setVelocityTarget(center); game.takePic(spherePoi); } } }
bool isAligned(){ api.getMyZRState(posicionSatelite); game.getPOILoc(posicionPOI, PoiID); float dir[3]; mathVecSubtract(dir,posicionPOI,posicionSatelite,3); mathVecNormalize(dir,3); float dotProd; dotProd = mathVecInner(dir,&posicionSatelite[6],3); if(dotProd>.985){ return true; } else{ return false; } }
static float timeToMS (float myState[12], float station[3]) { //BEGIN::PROC::timeToMS // distance / average velocity // + time to turn around float toStation[3]; //this is ugly so that code can fit mathVecSubtract(toStation, station, myState, 3); return (mathVecMagnitude(toStation, 3) / 0.065) + (acos(mathVecInner(&myState[3], toStation, 3)/(mathVecMagnitude(&myState[3],3)*mathVecMagnitude(toStation,3)))/18) + ((0.06 - mathVecMagnitude(&myState[3], 3)) / .01) + 8; //END::PROC::timeToMS }
void doOrbit(float targetPos[3],float speed){ float angle = 0.0f; float radius = mathVecMagnitude(myState,3); //Safety if(radius>.6f) radius = .6f; //Calculate Current Angle angle = acosf(myState[0]/(sqrtf(powf(myState[0],2)+powf(myState[1],2)+powf(myState[2],2)))); if(myState[1]<0){ angle=2*PI-angle; } mathVecSubtract(tempVec,targetPos,myState,3); if(tempVec[0]==0) tempVec[0]=-1; if(tempVec[1]==0) tempVec[1]=-1; //Calc Orbit Direction if(fabsf(tempVec[1])/tempVec[1]*fabsf(tempVec[0])/tempVec[0]>0){ angle-=(20*PI/180);//clockwise } else{ angle+=(20*PI/180); } //Calculate position on orbit circle targetPos[0] = radius * cosf(angle); targetPos[1] = radius * sinf(angle); //Rotate to 3D mathVecAdd(tempVec,myState,otherState,3); mathVecNormalize(tempVec,3); float xAxis[3] = {1.0f,0.0f,0.0f}; float yAxis[3] = {0.0f,1.0f,0.0f}; float zAxis[3] = {0.0f,0.0f,1.0f}; float rots[3] = {mathVecInner(tempVec,xAxis,3),mathVecInner(tempVec,yAxis,3),mathVecInner(tempVec,zAxis,3)}; rotate(targetPos,targetPos,rots); setPos(targetPos,speed); }
static void leaveOrbit (float myState[12], float asteroidLoc[3], float mPanel[3]) { //BEGIN::PROC::leaveOrbit #define SinAngle(a, b) sqrt(1 - mathSquare(mathVecInner((a), (b), 3)/(mathVecMagnitude((a), 3) * mathVecMagnitude((b), 3)))) float targetVel[3]; float r, toMS; float desiredMag; float vecToAst[3]; float vecToMS[3]; int i; for(i = 0; i < 3; i++){ vecToAst[i] = asteroidLoc[i] - myState[i]; vecToMS[i] = mPanel[i] - myState[i]; } toMS = mathVecMagnitude(vecToMS, 3); r = sqrt(mathVecInner(vecToAst, vecToAst, 3)); for(i = 0; i < 3; i++) targetVel[i] = mPanel[i] - myState[i]; if(toMS < .19){ if(mathVecMagnitude(&myState[3], 3) > 0.01) desiredMag = 0.002; else desiredMag = 0.01; } else if(toMS < 0.25) desiredMag = toMS/12; else if(toMS < 0.35) desiredMag = toMS/7; else desiredMag = r*3.141592/(45*SinAngle(vecToAst, &myState[3])); if(desiredMag > toMS/6) desiredMag = toMS/6; mathVecNormalize(targetVel, 3); for(i = 0; i < 3; i++) targetVel[i] *= desiredMag; ZRSetVelocityTarget(targetVel); //ZRSetAttitudeTarget(att); //END::PROC::leaveOrbit }
bool intersectsAsteroid(float targetPos[3]){ float dir[3]; mathVecSubtract(dir,myState,targetPos,3); float center[] = {0.0f,0.0f,0.0f}; float r = .31f; //Radius of danger zone mathVecSubtract(tempVec,myState,center,3); float res = powf(mathVecInner(dir,tempVec,3),2)-powf(mathVecMagnitude(tempVec,3),2)+powf(r,2); //Once we've found no intersections, can determine to orbit or not. Rest is to reinforce concept mathVecSubtract(tempVec,myState,targetPos,3); if(res<0){ //Does not intersect return false; }else if(res==0){ //Intersects at one point return true; }else{//if res>0 //Intersects at two points return true; } }
static float Vfunc(int which, float * v1, float * v2, float * vresult, float scalar) { int i; float vtemp[3]; if (which == 2) { // vresult = v1 - v2 Vfunc(4,v2,NULL,vtemp,-1); mathVecAdd(vresult,v1,vtemp,3); return 0; } if (which == 3) { // vresult = v1 / |v1|; if |v1| == 0, returns 0, else 1 memcpy(vresult, v1, sizeof(float)*3); mathVecNormalize(vresult,3); return 0; } if (which == 4) { // vresult = scalar * v1 for (i = 0; i < 3; ++i) vresult[i] = scalar * v1[i]; return 0; } if (which == 5) { // returns dot product: v1 * v2 return mathVecInner(v1,v2,3); } if (which == 6) { // returns distance between v1 and v2 float v3[3]; Vfunc(2,v1,v2,v3,0); // v3 = v1 - v2 return mathVecMagnitude(v3,3); } if (which == 8) { // angle between two vectors float dot = Vfunc(5,v1,v2,NULL,0)/(mathVecMagnitude(v1,3)*mathVecMagnitude(v2,3)); return acos(dot)*180.0/3.14159265; } if (which == 9) { // unit vector pointing from v1 toward v2 float v9[3]; Vfunc(2,v2,v1,v9,0); return Vfunc(3,v9,NULL,vresult,0); } }
//END::PAGE::adef //BEGIN::PAGE::ctrl void overshoot(vec targetPos) { vec nPos, att; float dis = disBetweenPts(statePos(myState), targetPos); attToTarget(statePos(myState), targetPos, att); nPos[0] = targetPos[0] + att[0] * osMag * dis; //set target location beyond target nPos[1] = targetPos[1] + att[1] * osMag * dis; nPos[2] = targetPos[2] + att[2] * osMag * dis; float velComponent = mathVecInner(stateVel(myState), att, 3); float disLeft = dis - stillDis; float velDis = disToHalt(velComponent); #if printDebug DEBUG(("Overshoot Info\n")); DEBUG(("\tOvershoot Magnitude\t%.3f\n", osMag)); DEBUG(("\tOvershoot Threshold\t%.3f\n", lim)); DEBUG(("\tDis to Target (m)\t%.3f\n", dis)); DEBUG(("\tVel Component (m/s)\t%.3f\n", velComponent)); DEBUG(("\tCur Vel Dis (m)\t%.3f\n", velDis)); printVec("\tTarget Pos (m)", targetPos); printVec("\tTarget Att (unitvec)", att); printVec("\tNew Pos (m)", nPos); #endif if (osMag <= floatZero || hitTarget(myState, targetPos)) { #if printDebug DEBUG(("\tsetPositionTarget\n")); #endif api.setPositionTarget(targetPos); } else if (dis < lim && velDis > disLeft + 0.001f) { #if printDebug DEBUG(("\tHalting\n")); #endif api.setVelocityTarget(zeroVec); } else { //overshoot #if printDebug DEBUG(("\tSpeeding Up\n")); #endif api.setPositionTarget(nPos); } }
float angleBetween(float a[], float b[]) { //returns the measure of the angle between vectors a and b return acosf(mathVecInner(a, b, 3) / (mathVecMagnitude(a,3) * mathVecMagnitude(b,3))); }
void loop(){ api.getMyZRState(me); api.getOtherZRState(other); getItemArray(); if((game.getCurrentTime() > 165 || game.getFuelRemaining() < 10) && game.getMemoryFilled() >= 1){ game.takePic(); state = UPLOAD; //game changer } if(game.getEnergy() < 1 && !game.posInLight(me)) state = STOP; if(game.getFuelRemaining() == 0) game.takePic(); switch(state) { case MAIN: //normal case if(distance(me,items[itemNum])<0.3 && distance(me,items[itemNum])>distance(other,items[itemNum]) ) itemBool[itemNum]=false; if(!itemBool[itemNum]){ itemNum = recalibrate(4,6); } memcpy(target,items[itemNum],3*sizeof(float)); moveTo(target); mathVecSubtract(facing, other, me, 3); api.setAttitudeTarget(facing); if( checkPhoto(me,other,facing) ) game.takePic(); if(game.getMemoryFilled() == 2) state = UPLOAD; break; case UPLOAD: //upload if(game.getEnergy() > 2) moveTo(target); else api.setVelocityTarget(origin); api.setAttitudeTarget(earth); if( game.getEnergy() > 1 && checkUpload(me) ) game.uploadPics(); if( game.getMemoryFilled() == 0){ state = MAIN; } break; case STOP: //chill out api.setVelocityTarget(origin); api.setAttRateTarget(origin); if(!mathVecInner(me+6,facing,3) > 0.9689f) api.setAttitudeTarget(facing); if(game.posInLight(me)){ if(game.getMemoryFilled() == 2) state = UPLOAD; else state = MAIN; } break; case LIGHTDARK: //if we are in light and they are in dark itemNum = recalibrate(7,8); if(!itemBool[itemNum]) //if both mirrors are taken itemNum = 6; memcpy(target,items[itemNum],3*sizeof(float)); api.setPositionTarget(target); mathVecSubtract(facing, other, me, 3); api.setAttitudeTarget(facing); break; } }
void mathVecProject(float c[], float a[], float b[], int n) { // finds the projection of a onto b, puts the result in c for (int i = 0; i < n; i++) { c[i] = mathVecInner(a, b, 3)/mathVecInner(b, b, 3) * b[i]; } }
//movement_moveto //dst -> Destiny //direct -> If true does not uses debris avoidance //return -> true if we have arrived, false otherwise bool movement_moveto(float dst[3], bool direct) { float delta[3]; float head[3]; mathVecSubtract(delta, dst, &our_state[POS], 3); //delta = dst - pos mathVecScalarMult(head, delta, 1.0f/mathVecMagnitude(delta, 3), 3); //head = delta / |delta| if(mathVecMagnitude(delta, 3) < MAX_ITEM_START_DIST && mathVecMagnitude(&our_state[VEL], 3) < MAX_ITEM_START_VEL) { return true; } else { //delta = zero delta[0] = delta[1] = delta[2] = 0.0f; api.setAttRateTarget(delta); } //delta = head_vel mathVecScalarMult(delta, head, mathVecInner(head, &our_state[VEL], 3), 3); float head_speed = mathVecMagnitude(delta, 3); float side_vel[3]; mathVecSubtract(side_vel, &our_state[VEL], delta, 3); const float danger_radius = (SPHERE_RADIUS + DEBRIS_RADIUS) + 0.03f; const float correction = danger_radius + 0.02f; if(direct || seconds >= 90) { api.setPositionTarget(dst); return false; } int debris_number; float debris_vector[5]; int nearest_debris = -1; float nearest_debris_distance = 1000000.0f; float nearest_debris_vector[5]; for(debris_number = 0; debris_number < NUMBER_OF_DEBRIS; debris_number++) { if(!is_debris_collected[debris_number]) { distanceToDebris(&our_state[POS], dst, debris_position[debris_number], debris_vector); if((debris_vector[SIDE_DIST] - (danger_radius + ((mathVecInner(side_vel, debris_vector, 3)/head_speed)*debris_vector[3]))) < 0.0f) { //Colisión if(debris_vector[HEAD_DIST] < nearest_debris_distance && debris_vector[HEAD_DIST] > 0.0f) { nearest_debris = debris_number; int i; for(i = 0; i < 5; ++i) { nearest_debris_vector[i] = debris_vector[i]; } nearest_debris_distance = debris_vector[3]; } } } } api.setPositionTarget(dst); if(nearest_debris >= 0) { //Si nearest_debris es un debris válido //delta = tmp float needed_speed_up = correction - (nearest_debris_vector[SIDE_DIST] + ((mathVecInner(side_vel, nearest_debris_vector, 3)/head_speed)*nearest_debris_vector[3])); mathVecScalarMult(delta, nearest_debris_vector, needed_speed_up, 3); api.setForces(delta); } #ifdef DEBUG_ACTIVE else { movement_last_debris = -1.0f; nearest_debris_distance = 0.0f; DEBUG(("movement:Clear!\n")); } #endif return false; }
//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); }
void ZRUser01(float *myState, float *otherState, float time) { //BEGIN::PROC::ZRUser float opulens[3] = {0.0, -0.6, 0.0}; float Laser[3] = {0.4, 0.0, 0.0}; float shield[3] = {0.0, 0.4, 0.0}; float disrupt[3] = {0.0,0.2,0.0}; float zero[3] = {0.0, 0.0, 0.0}; float difference[3]; float facing[3]; float Asteroid[3] = {0.0, -0.6, 0.0}; int recieved; switch((int)time) { case 0: SphereNumber = !!(myState[0] < 0) + 1; Station[0] = (SphereNumber == 1) ? 0.6 : -0.6; //station closer to where you started break; case 30: getUp = (otherState[1] <= 0); break; case 61: case 66: Spin = PisRevolving(otherState) ? 1 : 0; break; case 75: if(PinAsteroid(otherState) == 1) asteroid = 1; } Laser[0] = ((SphereNumber == 1) - (PotherHasLaser() == SphereNumber)) ? 0.4 : -0.4; recieved = (int)PgetMessage(); switch(recieved) { case 6: if(!(Station[0] < 0.0) && switchedStation++ < 3) Station[0] = -0.6; break; case 7: if(!(Station[0] > 0.0) && switchedStation++ < 3) Station[0] = 0.6; break; case 2: case 3: if(time < 45) asteroid = 1; break; case 4: case 5: if(time < 45) asteroid = 0; break; } if(time <= 120) PsendMessage(4 - 2*asteroid + (Spin == 0)); else PsendMessage((Station[0] < 0) + 6); Asteroid[1] = asteroid ? -0.6 : 0.6; switch(state) { case 0: ZRSetPositionTarget(Laser); shoot(myState, opulens, 0); if(PhaveLaser() || PgetPhase() == 2) state = 1; break; case 1: if(!getUp) { shield[1] = .3; disrupt[1] = .3; } ZRSetPositionTarget(disrupt); shoot(myState, opulens, 0); if(PotherDisruptorUpgraded() || PdisruptorUpgraded()) state = 2; if(PgetPhase() == 2) state = 3; break; case 2: ZRSetPositionTarget(shield); shoot(myState, opulens, 0); if(PotherHasShield() || PhaveShield() || PgetPhase() == 2) state = 3; break; case 3: if(PiceHits() < 15 && !asteroid) shoot(myState, opulens, (PgetPhase() > 1)); if(!(PiceMelted()) && asteroid) { mathVecSubtract(difference, myState, opulens, 3); if(mathVecMagnitude(difference, 3) > .8) ZRSetPositionTarget(opulens); else if(!Spin) ZRSetVelocityTarget(zero); shoot(myState, opulens, (PgetPhase() > 1)); return; } if(Spin){ ZRSetPositionTarget(Asteroid); spin(myState); } else orbit(myState, Asteroid, Station[0] == -0.6); if((!Spin && (time + timeToMS(myState, Station) >= 165)) || (Spin && (time >= 156))) state = 4; break; default: leaveOrbit(myState, Asteroid, Station); mathVecSubtract(facing, otherState, myState, 3); mathVecNormalize(facing, 3); ZRSetAttitudeTarget(facing); if (acos(mathVecInner(&myState[6], facing, 3) / mathVecMagnitude(&myState[6], 3)) < (0.1)) Ptractor(); break; } //END::PROC::ZRUser }
float angBetweenVecs (vec v1, vec v2) { return acosf(mathVecInner(v1, v2, 3) / (mathVecMagnitude(v1, 3) * mathVecMagnitude(v2, 3))); }