← Back to branch summary

~ubuntu-branches/ubuntu/precise/flightgear/precise

« back to all changes in this revision

Viewing changes to src/Airports/groundnetwork.cxx

  • Committer: Bazaar Package Importer
  • Author(s): Ove Kaaven
  • Date: 2011-01-30 15:46:35 UTC
  • mfrom: (3.1.7 sid)
  • Revision ID: james.westby@ubuntu.com-20110130154635-rlynmg9n5hzxq5xe
Tags: 2.0.0-3
* Recommend fgfs-aircraft-base and fgfs-models-base.
  Closes. #610276.
* Added note about scenery SharedModels.tgz to README.Debian.

Show diffs side-by-side

added added

removed removed

Lines of Context:
src/Airports/groundnetwork.cxx
27
27
#include <math.h>
28
28
#include <algorithm>
29
29
 
30
 
//#include <plib/sg.h>
31
 
//#include <plib/ul.h>
32
 
 
33
 
//#include <Environment/environment_mgr.hxx>
34
 
//#include <Environment/environment.hxx>
35
 
//#include <simgear/misc/sg_path.hxx>
36
 
//#include <simgear/props/props.hxx>
37
 
//#include <simgear/structure/subsystem_mgr.hxx>
38
30
#include <simgear/debug/logstream.hxx>
39
31
#include <simgear/route/waypoint.hxx>
40
 
//#include <Main/globals.hxx>
41
 
//#include <Main/fg_props.hxx>
42
 
//#include <Airports/runways.hxx>
43
32
 
44
33
#include <Airports/dynamics.hxx>
45
34
 
46
35
#include <AIModel/AIFlightPlan.hxx>
47
36
 
48
 
//#include <string>
49
 
 
50
37
#include "groundnetwork.hxx"
51
38
 
52
39
/***************************************************************************
92
79
// doing this.
93
80
void FGTaxiSegment::setTrackDistance()
94
81
{
95
 
  //double course;
96
 
  SGWayPoint first  (start->getLongitude(),
97
 
                     start->getLatitude(),
98
 
                     0);
99
 
  SGWayPoint second (end->getLongitude(),
100
 
                     end->getLatitude(),
101
 
                     0);
102
 
  first.CourseAndDistance(second, &course, &length);
 
82
  length = SGGeodesy::distanceM(start->getGeod(), end->getGeod());
103
83
}
104
84
 
105
85
 
287
267
      {
288
268
        if ((*j)->getEnd()->getIndex() == (*i)->getStart()->getIndex())
289
269
          {
290
 
            int start1 = (*i)->getStart()->getIndex();
291
 
            int end1   = (*i)->getEnd()  ->getIndex();
292
 
            int start2 = (*j)->getStart()->getIndex();
293
 
            int end2   = (*j)->getEnd()->getIndex();
294
 
            int oppIndex = (*j)->getIndex();
 
270
//          int start1 = (*i)->getStart()->getIndex();
 
271
//          int end1   = (*i)->getEnd()  ->getIndex();
 
272
//          int start2 = (*j)->getStart()->getIndex();
 
273
//          int end2   = (*j)->getEnd()->getIndex();
 
274
//          int oppIndex = (*j)->getIndex();
295
275
            //cerr << "Opposite of  " << (*i)->getIndex() << " (" << start1 << "," << end1 << ") "
296
276
            //   << "happens to be " << oppIndex      << " (" << start2 << "," << end2 << ") " << endl;
297
277
            (*i)->setOpposite(*j);
312
292
  //exit(1);
313
293
}
314
294
 
315
 
 
316
 
 
317
 
int FGGroundNetwork::findNearestNode(double lat, double lon)
 
295
int FGGroundNetwork::findNearestNode(const SGGeod& aGeod)
318
296
{
319
297
  double minDist = HUGE_VAL;
320
 
  double dist;
321
 
  int index;
322
 
  SGWayPoint first  (lon,
323
 
                     lat,
324
 
                     0);
 
298
  int index = -1;
325
299
  
326
 
  for (FGTaxiNodeVectorIterator 
327
 
         itr = nodes.begin();
328
 
       itr != nodes.end(); itr++)
 
300
  for (FGTaxiNodeVectorIterator itr = nodes.begin(); itr != nodes.end(); itr++)
 
301
  {
 
302
    double d = SGGeodesy::distanceM(aGeod, (*itr)->getGeod());
 
303
    if (d < minDist)
329
304
    {
330
 
      double course;
331
 
      SGWayPoint second ((*itr)->getLongitude(),
332
 
                         (*itr)->getLatitude(),
333
 
                         0);
334
 
      first.CourseAndDistance(second, &course, &dist);
335
 
      if (dist < minDist)
336
 
        {
337
 
          minDist = dist;
338
 
          index = (*itr)->getIndex();
339
 
          //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
340
 
        }
 
305
      minDist = d;
 
306
      index = (*itr)->getIndex();
 
307
      //cerr << "Minimum distance of " << minDist << " for index " << index << endl;
341
308
    }
 
309
  }
 
310
  
342
311
  return index;
343
312
}
344
313
 
345
 
FGTaxiNode *FGGroundNetwork::findNode(int idx)
 
314
int FGGroundNetwork::findNearestNode(double lat, double lon)
 
315
{
 
316
  return findNearestNode(SGGeod::fromDeg(lon, lat));
 
317
}
 
318
 
 
319
FGTaxiNode *FGGroundNetwork::findNode(unsigned idx)
346
320
{ /*
347
321
    for (FGTaxiNodeVectorIterator 
348
322
    itr = nodes.begin();
358
332
    return 0;
359
333
}
360
334
 
361
 
FGTaxiSegment *FGGroundNetwork::findSegment(int idx)
 
335
FGTaxiSegment *FGGroundNetwork::findSegment(unsigned idx)
362
336
{/*
363
337
  for (FGTaxiSegmentVectorIterator 
364
338
         itr = segments.begin();
580
554
   instruction. See below for the hold position instruction.
581
555
 
582
556
   Note that there currently still is one flaw in the logic that needs to be addressed. 
583
 
   can be situations where one aircraft is in front of the current aircraft, on a separate
 
557
   There can be situations where one aircraft is in front of the current aircraft, on a separate
584
558
   route, but really close after an intersection coming off the current route. This
585
559
   aircraft is still close enough to block the current aircraft. This situation is currently
586
560
   not addressed yet, but should be.
619
593
  double mindist = HUGE_VAL;
620
594
  if (activeTraffic.size()) 
621
595
    {
622
 
      double course, dist, bearing, minbearing;
623
 
      SGWayPoint curr  (lon,
624
 
                        lat,
625
 
                        alt);
 
596
      double course, dist, bearing, minbearing, az2;
 
597
      SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
626
598
      //TrafficVector iterator closest;
627
599
      closest = current;
628
600
      for (TrafficVectorIterator i = activeTraffic.begin(); 
629
601
           i != activeTraffic.end(); i++)
630
602
        {
631
 
          if (i != current) {
632
 
            //SGWayPoint curr  (lon,
633
 
            //        lat,
634
 
            //        alt);
635
 
            SGWayPoint other    (i->getLongitude  (),
636
 
                                 i->getLatitude (),
637
 
                                 i->getAltitude  ());
638
 
            other.CourseAndDistance(curr, &course, &dist);
639
 
            bearing = fabs(heading-course);
 
603
          if (i == current) {
 
604
        continue;
 
605
      }
 
606
      
 
607
        SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 
608
          i->getLatitude(), i->getAltitude()));
 
609
        SGGeodesy::inverse(curr, other, course, az2, dist);
 
610
        bearing = fabs(heading-course);
640
611
            if (bearing > 180)
641
612
              bearing = 360-bearing;
642
613
            if ((dist < mindist) && (bearing < 60.0))
645
616
                closest = i;
646
617
                minbearing = bearing;
647
618
              }
648
 
          }
649
619
        }
650
620
      //Check traffic at the tower controller
651
621
      if (towerController->hasActiveTraffic())
654
624
               i != towerController->getActiveTraffic().end(); i++)
655
625
            {
656
626
              //cerr << "Comparing " << current->getId() << " and " << i->getId() << endl;
657
 
              //SGWayPoint curr  (lon,
658
 
              //                  lat,
659
 
              //                  alt);
660
 
              SGWayPoint other    (i->getLongitude  (),
661
 
                                   i->getLatitude (),
662
 
                                   i->getAltitude  ());
663
 
              other.CourseAndDistance(curr, &course, &dist);
664
 
              bearing = fabs(heading-course);
 
627
              SGGeod other(SGGeod::fromDegM(i->getLongitude(),
 
628
                                            i->getLatitude(), 
 
629
                                            i->getAltitude()));
 
630
        SGGeodesy::inverse(curr, other, course, az2, dist);
 
631
        bearing = fabs(heading-course);
665
632
              if (bearing > 180)
666
 
                bearing = 360-bearing;
 
633
          bearing = 360-bearing;
667
634
              if ((dist < mindist) && (bearing < 60.0))
668
635
                {
669
636
                  mindist = dist;
676
643
      // Finally, check UserPosition
677
644
      double userLatitude  = fgGetDouble("/position/latitude-deg");
678
645
      double userLongitude = fgGetDouble("/position/longitude-deg");
679
 
      SGWayPoint user    (userLongitude,
680
 
                          userLatitude,
681
 
                          alt); // Alt is not really important here. 
682
 
      user.CourseAndDistance(curr, &course, &dist);
 
646
      SGGeod user(SGGeod::fromDeg(userLongitude,userLatitude));
 
647
      SGGeodesy::inverse(curr, user, course, az2, dist);
 
648
 
683
649
      bearing = fabs(heading-course);
684
650
      if (bearing > 180)
685
 
        bearing = 360-bearing;
 
651
      bearing = 360-bearing;
686
652
      if ((dist < mindist) && (bearing < 60.0))
687
653
        {
688
654
          mindist = dist;
690
656
          minbearing = bearing;
691
657
          otherReasonToSlowDown = true;
692
658
        }
693
 
      
694
 
      //        if (closest == current) {
695
 
      //          //SG_LOG(SG_GENERAL, SG_ALERT, "AI error: closest and current match");
696
 
      //          //return;
697
 
      //        }      
698
 
      //cerr << "Distance : " << dist << " bearing : " << bearing << " heading : " << heading 
699
 
      //   << " course : " << course << endl;
 
659
 
700
660
      current->clearSpeedAdjustment();
701
661
    
702
662
      if (current->checkPositionAndIntentions(*closest) || otherReasonToSlowDown) 
760
720
  }
761
721
  current = i;
762
722
  current->setHoldPosition(false);
763
 
  SGWayPoint curr  (lon,
764
 
                    lat,
765
 
                    alt);
766
 
  double course, dist, bearing, minbearing;
 
723
  SGGeod curr(SGGeod::fromDegM(lon, lat, alt));
 
724
  
767
725
  for (i = activeTraffic.begin(); 
768
726
       i != activeTraffic.end(); i++)
769
727
    {
772
730
          int node = current->crosses(this, *i);
773
731
          if (node != -1)
774
732
            {
 
733
          FGTaxiNode* taxiNode = findNode(node);
 
734
          
775
735
              // Determine whether it's save to continue or not. 
776
736
              // If we have a crossing route, there are two possibilities:
777
737
              // 1) This is an interestion
778
738
              // 2) This is oncoming two-way traffic, using the same taxiway.
779
739
              //cerr << "Hold check 1 : " << id << " has common node " << node << endl;
780
 
              SGWayPoint nodePos(findNode(node)->getLongitude  (),
781
 
                                 findNode(node)->getLatitude   (),
782
 
                                 alt);
783
 
 
784
 
              SGWayPoint other    (i->getLongitude  (),
785
 
                                   i->getLatitude (),
786
 
                                   i->getAltitude  ());
787
 
              //other.CourseAndDistance(curr, &course, &dist);
788
 
              bool needsToWait;
 
740
             
 
741
        SGGeod other(SGGeod::fromDegM(i->getLongitude(), i->getLatitude(), i->getAltitude()));
 
742
        bool needsToWait;
789
743
              bool opposing;
790
744
              if (current->isOpposing(this, *i, node))
791
745
                {
803
757
              else 
804
758
                {
805
759
                  opposing = false;
806
 
                  other.CourseAndDistance(nodePos, &course, &dist);
807
 
                  if (dist > 200) // 2.0*i->getRadius())
 
760
                  if (SGGeodesy::distanceM(other, taxiNode->getGeod()) > 200) // 2.0*i->getRadius())
808
761
                    {
809
762
                      needsToWait = false; 
810
763
                      //cerr << "Hold check 3 : " << id <<"  Other aircraft approaching node is still far away. (" << dist << " nm). Can safely continue " 
816
769
                      //cerr << "Hold check 4: " << id << "  Would need to wait for other aircraft : distance = " << dist << " meters" << endl;
817
770
                    }
818
771
                }
819
 
              curr.CourseAndDistance(nodePos, &course, &dist);
820
 
              if (!(i->hasHoldPosition()))
 
772
      
 
773
      double dist = SGGeodesy::distanceM(curr, taxiNode->getGeod());
 
774
      if (!(i->hasHoldPosition()))
821
775
                {
822
776
                  
823
777
                  if ((dist < 200) && //2.5*current->getRadius()) &&