XProfan

%XYZ2YBC convert form XYZ coordinate to YBC format (LRA)
%   XYZ2YBC(X,Y,Z) convert the X,Y,Z values to Y,B,C values. assuming the bend
%   radius is zero. the size of X,Y and Z must be the same.
%
%   XYZ2YBC(X,Y,Z,R) where R is the bend radius. convert the X,Y,Z values to
%   Y,B,C values, taking in to account the bend radius R.
%
%   XYZ2YBC(X,Y,Z,R) where R is the bend radius matrix for each bend. convert
%   the X,Y,Z values to Y,B,C values, taking in to account the bend radius R
%   for each bend.  size of R must be less than the number of XYZ points by 2.
%
%   returns Y length between bends, B rotation between bends in radius,+ for
%   clockwise and - for anticlockwise, C angle of bend in radius.
%
% File: xyz2ybc
% Author: Ammar Mousali
% Date:   29/04/2004
% Griffith University
% V1.1
%
% changes from V1.0
%   changed the output from degrees to radius.
%
function [Y,B,C] = xyz2ybc(Xc,Yc,Zc,R)
if( (length(Xc) ~= length(Yc)) | (length(Xc) ~= length(Zc)) )
error('X,Y,Z are not the same size');
end
if(nargin == 3)
% set the bend radius to zero for all bends.
R = zeros(1,length(Xc)-2);
elseif(length(R) == 1)
% set the bend radius to R for all bends.
R = ones(1,length(Xc)-2).*R;
elseif(length(R) ~= length(Xc)-2)
error('R size doesnt match the number of bends');
end
% number of bends is less than the number of XYZ points by 2.
for i = 1:length(Xc)-2
% calculate the 2 vectors BA and BC, representing an angle ABC.
V1 = [Xc(i)-Xc(i+1) ; Yc(i)-Yc(i+1) ; Zc(i)-Zc(i+1)];
V2 = [Xc(i+2)-Xc(i+1) ; Yc(i+2)-Yc(i+1) ; Zc(i+2)-Zc(i+1)];
% calculate the length of vectors BA and BC.
V1l = Vlength(V1);
V2l = Vlength(V2);
% calculate the angle between the 2 vectors BA and BC.
C(i) = pi - acos(dot(V1,V2) / (V1l*V2l));
% calculate the vector perpendicular to the plan ABC.
planeV(i,:) = cross(V1,V2)';
if(i>1)
% calculate the length of the straight by subtracting the tangent of
% the bend angle.
Y(i) = Y(i) - R(i)*tan(C(i)/2);
Y(i+1) = V2l - R(i)*tan(C(i)/2);
% calculate the length of vectors perpendicular to the plan ABC and
% the plan of the bend before it.
PV1l = Vlength(planeV(i-1,:));
PV2l = Vlength(planeV(i,:));
% calculate the rotation direction.  negative if counter clockwise.
s=sign(V1 .* cross(planeV(i-1,:),planeV(i,:))');
% calculate the rotation angle and multiply by the rotation direction.
B(i) = acos(dot(planeV(i-1,:),planeV(i,:))/(PV1l*PV2l))*s(1);

else

% calculate the length of the straight by subtracting the tangent of
% the bend angle.
Y(i) = V1l - R(i)*tan(C(i)/2);
Y(i+1) = V2l - R(i)*tan(C(i)/2);
% rotation of the first bend is always zero.
B(i) = 0;
end
end
% calculate the length of a 3D vector.
function l=Vlength(V)
l = sqrt(V(1)^2 + V(2)^2 + V(3)^2);