src/Navaids/mkrbeacons.cxx

   1 // mkrbeacons.cxx -- marker beacon management class
   2 //
   3 // Written by Curtis Olson, started March 2001.
   4 //
   5 // Copyright (C) 2001  Curtis L. Olson - curt@flightgear.org
   6 //
   7 // This program is free software; you can redistribute it and/or
   8 // modify it under the terms of the GNU General Public License as
   9 // published by the Free Software Foundation; either version 2 of the
  10 // License, or (at your option) any later version.
  11 //
  12 // This program is distributed in the hope that it will be useful, but
  13 // WITHOUT ANY WARRANTY; without even the implied warranty of
  14 // MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the GNU
  15 // General Public License for more details.
  16 //
  17 // You should have received a copy of the GNU General Public License
  18 // along with this program; if not, write to the Free Software
  19 // Foundation, Inc., 675 Mass Ave, Cambridge, MA 02139, USA.
  20 //
  21 // $Id$
  22
  23
  24 #include "mkrbeacons.hxx"
  25
  26
  27 // constructor
  28 FGMkrBeacon::FGMkrBeacon() {
  29     FGMkrBeacon( 0, 0, 0, NOBEACON );
  30 }
  31
  32 FGMkrBeacon::FGMkrBeacon( double _lon, double _lat, double _elev,
  33                     fgMkrBeacType _type ) {
  34     lon = _lon;
  35     lat = _lat;
  36     elev = _elev;
  37     type = _type;
  38
  39     Point3D pos = sgGeodToCart(Point3D(lon * SGD_DEGREES_TO_RADIANS, lat * SGD_DEGREES_TO_RADIANS, 0));
  40     // cout << "pos = " << pos << endl;
  41     x = pos.x();
  42     y = pos.y();
  43     z = pos.z();
  44 }
  45
  46 // destructor
  47 FGMkrBeacon::~FGMkrBeacon() {
  48 }
  49
  50
  51 // constructor
  52 FGMarkerBeacons::FGMarkerBeacons() {
  53 }
  54
  55 // destructor
  56 FGMarkerBeacons::~FGMarkerBeacons() {
  57 }
  58
  59
  60 // initialize the structures
  61 bool FGMarkerBeacons::init() {
  62     // erase the beacon map
  63     beacon_map.clear();
  64
  65     return true;
  66 }
  67
  68
  69 // real add a marker beacon
  70 bool FGMarkerBeacons::real_add( const int master_index, const FGMkrBeacon& b ) {
  71     // cout << "Master index = " << master_index << endl;
  72     beacon_map[master_index].push_back( b );
  73
  74     return true;
  75 }
  76
  77
  78 // front end for add a marker beacon
  79 bool FGMarkerBeacons::add( double lon, double lat, double elev,
  80                            FGMkrBeacon::fgMkrBeacType type ) {
  81     double diff;
  82
  83     int lonidx = (int)lon;
  84     diff = lon - (double)lonidx;
  85     if ( (lon < 0.0) && (fabs(diff) > SG_EPSILON) ) {
  86         lonidx -= 1;
  87     }
  88     double lonfrac = lon - (double)lonidx;
  89     lonidx += 180;
  90
  91     int latidx = (int)lat;
  92     diff = lat - (double)latidx;
  93     if ( (lat < 0.0) && (fabs(diff) > SG_EPSILON) ) {
  94         latidx -= 1;
  95     }
  96     double latfrac = lat - (double)latidx;
  97     latidx += 90;
  98
  99     int master_index = lonidx * 1000 + latidx;
 100     FGMkrBeacon b( lon, lat, elev, type );
 101
 102     // add to the actual bucket
 103     real_add( master_index, b );
 104
 105     // if we are close to the edge, add to adjacent buckets so we only
 106     // have to search one bucket at run time
 107
 108     // there are 8 cases since there are 8 adjacent tiles
 109
 110     if ( lonfrac < 0.2 ) {
 111         real_add( master_index - 1000, b );
 112         if ( latfrac < 0.2 ) {
 113             real_add( master_index - 1000 - 1, b );
 114         } else if ( latfrac > 0.8 ) {
 115             real_add( master_index - 1000 + 1, b );
 116         }
 117     } else if ( lonfrac > 0.8 ) {
 118         real_add( master_index + 1000, b );
 119         if ( latfrac < 0.2 ) {
 120             real_add( master_index + 1000 - 1, b );
 121         } else if ( latfrac > 0.8 ) {
 122             real_add( master_index+- 1000 + 1, b );
 123         }
 124     } else if ( latfrac < 0.2 ) {
 125         real_add( master_index - 1, b );
 126     } else if ( latfrac > 0.8 ) {
 127         real_add( master_index + 1, b );
 128     }
 129
 130     return true;
 131 }
 132
 133
 134 // returns marker beacon type if we are over a marker beacon, NOBEACON
 135 // otherwise
 136 FGMkrBeacon::fgMkrBeacType FGMarkerBeacons::query( double lon, double lat,
 137                                                 double elev ) {
 138     double diff;
 139
 140     int lonidx = (int)lon;
 141     diff = lon - (double)lonidx;
 142     if ( (lon < 0.0) && (fabs(diff) > SG_EPSILON) ) {
 143         lonidx -= 1;
 144     }
 145     lonidx += 180;
 146
 147     int latidx = (int)lat;
 148     diff = lat - (double)latidx;
 149     if ( (lat < 0.0) && (fabs(diff) > SG_EPSILON) ) {
 150         latidx -= 1;
 151     }
 152     latidx += 90;
 153
 154     int master_index = lonidx * 1000 + latidx;
 155
 156     beacon_list_type beacons = beacon_map[ master_index ];
 157     // cout << "Master index = " << master_index << endl;
 158     // cout << "beacon search length = " << beacons.size() << endl;
 159
 160     beacon_list_iterator current = beacons.begin();
 161     beacon_list_iterator last = beacons.end();
 162
 163     Point3D aircraft = sgGeodToCart(Point3D(lon*SGD_DEGREES_TO_RADIANS, lat*SGD_DEGREES_TO_RADIANS, 0));
 164
 165     double min_dist = 999999999.0;
 166
 167     for ( ; current != last ; ++current ) {
 168         // cout << "  testing " << current->get_locident() << endl;
 169         Point3D station = Point3D( current->get_x(),
 170                                    current->get_y(),
 171                                    current->get_z() );
 172         // cout << "    aircraft = " << aircraft << " station = " << station
 173         //      << endl;
 174
 175         double d = aircraft.distance3Dsquared( station ); // meters^2
 176         // cout << "  distance = " << d << " ("
 177         //      << FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
 178         //         * FG_ILS_DEFAULT_RANGE * SG_NM_TO_METER
 179         //      << ")" << endl;
 180
 181         // cout << "  range = " << sqrt(d) << endl;
 182
 183         if ( d < min_dist ) {
 184             min_dist = d;
 185         }
 186
 187         // cout << "elev = " << elev * SG_METER_TO_FEET
 188         //      << " current->get_elev() = " << current->get_elev() << endl;
 189         double delev = elev * SG_METER_TO_FEET - current->get_elev();
 190
 191         // max range is the area under r = 2.4 * alt or r^2 = 4000^2 - alt^2
 192         // whichever is smaller.  The intersection point is 1538 ...
 193         double maxrange2;       // feet^2
 194         if ( delev < 1538.0 ) {
 195             maxrange2 = 2.4 * 2.4 * delev * delev;
 196         } else if ( delev < 4000.0 ) {
 197             maxrange2 = 4000 * 4000 - delev * delev;
 198         } else {
 199             maxrange2 = 0.0;
 200         }
 201         maxrange2 *= SG_FEET_TO_METER * SG_FEET_TO_METER; // convert to meter^2
 202         // cout << "delev = " << delev << " maxrange = " << maxrange << endl;
 203
 204         // match up to twice the published range so we can model
 205         // reduced signal strength
 206         if ( d < maxrange2 ) {
 207             // cout << "lon = " << lon << " lat = " << lat
 208             //      << "  closest beacon = " << sqrt( min_dist ) << endl;
 209             return current->get_type();
 210         }
 211     }
 212
 213     // cout << "lon = " << lon << " lat = " << lat
 214     //      << "  closest beacon = " << sqrt( min_dist ) << endl;
 215
 216     return FGMkrBeacon::NOBEACON;
 217 }
 218
 219
 220 FGMarkerBeacons *current_beacons;