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 };