Nasal/geo.nas

   1 # geo functions
   2 # -------------------------------------------------------------------------------------------------
   3 # geo.Coord.new([<coord>])        ... class that holds and maintains geographical coordinates
   4 #                                     can be initialized with another geo.Coord instance
   5 #     Coord.set(<coord>)          ... sets coordinates from another geo.Coord instance
   6 #
   7 #     Coord.set_lat(<num>)        ... functions for setting latitude/longitude/altitude
   8 #     Coord.set_lon(<num>)
   9 #     Coord.set_alt(<num>)
  10 #     Coord.set_latlon(<num>, <num> [, <num>])      (altitude is optional; default=0)
  11 #
  12 #     Coord.set_x(<num>)          ... functions for setting cartesian x/y/z coordinates
  13 #     Coord.set_y(<num>)
  14 #     Coord.set_z(<num>)
  15 #     Coord.set_xyz(<num>, <num>, <num>)
  16 #
  17 #     Coord.lat()
  18 #     Coord.lon()                 ... functions for getting lat/lon/alt
  19 #     Coord.alt()                     ... returns altitude in m
  20 #     Coord.latlon()                  ... returns array  [<lat>, <lon>, <alt>]
  21 #
  22 #     Coord.x()                   ... functions for reading cartesian coords (in m)
  23 #     Coord.y()
  24 #     Coord.z()
  25 #     Coord.xyz()                     ... returns array  [<x>, <y>, <z>]
  26 #
  27 #     Coord.course_to(<coord>)    ... returns course to another geo.Coord instance (degree)
  28 #     Coord.distance_to(<coord>)  ... returns distance in m along Earth curvature, ignoring altitudes
  29 #                                     useful for map distance
  30 #     Coord.direct_distance_to(<coord>) ...   distance in m direct, considers altitude,
  31 #                                             but cuts through Earth surface
  32 #
  33 #     Coord.apply_course_distance(<course>, <distance>)       ... guess what
  34 #     Coord.dump()                ... outputs coordinates
  35 #     Coord.is_defined()          ... returns whether the coords are defined
  36 #
  37 # geo.aircraft_position()         ... returns current aircraft position as geo.Coord
  38 # geo.click_position()            ... returns last click coords as geo.Coord or nil before first click
  39 #
  40 # geo.tile_path(<lat>, <lon>)     ... returns tile path string (e.g. "w130n30/w123n37/942056.stg")
  41 # geo.elevation(<lat>, <lon>)     ... returns elevation in meter for given lat/lon, or nil on error
  42 # geo.normdeg(<angle>)            ... returns angle normalized to  0 <= angle < 360
  43 #
  44 # geo.put_model(<path>, <lat>, <lon> [, <elev:nil> [, <hdg:0> [, <pitch:0> [, <roll:0>]]]]);
  45 #                                 ... put model <path> at location <lat>/<lon> with given elevation
  46 #                                     (optional, default: surface). <hdg>/<pitch>/<roll> are optional
  47 #                                     and default to zero.
  48 # geo.put_model(<path>, <coord> [, <hdg:0> [, <pitch:0> [, <roll:0>]]]);
  49 #                                 ... same as above, but lat/lon/elev are taken from a Coord object
  50
  51
  52 var EPSILON = 0.0000000000001;
  53 var ERAD = 6378138.12;          # Earth radius (m)
  54 var D2R = math.pi / 180;
  55 var R2D = 180 / math.pi;
  56 var FT2M = 0.3048;
  57 var M2FT = 3.28083989501312335958;
  58
  59
  60 var printf = func { print(call(sprintf, arg)) }
  61 var floor = func(v) { v < 0.0 ? -int(-v) - 1 : int(v) }
  62 var sin = nil;
  63 var cos = nil;
  64 var atan2 = nil;
  65 var sqrt = nil;
  66 var asin = nil;
  67 var acos = nil;
  68 var mod = nil;
  69
  70
  71 # class that maintains one set of geographical coordinates
  72 #
  73 var Coord = {
  74         new : func(copy = nil) {
  75                 var m = { parents: [Coord] };
  76                 m._pdirty = 1;  # polar
  77                 m._cdirty = 1;  # cartesian
  78                 m._lat = nil;   # in radian
  79                 m._lon = nil;   #
  80                 m._alt = nil;   # ASL
  81                 m._x = nil;     # in m
  82                 m._y = nil;
  83                 m._z = nil;
  84                 if (copy != nil)
  85                         m.set(copy);
  86
  87                 return m;
  88         },
  89         _cupdate : func {
  90                 me._cdirty or return;
  91                 var xyz = geodtocart(me._lat * R2D, me._lon * R2D, me._alt);
  92                 me._x = xyz[0];
  93                 me._y = xyz[1];
  94                 me._z = xyz[2];
  95                 me._cdirty = 0;
  96         },
  97         _pupdate : func {
  98                 me._pdirty or return;
  99                 var lla = carttogeod(me._x, me._y, me._z);
 100                 me._lat = lla[0] * D2R;
 101                 me._lon = lla[1] * D2R;
 102                 me._alt = lla[2];
 103                 me._pdirty = 0;
 104         },
 105
 106         x : func { me._cupdate(); me._x },
 107         y : func { me._cupdate(); me._y },
 108         z : func { me._cupdate(); me._z },
 109         xyz : func { me._cupdate(); [me._x, me._y, me._z] },
 110
 111         lat : func { me._pupdate(); me._lat * R2D },  # return in degree
 112         lon : func { me._pupdate(); me._lon * R2D },
 113         alt : func { me._pupdate(); me._alt },
 114         latlon : func { me._pupdate(); [me._lat * R2D, me._lon * R2D, me._alt] },
 115
 116         set_x : func(x) { me._pupdate(); me._pdirty = 1; me._x = x; me },
 117         set_y : func(y) { me._pupdate(); me._pdirty = 1; me._y = y; me },
 118         set_z : func(z) { me._pupdate(); me._pdirty = 1; me._z = z; me },
 119
 120         set_lat : func(lat) { me._cupdate(); me._cdirty = 1; me._lat = lat * D2R; me },
 121         set_lon : func(lon) { me._cupdate(); me._cdirty = 1; me._lon = lon * D2R; me },
 122         set_alt : func(alt) { me._cupdate(); me._cdirty = 1; me._alt = alt; me },
 123
 124         set : func(c) {
 125                 c._pupdate();
 126                 me._lat = c._lat;
 127                 me._lon = c._lon;
 128                 me._alt = c._alt;
 129                 me._cdirty = 1;
 130                 me._pdirty = 0;
 131                 me;
 132         },
 133         set_latlon : func(lat, lon, alt = 0) {
 134                 me._lat = lat * D2R;
 135                 me._lon = lon * D2R;
 136                 me._alt = alt;
 137                 me._cdirty = 1;
 138                 me._pdirty = 0;
 139                 me;
 140         },
 141         set_xyz : func(x, y, z) {
 142                 me._x = x;
 143                 me._y = y;
 144                 me._z = z;
 145                 me._pdirty = 1;
 146                 me._cdirty = 0;
 147                 me;
 148         },
 149         apply_course_distance : func(course, dist) {
 150                 me._pupdate();
 151                 course *= D2R;
 152                 dist /= ERAD;
 153                 me._lat = asin(sin(me._lat) * cos(dist) + cos(me._lat) * sin(dist) * cos(course));
 154
 155                 if (cos(me._lat) > EPSILON)
 156                         me._lon = math.pi - mod(math.pi - me._lon - asin(sin(course) * sin(dist)
 157                                         / cos(me._lat)), 2 * math.pi);
 158
 159                 me._cdirty = 1;
 160                 me;
 161         },
 162         course_to : func(dest) {
 163                 me._pupdate();
 164                 dest._pupdate();
 165
 166                 if (me._lat == dest._lat and me._lon == dest._lon)
 167                         return 0;
 168
 169                 var dlon = dest._lon - me._lon;
 170                 return mod(atan2(sin(dlon) * cos(dest._lat), cos(me._lat) * sin(dest._lat)
 171                                 - sin(me._lat) * cos(dest._lat) * cos(dlon)), 2 * math.pi) * R2D;
 172         },
 173         # arc distance on an earth sphere; doesn't consider altitude
 174         distance_to : func(dest) {
 175                 me._pupdate();
 176                 dest._pupdate();
 177
 178                 if (me._lat == dest._lat and me._lon == dest._lon)
 179                         return 0;
 180
 181                 var a = sin((me._lat - dest._lat) * 0.5);
 182                 var o = sin((me._lon - dest._lon) * 0.5);
 183                 return 2.0 * ERAD * asin(sqrt(a * a + cos(me._lat) * cos(dest._lat) * o * o));
 184         },
 185         direct_distance_to : func(dest) {
 186                 me._cupdate();
 187                 dest._cupdate();
 188                 var dx = dest._x - me._x;
 189                 var dy = dest._y - me._y;
 190                 var dz = dest._z - me._z;
 191                 return sqrt(dx * dx + dy * dy + dz * dz);
 192         },
 193         is_defined : func {
 194                 return !(me._cdirty and me._pdirty);
 195         },
 196         dump : func {
 197                 if (me._cdirty and me._pdirty)
 198                         print("Coord.print(): coord undefined");
 199
 200                 me._cupdate();
 201                 me._pupdate();
 202                 printf("x=%f  y=%f  z=%f    lat=%f  lon=%f  alt=%f",
 203                                 me.x(), me.y(), me.z(), me.lat(), me.lon(), me.alt());
 204         },
 205 };
 206
 207
 208 # normalize degree to 0 <= angle < 360
 209 #
 210 var normdeg = func(angle) {
 211         while (angle < 0)
 212                 angle += 360;
 213         while (angle >= 360)
 214                 angle -= 360;
 215         return angle;
 216 }
 217
 218
 219 var bucket_span = func(lat) {
 220         if (lat >= 89.0)
 221                 360.0;
 222         elsif (lat >= 88.0)
 223                 8.0;
 224         elsif (lat >= 86.0)
 225                 4.0;
 226         elsif (lat >= 83.0)
 227                 2.0;
 228         elsif (lat >= 76.0)
 229                 1.0;
 230         elsif (lat >= 62.0)
 231                 0.5;
 232         elsif (lat >= 22.0)
 233                 0.25;
 234         elsif (lat >= -22.0)
 235                 0.125;
 236         elsif (lat >= -62.0)
 237                 0.25;
 238         elsif (lat >= -76.0)
 239                 0.5;
 240         elsif (lat >= -83.0)
 241                 1.0;
 242         elsif (lat >= -86.0)
 243                 2.0;
 244         elsif (lat >= -88.0)
 245                 4.0;
 246         elsif (lat >= -89.0)
 247                 8.0;
 248         else
 249                 360.0;
 250 }
 251
 252
 253 var tile_index = func(lat, lon) {
 254         var lat_floor = floor(lat);
 255         var lon_floor = floor(lon);
 256         var span = bucket_span(lat);
 257         var x = 0;
 258
 259         if (span < 0.0000001) {
 260                 lon = 0;
 261         } elsif (span <= 1.0) {
 262                 x = int((lon - lon_floor) / span);
 263         } else {
 264                 if (lon >= 0) {
 265                         lon = int(int(lon / span) * span);
 266                 } else {
 267                         lon = int(int((lon + 1) / span) * span - span);
 268                         if (lon < -180)
 269                                 lon = -180;
 270                 }
 271         }
 272
 273         var y = int((lat - lat_floor) * 8);
 274         return (lon_floor + 180) * 16384 + (lat_floor + 90) * 64 + y * 8 + x;
 275 }
 276
 277
 278 var format = func(lat, lon) {
 279         sprintf("%s%03d%s%02d", lon < 0 ? "w" : "e", abs(lon), lat < 0 ? "s" : "n", abs(lat));
 280 }
 281
 282
 283 var tile_path = func(lat, lon) {
 284         var p = format(floor(lat / 10.0) * 10, floor(lon / 10.0) * 10);
 285         p ~= "/" ~ format(floor(lat), floor(lon));
 286         p ~= "/" ~ tile_index(lat, lon) ~ ".stg";
 287 }
 288
 289
 290 var put_model = func(path, c, arg...) {
 291         call(_put_model, [path] ~ (isa(c, Coord) ? c.latlon() : [c]) ~ arg);
 292 }
 293
 294
 295 var _put_model = func(path, lat, lon, elev_m = nil, hdg = 0, pitch = 0, roll = 0) {
 296         if (elev_m == nil)
 297                 elev_m = elevation(lat, lon);
 298         if (elev_m == nil)
 299                 die("can't get elevation for " ~ lat ~ "/" ~ lon);
 300         var n = props.globals.getNode("/models");
 301         for (var i = 0; 1; i += 1)
 302                 if (n.getChild("model", i, 0) == nil)
 303                         break;
 304         n = n.getChild("model", i, 1);
 305         n.getNode("path", 1).setValue(path);
 306         n.getNode("latitude-deg", 1).setDoubleValue(lat);
 307         n.getNode("longitude-deg", 1).setDoubleValue(lon);
 308         n.getNode("elevation-ft", 1).setDoubleValue(elev_m * M2FT);
 309         n.getNode("heading-deg", 1).setDoubleValue(hdg);
 310         n.getNode("pitch-deg", 1).setDoubleValue(pitch);
 311         n.getNode("roll-deg", 1).setDoubleValue(roll);
 312         n.getNode("load", 1).setBoolValue(1);
 313         n.removeChildren("load");
 314         return n;
 315 }
 316
 317
 318 var elevation = func(lat, lon) {
 319         var d = geodinfo(lat, lon);
 320         return d == nil ? nil : d[0];
 321 }
 322
 323
 324 var aircraft_lat = nil;
 325 var aircraft_lon = nil;
 326 var aircraft_alt = nil;
 327
 328 var aircraft_position = func {
 329         var lat = aircraft_lat.getValue();
 330         var lon = aircraft_lon.getValue();
 331         var alt = aircraft_alt.getValue() * FT2M;
 332         return Coord.new().set_latlon(lat, lon, alt);
 333 }
 334
 335
 336 var click_lat = nil;
 337 var click_lon = nil;
 338 var click_elev = nil;
 339 var click_coord = Coord.new();
 340
 341 _setlistener("/sim/signals/click", func {
 342         var lat = click_lat.getValue();
 343         var lon = click_lon.getValue();
 344         var elev = click_elev.getValue();
 345         click_coord.set_latlon(lat, lon, elev);
 346 });
 347
 348 var click_position = func {
 349         return click_coord.is_defined() ? Coord.new(click_coord) : nil;
 350 }
 351
 352
 353
 354 _setlistener("/sim/signals/nasal-dir-initialized", func {
 355         sin = math.sin;
 356         cos = math.cos;
 357         atan2 = math.atan2;
 358         sqrt = math.sqrt;
 359         asin = math.asin;
 360         acos = math.acos;
 361         mod = math.mod;
 362
 363         aircraft_lat = props.globals.getNode("/position/latitude-deg", 1);
 364         aircraft_lon = props.globals.getNode("/position/longitude-deg", 1);
 365         aircraft_alt = props.globals.getNode("/position/altitude-ft", 1);
 366
 367         click_lat = props.globals.getNode("/sim/input/click/latitude-deg", 1);
 368         click_lon = props.globals.getNode("/sim/input/click/longitude-deg", 1);
 369         click_elev = props.globals.getNode("/sim/input/click/elevation-m", 1);
 370 });
 371