Generated: Sat Oct 24 16:35:23 2020 from ll2xyz.pl 2019/03/06 17.1 KB. text copy
#!/perl -w # NAME: ll2xyz.pl # AIM: Convert latitude, logitude to x,y,z co-ordinates # 2019-03-03 - update # 2008-12-17 - initial cut use strict; use warnings; use Time::HiRes qw(gettimeofday tv_interval); # provide more accurate timings # use constant PI => 4 * atan2(1, 1); use File::Basename; # split path ($name,$dir,$ext) = fileparse($file [, qr/\.[^.]*/] ) use File::stat; # to get the file date and size use Cwd; my $os = $^O; my $perl_dir = '/home/geoff/bin'; my $PATH_SEP = '/'; my $temp_dir = '/tmp'; if ($os =~ /win/i) { $perl_dir = 'C:\GTools\perl'; $temp_dir = $perl_dir; $PATH_SEP = "\\"; } unshift(@INC, $perl_dir); require 'logfile.pl' or die "Unable to load logfile.pl ...\n"; require 'fg_wsg84.pl' or die "Unable to load fg_wsg84.pl ...\n"; # log file stuff my ($LF); my $pgmname = $0; if ($pgmname =~ /\w{1}:\\.*/) { my @tmpsp = split(/\\/,$pgmname); $pgmname = $tmpsp[-1]; } my $outfile = "temp.$pgmname.txt"; open_log($outfile); # prt( "$0 ... Hello, World ...\n" ); my $do_dbg_sets = 0; my $do_dbg_timing = 0; my $do_dbg_tests = 0; ### 2516 VHHH HONG KONG INTL (22.3083428148148,113.917764876543) tile=e110n20 my $hklat = 22.3083428148148; my $hklon = 113.917764876543; ### NDME 49.20933100, 007.39577800, 1201, 10910, 25, 0.0, ZND, ZWEIBRUCKEN DME (1) my $zlat = 49.20933100; my $zlon = 7.39577800; #/** Meters to Nautical Miles. 1 nm = 6076.11549 feet */ my $METER_TO_NM = 0.0005399568034557235; my $PI = 3.1415926535897932384626433832795029; my $D2R = $PI / 180; my $R2D = 180 / $PI; my $ERAD = 6378138.12; #// These are hard numbers from the WGS84 standard. DON'T MODIFY #// unless you want to change the datum. my $HP_EQURAD = 6378137.0; my $HP_FLATTENING = 298.257223563; #// High-precision versions of the above produced with an arbitrary #// precision calculator (the compiler might lose a few bits in the FPU #// operations). These are specified to 81 bits of mantissa, which is #// higher than any FPU known to me: my $HP_SQUASH = 0.9966471893352525192801545; my $HP_STRETCH = 1.0033640898209764189003079; my $HP_POLRAD = 6356752.3142451794975639668; my $HP_MIN = 2.22507e-308; #my $D2_FACTOR = 10000000; my $D2_FACTOR = $ERAD; #my $D2_FACTOR = $HP_POLRAD; my $d_lat = 0.0; my $d_lon = 0.0; my $u_alt = 0.0; my $nlat = 0; my $nlon = 0; my $lastlon = 0.0; my $lastlat = 0.0; if ($do_dbg_tests) { my ($i); prt( "Using D2_FACTOR of $D2_FACTOR ...\n" ); #prt( "Perl PI=".PI.", vs PI=$PI. Diff=".abs(PI - $PI)."\n" ); if ($do_dbg_sets) { for ($i = 0; $i < 10; $i++) { $nlat = $i * 10; $nlon = $d_lon; do_lat_lon( $nlat, $nlon ); } for ($i = 0; $i < 10; $i++) { $nlat = $d_lat; $nlon = $i * 10; do_lat_lon( $nlat, $nlon ); } for ($i = 0; $i < 10; $i++) { $nlat = $i * 10; $nlon = $i * 10; do_lat_lon( $nlat, $nlon ); } } $lastlat = $hklat; $lastlon = $hklon; do_lat_lon( $zlat, $zlon ); $lastlat = 0.0; $lastlon = 0.0; do_lat_lon( $lastlat, 1.0 ); do_lat_lon( 1.0, $lastlon ); do_lat_lon( $lastlat, 2.0 ); do_lat_lon( $lastlat, 3.0 ); do_lat_lon( $lastlat, 89.9999 ); do_lat_lon( $lastlat, 179.3 ); if ($do_dbg_timing) { do_time_tests(); } close_log($outfile,1); exit(0); } sub do_time_tests { my ($d, @bgn, @end, $td1, $td2, $j, $ret, $az1, $az2, $s); my ($i); my $lat = $d_lat; my $lon = $d_lon; $lastlat = 1; $lastlon = 1; @bgn = gettimeofday(); for ($j = 0; $j < 1000; $j++) { for ($i = 0; $i < 10; $i++) { $nlat = $i * 10; $nlon = $lon; $d = fg_lat_lon_distance_m( $nlat, $nlon, $lastlat, $lastlon ); } for ($i = 0; $i < 10; $i++) { $nlat = $lat; $nlon = $i * 10; $d = fg_lat_lon_distance_m( $nlat, $nlon, $lastlat, $lastlon ); } for ($i = 0; $i < 10; $i++) { $nlat = $i * 10; $nlon = $i * 10; $d = fg_lat_lon_distance_m( $nlat, $nlon, $lastlat, $lastlon ); } } @end = gettimeofday(); $td1 = tv_interval( \@bgn, \@end ); prt( "Took $td1 seconds ...\n"); @bgn = gettimeofday(); for ($j = 0; $j < 1000; $j++) { for ($i = 0; $i < 10; $i++) { $nlat = $i * 10; $nlon = $lon; $ret = fg_geo_inverse_wgs_84( $nlat, $nlon, $lastlat, $lastlon, \$az1, \$az2, \$s ); } for ($i = 0; $i < 10; $i++) { $nlat = $lat; $nlon = $i * 10; $ret = fg_geo_inverse_wgs_84( $nlat, $nlon, $lastlat, $lastlon, \$az1, \$az2, \$s ); } for ($i = 0; $i < 10; $i++) { $nlat = $i * 10; $nlon = $i * 10; $ret = fg_geo_inverse_wgs_84( $nlat, $nlon, $lastlat, $lastlon, \$az1, \$az2, \$s ); } } @end = gettimeofday(); $td2 = tv_interval( \@bgn, \@end ); prt( "Took $td2 seconds ... factor of ".($td2 / $td1)."!\n"); } sub do_lat_lon { my ($lat, $lon) = @_; my ($x, $y, $z) = fg_ll2xyz($lon, $lat); my ($clon, $clat) = fg_xyz2ll($x, $y, $z); my ($lx, $ly, $lz) = fg_ll2xyz($lastlon, $lastlat); ###my $dist = sqrt ( coord_dist_sq( $lx, $ly, $lz, $x, $y, $z ) ) * $D2_FACTOR; my $dist = fg_coord_distance_m( $lx, $ly, $lz, $x, $y, $z ); my $dist2 = fg_lat_lon_distance_m( $lat, $lon, $lastlat, $lastlon ); my $nm = $dist * $METER_TO_NM; my $nm2 = $dist2 * $METER_TO_NM; prt( "Lon $lon, Lat $lat, is $x, $y, $z ($clon,$clat)\n" ); prt( " DIST1 = $dist m ($nm nm) to ($lastlat,$lastlon)\n" ); prt( " DIST2 = $dist2 m ($nm2 nm) to ($lastlat,$lastlon)\n" ); my ($az1, $az2, $s); $az1 = -1; $az2 = -1; $s = -1; my $ret = fg_geo_inverse_wgs_84( $lat, $lon, $lastlat, $lastlon, \$az1, \$az2, \$s ); if ($ret == 0) { my $nm2 = $s * $METER_TO_NM; my $diff = (abs($nm - $nm2) / $nm2) * 100.0; my $taz = $az1 + $az2 - 360; prt( " Distance = $s m ($nm2 nm), on azimuth $az1 ($taz) ret=$ret diff=".int($diff + 0.5)." pct\n" ); # sub fg_geo_direct_wgs_84 { # my ( $lat1, $lon1, $az1, $s, $lat2, $lon2, $az2 ) = @_; $ret = fg_geo_direct_wgs_84( $lat, $lon, $az1, $s, \$clat, \$clon, \$taz ); prt( sprintf(" Verify1 (%0.4f,%0.4f) on %0.2f, for %0.2f m, returns to (%0.4f,%0.4f) vs (%0.4f,%0.4f)\n", $lat, $lon, $az1, $s, $clat, $clon, $lastlat,$lastlon ) ); $ret = fg_geo_direct_wgs_84( $lastlat, $lastlon, $az2, $s, \$clat, \$clon, \$taz ); prt( sprintf(" Verify2 (%0.4f,%0.4f) on %0.2f, for %0.2f m, returns to (%0.4f,%0.4f) vs (%0.4f,%0.4f)\n", $lastlat, $lastlon, $az2, $s, $clat, $clon, $lat,$lon ) ); } else { prt( "fg_geo_inverse_wgs_84 FAILED to find solution!!!\n" ); } if ($do_dbg_sets) { $lastlat = $lat; $lastlon = $lon; } } sub do_lat_lon_ok { my ($lat, $lon) = @_; my ($x, $y, $z) = ll2xyz_ok($lon, $lat); my ($clon, $clat) = xyz2ll_ok($x, $y, $z); my ($lx, $ly, $lz) = ll2xyz_ok($lastlon, $lastlat); ###my $dist = sqrt ( coord_dist_sq( $lx, $ly, $lz, $x, $y, $z ) ) * $D2_FACTOR; my $dist = coord_distance_m_ok( $lx, $ly, $lz, $x, $y, $z ); my $dist2 = lat_lon_distance_m_ok( $lat, $lon, $lastlat, $lastlon ); my $nm = $dist * $METER_TO_NM; my $nm2 = $dist2 * $METER_TO_NM; prt( "Lon $lon, Lat $lat, is $x, $y, $z ($clon,$clat)\n" ); prt( " DIST1 = $dist m ($nm nm) to ($lastlat,$lastlon)\n" ); prt( " DIST2 = $dist2 m ($nm2 nm) to ($lastlat,$lastlon)\n" ); my ($az1, $az2, $s); $az1 = -1; $az2 = -1; $s = -1; my $ret = fg_geo_inverse_wgs_84( $lat, $lon, $lastlat, $lastlon, \$az1, \$az2, \$s ); if ($ret == 0) { my $nm2 = $s * $METER_TO_NM; my $diff = (abs($nm - $nm2) / $nm2) * 100.0; my $taz = $az1 + $az2 - 360; prt( " Distance = $s m ($nm2 nm), on azimuth $az1 ($taz) ret=$ret diff=".int($diff + 0.5)." pct\n" ); # sub fg_geo_direct_wgs_84 { # my ( $lat1, $lon1, $az1, $s, $lat2, $lon2, $az2 ) = @_; $ret = fg_geo_direct_wgs_84( $lat, $lon, $az1, $s, \$clat, \$clon, \$taz ); prt( sprintf(" Verify1 (%0.4f,%0.4f) on %0.2f, for %0.2f m, returns to (%0.4f,%0.4f) vs (%0.4f,%0.4f)\n", $lat, $lon, $az1, $s, $clat, $clon, $lastlat,$lastlon ) ); $ret = fg_geo_direct_wgs_84( $lastlat, $lastlon, $az2, $s, \$clat, \$clon, \$taz ); prt( sprintf(" Verify2 (%0.4f,%0.4f) on %0.2f, for %0.2f m, returns to (%0.4f,%0.4f) vs (%0.4f,%0.4f)\n", $lastlat, $lastlon, $az2, $s, $clat, $clon, $lat,$lon ) ); } else { prt( "fg_geo_inverse_wgs_84 FAILED to find solution!!!\n" ); } if ($do_dbg_sets) { $lastlat = $lat; $lastlon = $lon; } } ########################################### # *** These were just for development. *** # The final services have been placed in # the fg_wsg84.pl module. ########################################### sub ll2xyz_ok($$) { my $lon = (shift) * $D2R; my $lat = (shift) * $D2R; my $cosphi = cos $lat; my $di = $cosphi * cos $lon; my $dj = $cosphi * sin $lon; my $dk = sin $lat; return ($di, $dj, $dk); } sub xyz2ll_ok($$$) { my ($di, $dj, $dk) = @_; my $aux = $di * $di + $dj * $dj; my $lat = atan2($dk, sqrt $aux) * $R2D; my $lon = atan2($dj, $di) * $R2D; return ($lon, $lat); } sub coord_dist_sq_ok { my ($xa, $ya, $za, $xb, $yb, $zb) = @_; my $x = $xb - $xa; my $y = $yb - $ya; my $z = $zb - $za; return $x * $x + $y * $y + $z * $z; } sub coord_distance_m_ok { my ($xa, $ya, $za, $xb, $yb, $zb) = @_; return (sqrt( coord_dist_sq_ok( $xa, $ya, $za, $xb, $yb, $zb ) ) * $D2_FACTOR); } sub lat_lon_distance_m_ok { my ($lat1, $lon1, $lat2, $lon2) = @_; my ($xa, $ya, $za) = ll2xyz_ok($lon1, $lat1); my ($xb, $yb, $zb) = ll2xyz_ok($lon2, $lat2); return coord_distance_m_ok( $xa, $ya, $za, $xb, $yb, $zb ); } ################################################################## # user variables my $VERS = "0.0.9 2019-03-03"; ##my $VERS = "0.0.8 2008-12-17"; my $load_log = 0; my $in_file = ''; my $verbosity = 0; my $out_file = ''; # ### DEBUG ### my $debug_on = 0; my $def_file = 'def_file'; ### program variables my @warnings = (); my $cwd = cwd(); sub VERB1() { return $verbosity >= 1; } sub VERB2() { return $verbosity >= 2; } sub VERB5() { return $verbosity >= 5; } sub VERB9() { return $verbosity >= 9; } sub show_warnings($) { my ($val) = @_; if (@warnings) { prt( "\nGot ".scalar @warnings." WARNINGS...\n" ); foreach my $itm (@warnings) { prt("$itm\n"); } prt("\n"); } else { prt( "\nNo warnings issued.\n\n" ) if (VERB9()); } } sub pgm_exit($$) { my ($val,$msg) = @_; if (length($msg)) { $msg .= "\n" if (!($msg =~ /\n$/)); prt($msg); } show_warnings($val); close_log($outfile,$load_log); exit($val); } sub prtw($) { my ($tx) = shift; $tx =~ s/\n$//; prt("$tx\n"); push(@warnings,$tx); } sub process_in_file($) { my ($inf) = @_; if (! open INF, "<$inf") { pgm_exit(1,"ERROR: Unable to open file [$inf]\n"); } my @lines = <INF>; close INF; my $lncnt = scalar @lines; prt("Processing $lncnt lines, from [$inf]...\n"); my ($line,$inc,$lnn); $lnn = 0; foreach $line (@lines) { chomp $line; $lnn++; if ($line =~ /\s*#\s*include\s+(.+)$/) { $inc = $1; prt("$lnn: $inc\n"); } } } sub is_decimal($) { my $num = shift; # like -17.490550000,-149.763325000 return 1 if ($num =~ /^[-+]?[0-9]*\.?[0-9]+/); return 0; } sub in_world_range($$) { my ($lat,$lon) = @_; if (($lat < -90.0) || ($lat > 90.0) || ($lon < -180.0) || ($lon > 180.0)) { return 0; } return 1; } # ################################################################### # https://stackoverflow.com/questions/18759601/converting-lla-to-xyz # test: http://www.apsalin.com/convert-geodetic-to-cartesian.aspx sub ll2xyz_2($$) { my $lon = (shift) * $D2R; my $lat = (shift) * $D2R; my $alt = 0; my $cosLat = cos $lat; my $sinLat = sin $lat; my $cosLon = cos $lon; my $sinLon = sin $lon; my $R = 6378137; my $f_inv = 298.257224; my $f = 1.0 / $f_inv; my $e2 = 1 - (1 - $f) * (1 - $f); my $c = 1 / sqrt($cosLat * $cosLat + (1 - $f) * (1 - $f) * $sinLat * $sinLat); my $s = (1 - $f) * (1 - $f) * $c; my $x = ($R * $c + $alt) * $cosLat * $cosLon; my $y = ($R * $c + $alt) * $cosLat * $sinLon; my $z = ($R * $s + $alt) * $sinLat; return ($x, $y, $z); } #### BAD RESULTS sub lat_lon_2_xyz_NG { my ($lat, $lon) = @_; my ($x, $y, $z) = fg_ll2xyz($lon, $lat); #my ($x, $y, $z) = ll2xyz_2($lon, $lat); my ($nlon,$nlat) = fg_xyz2ll( $x, $y, $z ); my ($az1,$az2,$s,$res); $res = fg_geo_inverse_wgs_84($lat,$lon,$nlat,$nlon,\$az1,\$az2,\$s); prt("Input lat=$lat, lon=$lon,\n equal xyz $x, $y, $z,\n rev. lat=$nlat, lon=$nlon - diff $s m\n"); } sub lat_lon_alt_2_xyz($$$) { my ($lat, $lon, $alt) = @_; my $rlat = $lat * $D2R; my $rlon = $lon * $D2R; my ($x, $y, $z) = lla2xyz($rlon, $rlat, $alt); #my ($x, $y, $z) = ll2xyz_2($lon, $lat); my ($nlon,$nlat,$nalt) = xyz2lla( $x, $y, $z ); $nlon *= $R2D; $nlat *= $R2D; my ($az1,$az2,$s,$res); $res = fg_geo_inverse_wgs_84($lat,$lon,$nlat,$nlon,\$az1,\$az2,\$s); prt("Input lat=$lat, lon=$lon,\n equal xyz $x, $y, $z,\n rev. lat=$nlat, lon=$nlon - diff $s m\n"); } ######################################### ### MAIN ### parse_args(@ARGV); #process_in_file($in_file); #lat_lon_2_xyz( $d_lat, $d_lon ); lat_lon_alt_2_xyz( $d_lat, $d_lon, $u_alt ); pgm_exit(0,""); ######################################## sub need_arg { my ($arg,@av) = @_; pgm_exit(1,"ERROR: [$arg] must have a following argument!\n") if (!@av); } sub parse_args { my (@av) = @_; my ($arg,$sarg); my $verb = VERB2(); my $cnt = 0; while (@av) { $arg = $av[0]; if (($arg =~ /^-/) && !is_decimal($arg)) { $sarg = substr($arg,1); $sarg = substr($sarg,1) while ($sarg =~ /^-/); if (($sarg =~ /^h/i)||($sarg eq '?')) { give_help(); pgm_exit(0,"Help exit(0)"); } elsif ($sarg =~ /^v/) { if ($sarg =~ /^v.*(\d+)$/) { $verbosity = $1; } else { while ($sarg =~ /^v/) { $verbosity++; $sarg = substr($sarg,1); } } $verb = VERB2(); prt("Verbosity = $verbosity\n") if ($verb); } elsif ($sarg =~ /^l/) { if ($sarg =~ /^ll/) { $load_log = 2; } else { $load_log = 1; } prt("Set to load log at end. ($load_log)\n") if ($verb); } elsif ($sarg =~ /^o/) { need_arg(@av); shift @av; $sarg = $av[0]; $out_file = $sarg; prt("Set out file to [$out_file].\n") if ($verb); } else { pgm_exit(1,"ERROR: Invalid argument [$arg]! Try -?\n"); } } else { if ($cnt == 0) { $d_lat = $arg; prt("Set input lat=$d_lat\n") if ($verb); } elsif ($cnt == 1) { $d_lon = $arg; prt("Set input lon=$d_lon\n") if ($verb); } else { pgm_exit(1, "What is this '$arg'! Have lat=$d_lat, lon=$d_lon\n"); } $cnt++; } shift @av; } if ($debug_on) { prtw("WARNING: DEBUG is ON!\n"); if (length($in_file) == 0) { $in_file = $def_file; prt("Set DEFAULT input to [$in_file]\n"); } } if ($cnt != 2) { pgm_exit(1,"ERROR: No input lat lon found in command!\n"); } #if (length($in_file) == 0) { # pgm_exit(1,"ERROR: No input files found in command!\n"); #} #if (! -f $in_file) { # pgm_exit(1,"ERROR: Unable to find in file [$in_file]! Check name, location...\n"); #} } sub give_help { prt("$pgmname: version $VERS\n"); prt("Usage: $pgmname [options] lat lon\n"); prt("Options:\n"); prt(" --help (-h or -?) = This help, and exit 0.\n"); prt(" --verb[n] (-v) = Bump [or set] verbosity. def=$verbosity\n"); prt(" --load (-l) = Load LOG at end. ($outfile)\n"); prt(" --out <file> (-o) = Write output to this file.\n"); } # eof - ll2xyz.pl