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