//Begin page main #define SPH_RAD 0.11// __...------------._ #define hD 0.17095// ,-' `-. #define thrusterHealth 0.93// ,-' `. float target[3];float myState[12];float mySphereState[13] ;// ,' ,-`. float otherState[12];float otherSphereState[13] ;// ; `-' `. float otherStartQuat[4];float otherPredQuat[4] ;// ; .-. | int step;int time;int hMode;float spdm;float hCdown ;// ; .-. `-' | void init() {// ; `-' | spdm = 0.098 * thrusterHealth ;// ; `. hMode = 1 ;// ; : hCdown = 0.0 ;// ; | api.getOtherSphState(otherSphereState) ;// ; ; memcpy(otherStartQuat, otherSphereState + 6, 4 * sizeof(float)) ;// ; ___ ; step = 4 ;// ; ,-;-','.`.__ | time = 0 ;// _..; ,-' ;`,'.`,'.--`. | }// ///; ,-' `. ,-' ;` ;`,','_.--=: / void goTo(float pos[], float tv) {// |'': ,' : ;` ;,;,,-'_.-._`. ,' float t = spdm;float delta[3] ;// ' : ;_.-. `. :' ;;;'.ee. \| / mathVecSubtract(delta, pos, myState, 3) ;// \.' _..-'/8o. `. : :! ' ':8888) || / float myVel[3];memcpy(myVel, myState + 3, 3 * sizeof(float)) ;// ||`-'' \\88o\ : : :! : :`""' ;;/ float speed = mathVecMagnitude(myVel, 3);float myVelT[3] ;// || \"88o\; `. \ `. `. ;,' float t2 = 0.5 / t;float sot = 2.0 * speed * t2 ;// /) ___ `."'/(--.._ `. `.`. `-..-' ;--. float tmp = (-1.0 - t2 + sot / 6.0) * sot ;// \(.="""""==.. `'-' `.| `-`-..__.-' `. `. for (int i = 0; i < 3; i++) {// | `"==.__ ) ) ; myVelT[i] = (tmp * myVel[i] + delta[i]) ;// | || `"=== ' .' .' }// /\,,|||| | | \ .' .' api.setVelocityTarget(myVelT) ;// | |||'|' |'|' \| .' _.' | }// | |\' | | || || .' .' | float dist(float pos1[], float pos2[]) {// ' | \ ' |' . ``-- `| || .' .' | float v[3];mathVecSubtract(v, pos1, pos2, 3) ;// ' | ' | . ``-.._ | ; .' .' `. return mathVecMagnitude(v, 3) ;// _.--,;`. . -- ...._,' .' .' `.__ }// ,' ,'; `. . --..__..--'.' .' __/_| float travelTime(float pos1[], float pos2[]) {// ,' ; ; | . --..__.._.' .' ,' `. float q = dist(pos1, pos2);float accl = spdm ;// / ; : ; . -.. _.' _.' / ` return sqrtf(q / accl * 2.0) * 4.1 ;// / : `-._ | . _.--' _.' | }/// `. `--....--'' _.' | void getVFlip(float f, float* vFlip) {// `._ _..-' | vFlip[0] = vFlip[2] = 0.0;float f2 = 0.5 * f ;// `-..____...-'' | vFlip[1] = sinf(f2);vFlip[3] = cosf(f2) ;// | }// It's A Trap! | void getQFlip(float f, float* vFlip) { vFlip[1] = vFlip[2] = 0;float f2 = f / 2.0;vFlip[0] = sinf(f2) ; vFlip[3] = cosf(f2) ;// ,ooo888888888888888oooo, }// o8888YYYYYY77iiiiooo8888888o void getHFlip(float f, float* vFlip) {// 8888YYYY77iiYY8888888888888888 vFlip[0] = vFlip[1] = 0.0;float f2 = 0.5 * f ;// [88YYY77iiY88888888888888888888] vFlip[2] = sinf(f2);vFlip[3] = cosf(f2) ;// 88YY7iYY888888888888888888888888 }// [88YYi 88888888888888888888888888] float* predictOtherQuat(float t) {// i88Yo8888888888888888888888888888i float* quatVF = new float[4] ;// i] ^^^88888888^^^ o [i float deltaRot = (time - t) * PI / 75 ;// oi8 i o8o i 8io float* quatVFlip = new float[4] ;// ,77788o ^^ ,oooo8888888ooo, ^ o88777, getVFlip(otherSphereState[10] * (t - time), quatVFlip) ;// 7777788888888888888888888888888888877777 quatMult(quatVF, quatVFlip, otherSphereState + 6);return quatVF ;// 77777888888888888888888888888888877777 }// 77777788888888^7777777^8888888777777 float* predictOtherQuatChamp(float t) {// ,oooo888 ooo 88888778888^7777ooooo7777^8887788888 ,o88^^^^888oo float* quatVF = new float[4];float* quatVFlip = new float[4] ;// o8888777788[];78 88888888888888888888888888888888888887 7;8^ 888888888oo^88 getVFlip(t, quatVFlip) ;// o888888iii788 ]; o 78888887788788888^;;^888878877888887 o7;[]88888888888888o quatMult(quatVF, quatVFlip, otherStartQuat);return quatVF ;// 88888877 ii78[]8;7o 7888878^ ^8788^;;;;;;^878^ ^878877 o7;8 ]878888888888888 }// [88888888887888 87;7oo 777888o8888^;ii;;ii;^888o87777 oo7;7[]8778888888888888 float* predictOtherDriftPos(float t) {// 88888888888888[]87;777oooooooooooooo888888oooooooooooo77;78]88877i78888888888 float deltaD = (t - ((float)time));float* cpos = new float[3] ;// o88888888888888 877;7877788777iiiiiii;;;;;iiiiiiiii77877i;78] 88877i;788888888 for (int i = 0; i < 3; i++) {// 88^;iiii^88888 o87;78888888888888888888888888888888888887;778] 88877ii;7788888 cpos[i] = otherState[i] + deltaD * otherSphereState[i + 3] ;//;;;iiiii7iiii^ 87;;888888888888888888888888888888888888887;778] 888777ii;78888 }//;iiiii7iiiii7iiii77;i88888888888888888888i7888888888888888877;77i 888877777ii78 return cpos ;//iiiiiiiiiii7iiii7iii;;;i7778888888888888ii7788888888888777i;;;;iiii 88888888888 }//i;iiiiiiiiiiii7iiiiiiiiiiiiiiiiiiiiiiiiii8877iiiiiiiiiiiiiiiiiii877 88888 float predictOtherCircleAngle() {//ii;;iiiiiiiiiiiiii;;;ii^^^;;;ii77777788888888888887777iii;; 77777 78 api.getOtherSphState(otherSphereState);float radi = 0.2f ;//77iii;;iiiiiiiiii;;;ii;;;;;;;;;^^^^8888888888888888888777ii;; ii7 ;i78 float cirX = otherSphereState[2] ;//^ii;8iiiiiiii ';;;;ii;;;;;;;;;;;;;;;;;;^^oo ooooo^^^88888888;;i7 7;788 float cirY = otherSphereState[0] ;//o ^;;^^88888^ 'i;;;;;;;;;;;;;;;;;;;;;;;;;;;^^^88oo^^^^888ii7 7;i788 float inA = atan2(cirY, -cirX);return inA ;//88ooooooooo ;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;; 788oo^;; 7;i888 }//887ii8788888 ;;;;;;;ii;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;^87 7;788 float* coordConvert(float vin[], bool worldToHook) {//887i8788888^ ;;;;;;;ii;;;;;;;oo;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;,,, ;;888 float up[3], forward[3], side[3], attbase[3];attbase[0] = -1.0 ;//87787888888 ;;;;;;;ii;;;;;;;888888oo;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;;,,;i788 attbase[1] = 0.0;attbase[2] = 0.0 ;//87i8788888^ ';;;ii;;;;;;;8888878777ii8ooo;;;;;;;;;;;;;;;;;;;;;;;;;;i788 7 api.quat2AttVec(attbase, otherSphereState + 6, forward) ;//77i8788888 ioo;;;;;;oo^^ooooo ^7i88^ooooo;;;;;;;;;;;;;;;;;;;;i7888 78 attbase[0] = 0.0;attbase[1] = 0.0;attbase[2] = -1.0 ;//7i87788888o 7;ii788887i7;7;788888ooooo7888888ooo;;;;;;;;;;;;;;oo ^^^ 78 api.quat2AttVec(attbase, otherSphereState + 6, side) ;//i; 7888888^ 8888^o;ii778877;7;7888887;;7;7788878;878;; ;;;;;;;i78888o ^ attbase[0] = 0.0;attbase[1] = -1.0;attbase[2] = 0.0 ;//i8 788888 [88888^^ ooo ^^^^^;;77888^^^^;;7787^^^^ ^^;;;; iiii;i78888888 api.quat2AttVec(attbase, otherSphereState + 6, up) ;//^8 7888^ [87888 87 ^877i;i8ooooooo8778oooooo888877ii; iiiiiiii788888888 float otherHookPos[3];float relTipCoords[3] ;// ^^^ [7i888 87;; ^8i;;i7888888888888888887888888 i7iiiiiii88888^^ float *vout = new float[3] ;// 87;88 o87;;;;o 87i;;;78888788888888888888^^ o 8ii7iiiiii;; for (int i = 0; i < 3; i++) {// 87;i8 877;77888o ^877;;;i7888888888888^^ 7888 78iii7iii7iiii otherHookPos[i] = otherState[i] + otherState[i + 6] * hD ;// ^87; 877;778888887o 877;;88888888888^ 7ii7888 788oiiiiiiiii }// ^ 877;7 7888888887 877i;;8888887ii 87i78888 7888888888 for (int i = 0; i < 3; i++) {// [87;;7 78888888887 87i;;888887i 87ii78888 7888888888] if (worldToHook) relTipCoords[i] = vin[i] - otherHookPos[i] ;// 877;7 7788888888887 887i;887i^ 87ii788888 78888888888 else {// 87;i8 788888888888887 887ii;;^ 87ii7888888 78888888888 relTipCoords[i] = forward[i] * vin[0] +// [87;i8 7888888888888887 ^^^^ 87ii77888888 78888888888 side[i] * vin[1] + up[i] * vin[2] ;// 87;;78 7888888888888887ii 87i78888888 778888888888 }// 87;788 7888888888888887i] 87i78888888 788888888888 }// [87;88 778888888888888887 7ii78888888 788888888888 if (worldToHook) {// 87;;88 78888888888888887] ii778888888 78888888888] vout[0] = mathVecInner(relTipCoords, forward, 3) ;// 7;;788 7888888888888888] i7888888888 78888888888' vout[1] = mathVecInner(relTipCoords, side, 3) ;// 7;;788 7888888888888888 'i788888888 78888888888 vout[2] = mathVecInner(relTipCoords, up, 3) ;// 7;i788 788888888888888] 788888888 77888888888] } else {// '7;788 778888888888888] [788888888 78888888888' for (int i = 0; i < 3; i++) {// ';77888 78888888888888 8888888888 7888888888] vout[i] = relTipCoords[i] + otherHookPos[i] ;// 778888 78888888888888 8888888888 7888888888] }// 78888 7888888888888] [8888888888 7888888888 }// 7888 788888888888] 88888888888 788888888] return vout ;// 778 78888888888] ]888888888 778888888] }// oooooo ^88888^ ^88888^^^^^^^^8888] void loop() {// 87;78888ooooooo8o ,oooooo oo888oooooo spdm = 0.1 * thrusterHealth;api.getMyZRState(myState) ;// [877;i77888888888] [;78887i8888878i7888; api.getOtherZRState(otherState) ;// ^877;;ii7888ii788 ;i777;7788887787;778; api.getOtherSphState(otherSphereState) ;// ^87777;;;iiii777 ;77^^^^^^^^^^^^^^^^;; api.getMySphState(mySphereState) ;// ^^^^^^^^^ii7] ^ o88888888877iiioo DEBUG(("Step: %d", step)) ;// 77777o [88777777iiiiii;;778 if (game.getGamePhase() == 3 && step > 4) {// 77777iii 8877iiiii;;;77888888] step = 4 ;// 77iiii;8 [77ii;778 788888888888 }// 7iii;;88 iii;78888 778888888888 if (step == 4) {// 77i;78888] ;;;;i88888 78888888888 float thresh = -0.16;float shiftT = -0.1;spdm = 0.098 ;// ,7;78888888 [;;i788888 7888888888] spdm = spdm * thrusterHealth;float* myHP = new float[3] ;// i;788888888 ;i7888888 7888888888 for (int ooo = 0; ooo < 3; ooo++) {// ;788888888] i77888888 788888888] myHP[ooo] = mySphereState[ooo] + myState[ooo + 6] * hD ;// ';88888888' [77888888 788888888] }// [[8ooo88] 78888888 788888888 float* hookLocalCoords = coordConvert(myHP, true) ;// [88888] 78888888 788888888 float sideDist = sqrtf(hookLocalCoords[1] * // ^^^ [7888888 77888888] hookLocalCoords[1] + hookLocalCoords[2] * // 88888888 7888887 hookLocalCoords[2]) ;// 77888888 7888887 float spd = mathVecMagnitude(myState + 3, 3) ;// ;i88888 788888i float hookMi = mathVecMagnitude(hookLocalCoords, 3) ;// ,;;78888 788877i7 float targetLocal[3];targetLocal[0] = -hD ;// ,7;;i;777777i7i;;7 targetLocal[1] = 0.0;targetLocal[2] = 0.0 ;// 87778^^^ ^^^^87778 bool pHM = ((float)time - hCdown) < 4.0 ;// ^^^^ o777777o ^^^ bool flag = sideDist > 0.006 ;// o77777iiiiii7777o if (((hookMi > 0.05 || hookLocalCoords[0] < -0.005) &&// 7777iiii88888iii777 hMode == 0) || pHM) hMode = 1 ;// ;;;i7778888888877ii;; if (hMode == 1) {// [i77888888^^^^8888877i] targetLocal[0] = 0.01 - hD;targetLocal[1] = 0.01 ;// 77888^oooo8888oooo^8887] targetLocal[2] = -0.03 ;// [788888888888888888888888] if (spd < 0.01 && hookLocalCoords[0] > 0.005 && !pHM) {// 88888888888888888888888888 hMode = 2 ;// ]8888888^iiiiiiiii^888888] }// iiiiiiiiiiiiiiiiiiiiii }// ^^^^^^^^^^^^^ if (hMode == 2) { targetLocal[0] = 0.01 - hD;targetLocal[1] = -0.005 ; targetLocal[2] = 0.005 ; if (spd < 0.01 && sideDist < 0.01) hMode = 0 ; } if (hMode == 0) { targetLocal[0] = -hD;targetLocal[1] = 0.0 ; targetLocal[2] = 0.0 ; } DEBUG(("HOOK MODE: %d", hMode));float tme = 4.0 ; float champPos[3];float champ = 0.0;champPos[0] = 0 ; champPos[1] = -0.5;champPos[2] = 0 ; if (dist(otherState, champPos) < 0.03) { DEBUG(("I AM SUCH A CHAMP"));champ = 1.0;tme = 4.0 ; } float* quatt = new float[4] ; quatt = predictOtherQuat((float)time + tme) ; if (champ > 0.5 && time < 25) { quatt = predictOtherQuatChamp(2.5) ; } memcpy(otherPredQuat, quatt, 4 * sizeof(float)) ; float rott = 0;float quatVFlip[4];getHFlip(PI, quatVFlip) ; float* quatVFlip2 = new float[4];getVFlip(rott, quatVFlip2) ; float quatVF[4];quatMult(quatVF, otherPredQuat, quatVFlip) ; api.setQuatTarget(quatVF) ; float* newTargetHook = coordConvert(targetLocal, false) ; memcpy(target, newTargetHook, 3 * sizeof(float)) ; goTo(target, 0.0) ; if (game.getGamePhase() > 3) { step++ ; } } if (step == 5) { float zeros[3];memset(zeros, 0.0, 3 * sizeof(float)) ; if (otherState[1] < 0) { zeros[2] = -0.1 ; api.setAttRateTarget(zeros) ; zeros[2] = 0.0;zeros[1] = 10.0 ; api.setForces(zeros) ; } else { zeros[2] = -0.05;float niceVel[3] ; niceVel[1] = -myState[6 + 1] * 2.0 ; niceVel[2] = -myState[6 + 2] * 2.0 ; niceVel[0] = -myState[6 + 0] * 2.0 ; api.setTorques(zeros);api.setForces(niceVel) ; } } if (time == 81) { DEBUG(("Q: How did the satellite cross the debris field?")) ; DEBUG(("A: float forces[3];")) ; DEBUG(("\tmemset(zeros,0.0,3*sizeof(float));")) ; DEBUG(("\tforces[1]=1.0;")) ; DEBUG(("\tapi.setForces(forces);")) ; } time++ ; } //End page main