%
% Plot a helix, draw a tangent line, approximate the helix with the Taylor
% polynomial up to the 5th degree.
% The helix is described by: 
% r(1) = a * cos( omega * t )
% r(2) = b * sin( omega * t )
% r(3) = k * t
%


% Standard header :-)
clear
clf
view(3)
hold on
axis equal

% Set helix paramaters.
omega = 1.0;
k = 1.0;
a = 1.0;
b = 1.0;

%
% Create and draw helix.
%

% Set time parameters.

t0 = 0.5;
t1 = 2.5 *  pi;
dt = 0.1;

% Create time vector.
t = t0:dt:t1;

% Compute helix points.
r = [a * cos( omega * t ); b * sin( omega * t ); k * t];

% Plot helix.
plot3( r(1,:), r(2,:), r(3,:), 'b' );

%
% Tangent line at time td.
%
td = 5;

% Tangent line parameter.
s = [-1 1];

tmp = ones( size( s ) );

% Tangent vector for time td.
r_der1 = [-a * omega * sin( omega * td )
  b * omega * cos( omega * td )
  k];
% Helix point for time td.
r0 = [a * cos( omega * td ); b * sin( omega * td ); k * td];
% Tangent line parametrized by s;
p =  r0 * tmp + r_der1 * s;

% Plot tangent line.
plot3( r0(1,:), r0(2,:), r0(3,:), 'g*' );
plot3( p(1,:), p(2,:), p(3,:), 'r' );

%
% Approximate helix by Taylor polynomial at time td.
%

% Prepare derivatives.
r_der2 = [-a * omega^2 * cos( omega * td )
  -b * omega^2 * sin( omega * td )
  0];
r_der3 = [a * omega^3 * sin( omega * td )
  -b * omega^3 * cos( omega * td )
  0];
r_der4 = [a * omega^4 * cos( omega * td )
  b * omega^4 * sin( omega * td )
  0];
r_der5 = [-a * omega^5 * sin( omega * td )
  b * omega^5 * cos( omega * td )
  0];

% Put them all into one array.
r_der = [r_der1 r_der2 r_der3 r_der4 r_der5];

% Set order reduction - must be in 0 .. 4.
o_red = 0;

% Compute approximated helix points for the same time as the original.
t = t0:dt:t1;
r_aprox = zeros( 3, length( t ) );
lt = length( t );
for i = 1:lt;
  r_aprox(:,i) = r0;
  div = 1;
  for j = 1:size( r_der, 2 ) - o_red
    div = div / j;
    r_aprox(:,i) = r_aprox(:,i) + (t(i) - td)^j * r_der(:,j) * div;
  end
end
% Draw it.
plot3( r_aprox(1,:), r_aprox(2,:), r_aprox(3,:), 'm' )

% And now the same without cycles (faster!) (only to the 2nd order).
r0 = repmat( r0, 1, lt );
rx = r0(1,:) + (t - td) * r_der(1,1) + 0.5 * (t - td).^2 * r_der(1,2);
ry = r0(2,:) + (t - td) * r_der(2,1) + 0.5 * (t - td).^2 * r_der(2,2);
rz = r0(3,:) + (t - td) * r_der(3,1) + 0.5 * (t - td).^2 * r_der(3,2);
% Draw it.
plot3( rx, ry, rz, 'g' )

% Enable figure rotation by mouse.
rotate3d
