function [d,p]=uvwrot(d,p,iup)
% function [d,p]=uvwrot(d,p,iup)
% rotate velocities by p.drot p.dpit p.drol
% iup=1, up-looking data olny
% iup=0, down-looking data only
p.uwvrot=1;
p=setdefv(p,'drot',0);
p=setdefv(p,'dpit',0);
p=setdefv(p,'drol',0);
% which bins to transform
if nargin<3,
iz=1:size(d.ru,1);
it=1;
else
if iup
% up looking only
iz=d.izu;
it=2;
disp(' up-looking data pitch-roll offset')
disp([' pitch: ',num2str(p.dpit),' roll: ',num2str(p.dpit)])
else
% down looking only
iz=d.izd;
it=1;
disp(' down-looking data pitch-roll offset')
disp([' pitch: ',num2str(p.dpit),' roll: ',num2str(p.dpit)])
end
end
d=setdefv(d,'drot',[0 0]);
d=setdefv(d,'dpit',[0 0]);
d=setdefv(d,'drol',[0 0]);
drot=p.drot-d.drot(it);
dpit=p.dpit-d.dpit(it);
drol=p.drol-d.drol(it);
d.drot(it)=p.drot;
d.dpit(it)=p.dpit;
d.drol(it)=p.drol;
% precompute some constants
d2r=pi/180; % conversion from degrees to radians
[nb,ne]=size(d.ru);
%big loop
for ii=1:ne
% get roll, pitch and heading
roll=d.rol(it,ii);
pitch=d.pit(it,ii);
head=d.hdg(it,ii);
roll2=roll+drol;
pitch2=pitch+dpit;
head2=head+drot;
earth=[d.ru(:,ii),d.rv(:,ii),d.rw(:,ii)];
% Step 1 - determine rotation angles from sensor readings
% fixed sensor case
% make sure everything is expressed in radians for MATLAB
RR=roll.*d2r;
KA=sqrt(1.0 - (sin(pitch.*d2r).*sin(roll.*d2r)).^2);
PP=asin(sin(pitch.*d2r).*cos(roll.*d2r)./KA);
HH=head.*d2r;
RR2=roll2.*d2r;
KA=sqrt(1.0 - (sin(pitch2.*d2r).*sin(roll2.*d2r)).^2);
PP2=asin(sin(pitch2.*d2r).*cos(roll2.*d2r)./KA);
HH2=head2.*d2r;
% Step 2 - calculate trig functions and scaling factors
CP=cos(PP); CR=cos(RR);
SP=sin(PP); SR=sin(RR);
CP2=cos(PP2); CR2=cos(RR2);
SP2=sin(PP2); SR2=sin(RR2);
CH=cos(HH); SH=sin(HH);
CH2=cos(HH2); SH2=sin(HH2);
% rotation matrix
% VX = VXE.*(CH*CR + SH*SR*SP) + VYE.*SH.*CP + VZE.*(CH*SR - SH*CR*SP);
% VY = -VXE.*(SH*CR - CH*SR*SP) + VYE.*CH.*CP - VZE.*(SH*SR + CH*SP*CR);
% VZ = -VXE.*(SR*CP) + VYE.*SP + VZE.*(CP*CR);
R(1,1) = (CH*CR + SH*SR*SP);
R(1,2) = SH.*CP ;
R(1,3) = (CH*SR - SH*CR*SP);
R(2,1) = -(SH*CR - CH*SR*SP);
R(2,2) = CH.*CP;
R(2,3) = -(SH*SR + CH*SP*CR);
R(3,1) = -(SR*CP);
R(3,2) = SP;
R(3,3) = (CP*CR);
RI=inv(R);
R2(1,1) = (CH2*CR2 + SH2*SR2*SP2);
R2(1,2) = SH2.*CP2 ;
R2(1,3) = (CH2*SR2 - SH2*CR2*SP2);
R2(2,1) = -(SH2*CR2 - CH2*SR2*SP2);
R2(2,2) = CH2.*CP2;
R2(2,3) = -(SH2*SR2 + CH2*SP2*CR2);
R2(3,1) = -(SR2*CP2);
R2(3,2) = SP2;
R2(3,3) = (CP2*CR2);
for IB=iz,
% Step 4: rotate to instument coordinates
vb = RI*earth(IB,:)';
% Step 4: rotate to earth coordinates
ve = R2*vb;
earth(IB,:)=ve';
end
d.ru(iz,ii)=earth(iz,1);
d.rv(iz,ii)=earth(iz,2);
d.rw(iz,ii)=earth(iz,3);
end % Big Loop