From 54de08a3b697bd343dffcdf9b8b5604a9d5e3b83 Mon Sep 17 00:00:00 2001 From: portree_kid Date: Sun, 18 Apr 2021 21:03:21 +0200 Subject: [PATCH] Fix #2576 Added findWithHeading --- src/AIModel/AIFlightPlanCreatePushBack.cxx | 13 +++++++--- src/Airports/groundnetwork.cxx | 28 +++++++++++++++++++-- src/Airports/groundnetwork.hxx | 12 ++++++--- test_suite/unit_tests/AI/test_groundnet.cxx | 27 ++++++++++++++++++++ test_suite/unit_tests/AI/test_groundnet.hxx | 3 +++ 5 files changed, 73 insertions(+), 10 deletions(-) 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(); globals->add_new_subsystem(); @@ -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(); };