AU_UAV_ROS::mathVector fsquared::calculateRepulsiveForce(AU_UAV_ROS::PlaneObject &me, AU_UAV_ROS::PlaneObject &enemy){
double fieldAngle, planeAngle, rMagnitude, rAngle;
fsquared::relativeCoordinates relativePosition;
bool insideEnemyField;
fieldAngle = fsquared::findFieldAngle(me, enemy);
//find the angle from enemy's position to "me"s position
planeAngle = enemy.findAngle(me);
//find "me" coordinates from enemy's POV
relativePosition = fsquared::findRelativePosition(me, enemy);
//determine whether or not "me" is in enemy's field
insideEnemyField = fsquared::inEnemyField(enemy, relativePosition, fieldAngle, planeAngle);
//if "me" is in enemy field
if(insideEnemyField){
//calculate the force exerted by the field on "me"
rMagnitude = enemy.getField()->findForceMagnitude(relativePosition);
//calculate the angle of the force exerted by the field onto me
rAngle = planeAngle; //changed from toCartesian(planeAngle - 180) to planeAngle
AU_UAV_ROS::mathVector repulsiveForceVector(rMagnitude, rAngle);
return repulsiveForceVector;
}
//"me" is not in the enemy's field, return a vector with 0 magnitude (no contribution to force)
else{
AU_UAV_ROS::mathVector repulsiveForceVector(0,0);
return repulsiveForceVector;
}
}
/*
* findFieldAngle
* Preconditions: assumes valid planes
* params: me: plane that is potentially in enemy's field
* enemy: plane that is producing field
* returns: field angle - angle between enemy's bearing and my location.
* 0 < x < 180 = to enemy's right
* -180< x < 0= to enemy's left
*
* note: Different from AU_UAV_ROS::PlaneObject::findAngle(). FindAngle finds
* the angle between the relative position vector from one plane to another and the
* x axis in a global, absolute x/y coordinate system based on latitude and
* longitude.
*/
double fsquared::findFieldAngle(AU_UAV_ROS::PlaneObject& me, AU_UAV_ROS::PlaneObject &enemy) {
//Make two vectors - one representing bearing of enemy plane
// the other representing relative position from
// enemy to me
AU_UAV_ROS::mathVector enemyBearing(1, toCartesian(enemy.getCurrentBearing()));
AU_UAV_ROS::mathVector positionToMe(1, enemy.findAngle(me));
//Find angle between two vectors
return enemyBearing.findAngleBetween(positionToMe);
}