Пример #1
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;
	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;
}
Пример #2
0
/* 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);
}
Пример #3
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;
}
Пример #4
0
/* 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;
	}
}
Пример #5
0
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
Пример #6
0
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;
}
Пример #7
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;
}
Пример #8
0
/* 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;
}
Пример #9
0
// 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;
}
Пример #10
0
// 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
Пример #11
0
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);

}
Пример #12
0
double AU_UAV_ROS::CObject::findAngle(const AU_UAV_ROS::CObject& cobj) const {
	return findAngle(cobj.latitude, cobj.longitude);
}
Пример #13
0
/* 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;
}