//Begin page Utility //Begin page util void vecMultiply(float vec[4], float scale, int n = 3) { for(int i = 0; i < n; i++) { vec[i] *= scale; } } float distPoints(float x[4], float y[4], int i = 3) { float aux[4]; mathVecSubtract(aux, x, y, i); return mathVecMagnitude(aux, i); } float quatAlignment(float quat1[4], float quat2[4]) { float dQuatAdd[4], dQuatSubtract[4]; mathVecAdd(dQuatAdd, quat1, quat2, 4); mathVecSubtract(dQuatSubtract, quat1, quat2, 4); float magDQuatS = mathVecMagnitude(dQuatSubtract, 4); float magDQuatA = mathVecMagnitude(dQuatAdd, 4); if(magDQuatS < magDQuatA) { magDQuatA = magDQuatS; } return magDQuatA; } void quatBetween(float quat[4], float vec1[3], float vec2[3]) //Use only normalized vectors, not handling 180 degrees cases { //vec1 is the one which will be moved to align with vec2 mathVecCross(quat, vec1, vec2); //quat[3] = sqrt( pow( mathVecMagnitude(vec1, 3), 2) * pow( mathVecMagnitude(vec2, 3), 2) ) + mathVecInner(vec1, vec2, 3); quat[3] = 1.0f + mathVecInner(vec1, vec2, 3); mathVecNormalize(quat, 4); // Quaternion q; // vector a = crossproduct(v1, v2); // q.xyz = a; // q.w = sqrt((v1.Length ^ 2) * (v2.Length ^ 2)) + dotproduct(v1, v2); } //End page util //End page Utility //Begin page main //Begin page main #define RADIUS 0.10675f #define R_HOOK_CENTER 0.17095f #define R_HOOK_WIDTH 0.008f //These contain the telemetry from our SPHERE, and the OtherSPHERE (1) float pos[3], att[3], quat[4], vel[3], rot[3], pos1[3], att1[3], quat1[4], vel1[3], rot1[3], upAtt[3], upAtt1[3], rightAtt[3], rightAtt1[3]; float null[3]; //NULL vector of course float waypoint[3]; float hookDist; //Variable used for the difference of distance between the hooks --- more detail in the function -> hooking int hookStep; //Multi-purpose variable -> hooking void init() { DEBUG(("AIR ALLIANCE V1.0 HOOK N TUG ONLY")); //game.enableHookGT(); null[0] = null[1] = null[2] = 0.0f; //Init of the null vector DUH hookStep = 0; hookDist = 0.0f; } void loop(){ float myZRState[12],otherZRState[12]; api.getMyZRState(myZRState); api.getOtherZRState(otherZRState); // Do NOT change these variables, only copy them memcpy(pos, myZRState, sizeof(float)*3); memcpy(vel, myZRState + 3, sizeof(float)*3); memcpy(att, myZRState + 6, sizeof(float)*3); memcpy(rot, myZRState + 9, sizeof(float)*3); memcpy(pos1, otherZRState, sizeof(float)*3); memcpy(vel1, otherZRState + 3, sizeof(float)*3); memcpy(att1, otherZRState + 6, sizeof(float)*3); memcpy(rot1, otherZRState + 9, sizeof(float)*3); float mySphState[13], otherSphState[13]; api.getMySphState(mySphState); api.getOtherSphState(otherSphState); memcpy(quat, mySphState + 6, sizeof(float) * 4); memcpy(quat1, otherSphState + 6, sizeof(float) * 4); float zVec[3] = {0.0f, 0.0f, 1.0f}; api.quat2AttVec(zVec, quat, upAtt); api.quat2AttVec(zVec, quat1, upAtt1); float yVec[3] = {0.0f, 1.0f, 0.0f}; api.quat2AttVec(yVec, quat, rightAtt); api.quat2AttVec(yVec, quat1, rightAtt1); //Up until this point, all this code stores the telemetry of both satellites in the specific variables if(game.getGamePhase() == 3) { if(pos[1] > 0.4f) { float force[3] = {0.0f, -1.0f, 0.0f}; api.setForces(force); }else if(vel[1] < -0.4f) { float force[3]; mathVecSubtract(force, waypoint, pos, 3); force[1] = 0.3f; mathVecNormalize(force, 3); api.setForces(force); DEBUG(("STOP IN THE NAME OF THE LAW")); } // if(vel[1] < -0.4f) // { // float force[3] = {0.0f, 1.0f, 0.0f}; // api.setForces(force); // } } float quatReverse[4] = {0.0f, 0.0f, 1.0f, 0.0f}, quatF[4]; //quatF is the final orientation the blue satellite will try to achieve //quatReverse is the rotation the blue satellite has to make in order for it to be aligned properly quatMult(quatF, quat1, quatReverse); //When you see this function, there is basically a new rotation added to it //We chose quaternions bc of this feature they have over euler angles float posRedHookCenter[3]; // Copying the variable to keep the main telemetry variables untouched memcpy(posRedHookCenter, att1, sizeof(float) * 3); // Maybe useful for later vecMultiply(posRedHookCenter, R_HOOK_CENTER); mathVecAdd(posRedHookCenter, posRedHookCenter, pos1, 3); float posBlueHookCenter[3]; // Copying the variable to keep the main telemetry variables untouched memcpy(posBlueHookCenter, att, sizeof(float) * 3); // Maybe useful for later vecMultiply(posBlueHookCenter, R_HOOK_CENTER); mathVecAdd(posBlueHookCenter, posBlueHookCenter, pos, 3); float redAtt[3]; //Copying the variable to keep the main telemetry variables untouched memcpy(redAtt, att1, sizeof(float) * 3); vecMultiply(redAtt, 2 * R_HOOK_CENTER + hookDist); //Using this as a util function --- explained below mathVecAdd(waypoint, redAtt, pos1, 3); //Storing in the waypoint variable where the blue satellite should be --- at a distance hookDist from the edges of the hooks //The value used before is a constant that represents twice the amount of the distance between the center of the sphere and the end of the hook //Keep in mind that hookDist can take negative values ---the two satellites complete hooking when hookDist == 0.0f if(game.getGamePhase() == 3) { float posDistHooks[3]; mathVecSubtract(posDistHooks, posRedHookCenter, posBlueHookCenter, 3); float distHooks; distHooks = distPoints(posBlueHookCenter, posRedHookCenter); if(quatAlignment(quat, quatF) < 0.06f) { if(distHooks < 0.01f) //Used for hooking { float velocity[3]; memcpy(velocity, posDistHooks, sizeof(float) * 3); vecMultiply(velocity, 0.05f); api.setVelocityTarget(velocity); DEBUG(("PLEASE HOOK")); } if(distHooks < 0.035f && distPoints(vel, vel1) > 0.0015f) //Used for stopping { float forceStabilize[3]; //memcpy(forceStabilize, vel, sizeof(float) * 3); mathVecSubtract(forceStabilize, vel, vel1, 3); vecMultiply(forceStabilize, -1.5f); DEBUG(("STAHP")); api.setForces(forceStabilize); } if(distHooks < 0.2f && distHooks > 0.035f) //Used for speed { float velocity[3]; memcpy(velocity, posDistHooks, sizeof(float) * 3); vecMultiply(velocity, 0.1f); api.setVelocityTarget(velocity); DEBUG(("PLEASE HOOK FOR N00BZ")); float forceDeltaY[3]; memcpy(forceDeltaY, rightAtt1, sizeof(float) * 3); vecMultiply(forceDeltaY, mathVecInner(rightAtt1, posDistHooks, 3)); if(mathVecMagnitude(forceDeltaY, 3) > 0.01f) { vecMultiply(forceDeltaY, 0.2f); api.setForces(forceDeltaY); DEBUG(("GO WHERE YOU ARE SUPPOSED TO")); } } } api.setPositionTarget(waypoint); DEBUG(("DISTANCE BETWEEN HOOK CENTERS: %f%", distPoints(posBlueHookCenter, posRedHookCenter) )); api.setQuatTarget(quatF); } if(game.getGamePhase() == 4) { if(pos1[1] > -0.3f) { float force[3] = {0.0f, 0.0f, 1.0f}, rotRate[3] = {1.0f, 0.0f, 0.0f}; if(game.getHooked() == 0) { api.setForces(force); }else { api.setAttRateTarget(rotRate); } }else { float force[3] = {0.0f, 1.0f, 0.0f}; api.setForces(force); } DEBUG(("HOORAY %f-%f-%f",pos1[0],pos1[1],pos1[2])); } } //End page main //End page main