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;
}
}
static int getInTime (float cother[3], float other[12], float c[3], float state[12])
{
//BEGIN::PROC::getInTime
/*
* getInTime () returns a boolean
* value, which will be false if our
* opponent is able to reach his mining
* station before us, true otherwise
*/
float targetother[3];
float target[3];
float v1[3];
float v2[3];
mathVecSubtract(targetother,cother,other,3);
mathVecSubtract(target,c,state,3);
mathVecMul (other+3,other+3,2.05f);
mathVecMul (state+3,other+3,2.05f);
mathVecSubtract (v1,targetother,other+3,3);
mathVecSubtract (v2,target,state+3,3);
return ((mathVecMagnitude(v1, 3)+ 0.14f)>(mathVecMagnitude(v2, 3)));
//END::PROC::getInTime
}
int CheckPath(float WayPoint[3]) //checks if any debris is on path to a point
{
int DebID;
float Path[3];
mathVecSubtract(Path, WayPoint, self, 3);
mathVecNormalize(Path, 3);
float VectorBetween[3];
for(DebID = 0; DebID < 16; DebID++)
{
if(!(game.haveDebris(0, DebID) == true || game.haveDebris(1, DebID) == true))
{
mathVecSubtract(VectorBetween, debrisArr[DebID], self, 3);
if(dotproduct(Path, VectorBetween) >= 0)
{
float PathProjection[3] = {dotproduct(Path,VectorBetween) * Path[0],dotproduct(Path,VectorBetween) * Path[1],dotproduct(Path,VectorBetween)* Path[2]};
mathVecSubtract(PathProjection, VectorBetween, PathProjection, 3);
if(mathVecMagnitude(PathProjection, 3) < 0.15f)
{
return DebID;
}
}
}
}
return -1;
}
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;
}
}
float minDistanceFromOrigin(float target[]) {
float proj[3], meToTarget[3], testPoint[3];
mathVecSubtract(meToTarget, target, me, 3);
mathVecProject(proj, me, meToTarget, 3);
mathVecSubtract(testPoint, me, proj, 3);
return mathVecMagnitude(testPoint,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);
}
void loop(){
api.getMyZRState(me);
api.getOtherZRState(other);
for(int i = 0; i < 9; i++){
if(itemBool[i] && game.hasItem(i) != -1){
itemBool[i] = 0;
}
}
switch(state) {
case 0:
mathVecSubtract(facing,other,me,3);
mathVecNormalize(facing,3);
api.setAttitudeTarget(facing);
//api.setPositionTarget(target);
if(game.isCameraOn() && game.posInLight(other) && game.isFacingOther()){
DEBUG(("TAKING PICTURES"));
game.takePic();
}
if(game.getMemoryFilled() == 2)
state = 1;
break;
case 1:
game.uploadPics();
if(game.getMemoryFilled() == 0)
state = 0;
break;
case 2:
mathVecSubtract(facing,target,other,3);
mathVecNormalize(facing,2);
api.setAttitudeTarget(facing);
setPositionTarget(target,3);
if(distance(me,target) < 0.1){
setPositionTarget(target,8);
}
if(distance(me,target) < 0.03){
state = 0;
target[0] = 0.0;
target[1] = 0.6;
target[2] = 0.0;
}
break;
}
}
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;
}
void goToTarget(float target[]){
api.getMyZRState(me);
switch (state){
case 0: //initializing
if(pathToTarget(target,waypoint)){
float temp[3];
while(pathToTarget(waypoint, temp)){
for(int i = 0; i < 3; i++){
waypoint[i] = waypoint[i] + 0.05;
}
}
mathVecSubtract(originalVecBetween, waypoint, me, 3);
state = 1;
}
else{
state = 2;
}
break;
case 1:
mathVecSubtract(vecBetween, waypoint, me, 3);
float temp[3];
if(pathToTarget(target,temp)){
api.setVelocityTarget(originalVecBetween);
//magnitude stays the same as beginning magnitude
DEBUG(("Going to waypoint"));
DEBUG(("%f",mathVecMagnitude(originalVecBetween,3)));
}
else{
DEBUG(("going to target"));
api.setPositionTarget(target);
state = 2;
}
break;
case 2:
if(distance(me,target)>0.08){
haulAssTowardTarget(target,2);
DEBUG(("Going to Target haulAss"));
}
else{
api.setPositionTarget(target);
}
break;
}
}
void moveTo(float target[3]){
float cross[3];
float sub[3];
float o = zrstate[0] * target[0] + zrstate[1] * target[1] + zrstate[2] * target[2];
mathVecCross(cross,zrstate,target);
mathVecSubtract(sub,zrstate,target,3);
float d = mathVecMagnitude(cross,3) / mathVecMagnitude(sub,3);
if (d > 0.33 || o > 0.1){ //change o > _ to get difference tolerances
api.setPositionTarget(target);
}else{
float tmp[3];
float loc[3];
loc[0] = zrstate[0];
loc[1] = zrstate[1];
loc[2] = zrstate[2];
//mathVecAdd(tmp,target,?,3);
findTangPoint(tmp,loc,target,0.33);
DEBUG(("COLLISION COURSE -> NEW COURSE (%f, %f, %f)\n",tmp[0],tmp[1],tmp[2]));
mathVecNormalize(sub,3);
int i = 0;
for (;i < 3;i++){
sub[i] = 0.2 * sub[i];
}
mathVecAdd(tmp,tmp,sub,3);
api.setPositionTarget(tmp);
}
}
//User-defined procedures
static float checkTarget (float other[12], int n)
{
//BEGIN::PROC::checkTarget
/*
* checkTarget returns a value
* between 1 and 2: an high
* value means that the player
* "other" is headed towards
* the point "c".
*/
float v[3]={0,0,0};
float a[3];
float c[4][3]= { {-0.5f,+0.31f,-0.55f},
{+0.5f,-0.31f,+0.55f},
{0,-0.35f,-0.2f},
{0,0.35f,0.2f}
};
mathVecAdd (v,v,other+3,3);
mathVecSubtract (a,c[n],other,3);
mathVecNormalize(v, 3);
mathVecNormalize(a, 3);
mathVecAdd (a,a,v,3);
return mathVecMagnitude (a,3);
//END::PROC::checkTarget
}
//--------------------------------------HELPER METHODS-------------------------------------------//
void facePos(float target[3], float myPos[3]){
//Rotate to target
float attTarget[3];
mathVecSubtract(attTarget,target,myPos,3);
mathVecNormalize(attTarget,3);
api.setAttitudeTarget(attTarget);
if (distanceVec(myPos, target)< 0.50)
{
//DEBUG(("The SPHERE is close enough to take a picture. ")); //
if (game.alignLine(targetPOI)==true)
{
if (timeSinceArrival > 4) //5 seconds between pictures, 5 seconds after arriving at POI
{
DEBUG(("\n Pictures: %d", picturesTaken+1)); //
game.takePic(targetPOI);
picturesTaken++;
timeSinceArrival = 0;
}
timeSinceArrival++;
}
}
//return distance(attTarget, myPos);
}
static int revDir (float state[12], float c[3])
{
//BEGIN::PROC::revDir
/*
* revDir () returns a value
* between -1, 0 and 1:
* if 0 is returned, our
* opponent is not revolving,
* otherwise the number returned
* is related to the direction
* of the revolving (clockwise
* or counterclockwise)
*/
float asse[3];
float direction[3];
float result[3];
float directionAsteroid[3];
PgetAsteroidNormal(asse);
mathVecCross(direction, state+3, asse);
mathVecNormalize(direction,3);
mathVecSubtract(directionAsteroid, c, state,3);
mathVecNormalize(directionAsteroid,3);
mathVecAdd(result, direction, directionAsteroid,3);
if(mathVecMagnitude(result, 3) > 1.85) {
return 1;
}
else if(mathVecMagnitude(result, 3) < 0.15){
return -1;
}
else
return 0;
//END::PROC::revDir
}
void moveTo(float target[]) {
float disp[3];
float dist, speed;
mathVecSubtract(disp, target, me, 3);
dist = mathVecNormalize(disp, 3); //disp normalized to unit vector, holds direction of desired velocity
/* Condition here is based off of equation: d = (1/2)at^2 + vt
* a = 0.01, which is the approx maximum acceleration I have experimentally found of a SPHERE
* I assume t = 8s, which works and is faster then api.setPositionTarget
* You can change t if there is an overshoot
*/
if (dist > 0.5*0.01*64+mathVecMagnitude(me+3,3)*8) {
speed = dist;
for(n = 0; n < 3; n++) { //scale velocity (disp) to speed
disp[n] *= speed;
}
api.setVelocityTarget(disp);
}
else {
api.setPositionTarget(target);
}
}
void calcNewTarget(float newTarget[], float target[]) {
float magMe, meToTarget[3], normal[3], theta, rotationMatrix[3][3], test1[3], test2[3];
magMe = mathVecMagnitude(me, 3);
mathVecSubtract(meToTarget, target, me, 3);
mathVecCross(normal, me, meToTarget);
// normal to the plane containing Me, target, and origin
mathVecNormalize(normal, 3);
// normalize the normal
theta = asinf((0.33 - minDistanceFromOrigin(target)) / sqrtf(magMe * magMe - 0.33 * 0.33));
// angle between meToTarget and "me to tangent point"
DEBUG(("theta = %f radians\n", theta)); // THIS IS THE ERROR!!!
mathVecRotate(rotationMatrix, normal, theta);
DEBUG(("rotationMatrix = {{%f, %f, %f}, {%f, %f, %f}, {%f, %f, %f}}\n",
rotationMatrix[0][0], rotationMatrix[0][1], rotationMatrix[0][2],
rotationMatrix[1][0], rotationMatrix[1][1], rotationMatrix[1][2],
rotationMatrix[2][0], rotationMatrix[2][1], rotationMatrix[2][2]));
// creates the rotation matrix about the normal by an angle of theta
mathMatVecMult(test1, rotationMatrix, meToTarget);
// rotates meToTarget by an angle theta
mathVecAdd(test1, test1, me, 3);
mathVecRotate(rotationMatrix, normal, -theta);
// creates the rotation matrix about the normal by the opposite angle
mathMatVecMult(test2, rotationMatrix, meToTarget);
// rotates meToTarget by an angle theta
mathVecAdd(test2, test2, me, 3);
if (minDistanceFromOrigin(test1) > 0.32) {
for (int i = 0; i < 3; i++) newTarget[i] = test1[i];
}
else {
for (int i = 0; i < 3; i++) newTarget[i] = test2[i];
}
}
void calcularattitude(){
float vect[3];
game.getPOILoc(posicionPOI, PoiID);
api.getMyZRState(posicionSatelite);
mathVecSubtract(norm,posicionPOI,posicionSatelite,3);
mathVecNormalize(norm,3);
DEBUG(("Vector calculado.\n"));
}
float facePos(float target[3]){ //Points sphere to passed target
//Rotate to target
float attTarget[3];
mathVecSubtract(attTarget,target,myState,3);
mathVecNormalize(attTarget,3);
api.setAttitudeTarget(attTarget);
return distance(attTarget,&myState[6]);
}
void setPositionTarget(float target[3], float multiplier) {
float temp[3];
mathVecSubtract(temp,target,me,3);
for (int i = 0; i < 3; i++) temp[i] = me[i] + temp[i] * multiplier;
setPositionTarget(temp);
DEBUG(("Hauling ass! (*%.1f)\n", multiplier));
}
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;
}
}
void setPositionTarget(float target[3]) {
float temp[3];
if (minDistanceIfDilated(target,me) < 0.01) {
//if target is really close to me or a dilation of me
api.setPositionTarget(target);
//avoids divide-by-0 error when finding angle between me and target
}
else if (minDistanceFromOrigin(target) > 0.32) {
api.setPositionTarget(target);
}
else if (mathVecMagnitude(me,3) < 0.32) {
for (int i = 0; i < 3; i++) temp[i] = me[i] * 0.6 / mathVecMagnitude(me,3);
api.setPositionTarget(temp);
DEBUG(("DANGER! Asteroid Collision Imminent!\n"));
}
else {
float newTarget[3];
mathVecProject(temp,target,me,3);
mathVecSubtract(temp,target,temp,3);
//temp is orthogonal to me and located in the plane containing me and target
for (int i = 0; i < 3; i++) {
newTarget[i] = temp[i] / mathVecMagnitude(temp,3);
newTarget[i] *= sqrtf(1 / (1/(0.35 * 0.35) - 1/(mathVecMagnitude(me,3) * mathVecMagnitude(me,3))));
}
//newTarget is temp resized so that the altitude of right triangle Me-Origin-newTarget is 0.35
mathVecSubtract(temp,newTarget,me,3);
//temp goes from me to newTarget
mathVecSubtract(newTarget,target,me,3);
for (int i = 0; i < 3; i++) temp[i] *= mathVecMagnitude(newTarget,3);
//temp is resized so that it is as long as the original distance from me to target
mathVecAdd(newTarget,me,temp,3);
api.setPositionTarget(newTarget);
DEBUG(("Goddammit, there's an asteroid in the way!\n"));
}
}
void drift(float target[3], float speed) {
float diff[3];
mathVecSubtract(diff,target,self,3);
mathVecNormalize(diff,3);
for(int i = 0; i < 3; i ++) {
diff[i] *= speed;
}
api.setVelocityTarget(diff);
}
void speedMove(float loc[3],float tarSpeed, ZRState myZRState) {
float velTar[3];
mathVecSubtract(velTar, loc, myZRState, 3);
float ratio = tarSpeed / mathVecMagnitude(velTar, 3);
velTar[0] *= ratio;
velTar[1] *= ratio;
velTar[2] *= ratio;
api.setVelocityTarget(velTar);
}
void moveFast(float target[3]) {
if (dist(me, target) > 0.49) {
mathVecSubtract(vecBtwn, target, me, 3);
distance = mathVecMagnitude(vecBtwn, 3);
api.setVelocityTarget(vecBtwn);
}
else {
api.setPositionTarget(target);
}
}
/* 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 isInShot(){
float currentAimVector[3];
currentAimVector[0] = self[6];
currentAimVector[1] = self[7];
currentAimVector[2] = self[8];
mathVecNormalize(currentAimVector, 3);
float VectorBetween[3];
mathVecSubtract(VectorBetween, arbState, self, 3);
if(dotproduct(currentAimVector, VectorBetween) >= 0)
{
float PathProjection[3] = {dotproduct(currentAimVector,VectorBetween) * currentAimVector[0],dotproduct(currentAimVector,VectorBetween) * currentAimVector[1],dotproduct(currentAimVector,VectorBetween)* currentAimVector[2]};
mathVecSubtract(PathProjection, VectorBetween, PathProjection, 3);
if(mathVecMagnitude(PathProjection, 3) < 0.07f)
{
return true;
}
}
return false;
}
float minDistanceFromOrigin(float target[]) {
float temp[3] = {0,0,0}; //temp is the origin
if (cosf(angle(temp,myState,target)) < 0) { //going away from origin
return mathVecMagnitude(myState, 3);
}
else if (cosf(angle(temp,target,myState)) < 0) { //going in direction of origin
return mathVecMagnitude(target,3);
}
else {
mathVecSubtract(temp,target,myState,3);
mathVecProject(temp,myState,temp,3);
mathVecSubtract(temp,myState,temp,3);
return mathVecMagnitude(temp,3);
}
}
void setTargetAngVel(vec targetAngVel) {
vec torques;
mathVecSubtract(torques, targetAngVel, stateAngVel(myState), 3);
vecScale(torques, floatOne / accPerNM);
#if printDebug
DEBUG(("Angular Velocity Targeting Info\n"));
printVec("\tTarget AngVel (rad/s)", targetAngVel);
printVec("\tTorques (rad/s^2)", torques);
#endif
api.setTorques(torques);
}
// This must be called at the start of every loop.
void movementStart() {
// Setting motion data:
api.getMyZRState(ZRState);
//api.getOtherZRState(OZRState);
for (int i = 0; i < 3; i++) {
position[i] = ZRState[i];
velocity[i] = ZRState[i + 3];
}
mathVecSubtract(acceleration, velocity, lastVelocity, 3);
updateMass();
lastForceMagnitude = 0.f; // Very important in mass calculation [Positioning in loop included].
}
bool willCollide(int timeInFuture){
//Calculate future other position(velocity*time+currentState)
float futurePos[3];
for(int i=0;i<3;i++){
futurePos[i] = otherState[i+3]*timeInFuture+otherState[i];
}
//Check magnitude between otherFutureState and current State within collision sphere
mathVecSubtract(tempVec,futurePos,myState,3);
if(mathVecMagnitude(tempVec,3)<.15f)
return true;
return false;
}
//Joel
void adjustGoal(float target[], float stoppingDist){
float d = distance(me, target);
//DEBUG(("item location %f, %f", items[id][0], items[id][1]));
float tempTar[3];
memcpy(tempTar, target, 3*sizeof(float));
mathVecSubtract(holder, tempTar, me, 3);
//DEBUG(("item location %f, %f", items[id][0], items[id][1]));
for (n = 0; n < 3; n++){
holder[n] -= holder[n] * (stoppingDist / d);
holder[n] += me[n];
}
}
