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;
}
}
/* Assumptions:
* Enemy plane has a properly initialized field
* Description:
* All calculations are handeled by the ForceField class, so this function
* retrieves the ForceField associated with a plane and then calls the
* appropriate method to determine whether or not a point is in a specific
* field
*/
bool fsquared::inEnemyField(AU_UAV_ROS::PlaneObject &enemy, fsquared::relativeCoordinates locationOfMe, double fieldAngle, double planeAngle){
ForceField * enemyField = enemy.getField();
return enemyField->areCoordinatesInMyField(locationOfMe, fieldAngle, planeAngle);
}