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>)             ..this is in meters
18#     .set_latlon(<num>, <num> [, <num>])      (altitude (meters) 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#     .greatcircle_distance_to(<coord>, <coord>)  ... returns distance to a great circle (in m along Earth curvature)
49#                                                     defined by two points
50#     .horizon()                  ... returns distance to the horizon in m along Earth curvature, ignoring altitudes
51#
52#
53# MANIPULATION METHODS:
54#
55#     .apply_course_distance(<course>, <distance>)       ... moves the coord distance in meters in course direction (true)
56#
57#
58#
59#
60# -------------------------------------------------------------------------------------------------
61#
62# geo.aircraft_position()         ... returns current aircraft position as geo.Coord
63# geo.viewer_position()           ... returns viewer position as geo.Coord
64# geo.click_position()            ... returns last click coords as geo.Coord or nil before first click
65#
66# geo.tile_path(<lat>, <lon>)     ... returns tile path string (e.g. "w130n30/w123n37/942056.stg")
67# geo.elevation(<lat>, <lon> [, <top:10000>])
68#                                 ... returns elevation in meter for given lat/lon, or nil on error;
69#                                     <top> is the altitude at which the intersection test starts
70#
71# geo.normdeg(<angle>)            ... returns angle normalized to    0 <= angle < 360
72# geo.normdeg180(<angle>)         ... returns angle normalized to -180 < angle <= 360
73#
74# geo.put_model(<path>, <lat>, <lon> [, <elev:nil> [, <hdg:0> [, <pitch:0> [, <roll:0>]]]]);
75#                                 ... put model <path> at location <lat>/<lon> with given elevation
76#                                     (optional, default: surface). <hdg>/<pitch>/<roll> are optional
77#                                     and default to zero.
78# geo.put_model(<path>, <coord> [, <hdg:0> [, <pitch:0> [, <roll:0>]]]);
79#                                 ... same as above, but lat/lon/elev are taken from a Coord object
80
81
82var EPSILON = 1e-15;
83var ERAD = 6378138.12;		# Earth radius (m)
84
85# class that maintains one set of geographical coordinates
86#
87var Coord = {
88	new: func(copy = nil) {
89		var m = { parents: [Coord] };
90		m._pdirty = 1;  # polar
91		m._cdirty = 1;  # cartesian
92		m._lat = nil;   # in radian
93		m._lon = nil;   #
94		m._alt = nil;   # ASL
95		m._x = nil;     # in m
96		m._y = nil;
97		m._z = nil;
98		if (copy != nil)
99			m.set(copy);
100		return m;
101	},
102	_cupdate: func {
103		me._cdirty or return;
104		var xyz = geodtocart(me._lat * R2D, me._lon * R2D, me._alt);
105		me._x = xyz[0];
106		me._y = xyz[1];
107		me._z = xyz[2];
108		me._cdirty = 0;
109	},
110	_pupdate: func {
111		me._pdirty or return;
112		var lla = carttogeod(me._x, me._y, me._z);
113		me._lat = lla[0] * D2R;
114		me._lon = lla[1] * D2R;
115		me._alt = lla[2];
116		me._pdirty = 0;
117	},
118
119	x: func { me._cupdate(); me._x },
120	y: func { me._cupdate(); me._y },
121	z: func { me._cupdate(); me._z },
122	xyz: func { me._cupdate(); [me._x, me._y, me._z] },
123
124	lat: func { me._pupdate(); me._lat * R2D },  # return in degree
125	lon: func { me._pupdate(); me._lon * R2D },
126	alt: func { me._pupdate(); me._alt },
127	latlon: func { me._pupdate(); [me._lat * R2D, me._lon * R2D, me._alt] },
128
129	set_x: func(x) { me._cupdate(); me._pdirty = 1; me._x = x; me },
130	set_y: func(y) { me._cupdate(); me._pdirty = 1; me._y = y; me },
131	set_z: func(z) { me._cupdate(); me._pdirty = 1; me._z = z; me },
132
133	set_lat: func(lat) { me._pupdate(); me._cdirty = 1; me._lat = lat * D2R; me },
134	set_lon: func(lon) { me._pupdate(); me._cdirty = 1; me._lon = lon * D2R; me },
135	set_alt: func(alt) { me._pupdate(); me._cdirty = 1; me._alt = alt; me },
136
137	set: func(c) {
138		c._pupdate();
139		me._lat = c._lat;
140		me._lon = c._lon;
141		me._alt = c._alt;
142		me._cdirty = 1;
143		me._pdirty = 0;
144		me;
145	},
146	set_latlon: func(lat, lon, alt = 0) {
147		me._lat = lat * D2R;
148		me._lon = lon * D2R;
149		me._alt = alt;
150		me._cdirty = 1;
151		me._pdirty = 0;
152		me;
153	},
154	set_xyz: func(x, y, z) {
155		me._x = x;
156		me._y = y;
157		me._z = z;
158		me._pdirty = 1;
159		me._cdirty = 0;
160		me;
161	},
162	apply_course_distance: func(course, dist) {
163		me._pupdate();
164		course *= D2R;
165		dist /= ERAD;
166
167		if (dist < 0.0) {
168		  dist = abs(dist);
169		  course = course - math.pi;
170		}
171
172		me._lat = math.asin(math.sin(me._lat) * math.cos(dist)
173				+ math.cos(me._lat) * math.sin(dist) * math.cos(course));
174
175		if (math.cos(me._lat) > EPSILON)
176			me._lon = math.pi - math.mod(math.pi - me._lon
177					- math.asin(math.sin(course) * math.sin(dist)
178					/ math.cos(me._lat)), 2 * math.pi);
179
180		me._cdirty = 1;
181		me;
182	},
183	course_to: func(dest) {
184		me._pupdate();
185		dest._pupdate();
186
187		if (me._lat == dest._lat and me._lon == dest._lon)
188			return 0;
189
190		var dlon = dest._lon - me._lon;
191		var ret = nil;
192		call(func ret = math.mod(math.atan2(math.sin(dlon) * math.cos(dest._lat),
193				math.cos(me._lat) * math.sin(dest._lat)
194				- math.sin(me._lat) * math.cos(dest._lat)
195				* math.cos(dlon)), 2 * math.pi) * R2D, nil, var err = []);
196		if (size(err)) {
197			debug.printerror(err);
198			debug.dump(me._lat, me._lon, dlon, dest._lat, dest._lon, "--------------------------");
199		}
200		return ret;
201	},
202	# arc distance on an earth sphere; doesn't consider altitude
203	distance_to: func(dest) {
204		me._pupdate();
205		dest._pupdate();
206
207		if (me._lat == dest._lat and me._lon == dest._lon)
208			return 0;
209
210		var a = math.sin((me._lat - dest._lat) * 0.5);
211		var o = math.sin((me._lon - dest._lon) * 0.5);
212		return 2.0 * ERAD * math.asin(math.sqrt(a * a + math.cos(me._lat)
213				* math.cos(dest._lat) * o * o));
214	},
215	direct_distance_to: func(dest) {
216		me._cupdate();
217		dest._cupdate();
218		var dx = dest._x - me._x;
219		var dy = dest._y - me._y;
220		var dz = dest._z - me._z;
221		return math.sqrt(dx * dx + dy * dy + dz * dz);
222	},
223	# arc distance on an earth sphere to the great circle passing by A and B; doesn't consider altitude
224	greatcircle_distance_to: func(destA, destB) {
225		me._pupdate();
226		destA._pupdate();
227		destB._pupdate();
228
229		# AB is not a circle but a point
230		if (destA._lat == destB._lat and destA._lon == destB._lon) {
231		    return me.distance_to(destA);
232		}
233
234		var ca1 = math.cos(destA._lon);
235		var cd1 = math.cos(destA._lat);
236		var sa1 = math.sin(destA._lon);
237		var sd1 = math.sin(destA._lat);
238
239		var ca2 = math.cos(destB._lon);
240		var cd2 = math.cos(destB._lat);
241		var sa2 = math.sin(destB._lon);
242		var sd2 = math.sin(destB._lat);
243
244		var sa12 = math.sin(destA._lon - destB._lon);
245
246    		var ca3 = math.cos(me._lon);
247		var cd3 = math.cos(me._lat);
248		var sa3 = math.sin(me._lon);
249		var sd3 = math.sin(me._lat);
250
251		# this is sin(greatcircle_dist) * sin(arcAB)
252                var sDsAB = cd3 * sa3 * (ca2 * cd2 * sd1 - ca1 * cd1 * sd2 )
253		    + ca3 * cd3 * ( cd1 * sa1 * sd2 - cd2 * sa2 * sd1 )
254		    - cd1 * cd2 * sd3 * sa12;
255
256		# direct calculation of sin(arcAB) to not call sin(arcsin(distance_to))
257		var a = math.sin((destA._lat - destB._lat) * 0.5);
258		var o = math.sin((destA._lon - destB._lon) * 0.5);
259
260		var hs12 = a * a + cd1 * cd2 * o * o;
261		var hc12 = 1.0 - hs12;
262
263
264		# AB is undertermined; a great circle should be defined with non-colinear vectors
265		if (hs12*hc12 == 0.0) {
266		    die("Great circles are defined with non-colinear vectors");
267		}
268
269
270		return ERAD * math.abs( math.asin( 0.5 * sDsAB / math.sqrt( hs12 * hc12 ) ) );
271	},
272	# arc distance on an earth sphere to the horizon
273        horizon: func() {
274	        me._pupdate();
275		if (me._alt < 0.0) {
276		    return 0.0;
277		}
278		else {
279		    return ERAD*math.acos(ERAD/(ERAD+me._alt));
280		}
281	},
282	is_defined: func {
283		return !(me._cdirty and me._pdirty);
284	},
285	dump: func {
286		if (me._cdirty and me._pdirty)
287			print("Coord.dump(): coordinates undefined");
288
289		me._cupdate();
290		me._pupdate();
291		printf("x=%f  y=%f  z=%f    lat=%f  lon=%f  alt=%f",
292				me.x(), me.y(), me.z(), me.lat(), me.lon(), me.alt());
293	},
294};
295
296
297# normalize degree to 0 <= angle < 360
298#
299var normdeg = func(angle) {
300	while (angle < 0)
301		angle += 360;
302	while (angle >= 360)
303		angle -= 360;
304	return angle;
305}
306
307# normalize degree to -180 < angle <= 180
308#
309var normdeg180 = func(angle) {
310	while (angle <= -180)
311		angle += 360;
312	while (angle > 180)
313		angle -= 360;
314	return angle;
315}
316
317var tile_index = func(lat, lon) {
318    return tileIndex(lat, lon);
319}
320
321
322var format = func(lat, lon) {
323	sprintf("%s%03d%s%02d", lon < 0 ? "w" : "e", abs(lon), lat < 0 ? "s" : "n", abs(lat));
324}
325
326
327var tile_path = func(lat, lon) {
328	var p = tilePath(lat, lon) ~ "/" ~ tileIndex(lat, lon) ~ ".stg";
329}
330
331
332var put_model = func(path, c, arg...) {
333	call(_put_model, [path] ~ (isa(c, Coord) ? c.latlon() : [c]) ~ arg);
334}
335
336
337var _put_model = func(path, lat, lon, elev_m = nil, hdg = 0, pitch = 0, roll = 0) {
338	if (elev_m == nil)
339		elev_m = elevation(lat, lon);
340	if (elev_m == nil)
341		die("geo.put_model(): cannot get elevation for " ~ lat ~ "/" ~ lon);
342	fgcommand("add-model", var n = props.Node.new({ "path": path,
343		"latitude-deg": lat, "longitude-deg": lon, "elevation-m": elev_m,
344		"heading-deg": hdg, "pitch-deg": pitch, "roll-deg": roll,
345	}));
346	return props.globals.getNode(n.getNode("property").getValue());
347}
348
349
350var elevation = func(lat, lon, maxalt = 10000) {
351	var d = geodinfo(lat, lon, maxalt);
352	return d == nil ? nil : d[0];
353}
354
355
356var click_coord = Coord.new();
357
358_setlistener("/sim/signals/click", func {
359	var lat = getprop("/sim/input/click/latitude-deg");
360	var lon = getprop("/sim/input/click/longitude-deg");
361	var elev = getprop("/sim/input/click/elevation-m");
362	click_coord.set_latlon(lat, lon, elev);
363});
364
365var click_position = func {
366	return click_coord.is_defined() ? Coord.new(click_coord) : nil;
367}
368
369
370var aircraft_position = func {
371	var lat = getprop("/position/latitude-deg");
372	var lon = getprop("/position/longitude-deg");
373	var alt = getprop("/position/altitude-ft") * FT2M;
374	return Coord.new().set_latlon(lat, lon, alt);
375}
376
377
378var viewer_position = func {
379	var x = getprop("/sim/current-view/viewer-x-m");
380	var y = getprop("/sim/current-view/viewer-y-m");
381	var z = getprop("/sim/current-view/viewer-z-m");
382	return Coord.new().set_xyz(x, y, z);
383}
384
385# A object to handle differential positioned searches:
386# searchCmd executes and returns the actual search,
387# onAdded and onRemoved are callbacks,
388# and obj is a "me" reference (defaults to "me" in the
389# caller's namespace). If searchCmd returns nil, nothing
390# happens, i.e. the diff is cancelled.
391var PositionedSearch = {
392	new: func(searchCmd, onAdded, onRemoved, obj=nil) {
393		return {
394			parents:[PositionedSearch],
395			obj: obj == nil ? caller(1)[0]["me"] : obj,
396			searchCmd: searchCmd,
397			onAdded: onAdded,
398			onRemoved: onRemoved,
399			result: [],
400		};
401	},
402	_equals: func(a,b) {
403		return a == b; # positioned objects are created once
404		#return (a == b or a.id == b.id);
405	},
406	condense: func(vec) {
407		var ret = [];
408		foreach (var e; vec)
409			if (e != nil) append(ret, e);
410		return ret;
411	},
412	diff: func(old, new) {
413		if (new == nil)
414			return [old, [], []];
415		var removed = old~[]; #copyvec
416		var added = new~[];
417		# Mark common elements from removed and added:
418		forindex (OUTER; var i; removed)
419			forindex (var j; new)
420				if (removed[i] != nil and added[j] != nil and me._equals(removed[i], added[j])) {
421					removed[i] = added[j] = nil;
422					continue OUTER;
423				}
424		# And remove those common elements, returning the result:
425		return [new, me.condense(removed), me.condense(added)];
426	},
427	update: func(searchCmd=nil) {
428		if (searchCmd == nil) searchCmd = me.searchCmd;
429		if (me._equals == PositionedSearch._equals) {
430			# Optimized search using C code
431			var old = me.result~[]; #copyvec
432			me.result = call(searchCmd, nil, me.obj);
433			if (me.result == nil)
434			{ me.result = old; return }
435			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
436			else
437			positioned.diff( old,
438			                 me.result,
439			                 func call(me.onAdded, arg, me.obj),
440			                 func call(me.onRemoved, arg, me.obj) );
441		} else {
442			(me.result, var removed, var added) = me.diff(me.result, call(searchCmd, nil, me.obj));
443			foreach (var e; removed)
444				call(me.onRemoved, [e], me.obj);
445			foreach (var e; added)
446				call(me.onAdded, [e], me.obj);
447		}
448	},
449	# this is the worst case scenario: switching from 640 to 320 (or vice versa)
450	test: func(from=640, to=320) {
451	  var s= geo.PositionedSearch.new(
452	    func positioned.findWithinRange(from, 'fix'),
453	    func print('added:', arg[0].id),
454	    func print('removed:', arg[0].id)
455	  );
456	  debug.benchmark('Toggle '~from~'nm/'~to~'nm', func {
457	    s.update();
458	    s.update( func positioned.findWithinRange(to, 'fix') );
459	  }); # ~ takes
460	}, # of test
461};
462