gpsman 6.4.4.2-2 / usr / lib / gpsman / posncomp.tcl

#
# This file is part of:
#
#  gpsman --- GPS Manager: a manager for GPS receiver data
#
# Copyright (c) 1998-2013 Miguel Filgueiras migfilg@t-online.de
#
#    This program is free software; you can redistribute it and/or modify
#      it under the terms of the GNU General Public License as published by
#      the Free Software Foundation; either version 3 of the License, or
#      (at your option) any later version.
#
#      This program is distributed in the hope that it will be useful,
#      but WITHOUT ANY WARRANTY; without even the implied warranty of
#      MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
#      GNU General Public License for more details.
#
#      You should have received a copy of the GNU General Public License
#      along with this program.
#
#  File: posncomp.tcl
#  Last change:  6 October 2013
#

## algorithms and formulae taken or adapted from
#  gpstrans - a program to communicate with garmin gps
#      containing parts taken from John F. Waers (jfwaers _AT_ csn.net)
#      program MacGPS.
#        Copyright (c) 1995 by Carsten Tschach (tschach _AT_ zedat.fu-berlin.de) 
#      Uniform Finish grid support:
#        Copyright (c) 1996 Janne Sinkkonen (janne _AT_ iki.fi)
#
# some formulae have been simplified or corrected

## correction of a bug in the conversion from UPS coordinates due to
#      Peter H. Dana, University of Texas
##

proc ToDatum {latd longd datum0 datum1} {
    # convert position given in lat/long signed degrees from one datum
    #  to another
    # return list with lat and long in signed degrees

    if { $datum0 != $datum1 } {
	set phi [expr $latd*0.01745329251994329576]
	set lambda [expr $longd*0.01745329251994329576]
	set d0 [EllipsdData $datum0]
	set a0 [lindex $d0 0] ; set f0 [lindex $d0 1]
	set d1 [EllipsdData $datum1]
	set a1 [lindex $d1 0] ; set f1 [lindex $d1 1]
	set dx [expr [lindex $d1 2]-[lindex $d0 2]]
	set dy [expr [lindex $d1 3]-[lindex $d0 3]]
	set dz [expr [lindex $d1 4]-[lindex $d0 4]]
	set b0 [expr $a0*(1-$f0)] ; set es0 [expr $f0*(2-$f0)]
	set b1 [expr $a1*(1-$f1)] ; set es1 [expr $f1*(2-$f1)]
	       # convert geodetic latitude to geocentric latitude
	if { $latd==0 || $latd==90 || $latd==-90 } {
	    set psi $phi
	} else { set psi [expr atan((1-$es0)*tan($phi))] }
               # x and y axis coordinates with respect to original ellipsoid
	set t1 [expr tan($psi)]
	if { $longd==90 || $longd==-90.0 } {
	    set x 0
	    set y [expr abs($a0*$b0/sqrt($b0*$b0+$a0*$a0*$t1*$t1))]
	} else {
	    set t2 [expr tan($lambda)]
	    set x [expr abs(($a0*$b0)/ \
		            sqrt((1+$t2*$t2)*($b0*$b0+$a0*$a0*$t1*$t1)))]
	    set y [expr abs($x*$t2)]
	}
	if { $longd<-90 || $longd>90 } { set x [expr -$x] }
	if { $longd < 0 } { set y [expr -$y] }
	         # z axis coordinate with respect to the original ellipsoid
	if { $latd == 90 } {
	    set z $b0
	} elseif { $latd == -90 } {
	    set z [expr -$b0]
	} else {
	    set z [expr $t1*sqrt(($a0*$a0*$b0*$b0)/ \
		                 ($b0*$b0+$a0*$a0*$t1*$t1))]
	}
                 # geocentric latitude with respect to the new ellipsoid
	set ddx [expr $x-$dx] ; set ddy [expr $y-$dy] ; set ddz [expr $z-$dz]
	set psi1 [expr atan2($ddz,sqrt($ddx*$ddx+$ddy*$ddy))]
	         # geocentric latitude and longitude
	set clat [expr atan2(tan($psi1),1-$es1)*57.29577951308232087684]
	set clong [expr atan2($ddy,$ddx)*57.29577951308232087684]
    } else {
	set clat $latd ; set clong $longd
    }
    return [list $clat $clong]
}

       # reference TM lat
set UTMlat0 0
set UTMk0 0.9996

proc DegreesToUTM {lat long datum} {
    # convert position in lat/long signed degrees and given datum to
    #  UTM/UPS
    global UTMlat0 UTMk0
  
    if { $lat>=-80 && $lat<=84 } {
	set zn [expr 67+int(floor(($lat+80)/8.0))]
        # skip I, O
	if { $zn > 72 } { incr zn }
	if { $zn > 78 } { incr zn }
	set zn [format "%c" $zn]
	set long0 [expr 6*int(floor($long/6.0))+3]
	set ze [format "%02d" [expr int(floor(($long0+183)/6.0))]]
	foreach "x y" [ConvToTM $lat $long $UTMlat0 $long0 $UTMk0 $datum] {}
	set x [expr 500000+$x]
	if { $lat < 0 } { set y [expr 10000000+$y] }
    } else {
	set ze 00
	if { $lat > 0 } {
	    if { $long < 0 } { set zn Y } else { set zn Z }
	} else {
	    if { $long < 0 } { set zn A } else { set zn B }
	}
	foreach "x y" [ConvToUPS $lat $long $datum] {}
    }
    return [list $ze $zn $x $y]
}

proc UTMToDegrees {ze zn x y datum} {
    # convert from UTM/UPS to lat/long in signed degrees
    global UTMlat0 UTMk0

    scan $ze %0d zze
    if { $zze != 0 } {
	set long0 [expr -183+6.0*$zze]
	if { [string compare $zn M] <= 0 } {
	    set y [expr $y-1e7]
	}
	set x [expr $x-5e5]
	return [ConvFromTM $x $y $UTMlat0 $long0 $UTMk0 $datum]
    } else {
	return [ConvFromUPS $zn $x $y $datum]
    }
}

set BNGZONE1 STUQRNOPLMHJKFGCDEABXYZVW
set BNGZONE2 VWXYZQRSTULMNOPFGHJKABCDE

proc DegreesToBNG {lat long datum} {
    # convert from lat/long in signed degrees to British National Grid coords
    global BNGZONE1 BNGZONE2

    set cs [ConvToTM $lat $long 49.0 -2.0 0.9996012717 $datum]
    if { [set x [expr round(4e5+[lindex $cs 0])]] < 0 || $x > 1e6 || \
	    [set y [expr round([lindex $cs 1]-1e5)]] < 0 || $y > 2.5e6 } {
	return "-- 0 0"
    }
    set i0 [expr int($x/5e5)] ; set j0 [expr int($y/5e5)]
    set i1 [expr int($x/1e5)%5] ; set j1 [expr int($y/1e5)%5]
    set z1 [string index $BNGZONE1 [expr 5*$j0+$i0]]
    set z2 [string index $BNGZONE2 [expr 5*$j1+$i1]]
    return [list ${z1}$z2 [expr $x%100000] [expr $y%100000]]
}

proc BNGToDegrees {zone x y datum} {
    # convert from British National Grid coords to lat/long in signed degrees
    global BNGZONE1 BNGZONE2

    set z1 [string index $zone 0] ; set z2 [string index $zone 1]
    if { [set i1 [string first $z1 $BNGZONE1]] == -1 || $x < 0 || $x > 1e5 || \
	    [set i2 [string first $z2 $BNGZONE2]] == -1 || \
	    $y < 0 || $y > 1e5 } {
	return 0
    }
    set x [expr $x+1e5*(($i1%5)*5+($i2%5)-4)]
    set y [expr $y+1e5*(int($i1/5)*5+int($i2/5)+1)]
    return [ConvFromTM $x $y 49.0 -2.0 0.9996012717 $datum]
}

set ITMZONE VWXYZQRSTULMNOPFGHJKABCDE

proc DegreesToITM {lat long datum} {
    # convert from lat/long in signed degrees to Irish TM Grid coords
    global ITMZONE

    set cs [ConvToTM $lat $long 53.5 -8.0 1.000035 $datum]
    if { [set x [expr round(2e5+[lindex $cs 0])]] < 0 || $x > 5e5 || \
	    [set y [expr round([lindex $cs 1]+25e4)]] < 0 || $y > 5e5 } {
	return "-- 0 0"
    }
    set i [expr int($x/1e5)] ; set j [expr int($y/1e5)]
    set zone [string index $ITMZONE [expr 5*$j+$i]]
    return [list $zone [expr $x%100000] [expr $y%100000]]
}

proc ITMToDegrees {zone x y datum} {
    # convert from Irish TM Grid coords to lat/long in signed degrees
    global ITMZONE

    if { $x < 0 || $x > 1e5 || $y < 0 || $y > 1e5 || \
	    [set i [string first $zone $ITMZONE]] == -1 } {
	return 0
    }
    set x [expr $x+1e5*(($i%5)-2)]
    set y [expr $y+1e4*(int($i/5)*10-25)]
    return [ConvFromTM $x $y 53.5 -8.0 1.000035 $datum]
}

proc DegreesToKKJY {lat long datum} {
    # convert from lat/long in signed degrees to Uniform Finnish Grid coords

    set cs [ConvToTM $lat $long 0 27.0 1.0 $datum]
    if { [set x [expr round(5e5+[lindex $cs 0])]] < 0 || \
	    [set y [lindex $cs 1]] < 0 } {
	return "-- 0 0"
    }
    return [list 27E $x $y]
}

proc KKJYToDegrees {zone x y datum} {
    # convert from Uniform Finnish Grid coords to lat/long in signed degrees

    if { $zone != "27E" || $x < 0 || $y < 0 } { return 0 }
    return [ConvFromTM [expr $x-5e5] $y 0 27.0 1.0 $datum]
}

proc ConvToTM {lat long lat0 long0 k0 datum} {
    # convert to TM
    #  $lat0, $long0: centre coordinates in signed degrees
    #  $k0: scale at central meridian
    # value of $long will be adjusted to be closer to $long0

    set long [NormalLongCentred $long $long0]
    set d [EllipsdData $datum]
    set a [lindex $d 0] ; set f [lindex $d 1]
    set es [expr $f*(2-$f)]
    set phi [expr $lat*0.01745329251994329576]
    set lambda [expr $long*0.01745329251994329576]
    set phi0 [expr $lat0*0.01745329251994329576]
    set lambda0 [expr $long0*0.01745329251994329576]
    set m0 [TMAux $phi0 $a $es] ; set m [TMAux $phi $a $es]
    set et2 [expr $es/(1-$es)]
    set n [expr sin($phi)] ; set n [expr $a/sqrt(1-$es*$n*$n)]
    set t [expr tan($phi)] ; set t [expr $t*$t]
    set c [expr cos($phi)]
    set A [expr ($lambda-$lambda0)*$c] ; set c [expr $et2*$c*$c]
    set x [expr round($k0*$n*($A+(1-$t+$c)*$A*$A*$A/6+ \
	                      (5-18*$t+$t*$t+72*$c-58*$et2)* \
                               $A*$A*$A*$A*$A/120))]
    set y [expr round($k0*($m-$m0+$n*tan($phi)*($A*$A/2+ \
	                (5-$t+9*$c+4*$c*$c)*$A*$A*$A*$A/24+ \
			(61-58*$t+$t*$t+600*$c-330*$et2)* \
			$A*$A*$A*$A*$A*$A/720)))]
    return [list $x $y]
}

proc ConvFromTM {x y lat0 long0 k0 datum} {
    # convert from TM
    #  $lat0, $long0: centre coordinates in signed degrees
    #  $k0: scale at central meridian
    # return value of longitude in the -179,180 range

    set d [EllipsdData $datum]
    set a [lindex $d 0] ; set f [lindex $d 1]
    set es [expr $f*(2-$f)]
    set phi0 [expr $lat0*0.01745329251994329576]
    set lambda0 [expr $long0*0.01745329251994329576]
    set es [expr $f*(2-$f)]
    set m0 [TMAux $phi0 $a $es] ; set m [expr $m0+1.0*$y/$k0]
    set et2 [expr $es/(1-$es)]
    set e1 [expr sqrt(1-$es)] ; set e1 [expr (1-$e1)/(1+$e1)]
    set mu [expr $m/($a*(1-$es/4-3*$es*$es/64-0.01953125*$es*$es*$es))]
    set phi1 [expr $mu+(1.5*$e1-27*$e1*$e1*$e1/32)*sin($mu+$mu)+ \
	           (1.3125-1.71875*$e1*$e1)*$e1*$e1*sin(4*$mu)+ \
		   1.572916666667*$e1*$e1*$e1*sin(6*$mu)+ \
		   +2.142578125*$e1*$e1*$e1*$e1*sin(8*$mu)]
    set c1 [expr cos($phi1)] ; set c1 [expr $et2*$c1*$c1]
    set t1 [expr tan($phi1)] ; set t1 [expr $t1*$t1]
    set n1 [expr sin($phi1)]
    set r1 [expr $a*(1-$es)/pow(1-$es*$n1*$n1, 1.5)]
    set n1 [expr $a/sqrt(1-$es*$n1*$n1)]
    set d  [expr $x/($n1*$k0)]
    set lat [expr ($phi1-$n1*tan($phi1)/$r1* \
	           ($d*$d/2-(5+3*$t1+10*$c1-4*$c1*$c1-9*$et2)* \
		    $d*$d*$d*$d/24+ \
                   (61+90*$t1+298*$c1+45*$t1*$t1-252*$et2-3*$c1*$c1)* \
		   $d*$d*$d*$d*$d*$d/720))*57.29577951308232087684]
    set long [expr ($lambda0+($d-(1+($t1+$t1)+$c1)*$d*$d*$d/6+ \
	            (5-($c1+$c1)+28*$t1-3*$c1*$c1+8*$et2+24*$t1*$t1)* \
		     $d*$d*$d*$d*$d/120)/cos($phi1))*57.29577951308232087684]
    set long [NormalLong $long]
    return [list $lat $long]
}

proc TMAux {phi a es} {

    if { abs($phi) < 1e-20 } { return 0 }
    set es2 [expr $es*$es]
    set es3 [expr $es2*$es]
    return [expr $a*((1-$es/4-0.046875*$es2-0.01953125*$es3)*$phi- \
	             (0.375*$es+0.09375*$es2+0.0439453125*$es3)* \
                      sin($phi+$phi)+ \
		     (0.05859375*$es2+0.0439453125*$es3)*sin(4*$phi)- \
		     0.011393229166667*$es3*sin(6*$phi))]
}

set UPSk0 0.994

proc ConvToUPS {lat long datum} {
    global UPSk0

    set d [EllipsdData $datum]
    set a [lindex $d 0] ; set f [lindex $d 1]
    set es [expr $f*(2-$f)]
    set lambda [expr $long*0.01745329251994329576]
    set phi [expr abs($lat*0.01745329251994329576)]
    set e [expr sqrt($es)]
    set ee [expr $e*sin($phi)]
    set t [expr tan(0.78539816339744830961-$phi/2)/pow((1-$ee)/(1+$ee),($e/2))]
    set rho [expr 2*$a*$UPSk0*$t/sqrt(pow(1+$e,1+$e)*pow(1-$e,1-$e))]
    set x [expr round($rho*sin($lambda)+2e6)] ; set y [expr $rho*cos($lambda)]
    if { $lat > 0 } { set y [expr -$y] }
    return [list $x [expr round($y+2e6)]]
}

proc ConvFromUPS {zn x y datum} {
    global UPSk0

    set d [EllipsdData $datum]
    set a [lindex $d 0] ; set f [lindex $d 1]
    set es [expr $f*(2-$f)] ; set e [expr sqrt($es)]
    set x [expr $x-2e6] ; set y [expr $y-2e6]
    set rho [Hypot $x $y]
    set t [expr $rho*sqrt(pow(1+$e,1+$e)*pow(1-$e,1-$e))/(2*$a*$UPSk0)]
    set lat [expr [UPSAux $e $t]*57.29577951308232087684]
    if { abs($y) > 1e-20 } {
	set long [expr atan2(abs($x),abs($y))*57.29577951308232087684]
    } else {
	set long 90
	if { $x < 0 } { set long [expr -$long] }
    }
    if { [string compare $zn M] > 0 } {
	set y [expr -$y]
    }
    if { $y < 0 } { set long [expr 180-$long] }
    if { $x < 0 } { set long [expr -$long] }
    return [list $lat $long]
}

proc UPSAux {e t} {

    set phi [expr 1.57079632679489661923-2*atan($t)]
    set old 999.9e-99
    set maxIterations 20 ; set i 0
    while { abs(($phi-$old)/$phi)>1e-8  && $i<$maxIterations } { 
	incr i
	set old $phi
	set phi [expr 1.57079632679489661923- \
		 2*atan($t*pow((1-$e*sin($phi))/((1+$e*sin($phi))),$e/2.0))]
    }
    return $phi
}