/* Find the new collision avoidance waypoint for the plane to go to */
AU_UAV_ROS::waypoint AU_UAV_ROS::calculateWaypoint(PlaneObject &plane1, double turningRadius, bool turnRight){
AU_UAV_ROS::waypoint wp;
double V = MPS_SPEED;
double delta_T = TIME_STEP;
double cartBearing = toCartesian(plane1.getCurrentBearing())* PI/180;
double delta_psi = V / turningRadius * delta_T;
if (turnRight) delta_psi *= -1.0;
ROS_WARN("Delta psi: %f", delta_psi);
double psi = (cartBearing + delta_psi);
wp.longitude = plane1.getCurrentLoc().longitude + V*cos(psi)/DELTA_LON_TO_METERS;
wp.latitude = plane1.getCurrentLoc().latitude + V*sin(psi)/DELTA_LAT_TO_METERS;
wp.altitude = plane1.getCurrentLoc().altitude;
ROS_WARN("Plane%d new cbearing: %f", plane1.getID(), toCardinal( findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude) ) );
//ROS_WARN("Plane%d calc lat: %f lon: %f w/ act lat: %f lon: %f", plane1.getID(), wp.latitude, wp.longitude, plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
return wp;
}
/* Returns true if the original plane (plane1) should turn right to avoid plane2,
false if otherwise. Takes original plane and its greatest threat as parameters. */
bool sim::shouldTurnRight(PlaneObject &plane1, PlaneObject &plane2) {
/* For checking whether the plane should turn right or left */
double theta, theta_dot, R;
double cartBearing1 = toCartesian(plane1.getCurrentBearing());
double cartBearing2 = toCartesian(plane2.getCurrentBearing());
double V = MPS_SPEED;
/* Calculate theta, theta1, and theta2. Theta is the cartesian angle
from 0 degrees (due East) to plane2 (using plane1 as the origin). This
may be referred to as the LOS angle. */
theta = findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
R = findDistance(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
theta_dot = (V*sin((cartBearing2 - theta)*PI/180) - V*sin((cartBearing1 - theta)*PI/180)) / R;
return (theta_dot >= 0);
}
/* Find the new collision avoidance waypoint for the plane to go to */
au_uav_ros::waypoint au_uav_ros::calculateWaypoint(PlaneObject &plane1, double turningRadius, bool turnRight){
au_uav_ros::waypoint wp;
double V = MPS_SPEED * MPS_WAYPOINT_MULTIPLIER;
double delta_T = TIME_STEP;
double cartBearing = plane1.getCurrentBearing()* PI/180;
double delta_psi = V / turningRadius * delta_T;
if (turnRight) delta_psi *= -1.0;
ROS_WARN("Delta psi: %f", delta_psi);
double psi = (cartBearing + delta_psi);
V = V * MPS_WAYPOINT_MULTIPLIER;
wp.longitude = plane1.getCurrentLoc().longitude + V*cos(psi)/DELTA_LON_TO_METERS;
wp.latitude = plane1.getCurrentLoc().latitude + V*sin(psi)/DELTA_LAT_TO_METERS;
ROS_INFO("long+%f, lat+%f, distanceBetween UAV and AvoidWP%f", V*cos(psi)/DELTA_LON_TO_METERS, V*sin(psi)/DELTA_LON_TO_METERS,
distanceBetween(plane1.getCurrentLoc(), wp));
wp.altitude = plane1.getCurrentLoc().altitude;
ROS_WARN("Plane%d new cbearing: %f", plane1.getID(), toCardinal( findAngle(plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude) ) );
//ROS_WARN("Plane%d calc lat: %f lon: %f w/ act lat: %f lon: %f", plane1.getID(), wp.latitude, wp.longitude, plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
return wp;
}
/* This function is calculates any maneuvers that are necessary for the
current plane to avoid looping. Returns a waypoint based on calculations.
If no maneuvers are necessary, then the function returns the current
destination*/
AU_UAV_ROS::waypoint AU_UAV_ROS::takeDubinsPath(PlaneObject &plane1) {
/* Initialize variables*/
AU_UAV_ROS::coordinate circleCenter;
AU_UAV_ROS::waypoint wp = plane1.getDestination();
double minTurningRadius = 0.75*MINIMUM_TURNING_RADIUS;
bool destOnRight;
/* Calculate cartesian angle from plane to waypoint*/
double wpBearing = findAngle(plane1.getCurrentLoc().latitude,
plane1.getCurrentLoc().longitude, wp.latitude, wp.longitude);
/* Calculate cartesian current bearing of plane (currentBearing is stored as Cardinal)*/
double currentBearingCardinal = plane1.getCurrentBearing();
double currentBearingCartesian = toCartesian(currentBearingCardinal);
if (fabs(currentBearingCardinal) < 90.0)
/* Figure out which side of the plane the waypoint is on*/
if ((wpBearing < currentBearingCartesian) &&
(wpBearing > manipulateAngle(currentBearingCartesian - 180.0)))
destOnRight = true;
else destOnRight = false;
else
if ((wpBearing > currentBearingCartesian) &&
(wpBearing < manipulateAngle(currentBearingCartesian - 180.0)))
destOnRight = false;
else destOnRight = true;
/* Calculate the center of the circle of minimum turning radius on the side that the waypoint is on*/
circleCenter = calculateLoopingCircleCenter(plane1, minTurningRadius, destOnRight);
/* If destination is inside circle, must fly opposite direction before we can reach destination*/
if (findDistance(circleCenter.latitude, circleCenter.longitude, wp.latitude, wp.longitude) <
minTurningRadius) {
return calculateWaypoint(plane1, minTurningRadius, !destOnRight);
}
else {
//we can simply pass the current waypoint because it's accessible
//ROS_WARN("FINE: %f", findDistance(circleCenter.latitude, circleCenter.longitude, wp.latitude, wp.longitude));
return wp;
}
}
float computeElongation(float *Px, float *Py, float *Vx, float *Vy, int N) {
// Determine the average direction
float avgVx = mean(Vx, N);
float avgVy = mean(Vy, N);
// Determine the average position (centroid)
float centroidX = mean(Px, N);
float centroidY = mean(Py, N);
// printf("Center position is (%f, %f), average direction is (%f, %f)\n",centroidX, centroidY,avgVx,avgVy);
// Determine the rotation matrix to rotate the points.
//First, determine the angle.
float theta = findAngle(avgVx,avgVy,1.0,0.0);
float R11 = cosf(theta);
float R12 = -sinf(theta);
float R21 = sinf(theta);
float R22 = cosf(theta);
// Rotate the collection of points so that the average direction
// becomes the x-axis.
float *newPx = malloc(sizeof(float)*N);
float *newPy = malloc(sizeof(float)*N);
int i;
for (i=0; i < N; i++) {
newPx[i] = centroidX + (Px[i]-centroidX)*R11 + (Py[i]-centroidY)*R12;
newPy[i] = centroidY + (Px[i]-centroidX)*R21 + (Py[i]-centroidY)*R22;
}
// Now, the axis of direction is X
float xdist = max(newPx,N) - min(newPx,N);
float ydist = max(newPy,N) - min(newPy,N);
float E;
if (ydist == 0)
E = -1.0;
else
E = xdist/ydist;
return E;
} // end computeElongation
static void computeCurve( coOrd pos0, coOrd pos1, double radius, coOrd * center, double * a0, double * a1 )
/* translate between curves described by 2 end-points and a radius to
a curve described by a center, radius and angles.
*/
{
double d, a, aa, aaa, s;
d = findDistance( pos0, pos1 )/2.0;
a = findAngle( pos0, pos1 );
s = fabs(d/radius);
if (s > 1.0)
s = 1.0;
aa = R2D(asin( s ));
if (radius > 0) {
aaa = a + (90.0 - aa);
*a0 = normalizeAngle( aaa + 180.0 );
translate( center, pos0, aaa, radius );
} else {
aaa = a - (90.0 - aa);
*a0 = normalizeAngle( aaa + 180.0 - aa *2.0 );
translate( center, pos0, aaa, -radius );
}
*a1 = aa*2.0;
}
/*
* findTilt() : Angle of tilt calculated with distance
* from corners (max/min_x/y)
*
* findDirection() : Direction of tilt calculated with
* distance from side mid points (max/min_x/y, center_x/y)
*
* min_x, min_y center_x,min_y max_x, min_y
* +---#-----+---------+
* | | |
* | x2,y2 | x1,y1 #
* | | |
* min_x,center_y +---------+---------+ max_x,center_y
* | | |
* # x3,y3 | x4,y4 |
* | | |
* +---------+-----#---+
* min_x, max_y center_x,max_y max_x, max_y
*
*
* # - tilted anchor corner points ( x1/2/3/4, y1/2/3/4 )
*
*/
int
Shape::findTilt()
{
int x1 = center_x, y1 = center_y, p1 = grid_w * grid_h;
int x2 = center_x, y2 = center_y, p2 = grid_w * grid_h;
int x3 = center_x, y3 = center_y, p3 = grid_w * grid_h;
int x4 = center_x, y4 = center_y, p4 = grid_w * grid_h;
int angle = 0, a1=0, a2=0, a3=0, a4=0;
int x=0, y=0, p=0; // p : roximity to corner
int bbx = (max_x - min_x)/3, bby = (max_y - min_y)/3; //inner bounding box
int direction = 1;
if(debug){
cout << endl << "[W " ;
int c = max_y-min_y;
for(int i = 1; i < c/2; i++){
cout << widths_at_y[i] << "~" << widths_at_y[c-i] << " ";
}
cout << "W] " << endl;
cout << endl << "[H " ;
c = max_x-min_x;
for(int i = 1; i < c/2; i++){
cout << heights_at_x[i] << "~" << heights_at_x[c-i] << " ";
}
cout << "H] " << endl;
}
/*
* loop through all border pixels
* only select pixels close to bounding box edge
* (selected by outside inner bounding box)
* for each quandrant measure disance from coner
* closeset to corner for each quandrant is picked as
* the anchor corner point
*/
int i = 0;
while( i < mapcount ){
x = xmap[i];
y = ymap[i];
if( abs(x-center_x) > bbx || abs(y-center_y) > bby){//close to edge
if( x > center_x && y <= center_y ){ //top right Q1
p = (max_x - x) + (y - min_y);
if(p < p1){
p1 = p; x1 = x; y1 = y;
}
}else if( x <= center_x && y <= center_y ){ //top left Q2
p = (x - min_x) + (y - min_y);
if(p < p2){
p2 = p; x2 = x; y2 = y;
}
}else if( x <= center_x && y > center_y ){ //bot left Q3
p = (x - min_x) + (max_y - y);
if(p < p3){
p3 = p; x3 = x; y3 = y;
}
}else if( x > center_x && y > center_y ){ //bot right Q4
p = (max_x - x) + (max_y - y);
if(p < p4){
p4 = p; x4 = x; y4 = y;
}
}
}
i++;
}
a1 = findAngle(x2, y2, x1, y1);
a2 = findAngle(x1, y1, x4, y4);
a3 = findAngle(x4, y4, x3, y3);
a4 = findAngle(x3, y3, x2, y2);
angle = isDiagonal(a1, a2, a3, a4) ? maxAngle(a1, a2, a3, a4) : (a1+a2+a3+a4)/4;
if(angle != 45 ) direction = findDirection(x1, y1, x2, y2, x3, y3, x4, y4);
angle*=direction;
if(debug) cout << "[ " << angle << " | " << a1 << " " << a2 << " " << a3 << " " << a4 << " ]" << endl ;
if( pixdebug ){
d_pixmap->clearPixmap();
d_markAnchor();
d_pixmap->setPen(0, 255, 0);
d_pixmap->markPoint( x1, y1, 6);
d_pixmap->setPen(0, 0, 255);
d_pixmap->markPoint( x4, y4, 6);
d_pixmap->setPen(0, 255, 255);
d_pixmap->markPoint( x3, y3, 6);
d_pixmap->setPen(0, 0, 0);
d_pixmap->markPoint( x2, y2, 6);
d_pixmap->writeImage("corners");
}
return angle;
}
/* Function that returns the ID of the most dangerous neighboring plane and its ZEM */
AU_UAV_ROS::threatContainer AU_UAV_ROS::findGreatestThreat(PlaneObject &plane1, std::map<int, PlaneObject> &planes){
/* Set reference for origin (Northwest corner of the course)*/
AU_UAV_ROS::coordinate origin;
origin.latitude = 32.606573;
origin.longitude = -85.490356;
origin.altitude = 400;
/* Set preliminary plane to avoid as non-existent and most dangerous
ZEM as negative*/
int planeToAvoid = -1;
double mostDangerousZEM = -1;
/* Set the preliminary time-to-go to infinity*/
double minimumTimeToGo = std::numeric_limits<double>::infinity();
/* Declare second plane and ID variable */
PlaneObject plane2;
int ID;
/* Make a position vector representation of the current plane*/
double magnitude2, direction2;
double magnitude = findDistance(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
double direction = findAngle(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
AU_UAV_ROS::mathVector p1(magnitude,direction);
/* Make a heading vector representation of the current plane*/
AU_UAV_ROS::mathVector d1(1.0,toCartesian(plane1.getCurrentBearing()));
/* Declare variables needed for this loop*/
AU_UAV_ROS::mathVector pDiff;
AU_UAV_ROS::mathVector dDiff;
double timeToGo, zeroEffortMiss, distanceBetween, timeToDest;
std::map<int,AU_UAV_ROS::PlaneObject>::iterator it;
for ( it=planes.begin() ; it!= planes.end(); it++ ){
/* Unpacking plane to check*/
ID = (*it).first;
plane2 = (*it).second;
/* If it's not in the Check Zone, check the other plane*/
distanceBetween = plane1.findDistance(plane2);
if (distanceBetween > CHECK_ZONE || plane1.getID() == ID) continue;
else if (distanceBetween < MPS_SPEED) {
planeToAvoid = ID;
mostDangerousZEM = 0;
minimumTimeToGo = 0.1;
break;
}
/* Making a position vector representation of plane2*/
magnitude2 = findDistance(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
direction2 = findAngle(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
AU_UAV_ROS::mathVector p2(magnitude2,direction2);
/* Make a heading vector representation of the current plane*/
AU_UAV_ROS::mathVector d2(1.0,toCartesian(plane2.getCurrentBearing()));
/* Compute Time To Go*/
pDiff = p1-p2;
dDiff = d1-d2;
timeToGo = -1*pDiff.dotProduct(dDiff)/(MPS_SPEED*dDiff.dotProduct(dDiff));
/* Compute Zero Effort Miss*/
zeroEffortMiss = sqrt(pDiff.dotProduct(pDiff) +
2*(MPS_SPEED*timeToGo)*pDiff.dotProduct(dDiff) +
pow(MPS_SPEED*timeToGo,2)*dDiff.dotProduct(dDiff));
/* If the Zero Effort Miss is less than the minimum required
separation, and the time to go is the least so far, then avoid this plane*/
if(zeroEffortMiss <= DANGER_ZEM && timeToGo < minimumTimeToGo && timeToGo > 0){
// If the plane is behind you, don't avoid it
if ( fabs(plane2.findAngle(plane1)*180/PI - toCartesian(plane1.getCurrentBearing())) > 35.0) {
timeToDest = plane1.findDistance(plane1.getDestination().latitude,
plane1.getDestination().longitude) / MPS_SPEED;
/* If you're close to your destination and the other plane isn't
much of a threat, then don't avoid it */
if ( timeToDest < 5.0 && zeroEffortMiss > 3.0*MPS_SPEED ) continue;
planeToAvoid = ID;
mostDangerousZEM = zeroEffortMiss;
minimumTimeToGo = timeToGo;
}
}
}
AU_UAV_ROS::threatContainer greatestThreat;
greatestThreat.planeID = planeToAvoid;
greatestThreat.ZEM = mostDangerousZEM;
greatestThreat.timeToGo = minimumTimeToGo;
return greatestThreat;
}
// uses ear-clipping method, so won't work on polygons with holes, and is relatively slow at O(n^2)
// assumes size of _shape is at least 3
std::vector<std::vector<Util::Vector>> SteerLib::GJK_EPA::decompose(std::vector<Util::Vector> _shape)
{
// list of triangles that _shape decomposed into
std::vector<std::vector<Util::Vector>> triangleList;
// temporary shape that can be modified
std::vector<Util::Vector> tempShape = _shape;
int count = crossProductCount(_shape);
// if absolute value of count is same as size of shape, then shape is convex
if (std::abs(count) == _shape.size()) {
triangleList.push_back(_shape);
return triangleList;
}
bool isCCW;
// if count > 0, shape is ordered ccw
if (count > 0) {
isCCW = true;
}
// if count < 0, shape is ordered cw
else if (count < 0) {
isCCW = false;
}
// start the search at any point
int shapePos = 0;
// loop until there are only 3 points left in tempShape
// the loop will find ears in tempShape, and remove the ear, so the number of points in tempShape slowly decreases
while (tempShape.size() > 0) {
// if shapePos reaches the end, loop back to the beginning
if (shapePos == tempShape.size()) {
shapePos = 0;
}
// get the first point, and the two points adjacent to it
Util::Vector point = tempShape.at(shapePos);
// get the predecessor, if shapePos is 0, get the back of tempShape
Util::Vector predecessor;
if (shapePos == 0) {
predecessor = tempShape.back();
}
else {
predecessor = tempShape.at(shapePos - 1);
}
// get the predecessor, if shapePos is the end, get the front of tempShape
Util::Vector successor;
if (shapePos == tempShape.size()-1) {
successor = tempShape.front();
}
else {
successor = tempShape.at(shapePos + 1);
}
// find the angle of the first point
Util::Vector line1 = predecessor - point;
Util::Vector line2 = successor - point;
float angle = findAngle(line1, line2);
bool angleBool;
// if shape is ccw, angles less than pi are positive
if (isCCW) {
angleBool = angle < M_PI && angle > 0;
}
// if shape is cw, angles less than pi are negative
else {
angleBool = angle > -M_PI && angle < 0;
}
// if the angle is less than pi, it is possible for an ear to exist
if (angleBool) {
// make another temporary shape that does not include the point and it's adjacent points
std::vector<Util::Vector> triangleShape = tempShape;
int remove = indexOf(triangleShape, predecessor);
triangleShape.erase(triangleShape.begin() + remove);
remove = indexOf(triangleShape, point);
triangleShape.erase(triangleShape.begin() + remove);
remove = indexOf(triangleShape, successor);
triangleShape.erase(triangleShape.begin() + remove);
// check if any points in tempShape are in the triangle formed by the point and it's adjacent points
// if no points are in the triangle, then the point and it's adjacent points form an ear
if (!checkTriangle(triangleShape, predecessor, point, successor)) {
// construct the ear
std::vector<Util::Vector> ear;
ear.push_back(predecessor);
ear.push_back(point);
ear.push_back(successor);
// add the ear to the list of decomposed triangles
triangleList.push_back(ear);
// remove the point from tempShape since no other triangles in the decomposition can use it
remove = indexOf(tempShape, point);
tempShape.erase(tempShape.begin() + remove);
}
// there is another point inside the triangle, so it's not an ear
// move on to the next position
else {
shapePos++;
}
}
// if the angle is pi, then the point and it's adjacent points forms a line
// the point can be removed from tempShape
else if (angle == M_PI || angle == -M_PI) {
int remove = indexOf(tempShape, point);
tempShape.erase(tempShape.begin() + remove);
}
// if the angle is greater than pi, it can't be an ear
// move on to the next position
else {
shapePos++;
}
if (tempShape.size() == 3) {
// since there are only 3 points left in tempShape, it must form a triangle in the decomposition
triangleList.push_back(tempShape);
tempShape.clear();
}
}
for (int i = 0; i < triangleList.size(); i++) {
std::cout << "triangle " << i << "\n";
for (int j = 0; j < triangleList.at(i).size(); j++) {
std::cout << "point " << triangleList.at(i).at(j) << "\n";
}
}
return triangleList;
}
// Perform one step of the simulation.
// The current positions and directions are in Px, Py, Vx, and Vy
// There are N fish.
// The ratio of orientation to attraction is r
// The new positions and directions should be placed in
// newPx, newPy, newVx, and newVy.
// All position/direction vectors have been allocated and are of length N.
void fishStep(float *Px, float *Py, float *Vx, float *Vy, int N, float r,
float *newPx, float *newPy, float *newVx, float *newVy, float attractRadius) {
const float REPULSE_FACTOR=1.0f;
const float ATTRACT_FACTOR=attractRadius;
//in this:
//w_o=r, w_a=1. since normalized.
float w_a=1.0, w_o=r;
const float speed = 1.0;
const float tau = 0.2;
const float max_turn=115.0f;
const float blind_angle=10.0f;
int i,j;
//for each "goal" fish:
//calculate vector:
for(i=0;i<N;i++){
//if fish i finds at least one fish in zone of repulsion:
//need to update according to equation:
//v_i(t+\tao)=\sum_j\neq i \frac{c_j(t)-c_i(t)}{|c_j(t)-c_i(t)|}
//where c_j and c_i are actual locations of the fish.
//make a list of indices of fish in that zone(use j as index)
//then ,set new v_x[i] and new v_y[i] according to average direction to all
//the Px[j] and Py[j]
// ELSE:
// if agents are not found within zone of repultion, then it will align
// itself with...(see paper)
// make a list of fish in the zone
// set new vx, new vy using weighted average of positions and directions
// of those fish.
//END::update position
float newx_1=0.0f,newy_1=0.0f;
float newx_7=0.0f,newy_7=0.0f, newvectx_7=0.0f, newvecty_7=0.0f;
int fishInOne=0;
int fishInTwo=0;
for(j=0;j<N;j++){
//test for blind spot. first calc the vector:
float temp_x=Vx[j]-Vx[i];
float temp_y=Vy[j]-Vy[i];
if(fabs(findAngle(Vx[i],Vy[i],temp_x,temp_y))<(RAD((360.0-blind_angle)/2)) ){
//otherwise do nothing....
//if in repulsion:
float dist;
if((dist=distance(Px[i],Py[i],Px[j],Py[j]))<=REPULSE_FACTOR && i!=j){
//do equation 1(normalized)
newx_1-=(Px[j]-Px[i])/dist;
newy_1-=(Py[j]-Py[i])/dist;
fishInOne=1;
}
//attraction factor.
//according to Stephanie:
//"I do not exclude fish i from the list of fish within fish i's zone of
//attraction"
else if(dist<=ATTRACT_FACTOR){
if(i!=j){
newx_7+=(Px[j]-Px[i])/dist;
newy_7+=(Py[j]-Py[i])/dist;
}
newvectx_7+=(Vx[j])/(norm(Vx[j],Vy[j]));
newvecty_7+=(Vy[j])/(norm(Vx[j],Vy[j]));
fishInTwo=1;
}
}
}
//end calculation for one fish:
if(fishInOne){
//ADJUSTING for turning too much:
//if new angle is 115 degrees
float turn_angle;
if(fabs(turn_angle=findAngle(Vx[i],Vy[i],newx_1,newy_1))<=(RAD(max_turn)*tau)){
newVx[i]=newx_1;
newVy[i]=newy_1;
}
else{
//only turn max_turn degrees...
//BUT! need to take into account the speed and time step (tau)
//115 degress per 1 tau?
turn_angle=RAD(max_turn)*(turn_angle<0 ? -1:1)*(tau);
newVx[i]=Vx[i]*cos(turn_angle)-sin(turn_angle)*Vy[i];
newVy[i]=Vx[i]*sin(turn_angle)+cos(turn_angle)*Vy[i];
}
}
else if(fishInTwo){
newx_7*=w_a;
newy_7*=w_a;
newvectx_7*=w_o;
newvecty_7*=w_o;
newVx[i]=newx_7+newvectx_7;
newVy[i]=newy_7+newvecty_7;
float turn_angle;
if(fabs(turn_angle=findAngle(Vx[i],Vy[i],newVx[i],newVy[i]))<=(RAD(max_turn)*tau)){
//do nothing
}
else{
//only turn max_turn degrees...
turn_angle=RAD(max_turn)*(turn_angle<0 ? -1:1)*tau;
newVx[i]=Vx[i]*cos(turn_angle)-sin(turn_angle)*Vy[i];
newVy[i]=Vx[i]*sin(turn_angle)+cos(turn_angle)*Vy[i];
}
}
//correctDirection:
correctDirection((newVx+i),(newVy+i),Vx[i],Vy[i]);
//update locationm, eq(3):
newPx[i]=(speed*tau*newVx[i])+Px[i];
newPy[i]=(speed*tau*newVy[i])+Py[i];
}
} // end fishStep
ctktk5 (int iCluster, float target_pos[3], STAT * ctkStat, CLUSTER_INTPTS Clstr[MAXCLUSTERHITS], int *nClusters)
#endif
{
/* declarations */
float fom, minfom;
int curPerm, k;
int indx[MAXNOSEG];
float eg, th, thc, v1[3], v2[3];
int j;
#if (DEBUG)
FILE *fp;
char str[132];
float r1;
#endif
#ifdef KICKOUT
float oldfom = 0;
#endif
#ifdef GOODENOUGH
float FOMcutGoodenogh;
#endif
#ifdef JUMP
int distNextGrp, basejump;
float FOMcutJump;
#endif
#if(DEBUG)
if (Pars.nprint > 0)
{
sprintf (str, "F,track_%3.3i.list", ctkStat->nEvents);
fp = fopen (str, "w");
assert (fp != NULL);
};
#endif
/* count how many times we are called */
ctkStat->TrackingCalls++;
/* info */
#if(DEBUG)
if (Pars.nprint > 0)
{
fprintf (fp, "\n");
fprintf (fp, "--------------------------------------------------------------\n");
fprintf (fp, "\n");
#ifdef FULL
fprintf (fp, "entered ctktk0/FULL\n");
#endif
#ifdef MAXE
fprintf (fp, "entered ctktk0/MAXE\n");
#endif
#ifdef KICKOUT
fprintf (fp, "entered ctktk0/KICKOUT\n");
#endif
#ifdef GOODENOUGH
fprintf (fp, "entered ctktk0/GOODENOUGH\n");
#endif
#ifdef JUMP
fprintf (fp, "entered ctktk0/JUMP\n");
#endif
};
#endif
/* init */
Clstr[iCluster].fom = 0;
/* trap single hits, which are trivial. */
/* But we have to mark them tracked anyway */
if (Clstr[iCluster].ndet == 1)
{
Clstr[iCluster].tracked = 1;
Clstr[iCluster].fom = Pars.snglsFom;
Clstr[iCluster].bestPermutation = 0;
Clstr[iCluster].intpts[0].order = 0;
return (1);
};
/* trap if ndet is higher than we can handle */
if (Clstr[iCluster].ndet >= MAXNOSEG)
{
Clstr[iCluster].tracked = 0;
Clstr[iCluster].fom = MAXFOM;
return (2);
}
else
Clstr[iCluster].tracked = 1;
assert (Clstr[iCluster].tracked == 1);
#if(DEBUG)
if (Pars.nprint > 0)
{
fprintf (fp, "iCluster=%2i, # sectors (ndet)= %2i, ", iCluster, Clstr[iCluster].ndet);
fprintf (fp, "# perm=%10i; ", Pars.nperm[Clstr[iCluster].ndet]);
fprintf (fp, "target= %5.2f %5.2f %5.2f\n", target_pos[0], target_pos[1], target_pos[2]);
};
#endif
assert (Clstr[iCluster].ndet < MAXNOSEG);
#ifdef GOODENOUGH
FOMcutGoodenogh = Clstr[iCluster].ndet * Pars.fomGoodenough;
#endif
#ifdef MAXE
/* we just return the order we have as we */
/* sorted according to energy already */
j = 0;
for (k = 0; k < Clstr[iCluster].ndet; k++)
{
Clstr[iCluster].intpts[k].order = j;
j++;
};
/* mark as tracked */
Clstr[iCluster].tracked = 1;
/* assign the proper fom for it */
findVector (target_pos[0], target_pos[1], target_pos[2],
Clstr[iCluster].intpts[0].xx, Clstr[iCluster].intpts[0].yy,
Clstr[iCluster].intpts[0].zz, &v1[0], &v1[1], &v1[2]);
eg = Clstr[iCluster].esum;
fom = 0;
for (k = 0; k < (Clstr[iCluster].ndet - 1); k++)
{
/* find norm vector to next interaction point */
findVector (Clstr[iCluster].intpts[k].xx, Clstr[iCluster].intpts[k].yy,
Clstr[iCluster].intpts[k].zz, Clstr[iCluster].intpts[k + 1].xx,
Clstr[iCluster].intpts[k + 1].yy, Clstr[iCluster].intpts[k + 1].zz, &v2[0], &v2[1], &v2[2]);
// printf ("k =%i, %f %f %f\n", k, Clstr[iCluster].intpts[k].xx, Clstr[iCluster].intpts[k].yy, Clstr[iCluster].intpts[k].zz);
// printf ("k+1=%i, %f %f %f\n", k + 1, Clstr[iCluster].intpts[k + 1].xx, Clstr[iCluster].intpts[k + 1].yy, Clstr[iCluster].intpts[k + 1].zz);
/* find scattering angle */
findAngle (v1, v2, &th);
// printf ("vectors:\n");
// printf ("v1: (%9.3f,%9.3f,%9.3f)\n", v1[0], v1[1], v1[2]);
// printf ("v2: (%9.3f,%9.3f,%9.3f)\n", v2[0], v2[1], v2[2]);
// printf ("th=%9.4f\n", th);
/* check for sanity, invalidate so noone else will see it */
if (th < 0)
{
Clstr[iCluster].tracked = 0;
Clstr[iCluster].valid = 0;
return (5);
};
if (th > PI)
{
Clstr[iCluster].tracked = 0;
Clstr[iCluster].valid = 0;
return (6);
};
if (isnan (th))
{
Clstr[iCluster].tracked = 0;
Clstr[iCluster].valid = 0;
return (7);
};
/* find compton scattering angle */
findCAngle (eg, Clstr[iCluster].intpts[k].edet, &thc);
/* add to Figure of Merit (FOM) */
fom += (thc - th) * (thc - th);
// printf (" _%i, %f\n", k, fom);
/* shift norm vector */
v1[0] = v2[0];
v1[1] = v2[1];
v1[2] = v2[2];
/* subtract energy lost in this scatter */
eg = eg - Clstr[iCluster].intpts[k].edet;
}; /* loop over interaction points */
fom /= (Clstr[iCluster].ndet - 1);
Clstr[iCluster].fom = fom;
/* we are done */
// printf ("MAXE case fom=%f, ndet=%i\n", Clstr[iCluster].fom, Clstr[iCluster].ndet);
return (8);
#endif
#ifdef JUMP
FOMcutJump = Clstr[iCluster].ndet * Pars.fomJump;
/* distance to next group */
/* determinde by size of tail */
basejump = Pars.fac[Clstr[iCluster].ndet - Pars.jmpGrpLen[Clstr[iCluster].ndet]];
distNextGrp = basejump;
#if(0)
fprintf (fp, "iCluster=%i\n", iCluster);
fprintf (fp, "Clstr[iCluster].ndet=%i\n", Clstr[iCluster].ndet);
fprintf (fp, "Pars.jmpGrpLen[Clstr[iCluster].ndet]=%i\n", Pars.jmpGrpLen[Clstr[iCluster].ndet]);
fprintf (fp, "basejump=%i\n", basejump);
for (i = 0; i < MAXNOSEG; i++)
fprintf (fp, "%i, Pars.jmpGrpLen[i]=%5i\n", i, Pars.jmpGrpLen[i]);
exit (0);
#endif
#if(DEBUG)
if (Pars.nprint > 0)
{
fprintf (fp, "JUMP init: ndet=%i, basejump=%i in strategy ", Clstr[iCluster].ndet, basejump);
/* print jump nomenclature */
for (j = 0; j < Clstr[iCluster].ndet; j++)
if (j < Pars.jmpGrpLen[Clstr[iCluster].ndet])
fprintf (fp, "g");
else
fprintf (fp, "t");
for (j = Clstr[iCluster].ndet; j < MAXNOSEG; j++)
fprintf (fp, " ");
fprintf (fp, "\n");
}
#endif
#endif
/* find sum of interaction energies */
Clstr[iCluster].esum = 0;
for (k = 0; k < Clstr[iCluster].ndet; k++)
Clstr[iCluster].esum += Clstr[iCluster].intpts[k].edet;
/*-----------------------------*/
/* loop over ALL permutations */
/* full search/not tree search */
/*-----------------------------*/
minfom = MAXFLOAT;
for (curPerm = 0; curPerm < Pars.nperm[Clstr[iCluster].ndet]; curPerm++)
{
#ifdef KICKOUT
/* save previous FOM */
if (curPerm > 0)
oldfom = minfom;
#endif
/* count permutation we make */
ctkStat->nperm++;
eg = Clstr[iCluster].esum;
/* find index for lookup */
for (k = 0; k < Clstr[iCluster].ndet; k++)
indx[k] = Pars.permlkup[Clstr[iCluster].ndet][curPerm][k];
/* the lookup is then: */
/* XX Clstr[iCluster].intpts[i].xx[indx[i]] */
/* YY Clstr[iCluster].intpts[i].yy[indx[i]] */
/* ZZ Clstr[iCluster].intpts[i].zz[indx[i]] */
/* EE Clstr[iCluster].edet[indx[i]] */
#if(DEBUG)
if (Pars.nprint > 0)
{
#ifdef JUMP
if (distNextGrp == basejump)
fprintf (fp, "\n\n...start of new group\n");
#endif
fprintf (fp, "**permutation # %3i :: ", curPerm);
for (k = 0; k < Clstr[iCluster].ndet; k++)
fprintf (fp, "%i ", indx[k]);
fprintf (fp, " start eg=%5.3f\n", eg);
};
#endif
/* find norm vector to first interaction point */
findVector (target_pos[0], target_pos[1], target_pos[2],
Clstr[iCluster].intpts[indx[0]].xx, Clstr[iCluster].intpts[indx[0]].yy,
Clstr[iCluster].intpts[indx[0]].zz, &v1[0], &v1[1], &v1[2]);
fom = 0;
for (k = 0; k < (Clstr[iCluster].ndet - 1); k++)
{
/* find norm vector to next interaction point */
findVector (Clstr[iCluster].intpts[indx[k]].xx, Clstr[iCluster].intpts[indx[k]].yy,
Clstr[iCluster].intpts[indx[k]].zz, Clstr[iCluster].intpts[indx[k + 1]].xx,
Clstr[iCluster].intpts[indx[k + 1]].yy, Clstr[iCluster].intpts[indx[k + 1]].zz, &v2[0],
&v2[1], &v2[2]);
/* find scattering angle */
findAngle (v1, v2, &th);
#if(DEBUG &&0)
if (Pars.nprint > 0)
{
fprintf (fp, "vectors:");
fprintf (fp, "v1: (%9.3f,%9.3f,%9.3f)\n ", v1[0], v1[1], v1[2]);
fprintf (fp, "v2: (%9.3f,%9.3f,%9.3f)\n ", v2[0], v2[1], v2[2]);
fprintf (fp, "th=%9.4f\n", th);
};
#endif
/* check for sanity, invalidate so noone else will see it */
if (th < 0)
{
Clstr[iCluster].tracked = 0;
Clstr[iCluster].valid = 0;
return (5);
};
if (th > PI)
{
Clstr[iCluster].tracked = 0;
Clstr[iCluster].valid = 0;
return (6);
};
if (isnan (th))
{
Clstr[iCluster].tracked = 0;
Clstr[iCluster].valid = 0;
/*printf("oooopsie, th=%f, isnan=%i at event no # %i\n", th, isnan(th), ctkStat->nEvents); */
return (7);
};
/* find compton scattering angle */
findCAngle (eg, Clstr[iCluster].intpts[indx[k]].edet, &thc);
/* add to Figure of Merit (FOM) */
fom += (thc - th) * (thc - th);
/* shift norm vector */
v1[0] = v2[0];
v1[1] = v2[1];
v1[2] = v2[2];
/* subtract energy lost in this scatter */
eg = eg - Clstr[iCluster].intpts[indx[k]].edet;
#if(DEBUG)
if (Pars.nprint > 0)
{
fprintf (fp, "%6.2f %6.2f %6.2f; ", Clstr[iCluster].intpts[indx[k]].xx,
Clstr[iCluster].intpts[indx[k]].yy, Clstr[iCluster].intpts[indx[k]].zz);
fprintf (fp, "(%5.3f); ", Clstr[iCluster].intpts[indx[k]].edet);
fprintf (fp, "th/cth= %6.4f/%6.4f; ", th, thc);
r1 = sqrtf (fom) / (Clstr[iCluster].ndet - 1);
fprintf (fp, "sum square FOM now %9.6f (%9.6f), ", fom, r1);
fprintf (fp, "eg now %5.3f\n", eg);
};
#endif
}; /* loop over interaction points */
#if(DEBUG)
if (Pars.nprint > 0)
{
k = Clstr[iCluster].ndet - 1;
fprintf (fp, "%6.2f %6.2f %6.2f; ", Clstr[iCluster].intpts[indx[k]].xx,
Clstr[iCluster].intpts[indx[k]].yy, Clstr[iCluster].intpts[indx[k]].zz);
fprintf (fp, "(%5.3f); ", Clstr[iCluster].intpts[indx[k]].edet);
fprintf (fp, "(absorption)\n");
};
#endif
/*--------------------------------------------*/
/* keep record of the best permutation so far */
/*--------------------------------------------*/
if (fom < minfom)
{
minfom = fom;
Clstr[iCluster].bestPermutation = curPerm;
Clstr[iCluster].fom = fom;
/* store the permutation order, This is trickier */
/* than you might think */
for (k = 0; k < Clstr[iCluster].ndet; k++)
Clstr[iCluster].intpts[indx[k]].order = k;
#if(DEBUG)
if (Pars.nprint > 0)
{
fprintf (fp, "...best permutation so far\n");
fprintf (fp, "...Clstr[iCluster].bestPermutation= %i, \n", Clstr[iCluster].bestPermutation);
fprintf (fp, "...Clstr[iCluster].fom=%f\n", Clstr[iCluster].fom);
};
#endif
};
#ifdef KICKOUT
/* if we are doing worse than last */
/* permutation we tried, kickout here */
if (curPerm > 0)
if (fom > oldfom)
{
/* normalize FOM, debug print, return minfom */
minfom /= (Clstr[iCluster].ndet - 1);
Clstr[iCluster].fom = minfom;
#if(DEBUG)
if (Pars.nprint > 0)
fprintf (fp, "best (kickout) FM= %f for permutation # %i\n", minfom, Clstr[iCluster].bestPermutation);
#endif
return (3);
};
#endif
#ifdef GOODENOUGH
/* is this permutation FOM below */
/* cut? I.e., Goodenough */
if (fom < FOMcutGoodenogh)
{
/* normalize FOM, debug print, return minfom */
minfom /= (Clstr[iCluster].ndet - 1);
Clstr[iCluster].fom = minfom;
#if(DEBUG)
if (Pars.nprint > 0)
fprintf (fp, "best (goodenough) FM= %f for permutation # %i\n", minfom, Clstr[iCluster].bestPermutation);
#endif
return (4);
};
#endif
#ifdef JUMP
#if(DEBUG)
if (Pars.nprint > 0)
fprintf (fp, "...distNextGrp=%i\n", distNextGrp);
#endif
/* jump ahead if this fom is over the fom cut */
if (fom > Pars.fomJump)
{
#if(DEBUG)
if (Pars.nprint > 0)
{
fprintf (fp, "JUMP: fom>Pars.fomJump: %f > %f, jump %i [+=%i] ", fom, Pars.fomJump, distNextGrp,
distNextGrp - 1);
fprintf (fp, "(basejump=%i)\n", basejump);
};
#endif
/* forward curPerm, remember we gain one anyway */
curPerm += (distNextGrp - 1);
/* reset distNextGrp */
distNextGrp = basejump;
}
else
{
/* find distance to next group */
distNextGrp--;
if (distNextGrp <= 0)
distNextGrp = basejump;
};
#endif
}; /* loop over permutations */
/* normalize FOM, debug print, return minfom */
#if(0)
/* old way I had it, could argue OK, but... */
/* below I take the root and normalize */
minfom /= (Clstr[iCluster].ndet - 1);
#endif
minfom = sqrtf (minfom) / (Clstr[iCluster].ndet - 1);
Clstr[iCluster].fom = minfom;
#if(DEBUG)
if (Pars.nprint > 0)
{
fprintf (fp, "\n");
fprintf (fp, "tracking, final result:\n");
fprintf (fp, "best FOM is %10.6f for permutation # %i\n\n", minfom, Clstr[iCluster].bestPermutation);
for (k = 0; k < Clstr[iCluster].ndet; k++)
{
fprintf (fp, "%6.2f %6.2f %6.2f; ",
Clstr[iCluster].intpts[k].xx, Clstr[iCluster].intpts[k].yy, Clstr[iCluster].intpts[k].zz);
fprintf (fp, "(%5.3f); ", Clstr[iCluster].intpts[k].edet);
fprintf (fp, "interaction order: %i\n", Clstr[iCluster].intpts[k].order);
};
fprintf (fp, "reported FOM is: %10.6f\n", Clstr[iCluster].fom);
};
#endif
/* done */
#if(DEBUG)
if (Pars.nprint > 0)
{
fprintf (fp, "\n");
fclose (fp);
};
#endif
return (0);
}
double AU_UAV_ROS::CObject::findAngle(const AU_UAV_ROS::CObject& cobj) const {
return findAngle(cobj.latitude, cobj.longitude);
}
/* Function that returns the ID of the most dangerous neighboring plane and its ZEM. */
sim::threatContainer sim::findGreatestThreat(PlaneObject &plane1, std::map<int, PlaneObject> &planes) {
/* Set reference for origin (Northwest corner of the course)*/
sim::coordinate origin;
origin.latitude = 32.606573;
origin.longitude = -85.490356;
origin.altitude = 400;
/* Set preliminary plane to avoid as non-existent and most dangerous ZEM as negative */
int planeToAvoid = -1;
int iPlaneToAvoid = -1;
double mostDangerousZEM = -1.0;
double iMostDangerousZEM = -1.0;
/* Set the preliminary time-to-go high */
double minimumTimeToGo = MINIMUM_TIME_TO_GO;
double iMinimumTimeToGo = 3.5;
/* Declare second plane and ID variable */
PlaneObject plane2;
int ID;
/* Make a position vector representation of the current plane */
double magnitude2, direction2;
double magnitude = findDistance(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
double direction = findAngle(origin.latitude, origin.longitude,
plane1.getCurrentLoc().latitude, plane1.getCurrentLoc().longitude);
sim::mathVector p1(magnitude,direction);
/* Make a heading vector representation of the current plane */
sim::mathVector d1(1.0,toCartesian(plane1.getCurrentBearing()));
/* Declare variables needed for this loop */
sim::mathVector pDiff, dDiff;
double timeToGo, zeroEffortMiss, distanceBetween, timeToDest, bearingDiff;
std::map<int,sim::PlaneObject>::iterator it;
for (it=planes.begin() ; it!= planes.end(); it++) {
/* Unpacking plane to check */
ID = (*it).first;
plane2 = (*it).second;
/* If it's not in the Check Zone, check the other plane */
distanceBetween = plane1.findDistance(plane2);
if (distanceBetween > CHECK_ZONE || plane1.getID() == ID) continue;
/* Making a position vector representation of plane2 */
magnitude2 = findDistance(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
direction2 = findAngle(origin.latitude, origin.longitude,
plane2.getCurrentLoc().latitude, plane2.getCurrentLoc().longitude);
sim::mathVector p2(magnitude2,direction2);
/* Make a heading vector representation of the other plane */
sim::mathVector d2(1.0,toCartesian(plane2.getCurrentBearing()));
/* Compute time-to-go */
pDiff = p1-p2;
dDiff = d1-d2;
timeToGo = -1.0*pDiff.dotProduct(dDiff)/(MPS_SPEED*dDiff.dotProduct(dDiff));
/* Compute Zero Effort Miss */
zeroEffortMiss = sqrt(fabs(pDiff.dotProduct(pDiff) +
2.0*(MPS_SPEED*timeToGo)*pDiff.dotProduct(dDiff) +
pow(MPS_SPEED*timeToGo,2.0)*dDiff.dotProduct(dDiff)));
if( zeroEffortMiss > DANGER_ZEM || (timeToGo > minimumTimeToGo && timeToGo > iMinimumTimeToGo) || timeToGo < 0 ) continue;
timeToDest = plane1.findDistance(plane1.getDestination().latitude,
plane1.getDestination().longitude) / MPS_SPEED;
/* If you're close to your destination and the other plane isn't
much of a threat, then don't avoid it */
if ( timeToDest < 5.0 && zeroEffortMiss > 3.0*MPS_SPEED ) continue;
/* If you're likely to zigzag, don't avoid the other plane */
bearingDiff = fabs(plane1.getCurrentBearing() - planes[ID].getCurrentBearing());
if ( plane1.findDistance(planes[ID]) > 3.5*MPS_SPEED && bearingDiff < CHATTERING_ANGLE) continue;
/* Second Threshold, to prevent planes from flying into others when trying to avoid less imminent collisions */
if ( zeroEffortMiss <= SECOND_THRESHOLD && timeToGo <= iMinimumTimeToGo ) {
iPlaneToAvoid = ID;
iMostDangerousZEM = zeroEffortMiss;
iMinimumTimeToGo = timeToGo;
continue;
}
planeToAvoid = ID;
mostDangerousZEM = zeroEffortMiss;
minimumTimeToGo = timeToGo;
}
sim::threatContainer greatestThreat;
if (iPlaneToAvoid > -1) {
greatestThreat.planeID = iPlaneToAvoid;
greatestThreat.ZEM = iMostDangerousZEM;
greatestThreat.timeToGo = iMinimumTimeToGo;
}
else {
greatestThreat.planeID = planeToAvoid;
greatestThreat.ZEM = mostDangerousZEM;
greatestThreat.timeToGo = minimumTimeToGo;
}
return greatestThreat;
}