%3D coordinate system

% Define the new origin coordinates, use the node that attaches the tongue
% to the jaw: node 930: 0.07574595 0 0.05722117
customOriginX = 0.07574595;
customOriginY = 0;
customOriginZ = 0.05722117;

% Read the rectangular points from the file
nodeFile=readmatrix('nodePositions_fixed.csv');
Points=nodeFile(:,2:4);

% Calculate the number of points
numPoints = size(Points, 1);

% Translate function to move points to the custom origin, i.e the septum
% point 
translate = @(x, y, z) ([x + customOriginX, y + customOriginY, z + customOriginZ]);

% Convert rectangular points to cylindrical coordinates
cylindricalPoints = zeros(size(Points));
for i = 1:numPoints
    x = Points(i, 1);
    y = Points(i, 2);
    z = Points(i, 3);
    
    % Translate to custom origin and convert to cylindrical
    [rho, theta] = cart2pol(x - customOriginX, z - customOriginZ); %saggital plane on the right hand side 
    
    cylindricalPoints(i, :) = [rho, theta, y];% added in the axis of rotation. 
end
   
% Display the original rectangular points and the converted cylindrical points
disp('Original Rectangular Points:');
disp(Points);
disp('Converted Cylindrical Points:');
disp(cylindricalPoints);
%% Plot the cylindrical points
figure;
scatter3(cylindricalPoints(:, 1), cylindricalPoints(:, 2), cylindricalPoints(:, 3), 'filled');
xlabel('Rho');
ylabel('Theta');
zlabel('Right Left');
title('Cylindrical Points with new origin');
grid on;
view(3);