ll2xyz.pl to HTML.

index -|- end

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

index -|- top

checked by tidy  Valid HTML 4.01 Transitional