gpsman 6.4.3-1 / usr / lib / gpsman / acccomp.tcl

#
# This file is part of:
#
#  gpsman --- GPS Manager: a manager for GPS receiver data
#
# Copyright (c) 1998-2011 Miguel Filgueiras migf@portugalmail.pt
#
#    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: acccomp.tcl
#  Last change:  19 July 2011
#

# Replacement procedures for more accurate and more slow computations of
#   distances and bearings

# This file to be consulted after compute.tcl

## Formulae for distances and bearings taken from the program "inverse"
#     available from ftp://www.ngs.noaa.gov/pub/pcsoft/for_inv.3d/
#  They correspond to the modified Rainsford's Method with Helmert's
#     elliptical terms, and are effective in any azimuth and at any
#     distance short of antipodal, none of the points can be a pole
#  If one of the points is a pole, or the points are nearly antipodal
#     the Law of Cosines for Spherical Trigonometry, kindly supplied by
#     Luisa Bastos (Universidade do Porto) and Gil Goncalves (Universidade
#     de Coimbra), will be applied
##

proc ComputeDist {p1 p2 datum} {
    # distance between positions $p1 and $p2 with same datum
    # formulae from "inverse" program (see above)

    set lad1 [lindex $p1 0] ; set lod1 [lindex $p1 1]
    set lad2 [lindex $p2 0] ; set lod2 [lindex $p2 1]
    if { $lad1==$lad2 && $lod1==$lod2 } { return 0 }
    set la1 [expr $lad1*0.01745329251994329576]
    set lo1 [expr $lod1*0.01745329251994329576]
    set la2 [expr $lad2*0.01745329251994329576]
    set lo2 [expr $lod2*0.01745329251994329576]
    set dt [EllipsdData $datum]
    set a [lindex $dt 0] ; set f [lindex $dt 1]
    if { [expr abs(cos($la1))]<1.e-20 || [expr abs(cos($la2))]<1.e-20 || \
	 ( $lad1+$lad2 < 1e-4 && abs(abs($lod1-$lod2)-180) < 1e-4 ) } {
	# use Law of Cosines for Spherical Trigonometry
	set x [expr cos($lo1-$lo2)*cos($la1)*cos($la2)+sin($la1)*sin($la2)]
	if { $x >= 1 } { return 0 }
	return [expr 1e-3*$a*acos($x)]
    }
    set eps 5e-12
    set r [expr 1-$f]
    set tu1 [expr $r*tan($la1)] ; set tu2 [expr $r*tan($la2)]
    set cu1 [expr 1.0/sqrt($tu1*$tu1+1.0)]
    set su1 [expr $cu1*$tu1]
    set cu2 [expr 1.0/sqrt($tu2*$tu2+1.0)]
    set s [expr $cu1*$cu2]
    set baz [expr $s*$tu2] ; set faz [expr $baz*$tu1]
    set x [expr $lo2-$lo1] ; set d [expr $x+1]
    while { abs($d-$x) > $eps } {
	set sx [expr sin($x)] ; set cx [expr cos($x)]
	set tu1 [expr $cu2*$sx] ; set tu2 [expr $baz-$su1*$cu2*$cx]
	set sy [expr sqrt($tu1*$tu1+$tu2*$tu2)]
	set cy [expr $s*$cx+$faz] ; set y [expr atan2($sy,$cy)]
	set sa [expr $s*$sx/$sy] ; set c2a [expr -$sa*$sa+1.0]
	set cz [expr $faz+$faz]
	if { $cz > 0 } { set cz [expr -$cz/$c2a+$cy] }
	set e [expr $cz*$cz*2-1.0]
	set c [expr ((-3*$c2a+4.0)*$f+4.0)*$c2a*$f/16]
	set d $x
	set x [expr (($e*$cy*$c+$cz)*$sy*$c+$y)*$sa]
	set x [expr (1-$c)*$x*$f+$lo2-$lo1]
    }
    set faz [expr atan2($tu1,$tu2)]
    set baz [expr atan2($cu1*$sx,$baz*$cx-$su1*$cu2)+3.14159265358979323846]
    set x [expr sqrt((1.0/$r/$r-1)*$c2a+1)+1] ; set x [expr ($x-2.0)/$x]
    set c [expr 1-$x] ; set c [expr ($x*$x/4.0+1)/$c]
    set d [expr (0.375*$x*$x-1)*$x]
    set x [expr $e*$cy]
    set s [expr 1-$e-$e]
    set s [expr (((($sy*$sy*4-3)*$s*$cz*$d/6.0-$x)*$d/4.0+$cz)*$sy*$d+$y) \
	        *$c*$a*$r*1e-3]
    return $s
}

proc ComputeBear {p1 p2 datum} {
    # bearing from positions $p1 and $p2 with same datum
    # formulae from "inverse" program (see above)

    set lad1 [lindex $p1 0] ; set lod1 [lindex $p1 1]
    set lad2 [lindex $p2 0] ; set lod2 [lindex $p2 1]
    if { $lad1==$lad2 && $lod1==$lod2 } { return 0 }
    set la1 [expr $lad1*0.01745329251994329576]
    set lo1 [expr $lod1*0.01745329251994329576]
    set la2 [expr $lad2*0.01745329251994329576]
    set lo2 [expr $lod2*0.01745329251994329576]
    if { [expr abs(cos($la1))]<1.e-20 || [expr abs(cos($la2))]<1.e-20 } {
	# use Law of Cosines for Spherical Trigonometry
	# bearing
	set da [expr $la2-$la1] ; set do [expr $lo2-$lo1]
	if { [expr abs($da)] < 1e-20 } {
	    if { [expr abs($do)] < 1e-20 } {
		set b 0
	    } elseif { $do < 0 } {
		set b 270
	    } else { set b 90 }
	} elseif { [expr abs($do)] < 1e-20 } {
	    if { $da < 0 } {
		set b 180
	    } else { set b 0 }
	} else {
	    set b [expr round(atan2(sin($do), \
		                    tan($la2)*cos($la1)-sin($la1)*cos($do)) \
				    *57.29577951308232087684)]
	    if { $b < 0 } {
		if { $do < 0 } { incr b 360 } else { incr b 180 }
	    } elseif { $do < 0 } { incr b 180 }
	}
	return $b
    }
    set dt [EllipsdData $datum]
    set a [lindex $dt 0] ; set f [lindex $dt 1]
    set eps 5e-12
    set r [expr 1-$f]
    set tu1 [expr $r*tan($la1)] ; set tu2 [expr $r*tan($la2)]
    set cu1 [expr 1.0/sqrt($tu1*$tu1+1.0)]
    set su1 [expr $cu1*$tu1]
    set cu2 [expr 1.0/sqrt($tu2*$tu2+1.0)]
    set s [expr $cu1*$cu2]
    set baz [expr $s*$tu2] ; set faz [expr $baz*$tu1]
    set x [expr $lo2-$lo1] ; set d [expr $x+1]
    while { abs($d-$x) > $eps } {
	set sx [expr sin($x)] ; set cx [expr cos($x)]
	set tu1 [expr $cu2*$sx] ; set tu2 [expr $baz-$su1*$cu2*$cx]
	set sy [expr sqrt($tu1*$tu1+$tu2*$tu2)]
	set cy [expr $s*$cx+$faz] ; set y [expr atan2($sy,$cy)]
	set sa [expr $s*$sx/$sy] ; set c2a [expr -$sa*$sa+1.0]
	set cz [expr $faz+$faz]
	if { $cz > 0 } { set cz [expr -$cz/$c2a+$cy] }
	set e [expr $cz*$cz*2-1.0]
	set c [expr ((-3*$c2a+4.0)*$f+4.0)*$c2a*$f/16]
	set d $x
	set x [expr (($e*$cy*$c+$cz)*$sy*$c+$y)*$sa]
	set x [expr (1-$c)*$x*$f+$lo2-$lo1]
    }
    set faz [expr atan2($tu1,$tu2)]
    set b [expr round($faz*57.29577951308232087684)]
    if { $b < 0 } { incr b 360 }
    return $b
}

proc ComputeDistBear {p1 p2 datum} {
    # distance between and bearing from positions $p1 and $p2 with same datum
    # formulae from "inverse" program (see above)

    set lad1 [lindex $p1 0] ; set lod1 [lindex $p1 1]
    set lad2 [lindex $p2 0] ; set lod2 [lindex $p2 1]
    if { $lad1==$lad2 && $lod1==$lod2 } { return "0 0" }
    set la1 [expr $lad1*0.01745329251994329576]
    set lo1 [expr $lod1*0.01745329251994329576]
    set la2 [expr $lad2*0.01745329251994329576]
    set lo2 [expr $lod2*0.01745329251994329576]
    set dt [EllipsdData $datum]
    set a [lindex $dt 0] ; set f [lindex $dt 1]
    if { [expr abs(cos($la1))]<1.e-20 || [expr abs(cos($la2))]<1.e-20 } {
	# use Law of Cosines for Spherical Trigonometry
	# bearing
	set da [expr $la2-$la1] ; set do [expr $lo2-$lo1]
	if { [expr abs($da)] < 1e-20 } {
	    if { [expr abs($do)] < 1e-20 } {
		set b 0
	    } elseif { $do < 0 } {
		set b 270
	    } else { set b 90 }
	} elseif { [expr abs($do)] < 1e-20 } {
	    if { $da < 0 } {
		set b 180
	    } else { set b 0 }
	} else {
	    set b [expr round(atan2(sin($do), \
		                    tan($la2)*cos($la1)-sin($la1)*cos($do)) \
				    *57.29577951308232087684)]
	    if { $b < 0 } {
		if { $do < 0 } { incr b 360 } else { incr b 180 }
	    } elseif { $do < 0 } { incr b 180 }
	}
	return [list [expr 1e-3*$a*acos(cos($lo1-$lo2)*cos($la1)*cos($la2)+ \
		sin($la1)*sin($la2))] $b]
    }
    set eps 5e-12
    set r [expr 1-$f]
    set tu1 [expr $r*tan($la1)] ; set tu2 [expr $r*tan($la2)]
    set cu1 [expr 1.0/sqrt($tu1*$tu1+1.0)]
    set su1 [expr $cu1*$tu1]
    set cu2 [expr 1.0/sqrt($tu2*$tu2+1.0)]
    set s [expr $cu1*$cu2]
    set baz [expr $s*$tu2] ; set faz [expr $baz*$tu1]
    set x [expr $lo2-$lo1] ; set d [expr $x+1]
    while { abs($d-$x) > $eps } {
	set sx [expr sin($x)] ; set cx [expr cos($x)]
	set tu1 [expr $cu2*$sx] ; set tu2 [expr $baz-$su1*$cu2*$cx]
	set sy [expr sqrt($tu1*$tu1+$tu2*$tu2)]
	set cy [expr $s*$cx+$faz] ; set y [expr atan2($sy,$cy)]
	set sa [expr $s*$sx/$sy] ; set c2a [expr -$sa*$sa+1.0]
	set cz [expr $faz+$faz]
	if { $cz > 0 } { set cz [expr -$cz/$c2a+$cy] }
	set e [expr $cz*$cz*2-1.0]
	set c [expr ((-3*$c2a+4.0)*$f+4.0)*$c2a*$f/16]
	set d $x
	set x [expr (($e*$cy*$c+$cz)*$sy*$c+$y)*$sa]
	set x [expr (1-$c)*$x*$f+$lo2-$lo1]
    }
    set faz [expr atan2($tu1,$tu2)]
    set baz [expr atan2($cu1*$sx,$baz*$cx-$su1*$cu2)+3.14159265358979323846]
    set x [expr sqrt((1.0/$r/$r-1)*$c2a+1)+1] ; set x [expr ($x-2.0)/$x]
    set c [expr 1-$x] ; set c [expr ($x*$x/4.0+1)/$c]
    set d [expr (0.375*$x*$x-1)*$x]
    set x [expr $e*$cy]
    set s [expr 1-$e-$e]
    set s [expr (((($sy*$sy*4-3)*$s*$cz*$d/6.0-$x)*$d/4.0+$cz)*$sy*$d+$y) \
	        *$c*$a*$r*1e-3]
    set b [expr round($faz*57.29577951308232087684)]
    if { $b < 0 } { incr b 360 }
    return [list $s $b]
}

proc ComputeDistFD {p1 p2} {
    # compute distance between positions $p1 and $p2 assuming datum
    #  parameters where set by calling SetDatumData
    global DatumA DatumF
            # formulae from "inverse" program (see above)

    set lad1 [lindex $p1 0] ; set lod1 [lindex $p1 1]
    set lad2 [lindex $p2 0] ; set lod2 [lindex $p2 1]
    if { $lad1==$lad2 && $lod1==$lod2 } { return 0 }
    set la1 [expr $lad1*0.01745329251994329576]
    set lo1 [expr $lod1*0.01745329251994329576]
    set la2 [expr $lad2*0.01745329251994329576]
    set lo2 [expr $lod2*0.01745329251994329576]
    if { [expr abs(cos($la1))]<1.e-20 || [expr abs(cos($la2))]<1.e-20 } {
	# use Law of Cosines for Spherical Trigonometry
	set x [expr cos($lo1-$lo2)*cos($la1)*cos($la2)+sin($la1)*sin($la2)]
	if { $x >= 1 } { return 0 }
	return [expr 1e-3*$DatumA*acos($x)]
    }
    set eps 5e-12
    set r [expr 1-$DatumF]
    set tu1 [expr $r*tan($la1)] ; set tu2 [expr $r*tan($la2)]
    set cu1 [expr 1.0/sqrt($tu1*$tu1+1.0)]
    set su1 [expr $cu1*$tu1]
    set cu2 [expr 1.0/sqrt($tu2*$tu2+1.0)]
    set s [expr $cu1*$cu2]
    set baz [expr $s*$tu2] ; set faz [expr $baz*$tu1]
    set x [expr $lo2-$lo1] ; set d [expr $x+1]
    while { abs($d-$x) > $eps } {
	set sx [expr sin($x)] ; set cx [expr cos($x)]
	set tu1 [expr $cu2*$sx] ; set tu2 [expr $baz-$su1*$cu2*$cx]
	set sy [expr sqrt($tu1*$tu1+$tu2*$tu2)]
	set cy [expr $s*$cx+$faz] ; set y [expr atan2($sy,$cy)]
	set sa [expr $s*$sx/$sy] ; set c2a [expr -$sa*$sa+1.0]
	set cz [expr $faz+$faz]
	if { $cz > 0 } { set cz [expr -$cz/$c2a+$cy] }
	set e [expr $cz*$cz*2-1.0]
	set c [expr ((-3*$c2a+4.0)*$DatumF+4.0)*$c2a*$DatumF/16]
	set d $x
	set x [expr (($e*$cy*$c+$cz)*$sy*$c+$y)*$sa]
	set x [expr (1-$c)*$x*$DatumF+$lo2-$lo1]
    }
    set faz [expr atan2($tu1,$tu2)]
    set baz [expr atan2($cu1*$sx,$baz*$cx-$su1*$cu2)+3.14159265358979323846]
    set x [expr sqrt((1.0/$r/$r-1)*$c2a+1)+1] ; set x [expr ($x-2.0)/$x]
    set c [expr 1-$x] ; set c [expr ($x*$x/4.0+1)/$c]
    set d [expr (0.375*$x*$x-1)*$x]
    set x [expr $e*$cy]
    set s [expr 1-$e-$e]
    set s [expr (((($sy*$sy*4-3)*$s*$cz*$d/6.0-$x)*$d/4.0+$cz)*$sy*$d+$y) \
	        *$c*$DatumA*$r*1e-3]
    return $s
}

proc ComputeDistBearFD {p1 p2} {
    # compute distance between and bearing from positions $p1 and $p2
    #  assuming datum parameters where set by calling SetDatumData
    global DatumA DatumF
            # formulae from "inverse" program (see above)

    set lad1 [lindex $p1 0] ; set lod1 [lindex $p1 1]
    set lad2 [lindex $p2 0] ; set lod2 [lindex $p2 1]
    if { $lad1==$lad2 && $lod1==$lod2 } { return "0 0" }
    set la1 [expr $lad1*0.01745329251994329576]
    set lo1 [expr $lod1*0.01745329251994329576]
    set la2 [expr $lad2*0.01745329251994329576]
    set lo2 [expr $lod2*0.01745329251994329576]
    if { [expr abs(cos($la1))]<1.e-20 || [expr abs(cos($la2))]<1.e-20 } {
	# use Law of Cosines for Spherical Trigonometry
	# bearing
	set da [expr $la2-$la1] ; set do [expr $lo2-$lo1]
	if { [expr abs($da)] < 1e-20 } {
	    if { [expr abs($do)] < 1e-20 } {
		set b 0
	    } elseif { $do < 0 } {
		set b 270
	    } else { set b 90 }
	} elseif { [expr abs($do)] < 1e-20 } {
	    if { $da < 0 } {
		set b 180
	    } else { set b 0 }
	} else {
	    set b [expr round(atan2(sin($do), \
		                    tan($la2)*cos($la1)-sin($la1)*cos($do)) \
				    *57.29577951308232087684)]
	    if { $b < 0 } {
		if { $do < 0 } { incr b 360 } else { incr b 180 }
	    } elseif { $do < 0 } { incr b 180 }
	}
	return [list [expr 1e-3*$DatumA*acos(cos($lo1-$lo2)*cos($la1)* \
		cos($la2)+sin($la1)*sin($la2))] $b]
    }
    set eps 5e-12
    set r [expr 1-$DatumF]
    set tu1 [expr $r*tan($la1)] ; set tu2 [expr $r*tan($la2)]
    set cu1 [expr 1.0/sqrt($tu1*$tu1+1.0)]
    set su1 [expr $cu1*$tu1]
    set cu2 [expr 1.0/sqrt($tu2*$tu2+1.0)]
    set s [expr $cu1*$cu2]
    set baz [expr $s*$tu2] ; set faz [expr $baz*$tu1]
    set x [expr $lo2-$lo1] ; set d [expr $x+1]
    while { abs($d-$x) > $eps } {
	set sx [expr sin($x)] ; set cx [expr cos($x)]
	set tu1 [expr $cu2*$sx] ; set tu2 [expr $baz-$su1*$cu2*$cx]
	set sy [expr sqrt($tu1*$tu1+$tu2*$tu2)]
	set cy [expr $s*$cx+$faz] ; set y [expr atan2($sy,$cy)]
	set sa [expr $s*$sx/$sy] ; set c2a [expr -$sa*$sa+1.0]
	set cz [expr $faz+$faz]
	if { $cz > 0 } { set cz [expr -$cz/$c2a+$cy] }
	set e [expr $cz*$cz*2-1.0]
	set c [expr ((-3*$c2a+4.0)*$DatumF+4.0)*$c2a*$DatumF/16]
	set d $x
	set x [expr (($e*$cy*$c+$cz)*$sy*$c+$y)*$sa]
	set x [expr (1-$c)*$x*$DatumF+$lo2-$lo1]
    }
    set faz [expr atan2($tu1,$tu2)]
    set baz [expr atan2($cu1*$sx,$baz*$cx-$su1*$cu2)+3.14159265358979323846]
    set x [expr sqrt((1.0/$r/$r-1)*$c2a+1)+1] ; set x [expr ($x-2.0)/$x]
    set c [expr 1-$x] ; set c [expr ($x*$x/4.0+1)/$c]
    set d [expr (0.375*$x*$x-1)*$x]
    set x [expr $e*$cy]
    set s [expr 1-$e-$e]
    set s [expr (((($sy*$sy*4-3)*$s*$cz*$d/6.0-$x)*$d/4.0+$cz)*$sy*$d+$y) \
	        *$c*$DatumA*$r*1e-3]
    set b [expr round($faz*57.29577951308232087684)]
    if { $b < 0 } { incr b 360 }
    return [list $s $b]
}