function [TransSensorPosMat,TransFidPosMat]=TransSfpPos(SensorPosMat,FidPosMat);
%   This software is protected by german copyright and international treaties.             
%   Copyright 2004 Markus Junghöfer & Peter Peyk. All Rights Reserved.                     
%                                                                                          
%   THIS SOFTWARE AND DOCUMENTATION IS PROVIDED "AS IS," AND COPYRIGHT HOLDERS MAKE        
%   NO REPRESENTATIONS OR WARRANTIES, EXPRESS OR IMPLIED, INCLUDING BUT NOT LIMITED TO,    
%   WARRANTIES OF MERCHANTABILITY OR FITNESS FOR ANY PARTICULAR PURPOSE OR THAT THE USE    
%   OF THE SOFTWARE OR DOCUMENTATION WILL NOT INFRINGE ANY THIRD PARTY PATENTS, COPYRIGHTS,
%   TRADEMARKS OR OTHER RIGHTS. COPYRIGHT HOLDERS WILL NOT BE LIABLE FOR ANY DIRECT,       
%   INDIRECT, SPECIAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF ANY USE OF THE SOFTWARE      
%   OR DOCUMENTATION.                                                                      
if nargin<2; return; end

TransSensorPosMat=SensorPosMat;
TransSensorPosMat(:,3)=-SensorPosMat(:,3);
TransSensorPosMat(:,1)=-SensorPosMat(:,2);
TransSensorPosMat(:,2)=-SensorPosMat(:,1);
TransFidPosMat=FidPosMat./100;
TransFidPosMat(:,3)=-FidPosMat(:,3);
TransFidPosMat(:,1)=-FidPosMat(:,2);
TransFidPosMat(:,2)=-FidPosMat(:,1);
na=TransFidPosMat([1,6],:);
le=TransFidPosMat([2,8],:);
re=TransFidPosMat([3,9],:);
in=TransFidPosMat([4,7],:);
cz=TransFidPosMat(5,:);
dna=diff(na);
mna=mean(na);
dle=diff(le);
mle=mean(le);
dre=diff(re);
mre=mean(re);
din=diff(in);
nin=mean(in);
mx=(le(1,:)+re(1,:))/2;
if any([dna dle dre]>1)
	Message=char('Distance between first and second fid measure > 1 cm');
	hmsgbox=msgbox(Message,'Warning:','warn')
end
for i=1:size(FidPosMat,1)
	TransFidPosMat(i,:)=TransFidPosMat(i,:)-mx;
end
for i=1:size(SensorPosMat,1)
	TransSensorPosMat(i,:)=TransSensorPosMat(i,:)-mx;
end
PhiZ=TransFidPosMat(2,2)./TransFidPosMat(3,1);
TransFidPosMat=zrotation(TransFidPosMat,PhiZ,1);
TransSensorPosMat=zrotation(TransSensorPosMat,PhiZ,1);
PhiY=TransFidPosMat(3,3)./TransFidPosMat(3,1);
TransFidPosMat=yrotation(TransFidPosMat,PhiY,1);
TransSensorPosMat=yrotation(TransSensorPosMat,PhiY,1);
PhiX=-TransFidPosMat(1,3)./TransFidPosMat(1,2);
TransFidPosMat=xrotation(TransFidPosMat,PhiX,1);
TransSensorPosMat=xrotation(TransSensorPosMat,PhiX,1);

TransFidPosMat=TransFidPosMat./100;
TransSensorPosMat=TransSensorPosMat./100;
return;
