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;
	}
}
예제 #2
0
/* Copy constructor */
AU_UAV_ROS::PlaneObject::PlaneObject(const AU_UAV_ROS::PlaneObject& pobj):
		CObject(pobj.getLatitude(), pobj.getLongitude(), pobj.getCollisionRadius()){
	this->id = pobj.id;
	this->altitude = pobj.altitude;
	this->bearing = pobj.bearing;
	this->actualBearing = pobj.actualBearing;
	this->speed = pobj.speed;
	this->destination = pobj.destination;
	this->lastUpdateTime = pobj.lastUpdateTime;
}
/*
 * 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); 
}
예제 #4
0
/* Copy constructor */
AU_UAV_ROS::PlaneObject::PlaneObject(const AU_UAV_ROS::PlaneObject& pobj):
		CObject(pobj.getLatitude(), pobj.getLongitude(), pobj.getCollisionRadius()){
	this->id = pobj.id;
	this->altitude = pobj.altitude;
	this->bearing = pobj.bearing;
	this->actualBearing = pobj.actualBearing;
	this->speed = pobj.speed;
	this->destination = pobj.destination;
	this->lastUpdateTime = pobj.lastUpdateTime;
	this->tMinus1Lat = pobj.tMinus1Lat;
	this->tMinus1Lon = pobj.tMinus1Lon;
	this->tMinus2Lat = pobj.tMinus2Lat;
	this->tMinus2Lon = pobj.tMinus2Lon;
	this->leader = pobj.leader;
	this->follower = pobj.follower;
	this->checked = pobj.checked;
	this->flock = pobj.flock;
}
AU_UAV_ROS::mathVector fsquared::calculateAttractiveForce(AU_UAV_ROS::PlaneObject &me, AU_UAV_ROS::waypoint goal_wp){
	double aAngle, aMagnitude, destLat, destLon, currentLat, currentLon;
	//obtain current location by accessing PlaneObject's current coordinate
	currentLat = me.getCurrentLoc().latitude;
	currentLon = me.getCurrentLoc().longitude;
	destLat = goal_wp.latitude;
	destLon = goal_wp.longitude;
	aAngle = toCartesian(findAngle(currentLat, currentLon, destLat, destLon));
	aMagnitude = ATTRACTIVE_FORCE;
	//construct the attractrive force vector and return it
	AU_UAV_ROS::mathVector attractiveForceVector(aMagnitude, aAngle);
	return attractiveForceVector;
}
/*
 *Precondition: Assume valid planes
 *Use: Find "me's" position from enemy's POV
 *Params:
 *		me: Plane that is potentially in enemy's field
 *		enemy: Plane that is producing the field
 *Returns:	relativeCoordinates in meters of "me" from the enemy's POV, where enemy's bearing is towards the positive y axis.
 *Implementation:
 *			
 *who:		vw
*/
fsquared::relativeCoordinates fsquared::findRelativePosition(AU_UAV_ROS::PlaneObject &me, AU_UAV_ROS::PlaneObject &enemy ){
	fsquared::relativeCoordinates loc;

	double distance = enemy.findDistance(me);
	double fieldAngle = fsquared::findFieldAngle(me, enemy);

	//Find Y axis coordinate (in front or behind enemey)
	loc.y = cos(fieldAngle*PI/180.0)*distance;

	//Find X Axis coordinate (to the left or right)
	loc.x = sin(fieldAngle*PI/180.0)*distance;

	return loc;
}
/* 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);
}
예제 #8
0
/* Find angle between two PlaneObjects.  The calling plane gives the starting latitude and longitude, 
and the object passed as a parameter gives the final latitude and longitude. */
double AU_UAV_ROS::PlaneObject::findAngle(const AU_UAV_ROS::PlaneObject& pobj) const {
	return AU_UAV_ROS::CObject::findAngle(pobj.getLatitude(), pobj.getLongitude()); /* call inherited function */
}