Adding some more intelligence to the AI system step 2: Added a system to

detect interfering taxi routes and added a "hold position" instruction to
make one of two conflicting aircraft wait until it's route is cleared again

src/AIModel/AIAircraft.cxx

@@ -86,6 +86,8 @@ FGAIAircraft::FGAIAircraft(FGAISchedule *ref) :
     alt_lock = false;
     roll = 0;
     headingChangeRate = 0.0;
+
+    holdPos = false;
 }
 
 
@@ -226,7 +228,7 @@ void FGAIAircraft::Run(double dt) {
 			   pos.getLongitudeDeg(),
 			   hdg,
 			   speed,
-			   altitude_ft);
+			   altitude_ft, dt);
 	//if (controller->hasInstruction(getID()))
 	//  {
 	    processATC(controller->getInstruction(getID()));
@@ -979,20 +981,20 @@ void FGAIAircraft::announcePositionToController()
       //snprintf (buffer, 10, "%d", node);
       switch (leg) {
       case 3:
-	cerr << trafficRef->getRegistration() 
+	//cerr << trafficRef->getRegistration() 
-	     << " taxiing to runway at segment " 
+	//     << " taxiing to runway at segment " 
-	     << fp->getCurrentWaypoint()->routeIndex
+	//     << fp->getCurrentWaypoint()->routeIndex
-	     << endl;
+	//     << endl;
 	//cerr << "Match check between taxisegment and taxiroute : " << node << " " 
 	//     << fp->getCurrentWaypoint()->name << endl;
 	if (trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork()->exists())
 	  controller = trafficRef->getDepartureAirport()->getDynamics()->getGroundNetwork();
 	break;
       case 9:
-	cerr << trafficRef->getRegistration() 
+	//cerr << trafficRef->getRegistration() 
-	     << " taxiing to parking at segment " 
+	//     << " taxiing to parking at segment " 
-	     << fp->getCurrentWaypoint()->routeIndex
+	//     << fp->getCurrentWaypoint()->routeIndex
-	     << endl;
+	//     << endl;
 	if (trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork()->exists())
 	  controller = trafficRef->getArrivalAirport()->getDynamics()->getGroundNetwork();
 	break;
@@ -1006,8 +1008,8 @@ void FGAIAircraft::announcePositionToController()
     }
     if ((controller != prevController) && (prevController != 0)) {
       prevController->signOff(getID());
-      cerr << trafficRef->getRegistration() 
+      //cerr << trafficRef->getRegistration() 
-	   << " signing off " << endl;
+      //   << " signing off " << endl;
     }
     prevController = controller;
     if (controller) {
@@ -1024,13 +1026,29 @@ void FGAIAircraft::processATC(FGATCInstruction instruction)
   //cerr << "Processing ATC instruction (not Implimented yet)" << endl;
   if (instruction.getHoldPattern   ()) {
   }
+  
+  // Hold Position
   if (instruction.getHoldPosition  ()) {
+    if (!holdPos) {
+      if (trafficRef)
+	cerr << trafficRef->getCallSign() << "Holding Position " << endl;
+      holdPos = true;
     }
+    AccelTo(0.25);
+  } else {
+    if (holdPos) {
+      if (trafficRef)
+	cerr << trafficRef->getCallSign() << " Resuming Taxi " << endl;
+      holdPos = false;
+    }    
+    // Change speed Instruction. This can only be excecuted when there is no 
+    // Hold position instruction.
     if (instruction.getChangeSpeed   ()) {
       AccelTo(instruction.getSpeed());
     }else {
       if (fp) AccelTo(fp->getPreviousWaypoint()->speed);
     }
+  }
   if (instruction.getChangeHeading ()) { 
     hdg_lock = false;
     TurnTo(instruction.getHeading());

src/AIModel/AIAircraft.hxx

@@ -124,6 +124,8 @@ private:
     double prevSpeed;
     double prev_dist_to_go;
 
+  bool holdPos;
+
     bool _getGearDown() const;
     bool reachedWaypoint;
     string callsign;             // The callsign of this tanker.

src/Airports/groundnetwork.cxx

@@ -61,6 +61,7 @@ FGTaxiNode::FGTaxiNode()
  **************************************************************************/
 FGTaxiSegment::FGTaxiSegment()
 {
+  oppositeDirection = 0;
 }
 
 void FGTaxiSegment::setStart(FGTaxiNodeVector *nodes)
@@ -262,6 +263,102 @@ void FGTrafficRecord::setPositionAndHeading(double lat, double lon, double hdg,
   altitude = alt;
 }
 
+int FGTrafficRecord::crosses(FGGroundNetwork *net, FGTrafficRecord &other)
+{
+  if (checkPositionAndIntentions(other) || (other.checkPositionAndIntentions(*this)))
+    return -1;
+  intVecIterator i, j;
+  int currentTargetNode = 0, otherTargetNode = 0;
+  if (currentPos > 0)
+    currentTargetNode = net->findSegment(currentPos      )->getEnd()->getIndex(); // OKAY,... 
+  if (other.currentPos > 0)
+    otherTargetNode   = net->findSegment(other.currentPos)->getEnd()->getIndex(); // OKAY,...
+  if ((currentTargetNode == otherTargetNode) && currentTargetNode > 0)
+    return currentTargetNode;
+  if (intentions.size())
+    {
+      for (i = intentions.begin(); i != intentions.end(); i++)
+	{
+	  if (currentTargetNode == net->findSegment(*i)->getEnd()->getIndex())
+	    {
+	      cerr << "Current crosses at " << currentTargetNode <<endl;
+	      return currentTargetNode;
+	    }
+	  
+	}
+    }
+  if (other.intentions.size())
+    {
+      for (i = other.intentions.begin(); i != other.intentions.end(); i++)
+	{
+	  if (otherTargetNode == net->findSegment(*i)->getEnd()->getIndex())
+	    {
+	      cerr << "Other crosses at " << currentTargetNode <<endl;
+	      return otherTargetNode;
+	    }
+	}
+    }
+  if (intentions.size() && other.intentions.size())
+    {
+      for (i = intentions.begin(); i != intentions.end(); i++) 
+	{
+	  for (j = other.intentions.begin(); j != other.intentions.end(); j++)
+	    {
+	      //cerr << "finding segment " << *i << " and " << *j << endl;
+	      currentTargetNode = net->findSegment(*i)->getEnd()->getIndex();
+	      otherTargetNode   = net->findSegment(*j)->getEnd()->getIndex();
+	      if (currentTargetNode == otherTargetNode) 
+		{
+		  //cerr << "Routes will cross at " << currentTargetNode << endl;
+		  return currentTargetNode;
+		}
+	    }
+	}
+    }
+  return -1;
+}
+
+bool FGTrafficRecord::isOpposing (FGGroundNetwork *net, FGTrafficRecord &other, int node)
+{
+  // Check if current segment is the reverse segment for the other aircraft
+  FGTaxiSegment *opp;
+  //cerr << "Current segment " << currentPos << endl;
+  if ((currentPos > 0) && (other.currentPos > 0))
+    {
+      opp = net->findSegment(currentPos)->opposite();
+      if (opp) {
+	if (opp->getIndex() == other.currentPos)
+	  return true;
+      }
+      
+      for (intVecIterator i = intentions.begin(); i != intentions.end(); i++)
+	{
+	  for (intVecIterator j = intentions.begin(); j != intentions.end(); j++)
+	    {  
+	      // cerr << "Current segment 1 " << (*i) << endl;
+	      if (opp = net->findSegment(*i)->opposite())
+		{
+		  if (opp->getIndex() == 
+		      net->findSegment(*j)->getIndex())
+		    {
+		      cerr << "Nodes " << net->findSegment(*i)->getIndex()
+			   << " and  " << net->findSegment(*j)->getIndex()
+			   << " are opposites " << endl;
+		      if (net->findSegment(*i)->getStart()->getIndex() == node) {
+			{
+			  cerr << "Found the node" << endl;
+			  return true;
+			}
+		      }
+		    }
+		}
+	    }
+	}
+    }
+  return false;
+}
+
+
 /***************************************************************************
  * FGGroundNetwork()
  **************************************************************************/
@@ -273,6 +370,7 @@ FGGroundNetwork::FGGroundNetwork()
   totalDistance = 0;
   maxDistance = 0;
   currTraffic = activeTraffic.begin();
+
 }
 
 void FGGroundNetwork::addSegment(const FGTaxiSegment &seg)
@@ -306,16 +404,39 @@ void FGGroundNetwork::init()
 {
   hasNetwork = true;
   int index = 1;
+  sort(nodes.begin(), nodes.end());
+  sort(segments.begin(), segments.end());
   FGTaxiSegmentVectorIterator i = segments.begin();
   while(i != segments.end()) {
     //cerr << "initializing node " << i->getIndex() << endl;
     i->setStart(&nodes);
     i->setEnd  (&nodes);
     i->setTrackDistance();
-    i->setIndex(index++);
+    i->setIndex(index);
     //cerr << "Track distance = " << i->getLength() << endl;
     //cerr << "Track ends at"      << i->getEnd()->getIndex() << endl;
     i++;
+    index++;
+  }
+  i = segments.begin();
+  while(i != segments.end()) {
+    FGTaxiSegmentPointerVectorIterator j = i->getEnd()->getBeginRoute(); 
+    while (j != i->getEnd()->getEndRoute())
+      {
+	if ((*j)->getEnd()->getIndex() == i->getStart()->getIndex())
+	  {
+	    int start1 = i->getStart()->getIndex();
+	    int end1   = i->getEnd()  ->getIndex();
+	    int start2 = (*j)->getStart()->getIndex();
+	    int end2   = (*j)->getEnd()->getIndex();
+	    int oppIndex = (*j)->getIndex();
+	    cerr << "Oppossite of  " << i->getIndex() << " (" << start1 << "," << end1 << ") "
+		 << "happens to be " << oppIndex      << " (" << start2 << "," << end2 << ") " << endl;
+	    break;
+	  }
+	  j++;
+      }
+    i++;
   }
   //exit(1);
 }
@@ -349,19 +470,23 @@ int FGGroundNetwork::findNearestNode(double lat, double lon)
 }
 
 FGTaxiNode *FGGroundNetwork::findNode(int idx)
-{
+{ /*
     for (FGTaxiNodeVectorIterator 
     itr = nodes.begin();
     itr != nodes.end(); itr++)
     {
     if (itr->getIndex() == idx)
     return itr->getAddress();
-    }
+    }*/
+  
+  if ((idx >= 0) && (idx < nodes.size())) 
+    return nodes[idx].getAddress();
+  else
     return 0;
 }
 
 FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
-{
+{/*
   for (FGTaxiSegmentVectorIterator 
 	 itr = segments.begin();
        itr != segments.end(); itr++)
@@ -369,7 +494,14 @@ FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
       if (itr->getIndex() == idx)
 	return itr->getAddress();
     } 
+ */
+  if ((idx > 0) && (idx <= segments.size()))
+    return segments[idx-1].getAddress();
+  else
+    {
+      cerr << "Alert: trying to find invalid segment " << idx << endl;
       return 0;
+    }
 }
 
 FGTaxiRoute FGGroundNetwork::findShortestRoute(int start, int end) 
@@ -583,7 +715,8 @@ void FGGroundNetwork::signOff(int id) {
   }
 }
 
-void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt) {
+void FGGroundNetwork::update(int id, double lat, double lon, double heading, double speed, double alt, 
+			     double dt) {
   TrafficVectorIterator i = activeTraffic.begin();
   // Search search if the current id has an entry
   // This might be faster using a map instead of a vector, but let's start by taking a safe route
@@ -601,10 +734,42 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou
     current = i;
   }
   
+  setDt(getDt() + dt);
+  
+  // Update every three secs, but add some randomness
+  // to prevent all IA objects doing this in synchrony
+  //if (getDt() < (3.0) + (rand() % 10))
+  //  return;
+  //else
+  //  setDt(0);
+  checkSpeedAdjustment(id, lat, lon, heading, speed, alt);
+  checkHoldPosition   (id, lat, lon, heading, speed, alt);
+}
+
+void FGGroundNetwork::checkSpeedAdjustment(int id, double lat, 
+					   double lon, double heading, 
+					   double speed, double alt)
+{
+  
   // Scan for a speed adjustment change. Find the nearest aircraft that is in front
   // and adjust speed when we get too close. Only do this when current position and/or
   // intentions of the current aircraft match current taxiroute position of the proximate
-  // aircraft. 
+  // aircraft. For traffic that is on other routes we need to issue a "HOLD Position"
+  // instruction. See below for the hold position instruction.
+  TrafficVectorIterator current, closest;
+  TrafficVectorIterator i = activeTraffic.begin();
+  if (activeTraffic.size()) 
+    {
+      while ((i->getId() != id) && (i != activeTraffic.end()))
+	{
+	  i++;
+	}
+    }
+  else
+    {
+      return;
+    }
+  current = i;
   double mindist = HUGE;
   if (activeTraffic.size()) 
     {
@@ -690,31 +855,6 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou
 		    newSpeed = -1 * (maxAllowableDistance-mindist);
 		    current->setSpeedAdjustment(newSpeed);
 		    current->setHeadingAdjustment(heading);
-		    // 	      if (mindist < 5) {
-		    // 		double bank_sense = 0;
-		    // 		current->setSpeedAdjustment(-0.1);
-		    // 		// Do a heading adjustment
-		    // 		double diff = fabs(heading - bearing);
-		    // 		if (diff > 180)
-		    // 		  diff = fabs(diff - 360);
-		    
-		    // 		double sum = heading + diff;
-		    // 		if (sum > 360.0)
-		    // 		  sum -= 360.0;
-		    // 		if (fabs(sum - bearing) < 1.0) {
-		    // 		  bank_sense = -1.0;   // turn left for evasive action
-		    // 		} else {
-		    // 		  bank_sense = 1.0;  // turn right for evasive action
-		    // 		}
-		    // 		double newHeading = heading + bank_sense;
-		    // 		if (newHeading < 0) newHeading   += 360;
-		    // 		if (newHeading > 360) newHeading -= 360;
-		    // 		current->setHeadingAdjustment(newHeading);
-		    // 		//cerr << "Yikes: TOOOO close. backing up and turning to heading " << newHeading 
-		    // 		//  << endl;
-		    // 		cerr << "Troubleshooting: " << current->getId() << " Closest : " << closest->getId() 
-		    // 		     << endl;
-		    //	     }
 		  }
 		}
 	    }
@@ -722,12 +862,115 @@ void FGGroundNetwork::update(int id, double lat, double lon, double heading, dou
     }
 }
 
+void FGGroundNetwork::checkHoldPosition(int id, double lat, 
+					double lon, double heading, 
+					double speed, double alt)
+{
+  // Check for "Hold position instruction".
+  // The hold position should be issued under the following conditions:
+  // 1) For aircraft entering or crossing a runway with active traffic on it, or landing aircraft near it
+  // 2) For taxiing aircraft that use one taxiway in opposite directions
+  // 3) For crossing or merging taxiroutes.
+
+  TrafficVectorIterator current, closest;
+  TrafficVectorIterator i = activeTraffic.begin();
+  if (activeTraffic.size()) 
+    {
+      while ((i->getId() != id) && i != activeTraffic.end()) 
+	{
+	  i++;
+	}
+    }
+  else
+    {
+      return ;
+    }
+  current = i;
+  current->setHoldPosition(false);
+  double course, dist, bearing, minbearing;
+  for (i = activeTraffic.begin(); 
+       i != activeTraffic.end(); i++)
+    {
+      if (i != current) 
+	{
+	  int node = current->crosses(this, *i);
+	  if (node != -1)
+	    {
+	      // Determine whether it's save to continue or not. 
+	      // If we have a crossing route, there are two possibilities:
+	      // 1) This is an interestion
+	      // 2) This is oncoming two-way traffic, using the same taxiway.
+	      //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
+	      SGWayPoint nodePos(findNode(node)->getLongitude  (),
+				 findNode(node)->getLatitude   (),
+				 alt);
+	      SGWayPoint curr  (lon,
+				lat,
+				alt);
+	      
+	      SGWayPoint other    (i->getLongitude  (),
+				   i->getLatitude (),
+				   i->getAltitude  ());
+	      //other.CourseAndDistance(curr, &course, &dist);
+	      bool needsToWait;
+	      if (current->isOpposing(this, *i, node))
+		{
+		  needsToWait = true;
+		  //cerr << "Hold check 2 : " << id << "  has opposing segment " << endl;
+		  // issue a "Hold Position" as soon as we're close to the offending node
+		  // For now, I'm doing this as long as the other aircraft doesn't
+		  // have a hold instruction as soon as we're within a reasonable 
+		  // distance from the offending node.
+		  // This may be a bit of a conservative estimate though, as it may
+		  // be well possible that both aircraft can both continue to taxi 
+		  // without crashing into each other.
+		} 
+	      else 
+		{
+		  other.CourseAndDistance(nodePos, &course, &dist);
+		  if (dist > 2.0*i->getRadius())
+		    {
+		      needsToWait = false; 
+		      //cerr << "Hold check 3 : " << id <<"  Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue " 
+		      //	   << endl;
+		    }
+		  else 
+		    {
+		      needsToWait = true;
+		      //cerr << "Hold check 4: " << id << "  Would need to wait for other aircraft : distance = " << dist << " nm" << endl;
+		    }
+		}
+	      curr.CourseAndDistance(nodePos, &course, &dist);
+	      if (!(i->hasHoldPosition()))
+		{
+		  
+		  if ((dist < 2.5*current->getRadius()) && 
+		      (needsToWait) && 
+		      (!(current->getId() == i->getWaitsForId())) &&
+		      (!(current->getSpeedAdjustment())))
+		    
+		    {
+		      current->setHoldPosition(true);
+		      //cerr << "Hold check 5: " << id <<"  Setting Hold Position: distance to node : " << dist << " nm"<< endl;
+		    }
+		  else
+		    {
+		      //cerr << "Hold check 6: " << id << "  No need to hold yet: Distance to node : " << dist << " nm"<< endl;
+		    }
+		}
+	    }
+	}
+    }
+}
+
+
 bool FGGroundNetwork::hasInstruction(int id)
 {
    TrafficVectorIterator i = activeTraffic.begin();
   // Search search if the current id has an entry
   // This might be faster using a map instead of a vector, but let's start by taking a safe route
-  if (activeTraffic.size()) {
+  if (activeTraffic.size()) 
+    {
       while ((i->getId() != id) && i != activeTraffic.end()) {
       i++;
     }

src/Airports/groundnetwork.hxx

@@ -76,6 +76,7 @@ public:
   FGTaxiNode *getAddress() { return this;};
   FGTaxiSegmentPointerVectorIterator getBeginRoute() { return next.begin(); };
   FGTaxiSegmentPointerVectorIterator getEndRoute()   { return next.end();   }; 
+  bool operator<(const FGTaxiNode &other) const { return index < other.index; };
 };
 
 typedef vector<FGTaxiNode> FGTaxiNodeVector;
@@ -93,25 +94,32 @@ private:
   FGTaxiNode *start;
   FGTaxiNode *end;
   int index;
+  FGTaxiSegment *oppositeDirection;
 
 public:
   FGTaxiSegment();
-  FGTaxiSegment(FGTaxiNode *, FGTaxiNode *, int);
+  //FGTaxiSegment(FGTaxiNode *, FGTaxiNode *, int);
 
   void setIndex        (int val) { index     = val; };
   void setStartNodeRef (int val) { startNode = val; };
   void setEndNodeRef   (int val) { endNode   = val; };
 
+  void setOpposite(FGTaxiSegment *opp) { oppositeDirection = opp; };
+
   void setStart(FGTaxiNodeVector *nodes);
   void setEnd  (FGTaxiNodeVector *nodes);
   void setTrackDistance();
 
   FGTaxiNode * getEnd() { return end;};
+  FGTaxiNode * getStart() { return start; };
   double getLength() { return length; };
   int getIndex() { return index; };
 
  FGTaxiSegment *getAddress() { return this;};
 
+  bool operator<(const FGTaxiSegment &other) const { return index < other.index; };
+  FGTaxiSegment *opposite() { return oppositeDirection; };
+
   
 };
 
@@ -194,6 +202,7 @@ public:
   void setAlt         (double val) { alt     = val; };
 };
 
+class FGGroundNetwork;
 
 /**************************************************************************************
  * class FGTrafficRecord
@@ -219,6 +228,10 @@ public:
   bool hasInstruction() { return instruction.hasInstruction(); };
   void setPositionAndHeading(double lat, double lon, double hdg, double spd, double alt);
   bool checkPositionAndIntentions(FGTrafficRecord &other);
+  int  crosses                   (FGGroundNetwork *, FGTrafficRecord &other); 
+  bool isOpposing                (FGGroundNetwork *, FGTrafficRecord &other, int node);
+
+  bool getSpeedAdjustment() { return instruction.getChangeSpeed(); };
   
   double getLatitude () { return latitude ; };
   double getLongitude() { return longitude; };
@@ -237,6 +250,8 @@ public:
   void clearHeadingAdjustment() { instruction.setChangeHeading(false); };
 
   bool hasHeadingAdjustment() { return instruction.getChangeHeading(); };
+  bool hasHoldPosition() { return instruction.getHoldPosition(); };
+  void setHoldPosition (bool inst) { instruction.setHoldPosition(inst); };
 
   void setWaitsForId(int id) { waitsForId = id; };
 
@@ -265,7 +280,7 @@ public:
 				double hdg, double spd, double alt, double radius) = 0;
   virtual void             signOff(int id) = 0;
   virtual void             update(int id, double lat, double lon, 
-				  double heading, double speed, double alt) = 0;
+				  double heading, double speed, double alt, double dt) = 0;
   virtual bool             hasInstruction(int id) = 0;
   virtual FGATCInstruction getInstruction(int id) = 0;
 
@@ -296,6 +311,11 @@ private:
 
   void printRoutingError(string);
 
+  void checkSpeedAdjustment(int id, double lat, double lon, 
+			    double heading, double speed, double alt);
+  void checkHoldPosition(int id, double lat, double lon, 
+			 double heading, double speed, double alt);
+  
 public:
   FGGroundNetwork();
 
@@ -314,7 +334,7 @@ public:
   virtual void announcePosition(int id, FGAIFlightPlan *intendedRoute, int currentRoute, 
 				double lat, double lon, double hdg, double spd, double alt, double radius);
   virtual void signOff(int id);
-  virtual void update(int id, double lat, double lon, double heading, double speed, double alt);
+  virtual void update(int id, double lat, double lon, double heading, double speed, double alt, double dt);
   virtual bool hasInstruction(int id);
   virtual FGATCInstruction getInstruction(int id);
 };