Fix #2576 Added findWithHeading

src/AIModel/AIFlightPlanCreatePushBack.cxx

@@ -134,7 +134,8 @@ bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac,
         ac->setTaxiClearanceRequest(false);
         double az2 = 0.0;
 
-        FGTaxiSegment* pushForwardSegment = dep->groundNetwork()->findSegment(parking, 0);
+        FGTaxiSegment* pushForwardSegment = dep->groundNetwork()->findSegmentByHeading(parking, parking->getHeading()); 
+
         if (!pushForwardSegment) {
             // there aren't any routes for this parking, so create a simple segment straight ahead for 2 meters based on the parking heading
             SG_LOG(SG_AI, SG_DEV_WARN, "Gate " << parking->ident() << "/" << parking->getName()
@@ -156,13 +157,17 @@ bool FGAIFlightPlan::createPushBack(FGAIAircraft *ac,
 
         double parkingHeading = parking->getHeading();
 
-        for (int i = 1; i < 10; i++) {
+        SG_LOG(SG_AI, SG_BULK, "Creating Pushforward : \t" << pushForwardSegment->getEnd()->getIndex() << " Length : \t" << distance);
+
+        int numSegments = distance/3.0;
+        for (int i = 1; i < numSegments; i++) {
             SGGeod pushForwardPt;
             SGGeodesy::direct(parking->geod(), parkingHeading,
-                              ((i / 10.0) * distance), pushForwardPt, az2);
+                              ((i / numSegments) * distance), pushForwardPt, az2);
             char buffer[16];
-            snprintf(buffer, 16, "pushback-%02d", i);
+            snprintf(buffer, 16, "pushforward-%02d", i);
             FGAIWaypoint *wpt = createOnGround(ac, string(buffer), pushForwardPt, dep->getElevation(), vTaxiReduced);
+            SG_LOG(SG_AI, SG_BULK, "Created WP : \t" << wpt->getName() << "\t" << wpt->getPos());
 
             wpt->setRouteIndex(pushForwardSegment->getIndex());
             pushBackWaypoint(wpt);

src/Airports/groundnetwork.cxx

@@ -384,7 +384,7 @@ FGTaxiRoute FGGroundNetwork::findShortestRoute(FGTaxiNode* start, FGTaxiNode* en
             break;
         }
 
-        for (auto target : segmentsFrom(best)) {
+        for (auto target : findSegmentsFrom(best)) {
             double edgeLength = dist(best->cart(), target->cart());
             double alt = searchData[best].score + edgeLength + edgePenalty(target);
             if (alt < searchData[target].score) {    // Relax (u,v)
@@ -508,7 +508,7 @@ void FGGroundNetwork::addParking(const FGParkingRef &park)
     }
 }
 
-FGTaxiNodeVector FGGroundNetwork::segmentsFrom(const FGTaxiNodeRef &from) const
+FGTaxiNodeVector FGGroundNetwork::findSegmentsFrom(const FGTaxiNodeRef &from) const
 {
     FGTaxiNodeVector result;
     FGTaxiSegmentVector::const_iterator it;
@@ -521,6 +521,30 @@ FGTaxiNodeVector FGGroundNetwork::segmentsFrom(const FGTaxiNodeRef &from) const
     return result;
 }
 
+FGTaxiSegment* FGGroundNetwork::findSegmentByHeading(const FGTaxiNode* from, const double heading) const {
+    if (from == 0) {
+        return NULL;
+    }
+
+    FGTaxiSegment* best = nullptr;
+
+  // completely boring linear search of segments. Can be improved if/when
+  // this ever becomes a hot-spot
+    for (auto seg : segments) {
+        if (seg->startNode != from) {
+            continue;
+        }
+
+        if( !best || fabs(best->getHeading()-heading) > fabs(seg->getHeading()-heading)) {
+            best = seg;
+        }
+    }
+
+    return best; // not found
+}
+
+
+
 const intVec& FGGroundNetwork::getTowerFrequencies() const
 {
     return freqTower;

src/Airports/groundnetwork.hxx

@@ -199,8 +199,6 @@ private:
     void addSegment(const FGTaxiNodeRef& from, const FGTaxiNodeRef& to);
     void addParking(const FGParkingRef& park);
 
-    FGTaxiNodeVector segmentsFrom(const FGTaxiNodeRef& from) const;
-
     void addAwosFreq     (int val) {
         freqAwos.push_back(val);
     };
@@ -246,8 +244,6 @@ public:
 
     FGTaxiNodeRef findNearestNodeOffRunway(const SGGeod& aGeod, FGRunway* aRunway, double distanceM) const;
 
-    FGTaxiSegment *findSegment(unsigned int idx) const;
-
     FGTaxiSegment* findOppositeSegment(unsigned int index) const;
 
     const FGParkingList& allParkings() const;
@@ -263,6 +259,14 @@ public:
      * segment originating at 'from' is acceptable.
      */
     FGTaxiSegment *findSegment(const FGTaxiNode* from, const FGTaxiNode* to) const;
+    /** Find the taxiway segment best matching the heading*/
+    FGTaxiSegment *findSegmentByHeading(const FGTaxiNode* from, const double heading) const;
+    FGTaxiSegment *findSegment(unsigned int idx) const;
+    /**
+     * Find the segments connected to the node.
+    */
+    FGTaxiNodeVector findSegmentsFrom(const FGTaxiNodeRef& from) const;
+
   
     FGTaxiRoute findShortestRoute(FGTaxiNode* start, FGTaxiNode* end, bool fullSearch=true);
 

test_suite/unit_tests/AI/test_groundnet.cxx

@@ -64,6 +64,9 @@ void GroundnetTests::setUp()
     FGAirportRef egph = FGAirport::getByIdent("EGPH");
     egph->testSuiteInjectGroundnetXML(SGPath::fromUtf8(FG_TEST_SUITE_DATA) / "EGPH.groundnet.xml");
 
+    FGAirportRef ybbn = FGAirport::getByIdent("YBBN");
+    ybbn->testSuiteInjectGroundnetXML(SGPath::fromUtf8(FG_TEST_SUITE_DATA) / "YBBN.groundnet.xml");
+
 
     globals->add_new_subsystem<PerformanceDB>();
     globals->add_new_subsystem<FGATCManager>();
@@ -93,3 +96,27 @@ void GroundnetTests::testShortestRoute()
     CPPUNIT_ASSERT_EQUAL(true, network->exists());
     CPPUNIT_ASSERT_EQUAL(25, route.size());
 }
+
+/**
+ * Tests various find methods.
+ */
+
+void GroundnetTests::testFind()
+{
+    FGAirportRef ybbn = FGAirport::getByIdent("YBBN");
+
+    FGGroundNetwork* network = ybbn->groundNetwork();    
+    FGParkingRef startParking = network->findParkingByName("GA1");
+    CPPUNIT_ASSERT_EQUAL(1018, startParking->getIndex());
+    FGTaxiSegment* segment1 = network->findSegment(startParking, NULL);
+    CPPUNIT_ASSERT(segment1);
+    FGTaxiSegment* segment2 = network->findSegment(startParking, segment1->getEnd());
+    CPPUNIT_ASSERT(segment2);
+    FGTaxiNodeVector segmentList = network->findSegmentsFrom(startParking);
+    CPPUNIT_ASSERT_EQUAL(2, (int)segmentList.size());
+    CPPUNIT_ASSERT_EQUAL(1024, segmentList.front()->getIndex());
+    CPPUNIT_ASSERT_EQUAL(1025, segmentList.back()->getIndex());
+    FGTaxiSegment* pushForwardSegment = network->findSegmentByHeading(startParking, startParking->getHeading()); 
+    CPPUNIT_ASSERT(pushForwardSegment);
+    CPPUNIT_ASSERT_EQUAL(1025, pushForwardSegment->getEnd()->getIndex());
+}

test_suite/unit_tests/AI/test_groundnet.hxx

@@ -34,6 +34,8 @@ class GroundnetTests : public CppUnit::TestFixture
     // Set up the test suite.
     CPPUNIT_TEST_SUITE(GroundnetTests);
     CPPUNIT_TEST(testShortestRoute);
+    CPPUNIT_TEST(testFind);
+    
     CPPUNIT_TEST_SUITE_END();
 
 
@@ -46,4 +48,5 @@ public:
 
     // The tests.
     void testShortestRoute();
+    void testFind();
 };