]> git.mxchange.org Git - flightgear.git/blob - src/Navaids/mkrbeacons.cxx
MSVC fix from Frederic Bouvier
[flightgear.git] / 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;