d3trafo.m

Set of tools to perform transformations between projection, ellipsoidal and cartesian coordinates in

function xyz=d3trafo(XYZ,p,O,dir)

% D3TRAFO performs 3D-transformation with 7 parameters at arbitrary rotation center
%         in either transformation direction
%
% xyz=d3trafo(XYZ,p,O,dir)
%
% Inputs:  XYZ is a nx3-matrix to be transformed. 3xn-matrices are allowed, be careful with
%              3x3-matrices!
%            p is the vector of transformation parameters [dx dy dz ex ey ez s] with
%              dx,dy,dz = translations
%              ex,ey,ez = rotations in [rad]
%                     s = scale factor
%            O is the center of the rotation in [xO yO zO]. Default is [0 0 0]
%          dir is the transformation direction.
%              If dir=0 (default), p are the parameters to transform XYZ to xyz
%              If dir=1, p are the parameters to transform xyz to XYZ, i.e. the inverse
%                        transformation on p is performed to calculate xyz from XYZ.
%
% Output:  xyz is a nx3-matrix with the transformed coordinates
%
% d3trafo performs transformation on a right-handed system, i.e. x is the 1st, y is the 2nd axis.
% This function is meant for transforming cartesian coordinates from one system to another, in
% geodesy also called "datum transformation".
% This function works for Leica GeoOffice/SKI-Pro Output parameters.
%
% See also: ell2proj, proj2ell, ell2cart, cart2ell, Ellipsoids, Projections

% Author:
% Peter Wasmeier, Technical University of Munich
% [e-mail]
% Jan 18, 2006

% Do input checking and adjust vectors
if     nargin<4,         dir=0;
elseif ~isscalar(dir),   error('Parameter ''dir'' must be a scalar expression.')
end
if     nargin<3,         O=[0 0 0]';
elseif numel(O)~=3, error('Parameter ''O'' must be a 1x3-vector!')
else                    O=O(:)'; 
end
if     nargin<2,         error('Too few parameters for D3trafo execution. Check your inputs!')
end
if     numel(p)~=7, error('Parameter ''p'' must be a 1x7-vector!')
else                    p=p(:); 
end
if     (size(XYZ,1)~=3)&&(size(XYZ,2)~=3), error('Coordinate list XYZ must be a nx3-matrix!')
elseif (size(XYZ,1)==3)&&(size(XYZ,2)~=3), XYZ=XYZ';
end
% number of coordinate triplets to transform
n=size(XYZ,1);

% Create rotation matrix
c=cos(p(4:6));
s=sin(p(4:6));
D=zeros(3);
D(:,1)=[c(2)*c(3) -c(2)*s(3) sin(p(5))]';
D(:,2)=[s(1)*s(2)*c(3)+c(1)*s(3) -s(1)*s(2)*s(3)+c(1)*c(3) -s(1)*c(2)]';
D(:,3)=[-c(1)*s(2)*c(3)+s(1)*s(3) c(1)*s(2)*s(3)+s(1)*c(3) c(1)*c(2)]';

% Translate the rotation center to coordinate origin
dXYZ=XYZ-repmat(O,n,1);

% Perform transformation
if ~dir
    T=repmat(p(1:3),1,n)+p(7)*D*dXYZ';
else
    T=D'/p(7)*(dXYZ'-repmat(p(1:3),1,n));
end

% Translate the rotation center back again
xyz=T'+repmat(O,n,1);