% Illustration of Frenet frames in 2D.
% Author: Roman Putanowicz <putanowr AT L5.pk.edu.pl>
% Date: 2012 Mar 12 13:39:12 

% I put this file under FreeBSD license to clarify how it can be used.

%%----------------------------------------------------------------------------
%% Copyright (c) 2012, Roman Putanowicz
%% All rights reserved.
%%
%% Redistribution and use in source and binary forms, with or without 
%% modification, are permitted provided that the following conditions are met:
%%
%%    * Redistributions of source code must retain the above copyright 
%%      notice, this list of conditions and the following disclaimer.
%%    * Redistributions in binary form must reproduce the above copyright 
%%      notice, this list of conditions and the following disclaimer 
%%      in the documentation and/or other materials provided with the 
%%      distribution.
%%
%% THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS 
%% "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT 
%% LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS 
%% FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE 
%% COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, 
%% INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, 
%% BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS 
%% OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 
%% AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY,
%% OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT 
%% OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY 
%% OF SUCH DAMAGE.
%%----------------------------------------------------------------------------

% Define Lagrange base of 2-nd degree and 
% the first and second derivative if it
function L = L1(t) 
   L = 2*(t-0.5).*(t-1);
end

function L = Lp1(t)
   L = 4*t-3;
end

function L=Lpp1(t)
  L = 4*ones(size(t));
end 

function L=L2(t)
  L = -4*(t).*(t-1);
end


function L= Lp2(t)
  L = -8*t+4;
end

function L=Lpp2(t)
  L = -8*ones(size(t));
end 

function L=L3(t)
  L = 2*t.*(t-0.5);
end

function L=Lp3(t)
  L = 4*t-1;
end 

function L=Lpp3(t)
  L = 4*ones(size(t));
end 

% Define function returning position vector for points on the curve
% The function returns 3D vector -- this is necessary for the 'cross'
% function used later.

function r = r(t,x,y)
  xr = x(1)*L1(t) + x(2)*L2(t) + x(3)*L3(t);
  yr = y(1)*L1(t) + y(2)*L2(t) + y(3)*L3(t);
  zr = zeros(size(t));
  r = [xr;yr;zr];
end

% Function r'(t) -- first derivative
function rp = rp(t,x,y)
  xrp = x(1)*Lp1(t) + x(2)*Lp2(t) + x(3)*Lp3(t);
  yrp = y(1)*Lp1(t) + y(2)*Lp2(t) + y(3)*Lp3(t);
  zrp = zeros(size(t));
  rp = [xrp;yrp;zrp];
end

% Function r''(t) -- second derivative 
function rpp = rpp(t,x,y)
  xrpp = x(1)*Lpp1(t) + x(2)*Lpp2(t) + x(3)*Lpp3(t);
  yrpp = y(1)*Lpp1(t) + y(2)*Lpp2(t) + y(3)*Lpp3(t);
  zrpp = zeros(size(t));
  rpp = [xrpp;yrpp;zrpp];
end

% Interpolation nodes
x = [-1,2,-1]
y = [5 0,-1]

% Dense sampling of curve for plotting purpose
t = linspace(0,1,50);
RM = r(t,x,y);

% Plot the curve
plot(RM(1,:), RM(2,:));
hold on

% Sampling of curve for calculation of Frenet frame
tv = linspace(0,1,5);

% Pointing vector for Frenet frame origins
RV = r(tv, x,y);

% The first and second derivative of position vector
RPV = rp(tv, x,y);
RPPV = rpp(tv, x, y);

% Calculate norm of the first derivative of the position vector
% in Octave 
% S = norm(RPV, 2, 'columns')
% in Matlab
for i=1:size(RPV,2)
S(i) = norm(RPV(:,i));
end

% The matrix of tangent vectors
T = [RPV(1,:)./S;RPV(2,:)./S; RPV(3,:)./S]


% Allocate matrix for normal vectors
N = zeros(3, size(RPV,2));

% Calculate the normal vectors
for i = 1:size(RPV,2)
   rp1 = RPV(:,i);
   rp2 = RPPV(:,i);
   n = cross(rp1,cross(rp1,rp2));
   n = n/norm(n);
   N(:,i) = n;
end

% Plot tangent vectors in red
ht = quiver(RV(1,:), RV(2,:), T(1,:), T(2,:),'color', [1,0,0])
set (ht, 'maxheadsize', 0.3);

% Plot tangent vectors in green 
hn = quiver(RV(1,:), RV(2,:), N(1,:), N(2,:),'color', [0,1,0])
set (hn, 'maxheadsize', 0.3);

axis('equal')
pause();

print('FrenetFrame2d.fig');