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; } }
/* 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); }
/* 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); }
/* 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 */ }