local_forces.m

function [lift,drag,torque] = local_forces(s,theta,dTheta,bod_x,bod_y,pitch,heave,...
                                p_prime,h_prime,Vbod_x,Vbod_y,cLift)
% Calculate the forces acting on the body and generated by the fin

%% Check inputs
if max(abs(heave)>pi/2)
    error('These calculations assume heave < pi/2');
elseif max(abs(pitch)>pi/2)
    error('These calculations assume pitch < pi/2');
elseif max(abs(pitch+heave)>pi/2)
    error('These calculations assume (pitch+heave) < pi/2');
end


%% Global to local FOR

% Landmarks for local system
rost   = [bod_x+cos(theta) bod_y+sin(theta) theta.*0];
origin = [bod_x bod_y theta.*0];

% Get local velocities and transformation
for i = 1:size(rost,1)
    
    % Rotation matrix
    R{i} = local_system(origin(i,:),rost(i,:));
    
    % Transform velocities
    velG(i,:) = [Vbod_x(i) Vbod_y(i) dTheta(i)];
    velL(i,:) = global_to_local(origin(i,:),R{i},velG(i,:));
end


%% Fin kinematics in local FOR

% Distance from COM to posterior margin of trunk
tr_len = 0.7*s.bodyL;

% Length of peduncle
pd_len = s.pedL;

% Distance of COP along chord length
tl_len = s.finL/4;

% Coordinates of peduncle 
pd_pos = [-tr_len-pd_len.*cos(heave) pd_len.*sin(heave)];

% Velocity of peduncle
pd_vel = [pd_len*h_prime.*sin(heave) pd_len*h_prime.*cos(heave)];

% Coordinates of tail
tl_pos(:,1) = pd_pos(:,1) - tl_len.*cos(heave+pitch);
tl_pos(:,2) = pd_pos(:,2) - tl_len.*sin(heave+pitch);

% Velocity of tail
tl_vel(:,1) = pd_vel(:,1) + tl_len.*p_prime.*sin(heave+pitch);
tl_vel(:,2) = pd_vel(:,2) + tl_len.*p_prime.*cos(heave+pitch);

% Check on above calculations
if 0 %max(abs(heave)>pi/8)
   %figure
   plot([0 -tr_len pd_pos(1) tl_pos(1)],[0 0 pd_pos(2) tl_pos(2)],'k-',...
       'LineWidth',3);
   axis equal
   grid on 
end

clear tr_len pd_len tl_len


%% Calculate flows (local FOR)

% Flow due to body rotation is opposite to translation velocity
U_trans = -velL(:,1:2);

% Distance from tail to COM
distT = sqrt(tl_pos(:,1).^2 + tl_pos(:,2).^2);

% Angular position of tail
angT = atan(tl_pos(:,2)./abs(tl_pos(:,1)));

% Flow at the tail due to body rotation
U_rot_tail(:,1) = -velL(:,3).*distT.*sin(angT);
U_rot_tail(:,2) = -velL(:,3).*distT.*cos(angT);

% Flow at tail: sum of oppisite of self motion, and body translation and rotation
Utail(:,1) = -tl_vel(:,1) + U_trans(:,1) + U_rot_tail(:,1);
Utail(:,2) = -tl_vel(:,2) + U_trans(:,2) + U_rot_tail(:,2);
%Utail(:,1) = -tl_vel(:,1);
%Utail(:,2) = -tl_vel(:,2);

clear angT distT


%% Body forces (drag)

% Components of drag on body
% Note: assumes to act at COM (i.e. no torque)
dragL(:,1) = 0.5 * s.cDrag * s.rho * s.SA .* abs(U_trans(:,1)) .* U_trans(:,1);
dragL(:,2) = 0.5 * s.cDrag * s.rho * s.SA .* abs(U_trans(:,2)) .* U_trans(:,2);

% Viscous drag that resists rotation
dragtorque  = - s.cDrag_rot * (s.bodyL/2)^3 * s.visc * velL(:,3);


%% Lift & lift-torque on tail fin

% Lift
liftL(:,1) = 0.5*s.rho*s.finA.*cLift.*abs(Utail(:,1)).*Utail(:,1);
liftL(:,2) = 0.5*s.rho*s.finA.*cLift.*abs(Utail(:,2)).*Utail(:,2);

for i = 1:size(tl_pos,1)
    % Torque due to lift force (cross product of fin position and lift vector)
    tmp = cross([tl_pos(i,:) 0],[liftL(i,:) 0],2);
    lifttorque(i,1) = tmp(3);
end


%% Sum forces and torques

% Total force: sum of drag and lift
%force = drag(:,1:2) + lift(:,1:2);

% Total torque
torque = dragtorque + lifttorque;
%torque = 0.*dragtorque ;

%forceL = [force torque];

if abs(heave)>pi/16
    ttt=3;
end


%% Local to global FOR

for i = 1:size(rost,1)
    % Lift
    tmpG = local_to_global(origin,R{i},[liftL(i,:) 0]);
    lift(i,:) = tmpG(1:2);
    
    % Drag
    tmpG = local_to_global(origin,R{i},[dragL(i,:) 0]);
    drag(i,:) = tmpG(1:2);
end
%torque = tmpG(:,3);




function R = local_system(origin,rost)
% Defines rotation matrix for a coordinate system

% Check dimensions
if size(origin,1)~=1 || size(origin,2)~=3 || ...
   size(rost,1)~=1 || size(rost,2)~=3
    error('inputs have incorrect dimensions')
end

% Retrieve local x axis to determine coordinate system
xaxis(1,1) = rost(1) - origin(1);
xaxis(1,2) = rost(2) - origin(2);
xaxis(1,3) = 0;

% Normalize to create a unit vector
xaxis = xaxis./norm(xaxis);

%Determine local y axis
%Short hand of cross product of inertial z axis and local x axis
yaxis = [-xaxis(2) xaxis(1) 0];

% Normalize to create a unit vector
yaxis = yaxis./norm(yaxis);

%Determine local z axis
zaxis = cross(xaxis,yaxis);

% Normalize to create a unit vector
zaxis = zaxis./norm(zaxis);

%Create rotation matrix (from inertial axes to local axes)
%R = [xaxis(1:2); yaxis(1:2)];
R = [xaxis; yaxis; zaxis];
%end

function ptsT = global_to_local(origin,R,pts)
% Assumes columns vectors for coordinates

%pts = [x y z];

% Translate
pts(:,1) = pts(:,1) - origin(1);
pts(:,2) = pts(:,2) - origin(2);
pts(:,3) = pts(:,3) - origin(3);

% Rotate points
ptsT = [R * pts']';

% Extract columns of points
% xT = ptsT(:,1);
% yT = ptsT(:,2);
% zT = ptsT(:,3);
%end

function ptsT = local_to_global(origin,R,pts)
% Assumes columns vectors for coordinates

%pts = [x y];

% Rotate points
ptsT = [inv(R) * pts']';

% Translate global coordinates wrt origin
ptsT(:,1) = ptsT(:,1) + origin(1);
ptsT(:,2) = ptsT(:,2) + origin(2);
ptsT(:,3) = ptsT(:,3) + origin(3);
% Extract columns of points
%xT = ptsT(:,1);
%yT = ptsT(:,2);
%end