//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
}
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];
}
}
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[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);
}
}
static void SetPosFaster (float c[3], float state[12])
{
//BEGIN::PROC::SetPosFaster
/*
* SetPosFaster, as the name says,
* is used to reach a point in
* a shorter amount of seconds
* than ZRSetPositionTarget ().
*/
float v[3];
mathVecSubtract(v,c,state,3);
mathVecMul (v,v,1.315f);
mathVecAdd (v,v,state,3);
ZRSetPositionTarget(v);
//END::PROC::SetPosFaster
}
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"));
}
}
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);
}
}
static void orbit (float myState[12], float center[3], unsigned char CCW)
{
//BEGIN::PROC::orbit
float difference[3], target[3];
float theta;
float thetastep = .66;
mathVecSubtract(difference, myState, center, 3);
theta = atan2f(difference[1], difference[0]);
if(CCW)
thetastep *= -1;
theta += thetastep;
difference[0] = .4 * sinf(PI/2 - theta);
difference[1] = .4 * sinf(theta);
mathVecAdd(target, center, difference, 3);
ZRSetPositionTarget(target);
//END::PROC::orbit
}
void findTangPoint(float result[3], float point[3],float targ[3], float r) {
float DNorms[3];
float D = mathVecMagnitude(point,3);
mathVecNormalize(point,3);
mathVecNormalize(targ,3);
mathVecSubtract(DNorms,targ,point,3);
mathVecNormalize(DNorms,3);
float alpha = acosf(r/D);
float beta = angleBetween(point,DNorms);
float theta = beta - alpha;
float dist = sinf(alpha)/sinf(theta);
DNorms[0] = DNorms[0]*dist;
DNorms[1] = DNorms[1]*dist;
DNorms[2] = DNorms[2]*dist;
mathVecAdd(result,point,DNorms,3);
mathVecNormalize(result,3);
result[0] = result[0] * r;
result[1] = result[1] * r;
result[2] = result[2] * r;
}
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);
}
void setPositionTarget(float target[3], float multiplier) {
api.getMyZRState(me);
float myPos[3],meMag;
memcpy(myPos, me, 3*sizeof(float));
meMag = mathVecMagnitude(myPos,3);
if (minDistanceFromOrigin(target) > 0.32) {
//if (distance(me, target) < 0.6) { // Save braking distance
if(distance(me,target)<0.15){
haulAssTowardTarget(target,2.5);
}
else if(distance(me,target)<0.2){
haulAssTowardTarget(target,1.8);
}
haulAssTowardTarget(target,1.3);
//}
//else { // Or haul ass towards target
// float temp[3];
// mathVecSubtract(temp,target,me,3);
// for (int i = 0 ; i < 3 ; i++) {
// temp[i] = me[i] + temp[i] * multiplier;
// }
// api.setPositionTarget(temp);
//}
DEBUG(("GOING STRAIGHT\n"));
}
else if (meMag >= 0.22 && meMag <= 0.315) {
for (int i = 0; i < 3; i++) {
myPos[i] = myPos[i] * 2;
}
api.setPositionTarget(myPos);
DEBUG(("TOO CLOSE\n"));
}
else {
float opposite[3], perpendicular[3], mePrep[3], path[3], temp[3];
mathVecProject(opposite,target,myPos,3);
mathVecSubtract(perpendicular,target,opposite,3);
for (int i = 0; i < 3; i++) {
mePrep[i] = perpendicular[i] / mathVecMagnitude(perpendicular,3);
}
for (int i = 0; i < 3; i++) {
mePrep[i] = (mePrep[i] * 0.325 * meMag) / (sqrtf(meMag*meMag - 0.315*0.315));
}
mathVecSubtract(path,mePrep,myPos,3);
for (int i = 0; i < 3; i++) {
path[i] = path[i] * multiplier;
}
mathVecAdd(temp,myPos,path,3);
api.setPositionTarget(temp);
DEBUG(("TAKING THE TANGENT\n"));
}
}
/* MAIN ***********************************************************************/
void loop() {
//blah();
updateGameState();
changeState();
if (game.getEnergy() < ENERGY_SHUTDOWN_THRESHOLD) {
api.setVelocityTarget(origin);
return;
}
switch (att_state) {
case FACE_OTHER:
mathVecSubtract(temp, other, me, 3);
api.setAttitudeTarget(temp);
if (checkPhoto()) {
game.takePic();
}
break;
case UPLOAD:
temp[0] = 0.0f;
temp[1] = 0.0f;
temp[2] = 1.0f;
api.setAttitudeTarget(temp);
if (checkUpload()) {
game.uploadPics();
}
break;
}
switch (move_state) {
case GET_SCORE_PACKS:
if (game.getScore() == 0) {
mathVecAdd(temp, me, me + 3, 3); // where we will be
if (!game.posInDark(temp)) {
api.setVelocityTarget(origin); // steady...
}
else {
temp[0] = game.getCurrentTime();
if (temp[0] < 4 || temp[0] > 17) {
api.setPositionTarget(items[itemID]);
}
}
}
else {
api.setPositionTarget(items[itemID]);
}
break;
case END_GAME:
memcpy(temp, other, 3 * sizeof(float));
mathVecNormalize(temp, 3);
mathVecMultiply(temp, -0.3, 3);
mathVecAdd(target, temp, other, 3);
api.setPositionTarget(target);
break;
case TO_ORIGIN:
api.setPositionTarget(origin);
break;
}
}
void loop() {
api.getMyZRState(me);
api.getOtherZRState(other);
currentEnergy = game.getEnergy();
if (state != upload) {
if (currentEnergy < 1.5) {
state = getEnergyState;
}
else if (game.posInArea(other) == 1) {
state = getInPosForPic;
}
else {
state = getItem;
}
}
DEBUG(("%d\n", state));
switch(state) {
case getInPosForPic: // if other in light, point towards other
if (game.getMemoryFilled() == 2) {
state = upload;
}
else if (brakingPt[0] > 40 && brakingPt[1] > 40) {
mathVecSubtract(facing, me, other, 3);
mathVecNormalize(facing, 3);
mathVecMultiply(facing, 0.5);
mathVecAdd(brakingPt, other, facing, 3);
}
else {
mathVecSubtract(facing, other, me, 3);
mathVecNormalize(facing, 3);
api.setPositionTarget(brakingPt);
api.setAttitudeTarget(facing);
if (game.isCameraOn() && game.isFacingOther()) {
DEBUG(("TAKING PIC"));
game.takePic();
}
}
break;
case upload: // upload
game.uploadPics();
if (game.getMemoryFilled() == 0) {
if (game.posInArea(other) == 1) {
state = getInPosForPic;
}
else {
state = getItem;
}
}
brakingPt[0] = 50.0f;
brakingPt[1] = 50.0f;
brakingPt[2] = 50.0f;
break;
case getItem: // if other in dark, go for the point item
targetID = minDistEl(scores, me); // FIXME this doesn't work right
memcpy(target, scores + targetID, 3 * sizeof(float));
if (areWeThereYet(target, me)) {
float tmp[3];
tmp[0] = 0;
tmp[1] = 0;
tmp[2] = 1;
api.setAttRateTarget(tmp); // supposedly will rotate
//if (game.hasItem(targetID + 4) != -1) { // FIXME this should work but does not
// scores[targetID][0] = 99999;
// scores[targetID][1] = 99999;
// scores[targetID][2] = 99999;
//}
}
else {
api.setPositionTarget(target);
}
break;
case getEnergyState: // if low battery, go grab energy pack
targetID = minDistEl(energy, me);
memcpy(target, energy + targetID, 3 * sizeof(float));
if (areWeThereYet(target, me)) {
float tmp[3];
tmp[0] = 0;
tmp[1] = 0;
tmp[2] = 1;
api.setAttRateTarget(tmp); // supposedly will rotate
if (game.hasItem(targetID) != -1) {
energy[targetID][0] = -999;
energy[targetID][1] = -999;
energy[targetID][2] = -999;
}
}
break;
}
}
