function [nMat, scp] = center_and_align_body(nMat, scp)
% Centre and align body.
%
% Steps: 1. Translate the body so that the average nodal coordinate is [0, 0, 0]
%        2. Select the top and the bottom control point along the Z axis.
%        3. Form a triangle using these two points and the Z axis.
%        4. Calculate the angle relative to the Z axis.
%        5. For each node and each source control point, rotate it by the angle.
%        6. Centre again.
%
% INPUTS:     nMat - [n x 4] matrix of nodal IDs and x, y, z coordinates.
%             scp  - [m x 3] matrix of source control point x, y, z coordinates.
%
% OUTPUTS:    nMat - [n x 4] matrix of nodal IDs and x, y, z coordinates, with average nodal
%                    coordinate 0, 0, 0 and translated to align with the Z-axis.
%             scp  - [m x 3] matrix of source control points, translated using the same
%                    vector as for the nodes to ensure that nodes and control points remain coincident.
%
% created by : ABR
% date : 2024-04-16

% Perform rigid body translation to center the body and the control points
[nMat, scp] = rigid_body_translation_to_triple_zero(nMat, scp);

% Select extreme points along z axis from the control points
[~, top_scp] = max(scp(:,3));
[~, bottom_scp] = min(scp(:,3));

% Calculate an angle of rotation.
angle_of_body_rotation = atand(diff(scp([top_scp bottom_scp ],1))/diff(scp([top_scp bottom_scp ],3)));

% Form a rotation matrix.
rigid_2D_rot_matrix = [sind(-angle_of_body_rotation+90) -cosd(-angle_of_body_rotation+90) ;
                       cosd(-angle_of_body_rotation+90)  sind(-angle_of_body_rotation+90)];


nMat_rotated = nan(size(nMat));
scp_rotated = nan(size(scp));
for aLoop = 1:size(nMat,1)
  nMat_rot_temp = rigid_2D_rot_matrix * nMat(aLoop,[2 4])';
  nMat_rotated(aLoop,:) = [nMat(aLoop,1) nMat_rot_temp(1) nMat(aLoop,3) nMat_rot_temp(2)];
end
for bLoop = 1:size(scp,1)
  scp_rot_temp =  rigid_2D_rot_matrix * scp(bLoop,[1 3])';
  scp_rotated(bLoop,:) = [scp_rot_temp(1) scp(bLoop,2) scp_rot_temp(2)];
end

% Assign the rotated nodes and source control points back to the nMat and scp variables.
nMat = nMat_rotated;
scp = scp_rotated;

% Perform rigid body translation to center the body and the control points
[nMat, scp] = rigid_body_translation_to_triple_zero(nMat, scp);
