diff --git a/src/AIModel/AIFlightPlanCreatePushBack.cxx b/src/AIModel/AIFlightPlanCreatePushBack.cxx
index ffd4391e8..6be57d0d3 100644
--- a/src/AIModel/AIFlightPlanCreatePushBack.cxx
+++ b/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);
diff --git a/src/Airports/groundnetwork.cxx b/src/Airports/groundnetwork.cxx
index e322e2156..c25951aa6 100644
--- a/src/Airports/groundnetwork.cxx
+++ b/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;
diff --git a/src/Airports/groundnetwork.hxx b/src/Airports/groundnetwork.hxx
index ce039e24b..d8f6e54cf 100644
--- a/src/Airports/groundnetwork.hxx
+++ b/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);
diff --git a/test_suite/unit_tests/AI/test_groundnet.cxx b/test_suite/unit_tests/AI/test_groundnet.cxx
index 81e739fff..dbbbf9038 100644
--- a/test_suite/unit_tests/AI/test_groundnet.cxx
+++ b/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());
+}
diff --git a/test_suite/unit_tests/AI/test_groundnet.hxx b/test_suite/unit_tests/AI/test_groundnet.hxx
index 6472679b7..653086bbd 100644
--- a/test_suite/unit_tests/AI/test_groundnet.hxx
+++ b/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();
};