old mode 120000
new mode 100644
--- a/libRDI_Coords.pl
+++ b/libRDI_Coords.pl
@@ -1,1 +1,640 @@
-/Data/src/Oceanography/ADCP_tools/libRDI_Coords.pl
\ No newline at end of file
+#======================================================================
+# R D I _ C O O R D S . P L
+# doc: Sun Jan 19 17:57:53 2003
+# dlm: Mon Nov 27 07:13:25 2017
+# (c) 2003 A.M. Thurnherr
+# uE-Info: 58 62 NIL 0 0 72 10 2 4 NIL ofnI
+#======================================================================
+
+# RDI Workhorse Coordinate Transformations
+
+# HISTORY:
+# Jan 19, 2003: - written
+# Jan 21, 2003: - made it obey HEADING_BIAS (magnetic declination)
+# Jan 22, 3003: - corrected magnetic declination
+# Feb 16, 2003: - use pitch correction from RDI manual
+# Oct 11, 2003: - BUG: return value of atan() had been interpreted
+# as degrees instead of radians
+# Feb 27, 2004: - added velApplyHdgBias()
+# - changed non-zero HEADING_ALIGNMENT from error to warning
+# Sep 16, 2005: - added deg() for [mkprofile]
+# Aug 26, 2006: - BUG: incorrect transformation for uplookers
+# Nov 30, 2007: - optimized &velInstrumentToEarth(), velBeamToInstrument()
+# - added support for 3-beam solutions
+# Feb 12, 2008: - added threeBeamFlag
+# Mar 18, 2009: - added &gimbal_pitch(), &angle_from_vertical()
+# May 19, 2009: - added &velBeamToVertical()
+# May 23, 2009: - debugged & renamed to &velBeamToBPEarth
+# May 23, 2010: - changed prototypes of rad() & deg() to conform to ANTS
+# Dec 20, 2010: - cosmetics
+# Dec 23, 2010: - added &velBeamToBPInstrument
+# Jan 22, 2011: - made velApplyHdgBias calculate sin/cos every time to allow
+# per-ensemble corrections
+# Jan 15, 2012: - replaced defined(@...) by (@...) to get rid of warning
+# Aug 7, 2013: - BUG: &velBeamToBPInstrument did not return any val unless
+# all beam velocities are defined
+# Nov 27, 2013: - added &RDI_pitch(), &tilt_azimuth()
+# Mar 4, 2014: - added support for ensembles with missing PITCH/ROLL/HEADING
+# May 29, 2014: - BUG: vertical velocity can be calculated even without
+# heading
+# - removed some old debug statements
+# - removed unused code from &velBeamToBPInstrument
+# Jan 5, 2016: - added &velEarthToInstrument(@), &velInstrumentToBeam(@)
+# Jan 9, 2016: - added &velEarthToBeam(), &velBeamToEarth()
+# Feb 29, 2016: - debugged & verified velEarthToInstrument(), velInstrumentToBeam()
+# - added velBeamToEarth()
+# May 19, 2016: - begin implemeting bin interpolation
+# May 25, 2016: - continued
+# May 26, 2016: - made it work
+# May 30, 2016: - begin implementing 2nd order attitude transformations
+# Jun 6, 2016: - toEarth transformation in beamToBPEarth was but crude approximation;
+# updated with transformation taken from Lohrman et al. (JAOT 1990)
+# - BUG: v34 sign was inconsistent with RDI coord manual
+# Jun 8, 2016: - added $ens as arg to velInstrumentToBeam() for consistency
+# Jul 7, 2016: - added velEarthToBPw() with algorithm debugged and verified
+# by Paul Wanis from TRDI
+# Oct 12, 2017: - documentation
+# Nov 26, 2017: - BUG: velBeamtoBPEarth() did not respect missing values
+# Nov 27, 2017: - BUG: numbersp() from [antslib.pl] was used
+
+use strict;
+use POSIX;
+
+my($PI) = 3.14159265358979;
+
+sub rad(@) { return $_[0]/180 * $PI; }
+sub deg(@) { return $_[0]/$PI * 180; }
+
+#----------------------------------------------------------------------
+# Tweakables
+#----------------------------------------------------------------------
+
+$RDI_Coords::minValidVels = 3; # 3-beam solutions ok (velBeamToInstrument)
+$RDI_Coords::binMapping = 'linterp'; # 'linterp' or 'none' (earthVels, BPearthVels)
+$RDI_Coords::beamTransformation = 'LHR90'; # set to 'RDI' to use 1st order transformations from RDI manual
+
+#----------------------------------------------------------------------
+# beam to earth transformation
+#----------------------------------------------------------------------
+
+$RDI_Coords::threeBeam_1 = 0; # stats from velBeamToInstrument
+$RDI_Coords::threeBeam_2 = 0;
+$RDI_Coords::threeBeam_3 = 0;
+$RDI_Coords::threeBeam_4 = 0;
+$RDI_Coords::fourBeam = 0;
+
+$RDI_Coords::threeBeamFlag = 0; # flag last transformation
+
+{ # STATIC SCOPE
+ my(@B2I);
+
+ sub velBeamToInstrument(@)
+ {
+ my($ADCP,$ens,$v1,$v2,$v3,$v4) = @_;
+ return undef unless (defined($v1) + defined($v2) +
+ defined($v3) + defined($v4)
+ >= $RDI_Coords::minValidVels);
+
+ unless (@B2I) {
+ my($a) = 1 / (2 * sin(rad($ADCP->{BEAM_ANGLE})));
+ my($b) = 1 / (4 * cos(rad($ADCP->{BEAM_ANGLE})));
+ my($c) = $ADCP->{CONVEX_BEAM_PATTERN} ? 1 : -1;
+ my($d) = $a / sqrt(2);
+ @B2I = ([$c*$a, -$c*$a, 0, 0 ],
+ [0, 0, -$c*$a, $c*$a],
+ [$b, $b, $b, $b ],
+ [$d, $d, -$d, -$d ]);
+ }
+
+ if (!defined($v1)) { # 3-beam solutions
+ $RDI_Coords::threeBeamFlag = 1;
+ $RDI_Coords::threeBeam_1++;
+ $v1 = -($v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3])/$B2I[3][0];
+ } elsif (!defined($v2)) {
+ $RDI_Coords::threeBeamFlag = 1;
+ $RDI_Coords::threeBeam_2++;
+ $v2 = -($v1*$B2I[3][0]+$v3*$B2I[3][2]+$v4*$B2I[3][3])/$B2I[3][1];
+ } elsif (!defined($v3)) {
+ $RDI_Coords::threeBeamFlag = 1;
+ $RDI_Coords::threeBeam_3++;
+ $v3 = -($v1*$B2I[3][0]+$v2*$B2I[3][1]+$v4*$B2I[3][3])/$B2I[3][2];
+ } elsif (!defined($v4)) {
+ $RDI_Coords::threeBeamFlag = 1;
+ $RDI_Coords::threeBeam_4++;
+ $v4 = -($v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2])/$B2I[3][3];
+ } else {
+ $RDI_Coords::threeBeamFlag = 0;
+ $RDI_Coords::fourBeam++;
+ }
+
+ return ($v1*$B2I[0][0]+$v2*$B2I[0][1],
+ $v3*$B2I[1][2]+$v4*$B2I[1][3],
+ $v1*$B2I[2][0]+$v2*$B2I[2][1]+$v3*$B2I[2][2]+$v4*$B2I[2][3],
+ $v1*$B2I[3][0]+$v2*$B2I[3][1]+$v3*$B2I[3][2]+$v4*$B2I[3][3]);
+ }
+} # STATIC SCOPE
+
+#----------------------------------------------------------------------
+# velInstrumentToEarth(\%ADCP,ens,v1,v2,v3,v4) => (u,v,w,e)
+# - $RDI_Coords::beamTransformation = 'LHR90'
+# - from Lohrmann, Hackett & Roet (J. Tech., 1990)
+# - eq A1 maps to RDI matrix M (sec 5.6) with
+# alpha = roll
+# beta = gimball_pitch
+# psi = calculation_pitch
+# psi = asin{sin(beta) cos(alpha) / sqrt[1- sin(alpha)^2 sin(beta)^2]}
+# - (I only checked for 0 heading, but this is sufficient)
+# - $RDI_Coords::beamTransformation = 'RDI'
+# - default prior to LADCP_w V1.3
+# - from RDI manual
+# - 99% accurate for p/r<8deg
+# => 1cm/s error for 1m/s winch speed!
+#----------------------------------------------------------------------
+
+{ # STATIC SCOPE
+ my($hdg,$pitch,$roll,@I2E);
+
+ sub velInstrumentToEarth(@)
+ {
+ my($ADCP,$ens,$v1,$v2,$v3,$v4) = @_;
+ return undef unless (defined($v1) && defined($v2) &&
+ defined($v3) && defined($v4) &&
+ defined($ADCP->{ENSEMBLE}[$ens]->{PITCH}) &&
+ defined($ADCP->{ENSEMBLE}[$ens]->{ROLL}));
+
+ unless (@I2E &&
+ $pitch == $ADCP->{ENSEMBLE}[$ens]->{PITCH} &&
+ $roll == $ADCP->{ENSEMBLE}[$ens]->{ROLL}) {
+ printf(STDERR "$0: warning HEADING_ALIGNMENT == %g ignored\n",
+ $ADCP->{HEADING_ALIGNMENT})
+ if ($ADCP->{HEADING_ALIGNMENT});
+ $hdg = $ADCP->{ENSEMBLE}[$ens]->{HEADING} - $ADCP->{HEADING_BIAS}
+ if defined($ADCP->{ENSEMBLE}[$ens]->{HEADING});
+ $pitch = $ADCP->{ENSEMBLE}[$ens]->{PITCH};
+ $roll = $ADCP->{ENSEMBLE}[$ens]->{ROLL};
+ my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll)));
+ my($rad_calc_pitch) = ($RDI_Coords::beamTransformation eq 'RDI') ? $rad_gimbal_pitch :
+ asin(sin($rad_gimbal_pitch)*cos(rad($roll)) /
+ sqrt(1-sin(rad($roll))**2*sin($rad_gimbal_pitch)**2));
+ my($sh,$ch) = (sin(rad($hdg)),cos(rad($hdg)))
+ if defined($hdg);
+ my($sp,$cp) = (sin($rad_calc_pitch),cos($rad_calc_pitch));
+ my($sr,$cr) = (sin(rad($roll)), cos(rad($roll)));
+ @I2E = $ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}
+ ? (
+ [-$ch*$cr-$sh*$sp*$sr, $sh*$cp,-$ch*$sr+$sh*$sp*$cr],
+ [-$ch*$sp*$sr+$sh*$cr, $ch*$cp, $sh*$sr+$ch*$sp*$cr],
+ [+$cp*$sr, $sp, -$cp*$cr, ],
+ ) : (
+ [$ch*$cr+$sh*$sp*$sr, $sh*$cp, $ch*$sr-$sh*$sp*$cr],
+ [$ch*$sp*$sr-$sh*$cr, $ch*$cp,-$sh*$sr-$ch*$sp*$cr],
+ [-$cp*$sr, $sp, $cp*$cr, ],
+ );
+ }
+ return defined($ADCP->{ENSEMBLE}[$ens]->{HEADING})
+ ? ($v1*$I2E[0][0]+$v2*$I2E[0][1]+$v3*$I2E[0][2],
+ $v1*$I2E[1][0]+$v2*$I2E[1][1]+$v3*$I2E[1][2],
+ $v1*$I2E[2][0]+$v2*$I2E[2][1]+$v3*$I2E[2][2],
+ $v4)
+ : (undef,undef,
+ $v1*$I2E[2][0]+$v2*$I2E[2][1]+$v3*$I2E[2][2],
+ $v4);
+ }
+} # STATIC SCOPE
+
+
+sub velBeamToEarth(@)
+{
+ my($ADCP,$e,@v) = @_;
+ return velInstrumentToEarth($ADCP,$e,velBeamToInstrument($ADCP,$e,@v));
+}
+
+
+#----------------------------------------------------------------------
+# velEarthToInstrument() transforms earth to instrument coordinates
+# - based on manually inverted rotation matrix M (Sec 5.6 in coord-trans manual)
+# - Paul Wanis from TRDI pointed out that M is orthonormal, which
+# implies that M^-1 = M' (where M' is the transpose), confirming
+# the (unnecessary) derivation
+# - code was verified for both down- and uplookers
+# - missing heading data (IMP) causes undef beam velocities
+#----------------------------------------------------------------------
+
+{ # STATIC SCOPE
+ my($hdg,$pitch,$roll,@E2I);
+
+ sub velEarthToInstrument(@)
+ {
+ my($ADCP,$ens,$u,$v,$w,$ev) = @_;
+
+ unless (@E2I &&
+ $pitch == $ADCP->{ENSEMBLE}[$ens]->{PITCH} &&
+ $roll == $ADCP->{ENSEMBLE}[$ens]->{ROLL}) {
+ $hdg = $ADCP->{ENSEMBLE}[$ens]->{HEADING} - $ADCP->{HEADING_BIAS}
+ if defined($ADCP->{ENSEMBLE}[$ens]->{HEADING});
+ $pitch = $ADCP->{ENSEMBLE}[$ens]->{PITCH};
+ $roll = $ADCP->{ENSEMBLE}[$ens]->{ROLL};
+ my($rad_gimbal_pitch) = atan(tan(rad($pitch)) * cos(rad($roll)));
+ my($useRoll) = ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}) ? $roll+180 : $roll;
+ my($sh,$ch) = (sin(rad($hdg)),cos(rad($hdg)))
+ if defined($hdg);
+ my($sp,$cp) = (sin($rad_gimbal_pitch),cos($rad_gimbal_pitch));
+ my($sr,$cr) = (sin(rad($useRoll)), cos(rad($useRoll)));
+ @E2I = ([$ch*$cr+$sh*$sp*$sr, $ch*$sp*$sr-$sh*$cr, -$cp*$sr], # M^-1 = R^-1 * P^-1 * R^-1
+ [$sh*$cp, $ch*$cp, $sp ],
+ [$ch*$sr-$sh*$sp*$cr, -$sh*$sr-$ch*$sp*$cr, $cp*$cr]);
+ }
+
+ return defined($ADCP->{ENSEMBLE}[$ens]->{HEADING})
+ ? ($u*$E2I[0][0]+$v*$E2I[0][1]+$w*$E2I[0][2],
+ $u*$E2I[1][0]+$v*$E2I[1][1]+$w*$E2I[1][2],
+ $u*$E2I[2][0]+$v*$E2I[2][1]+$w*$E2I[2][2],
+ $ev)
+ : (undef,undef,undef,undef);
+
+ } # velEarthToIntrument()
+} # STATIC SCOPE
+
+#----------------------------------------------------------------------
+# velInstrumentToBeam() transforms instrument to beam coordinates
+# - based on manually solved eq system in sec 5.3 of coord manual
+# - does not implement bin-remapping
+# - returns undef for 3-beam solutions, as it is not known which
+# beam was bad
+#----------------------------------------------------------------------
+
+{ # STATIC SCOPE
+ my($a,$b,$c,$d);
+
+ sub velInstrumentToBeam(@)
+ {
+ my($ADCP,$ens,$x,$y,$z,$ev) = @_;
+ return undef unless (defined($x) + defined($y) +
+ defined($z) + defined($ev) == 4);
+
+ unless (defined($a)) {
+ $a = 1 / (2 * sin(rad($ADCP->{BEAM_ANGLE})));
+ $b = 1 / (4 * cos(rad($ADCP->{BEAM_ANGLE})));
+ $c = $ADCP->{CONVEX_BEAM_PATTERN} ? 1 : -1;
+ $d = $a / sqrt(2);
+ }
+
+ return ( $x/(2*$a*$c) + $z/(4*$b) + $ev/(4*$d),
+ -$x/(2*$a*$c) + $z/(4*$b) + $ev/(4*$d),
+ -$y/(2*$a*$c) + $z/(4*$b) - $ev/(4*$d),
+ $y/(2*$a*$c) + $z/(4*$b) - $ev/(4*$d));
+
+ }
+} # STATIC SCOPE
+
+#----------------------------------------------------------------------
+# velEarthToBeam() combines velEarthToInstrument and velInstrumentToBeam
+#----------------------------------------------------------------------
+
+sub velEarthToBeam(@)
+{
+ my($ADCP,$ens,$u,$v,$w,$ev) = @_;
+ return velInstrumentToBeam($ADCP,$ens,
+ velEarthToInstrument($ADCP,$ens,$u,$v,$w,$ev));
+}
+
+#----------------------------------------------------------------------
+# velEarthToBPw() returns w12 and w34 for beam-coordinate data
+# - I am grateful for Paul Wanis from TRDI who corrected a
+# bug in my transformation (fixed in V1.3). [The bug did not
+# affect the final w profiles significantly, because w12 and w34
+# are used only as diagnostics.]
+# - algorithm:
+# 1) rotate into instrument coordinates
+# 2) w12 = w + e*tan(beam_angle)/sqrt(2)
+# w34 = w - e*tan(beam_angle)/sqrt(2)
+# 3) rotate into horizontal coords (earth coords w/o
+# considering heading, i.e. same as earth coords
+# in case of w
+# - the commented-out version above is a "brute-force"
+# implementation which should give the same result
+#----------------------------------------------------------------------
+
+#sub velEarthToBPw(@)
+#{
+# my(@bpv) = velBeamToBPEarth(&velEarthToBeam(@_));
+# return ($bpv[1],$bpv[3]);
+#}
+
+sub velEarthToBPw(@)
+{
+ my($ADCP,$ens,$u,$v,$w,$ev) = @_;
+ my(@iv) = velEarthToInstrument(@_);
+ my(@iv12) = my(@iv34) = @iv;
+ $iv12[2] += $iv[3] * tan(rad($ADCP->{BEAM_ANGLE}))/sqrt(2);
+ $iv34[2] -= $iv[3] * tan(rad($ADCP->{BEAM_ANGLE}))/sqrt(2);
+ my(@ev12) = velInstrumentToEarth($ADCP,$ens,@iv12);
+ my(@ev34) = velInstrumentToEarth($ADCP,$ens,@iv34);
+ return ($ev12[2],$ev34[2]);
+}
+
+#======================================================================
+# velBeamToBPEarth(@) calculates the vertical- and horizontal vels
+# from the two beam pairs separately. Note that (w1+w2)/2 is
+# identical to the w estimated according to RDI (ignoring 3-beam
+# solutions).
+#======================================================================
+
+{ # STATIC SCOPE
+ my($TwoCosBAngle,$TwoSinBAngle);
+
+ sub velBeamToBPEarth(@)
+ {
+ my($ADCP,$ens,$b1,$b2,$b3,$b4) = @_;
+ my($v12,$w12,$v34,$w34);
+
+ return (undef,undef,undef,undef)
+ unless (defined($ADCP->{ENSEMBLE}[$ens]->{PITCH}) &&
+ defined($ADCP->{ENSEMBLE}[$ens]->{ROLL}));
+
+ unless (defined($TwoCosBAngle)) {
+ $TwoCosBAngle = 2 * cos(rad($ADCP->{BEAM_ANGLE}));
+ $TwoSinBAngle = 2 * sin(rad($ADCP->{BEAM_ANGLE}));
+ }
+ my($rad_roll) = rad($ADCP->{ENSEMBLE}[$ens]->{ROLL});
+ my($sr) = sin($rad_roll); my($cr) = cos($rad_roll);
+ my($rad_gimbal_pitch) = atan(tan(rad($ADCP->{ENSEMBLE}[$ens]->{PITCH})) * $cr); # gimbal pitch
+ my($rad_calc_pitch) = ($RDI_Coords::beamTransformation eq 'RDI') ? $rad_gimbal_pitch :
+ asin(sin($rad_gimbal_pitch)*cos($rad_roll) /
+ sqrt(1-sin($rad_roll)**2*sin($rad_gimbal_pitch)**2));
+ my($sp) = sin($rad_calc_pitch); my($cp) = cos($rad_calc_pitch);
+
+ # Sign convention:
+ # - refer to Coord manual Fig. 3
+ # - v12 is horizontal velocity from beam1 to beam2, i.e. westward for upward-looking ADCP
+ # with beam 3 pointing north (heading = 0)
+
+ my($v12_ic,$w12_ic,$v34_ic,$w34_ic,$w_ic);
+
+ if (numberp($b1) && numberp($b2)) {
+ $v12_ic = ($b1-$b2)/$TwoSinBAngle; # instrument coords...
+ $w12_ic = ($b1+$b2)/$TwoCosBAngle; # consistent with RDI convention
+ }
+ if (numberp($b3) && numberp($b4)) {
+ $v34_ic = ($b4-$b3)/$TwoSinBAngle;
+ $w34_ic = ($b3+$b4)/$TwoCosBAngle;
+ }
+
+ if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_DOWN}) { # beampair Earth coords
+ if (numberp($w12_ic) && numberp($w34_ic)) {
+ $w_ic = ($w12_ic+$w34_ic) / 2;
+ $v12 = $v12_ic*$cr + $v34_ic*0 + $w_ic*$sr; # Lohrman et al. (1990) A1
+ $v34 = $v12_ic*$sp*$sr + $v34_ic*$cp - $w_ic*$sp*$cr; # - defined for z upward => DL
+ $w12 =-$v12_ic*$cp*$sr + $v34_ic*$sp + $w12_ic*$cp*$cr;
+ $w34 =-$v12_ic*$cp*$sr + $v34_ic*$sp + $w34_ic*$cp*$cr;
+ } elsif (numberp($w12_ic)) {
+ $v12 = $v12_ic*$cr + $w12_ic*$sr;
+ $w12 =-$v12_ic*$cp*$sr + $w12_ic*$cp*$cr;
+ } elsif (numberp($w34_ic)) {
+ $v34 = $v34_ic*$cp - $w34_ic*$sp*$cr;
+ $w34 = $v34_ic*$sp + $w34_ic*$cp*$cr;
+ }
+ } else {
+ if (numberp($w12_ic) && numberp($w34_ic)) {
+ $w_ic = ($w12_ic+$w34_ic) / 2;
+ $v12 =-$v12_ic*$cr + $v34_ic*0 - $w_ic*$sr; # - as above with 1st & 3rd cols negated
+ $v34 =-$v12_ic*$sp*$sr + $v34_ic*$cp + $w_ic*$sp*$cr;
+ $w12 = $v12_ic*$cp*$sr + $v34_ic*$sp - $w12_ic*$cp*$cr;
+ $w34 = $v12_ic*$cp*$sr + $v34_ic*$sp - $w34_ic*$cp*$cr;
+ } elsif (numberp($w12_ic)) {
+ $v12 =-$v12_ic*$cr - $w12_ic*$sr;
+ $w12 = $v12_ic*$cp*$sr - $w12_ic*$cp*$cr;
+ } elsif (numberp($w34_ic)) {
+ $v34 = $v34_ic*$cp + $w34_ic*$sp*$cr;
+ $w34 = $v34_ic*$sp - $w34_ic*$cp*$cr;
+ }
+ }
+
+ return ($v12,$w12,$v34,$w34);
+ }
+}
+
+#===================================================================
+# velBeamToBPInstrument(@) calculates the instrument-coordinate vels
+# from the two beam pairs separately.
+# - in spite of the function name, the output is in ship
+# coordinates (instr coords with w up)
+#===================================================================
+
+{ # STATIC SCOPE
+ my($TwoCosBAngle,$TwoSinBAngle);
+
+ sub velBeamToBPInstrument(@)
+ {
+ my($ADCP,$ens,$b1,$b2,$b3,$b4) = @_;
+ my($v12,$w12,$v34,$w34);
+
+ return (undef,undef,undef,undef)
+ unless (defined($ADCP->{ENSEMBLE}[$ens]->{PITCH}) &&
+ defined($ADCP->{ENSEMBLE}[$ens]->{ROLL}));
+
+ unless (defined($TwoCosBAngle)) {
+ $TwoCosBAngle = 2 * cos(rad($ADCP->{BEAM_ANGLE}));
+ $TwoSinBAngle = 2 * sin(rad($ADCP->{BEAM_ANGLE}));
+ }
+
+ # Sign convention:
+ # - refer to Coord manual Fig. 3
+ # - v12 is horizontal velocity from beam1 to beam2
+ # - w is +ve upward, regardless of instrument orientation
+
+ if (defined($b1) && defined($b2)) {
+ $v12 = ($b1-$b2)/$TwoSinBAngle;
+ $w12 = ($b1+$b2)/$TwoCosBAngle;
+ $w12 *= -1 if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP});
+ }
+ if (defined($b3) && defined($b4)) {
+ $v34 = ($b4-$b3)/$TwoSinBAngle;
+ $w34 = ($b3+$b4)/$TwoCosBAngle;
+ $w34 *= -1 if ($ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP});
+ }
+
+ return ($v12,$w12,$v34,$w34);
+ }
+}
+
+#======================================================================
+# velApplyHdgBias() applies the heading bias, which is used to correct
+# for magnetic declination for data recorded in Earth-coordinates ONLY.
+# Bias correction for beam-coordinate data is done in velInstrumentToEarth()
+#======================================================================
+
+sub velApplyHdgBias(@)
+{
+ my($ADCP,$ens,$v1,$v2,$v3,$v4) = @_;
+ return (undef,undef,undef,undef)
+ unless (defined($v1) && defined($v2) &&
+ defined($ADCP->{ENSEMBLE}[$ens]->{HEADING}));
+
+ my($sh) = sin(rad(-$ADCP->{HEADING_BIAS}));
+ my($ch) = cos(rad(-$ADCP->{HEADING_BIAS}));
+
+ return ( $v1*$ch + $v2*$sh,
+ -$v1*$sh + $v2*$ch,
+ $v3 ,
+ $v4 );
+}
+
+#----------------------------------------------------------------------
+# Pitch/Roll Functions
+#----------------------------------------------------------------------
+
+sub gimbal_pitch($$) # RDI coord trans manual
+{
+ my($RDI_pitch,$RDI_roll) = @_;
+ return 'nan' unless defined($RDI_pitch) && defined($RDI_roll);
+ return deg(atan(tan(rad($RDI_pitch)) * cos(rad($RDI_roll))));
+}
+
+sub RDI_pitch($$)
+{
+ my($gimbal_pitch,$roll) = @_;
+ return 'nan' unless defined($gimbal_pitch) && defined($roll);
+ return deg(atan(tan(rad($gimbal_pitch))/cos(rad($roll))));
+}
+
+sub tilt_azimuth($$)
+{
+ my($gimbal_pitch,$roll) = @_;
+ return 'nan' unless defined($gimbal_pitch) && defined($roll);
+ return angle(deg(atan2(sin(rad($gimbal_pitch)),sin(rad($roll)))));
+}
+
+# - angle from vertical is home grown
+# - angle between two unit vectors given by acos(v1 dot v2)
+# - vertical unit vector v1 = (0 0 1) => dot product = z-component of v2
+# - when vertical unit vector is pitched in x direction, followed by
+# roll in y direction:
+# x = sin(pitch)
+# y = cos(pitch) * sin(roll)
+# z = cos(pitch) * cos(roll)
+# has been checked with sqrt(x^2+y^2+z^2) == 1
+# - for small angles, this is very similar to sqrt(pitch^2+roll^2)
+
+sub angle_from_vertical($$)
+{
+ my($RDI_pitch,$RDI_roll) = @_;
+ return 'nan' unless defined($RDI_pitch) && defined($RDI_roll);
+ my($rad_pitch) = atan(tan(rad($RDI_pitch)) * cos(rad($RDI_roll)));
+ return deg(acos(cos($rad_pitch) * cos(rad($RDI_roll))));
+}
+
+#----------------------------------------------------------------------
+# alongBeamDZ(ADCP_dta,ens,beam) => (dz_to_bin1_center,bin_dz)
+# - calculate vertical distances:
+# - between transducer and bin1
+# - between adjacent bins
+# - no soundspeed correction
+# - for UL (Fig. 3 Coord Manual):
+# b1 = phi + roll b2 = phi - roll
+# b3 = phi - pitch b4 = phi + pitch
+# - for DL:
+# b1 = phi + roll b2 = phi - roll
+# b3 = phi + pitch b4 = phi - pitch
+#----------------------------------------------------------------------
+
+sub alongBeamDZ($$$)
+{
+ my($ADCP,$ens,$beam) = @_;
+
+ my($tilt); # determine tilt of given beam
+ my($pitch) = $ADCP->{ENSEMBLE}[$ens]->{PITCH};
+ my($roll) = $ADCP->{ENSEMBLE}[$ens]->{ROLL};
+ if ($beam == 0) { # beam 1
+ $tilt = &angle_from_vertical($pitch,$ADCP->{BEAM_ANGLE}+$roll);
+ } elsif ($beam == 1) { # beam 2
+ $tilt = &angle_from_vertical($pitch,$ADCP->{BEAM_ANGLE}-$roll);
+ } elsif ($beam == 2) { # beam 3
+ $tilt = $ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}
+ ? &angle_from_vertical($ADCP->{BEAM_ANGLE}-$pitch,$roll)
+ : &angle_from_vertical($ADCP->{BEAM_ANGLE}+$pitch,$roll);
+ } else { # beam 4
+ $tilt = $ADCP->{ENSEMBLE}[$ens]->{XDUCER_FACING_UP}
+ ? &angle_from_vertical($ADCP->{BEAM_ANGLE}+$pitch,$roll)
+ : &angle_from_vertical($ADCP->{BEAM_ANGLE}-$pitch,$roll);
+ }
+ return ($ADCP->{DISTANCE_TO_BIN1_CENTER}*cos(rad($tilt)),
+ $ADCP->{BIN_LENGTH}*cos(rad($tilt)));
+}
+
+#----------------------------------------------------------------------
+# binterp(ADCP_dta,ens,bin,ADCP_field) => @interpolated_vals
+# - interpolate beam velocities to nominal bin center
+# - field can be VELOCITY, ECHO_AMPLITUDE, ...
+#
+# earthVels(ADCP_dta,ens,bin) => (u,v,w,err_vel)
+# BPEarthVels(ADCP_dta,ens,bin) => (v12,w12,v34,w34)
+# - new interface (V1.7)
+#----------------------------------------------------------------------
+
+sub binterp1($$$$$) # interpolate along a single beam
+{
+ my($ADCP,$ens,$target_dz,$ADCP_field,$beam) = @_;
+
+ my($dz2bin1,$bin_dz) = &alongBeamDZ($ADCP,$ens,$beam);
+ my($floor_bin) = int(($target_dz-$dz2bin1) / $bin_dz);
+ $floor_bin-- if ($floor_bin == $ADCP->{N_BINS}-1);
+
+ my($y1) = $ADCP->{ENSEMBLE}[$ens]->{$ADCP_field}[$floor_bin][$beam];
+ my($y2) = $ADCP->{ENSEMBLE}[$ens]->{$ADCP_field}[$floor_bin+1][$beam];
+ $y2 = $y1 unless defined($y2);
+ $y1 = $y2 unless defined($y1);
+ return undef unless defined($y1);
+
+ my($dz1) = $dz2bin1 + $floor_bin * $bin_dz;
+ my($dz2) = $dz1 + $bin_dz;
+ my($ifac) = ($target_dz - $dz1) / ($dz2 - $dz1);
+ die("assertion failed\nifac = $ifac (target_dz = $target_dz, dz1 = $dz1, dz2 = $dz2)")
+ unless ($ifac>= -0.5 && $ifac<=2);
+ return $y1 + $ifac*($y2-$y1);
+}
+
+sub binterp($$$$)
+{
+ my($ADCP,$ens,$target_bin,$ADCP_field) = @_;
+
+ my($crt) = cos(rad($ADCP->{ENSEMBLE}[$ens]->{TILT})); # calc center depth of target bin
+ my($target_dz) = ($ADCP->{DISTANCE_TO_BIN1_CENTER} + $target_bin*$ADCP->{BIN_LENGTH}) * $crt;
+
+ return (&binterp1($ADCP,$ens,$target_dz,$ADCP_field,0), # interpolate all four beams
+ &binterp1($ADCP,$ens,$target_dz,$ADCP_field,1),
+ &binterp1($ADCP,$ens,$target_dz,$ADCP_field,2),
+ &binterp1($ADCP,$ens,$target_dz,$ADCP_field,3));
+}
+
+sub earthVels($$$)
+{
+ my($ADCP,$ens,$bin) = @_;
+ if ($RDI_Coords::binMapping eq 'linterp') {
+ return velInstrumentToEarth($ADCP,$ens,
+ velBeamToInstrument($ADCP,$ens,
+ binterp($ADCP,$ens,$bin,'VELOCITY')));
+ } elsif ($RDI_Coords::binMapping eq 'none') {
+ return velInstrumentToEarth($ADCP,$ens,
+ velBeamToInstrument($ADCP,$ens,
+ @{$ADCP->{ENSEMBLE}[$ens]->{VELOCITY}[$bin]}));
+ } else {
+ die("earthVels(): unknown bin mapping '$RDI_Coords::binMapping '\n");
+ }
+}
+
+sub BPEarthVels($$$)
+{
+ my($ADCP,$ens,$bin) = @_;
+ if ($RDI_Coords::binMapping eq 'linterp') {
+ return velBeamToBPEarth($ADCP,$ens,binterp($ADCP,$ens,$bin,'VELOCITY'));
+ } elsif ($RDI_Coords::binMapping eq 'none') {
+ return velBeamToBPEarth($ADCP,$ens,@{$ADCP->{ENSEMBLE}[$ens]->{VELOCITY}[$bin]});
+ } else {
+ die("BPEarthVels(): unknown bin mapping '$RDI_Coords::binMapping '\n");
+ }
+}
+
+#----------------------------------------------------------------------
+
+1;