Phi: nicer scroll animation for METAR widget
[fg:fgdata.git] / Nasal / geo.nas
1 # geo functions
2 # -------------------------------------------------------------------------------------------------
3 #
4 #
5 # geo.Coord class
6 # -------------------------------------------------------------------------------------------------
7 #
8 # geo.Coord.new([<coord>])        ... class that holds and maintains geographical coordinates
9 #                                     can be initialized with another geo.Coord instance
10 #
11 # SETTER METHODS:
12 #
13 #     .set(<coord>)               ... sets coordinates from another geo.Coord instance
14 #
15 #     .set_lat(<num>)             ... functions for setting latitude/longitude/altitude
16 #     .set_lon(<num>)
17 #     .set_alt(<num>)
18 #     .set_latlon(<num>, <num> [, <num>])      (altitude is optional; default=0)
19 #
20 #     .set_x(<num>)               ... functions for setting cartesian x/y/z coordinates
21 #     .set_y(<num>)
22 #     .set_z(<num>)
23 #     .set_xyz(<num>, <num>, <num>)
24 #
25 #
26 # GETTER METHODS:
27 #
28 #     .lat()
29 #     .lon()                      ... functions for getting lat/lon/alt
30 #     .alt()                          ... returns altitude in m
31 #     .latlon()                       ... returns vector  [<lat>, <lon>, <alt>]
32 #
33 #     .x()                        ... functions for reading cartesian coords (in m)
34 #     .y()
35 #     .z()
36 #     .xyz()                          ... returns vector  [<x>, <y>, <z>]
37 #
38 #
39 # QUERY METHODS:
40 #
41 #     .is_defined()               ... returns whether the coords are defined
42 #     .dump()                     ... outputs coordinates
43 #     .course_to(<coord>)         ... returns course to another geo.Coord instance (degree)
44 #     .distance_to(<coord>)       ... returns distance in m along Earth curvature, ignoring altitudes
45 #                                     useful for map distance
46 #     .direct_distance_to(<coord>)      ...   distance in m direct, considers altitude,
47 #                                             but cuts through Earth surface
48 #
49 #
50 # MANIPULATION METHODS:
51 #
52 #     .apply_course_distance(<course>, <distance>)       ... guess what
53 #
54 #
55 #
56 #
57 # -------------------------------------------------------------------------------------------------
58 #
59 # geo.aircraft_position()         ... returns current aircraft position as geo.Coord
60 # geo.viewer_position()           ... returns viewer position as geo.Coord
61 # geo.click_position()            ... returns last click coords as geo.Coord or nil before first click
62 #
63 # geo.tile_path(<lat>, <lon>)     ... returns tile path string (e.g. "w130n30/w123n37/942056.stg")
64 # geo.elevation(<lat>, <lon> [, <top:10000>])
65 #                                 ... returns elevation in meter for given lat/lon, or nil on error;
66 #                                     <top> is the altitude at which the intersection test starts
67 #
68 # geo.normdeg(<angle>)            ... returns angle normalized to    0 <= angle < 360
69 # geo.normdeg180(<angle>)         ... returns angle normalized to -180 < angle <= 360
70 #
71 # geo.put_model(<path>, <lat>, <lon> [, <elev:nil> [, <hdg:0> [, <pitch:0> [, <roll:0>]]]]);
72 #                                 ... put model <path> at location <lat>/<lon> with given elevation
73 #                                     (optional, default: surface). <hdg>/<pitch>/<roll> are optional
74 #                                     and default to zero.
75 # geo.put_model(<path>, <coord> [, <hdg:0> [, <pitch:0> [, <roll:0>]]]);
76 #                                 ... same as above, but lat/lon/elev are taken from a Coord object
77
78
79 var EPSILON = 1e-15;
80 var ERAD = 6378138.12;          # Earth radius (m)
81
82
83 var floor = func(v) v < 0.0 ? -int(-v) - 1 : int(v);
84
85
86 # class that maintains one set of geographical coordinates
87 #
88 var Coord = {
89         new: func(copy = nil) {
90                 var m = { parents: [Coord] };
91                 m._pdirty = 1;  # polar
92                 m._cdirty = 1;  # cartesian
93                 m._lat = nil;   # in radian
94                 m._lon = nil;   #
95                 m._alt = nil;   # ASL
96                 m._x = nil;     # in m
97                 m._y = nil;
98                 m._z = nil;
99                 if (copy != nil)
100                         m.set(copy);
101                 return m;
102         },
103         _cupdate: func {
104                 me._cdirty or return;
105                 var xyz = geodtocart(me._lat * R2D, me._lon * R2D, me._alt);
106                 me._x = xyz[0];
107                 me._y = xyz[1];
108                 me._z = xyz[2];
109                 me._cdirty = 0;
110         },
111         _pupdate: func {
112                 me._pdirty or return;
113                 var lla = carttogeod(me._x, me._y, me._z);
114                 me._lat = lla[0] * D2R;
115                 me._lon = lla[1] * D2R;
116                 me._alt = lla[2];
117                 me._pdirty = 0;
118         },
119
120         x: func { me._cupdate(); me._x },
121         y: func { me._cupdate(); me._y },
122         z: func { me._cupdate(); me._z },
123         xyz: func { me._cupdate(); [me._x, me._y, me._z] },
124
125         lat: func { me._pupdate(); me._lat * R2D },  # return in degree
126         lon: func { me._pupdate(); me._lon * R2D },
127         alt: func { me._pupdate(); me._alt },
128         latlon: func { me._pupdate(); [me._lat * R2D, me._lon * R2D, me._alt] },
129
130         set_x: func(x) { me._pupdate(); me._pdirty = 1; me._x = x; me },
131         set_y: func(y) { me._pupdate(); me._pdirty = 1; me._y = y; me },
132         set_z: func(z) { me._pupdate(); me._pdirty = 1; me._z = z; me },
133
134         set_lat: func(lat) { me._cupdate(); me._cdirty = 1; me._lat = lat * D2R; me },
135         set_lon: func(lon) { me._cupdate(); me._cdirty = 1; me._lon = lon * D2R; me },
136         set_alt: func(alt) { me._cupdate(); me._cdirty = 1; me._alt = alt; me },
137
138         set: func(c) {
139                 c._pupdate();
140                 me._lat = c._lat;
141                 me._lon = c._lon;
142                 me._alt = c._alt;
143                 me._cdirty = 1;
144                 me._pdirty = 0;
145                 me;
146         },
147         set_latlon: func(lat, lon, alt = 0) {
148                 me._lat = lat * D2R;
149                 me._lon = lon * D2R;
150                 me._alt = alt;
151                 me._cdirty = 1;
152                 me._pdirty = 0;
153                 me;
154         },
155         set_xyz: func(x, y, z) {
156                 me._x = x;
157                 me._y = y;
158                 me._z = z;
159                 me._pdirty = 1;
160                 me._cdirty = 0;
161                 me;
162         },
163         apply_course_distance: func(course, dist) {
164                 me._pupdate();
165                 course *= D2R;
166                 dist /= ERAD;
167                 
168                 if (dist < 0.0) {
169                   dist = abs(dist);
170                   course = course - math.pi;            
171                 }
172                 
173                 me._lat = math.asin(math.sin(me._lat) * math.cos(dist)
174                                 + math.cos(me._lat) * math.sin(dist) * math.cos(course));
175
176                 if (math.cos(me._lat) > EPSILON)
177                         me._lon = math.pi - math.mod(math.pi - me._lon
178                                         - math.asin(math.sin(course) * math.sin(dist)
179                                         / math.cos(me._lat)), 2 * math.pi);
180
181                 me._cdirty = 1;
182                 me;
183         },
184         course_to: func(dest) {
185                 me._pupdate();
186                 dest._pupdate();
187
188                 if (me._lat == dest._lat and me._lon == dest._lon)
189                         return 0;
190
191                 var dlon = dest._lon - me._lon;
192                 var ret = nil;
193                 call(func ret = math.mod(math.atan2(math.sin(dlon) * math.cos(dest._lat),
194                                 math.cos(me._lat) * math.sin(dest._lat)
195                                 - math.sin(me._lat) * math.cos(dest._lat)
196                                 * math.cos(dlon)), 2 * math.pi) * R2D, nil, var err = []);
197                 if (size(err)) {
198                         debug.printerror(err);
199                         debug.dump(me._lat, me._lon, dlon, dest._lat, dest._lon, "--------------------------");
200                 }
201                 return ret;
202         },
203         # arc distance on an earth sphere; doesn't consider altitude
204         distance_to: func(dest) {
205                 me._pupdate();
206                 dest._pupdate();
207
208                 if (me._lat == dest._lat and me._lon == dest._lon)
209                         return 0;
210
211                 var a = math.sin((me._lat - dest._lat) * 0.5);
212                 var o = math.sin((me._lon - dest._lon) * 0.5);
213                 return 2.0 * ERAD * math.asin(math.sqrt(a * a + math.cos(me._lat)
214                                 * math.cos(dest._lat) * o * o));
215         },
216         direct_distance_to: func(dest) {
217                 me._cupdate();
218                 dest._cupdate();
219                 var dx = dest._x - me._x;
220                 var dy = dest._y - me._y;
221                 var dz = dest._z - me._z;
222                 return math.sqrt(dx * dx + dy * dy + dz * dz);
223         },
224         is_defined: func {
225                 return !(me._cdirty and me._pdirty);
226         },
227         dump: func {
228                 if (me._cdirty and me._pdirty)
229                         print("Coord.dump(): coordinates undefined");
230
231                 me._cupdate();
232                 me._pupdate();
233                 printf("x=%f  y=%f  z=%f    lat=%f  lon=%f  alt=%f",
234                                 me.x(), me.y(), me.z(), me.lat(), me.lon(), me.alt());
235         },
236 };
237
238
239 # normalize degree to 0 <= angle < 360
240 #
241 var normdeg = func(angle) {
242         while (angle < 0)
243                 angle += 360;
244         while (angle >= 360)
245                 angle -= 360;
246         return angle;
247 }
248
249 # normalize degree to -180 < angle <= 180
250 #
251 var normdeg180 = func(angle) {
252         while (angle <= -180)
253                 angle += 360;
254         while (angle > 180)
255                 angle -= 360;
256         return angle;
257 }
258
259 var tile_index = func(lat, lon) {
260     return tileIndex(lat, lon);
261 }
262
263
264 var format = func(lat, lon) {
265         sprintf("%s%03d%s%02d", lon < 0 ? "w" : "e", abs(lon), lat < 0 ? "s" : "n", abs(lat));
266 }
267
268
269 var tile_path = func(lat, lon) {
270         var p = tilePath(lat, lon) ~ "/" ~ tileIndex(lat, lon) ~ ".stg";
271 }
272
273
274 var put_model = func(path, c, arg...) {
275         call(_put_model, [path] ~ (isa(c, Coord) ? c.latlon() : [c]) ~ arg);
276 }
277
278
279 var _put_model = func(path, lat, lon, elev_m = nil, hdg = 0, pitch = 0, roll = 0) {
280         if (elev_m == nil)
281                 elev_m = elevation(lat, lon);
282         if (elev_m == nil)
283                 die("geo.put_model(): cannot get elevation for " ~ lat ~ "/" ~ lon);
284         fgcommand("add-model", var n = props.Node.new({ "path": path,
285                 "latitude-deg": lat, "longitude-deg": lon, "elevation-m": elev_m,
286                 "heading-deg": hdg, "pitch-deg": pitch, "roll-deg": roll,
287         }));
288         return props.globals.getNode(n.getNode("property").getValue());
289 }
290
291
292 var elevation = func(lat, lon, maxalt = 10000) {
293         var d = geodinfo(lat, lon, maxalt);
294         return d == nil ? nil : d[0];
295 }
296
297
298 var click_coord = Coord.new();
299
300 _setlistener("/sim/signals/click", func {
301         var lat = getprop("/sim/input/click/latitude-deg");
302         var lon = getprop("/sim/input/click/longitude-deg");
303         var elev = getprop("/sim/input/click/elevation-m");
304         click_coord.set_latlon(lat, lon, elev);
305 });
306
307 var click_position = func {
308         return click_coord.is_defined() ? Coord.new(click_coord) : nil;
309 }
310
311
312 var aircraft_position = func {
313         var lat = getprop("/position/latitude-deg");
314         var lon = getprop("/position/longitude-deg");
315         var alt = getprop("/position/altitude-ft") * FT2M;
316         return Coord.new().set_latlon(lat, lon, alt);
317 }
318
319
320 var viewer_position = func {
321         var x = getprop("/sim/current-view/viewer-x-m");
322         var y = getprop("/sim/current-view/viewer-y-m");
323         var z = getprop("/sim/current-view/viewer-z-m");
324         return Coord.new().set_xyz(x, y, z);
325 }
326
327 # A object to handle differential positioned searches:
328 # searchCmd executes and returns the actual search,
329 # onAdded and onRemoved are callbacks,
330 # and obj is a "me" reference (defaults to "me" in the
331 # caller's namespace). If searchCmd returns nil, nothing
332 # happens, i.e. the diff is cancelled.
333 var PositionedSearch = {
334         new: func(searchCmd, onAdded, onRemoved, obj=nil) {
335                 return {
336                         parents:[PositionedSearch],
337                         obj: obj == nil ? caller(1)[0]["me"] : obj,
338                         searchCmd: searchCmd,
339                         onAdded: onAdded,
340                         onRemoved: onRemoved,
341                         result: [],
342                 };
343         },
344         _equals: func(a,b) {
345                 return a == b; # positioned objects are created once
346                 #return (a == b or a.id == b.id);
347         },
348         condense: func(vec) {
349                 var ret = [];
350                 foreach (var e; vec)
351                         if (e != nil) append(ret, e);
352                 return ret;
353         },
354         diff: func(old, new) {
355                 if (new == nil)
356                         return [old, [], []];
357                 var removed = old~[]; #copyvec
358                 var added = new~[];
359                 # Mark common elements from removed and added:
360                 forindex (OUTER; var i; removed)
361                         forindex (var j; new)
362                                 if (removed[i] != nil and added[j] != nil and me._equals(removed[i], added[j])) {
363                                         removed[i] = added[j] = nil;
364                                         continue OUTER;
365                                 }
366                 # And remove those common elements, returning the result:
367                 return [new, me.condense(removed), me.condense(added)];
368         },
369         update: func(searchCmd=nil) {
370                 if (searchCmd == nil) searchCmd = me.searchCmd;
371                 if (me._equals == PositionedSearch._equals) {
372                         # Optimized search using C code
373                         var old = me.result~[]; #copyvec
374                         me.result = call(searchCmd, nil, me.obj);
375                         if (me.result == nil)
376                         { me.result = old; return }
377                         if (typeof(me.result) != 'vector') die("geo.PositionedSearch(): A searchCmd must return a vector of elements or nil !!"); # TODO: Maybe make this a hash instead to wrap a vector, so that we can implement basic type-checking - e.g. doing isa(PositionedSearchResult, me.result) would be kinda neat and could help troubleshooting
378                         else
379                         positioned.diff( old,
380                                          me.result,
381                                          func call(me.onAdded, arg, me.obj),
382                                          func call(me.onRemoved, arg, me.obj) );
383                 } else {
384                         (me.result, var removed, var added) = me.diff(me.result, call(searchCmd, nil, me.obj));
385                         foreach (var e; removed)
386                                 call(me.onRemoved, [e], me.obj);
387                         foreach (var e; added)
388                                 call(me.onAdded, [e], me.obj);
389                 }
390         },
391         # this is the worst case scenario: switching from 640 to 320 (or vice versa)
392         test: func(from=640, to=320) {
393           var s= geo.PositionedSearch.new(
394             func positioned.findWithinRange(from, 'fix'),
395             func print('added:', arg[0].id),
396             func print('removed:', arg[0].id)
397           );
398           debug.benchmark('Toggle '~from~'nm/'~to~'nm', func {
399             s.update();
400             s.update( func positioned.findWithinRange(to, 'fix') );
401           }); # ~ takes 
402         }, # of test
403 };