%  Daniel Kawano, Rose-Hulman Institute of Technology
%  Last modified:  Feb 23, 2016

clear all
close all
clc

%  Load the symbolic function handles for the state equations in
%  mass-matrix form, M(xi,Y)*Y'(xi) = F(xi,Y):

load directed_curve_ODEs

%  Physical parameters:

L = 1.5;                %  m
r0 = [0, 0, 0]';        %  m

nu1 = @(xi) 1;
nu2 = @(xi) 0;
nu3 = @(xi) 0;

%  Simulation parameters:

dxi = 0.005;                    %  m
xisim = [0 : dxi : L]';         %  m
tol = 1e-6;

psi0 = 0;                       %  rad
theta0 = 30*(pi/180);           %  rad
phi0 = 0;                       %  rad

Y0 = [psi0, theta0, phi0]';

%  Convert M(xi,Y) and F(xi,Y) to purely numeric function handles for
%  numerical integration:

M = @(xi, Y) M(xi, Y, nu1(xi), nu2(xi), nu3(xi));
F = @(xi, Y) F(xi, Y, nu1(xi), nu2(xi), nu3(xi));

%  Numerically integrate the state equations:

options = odeset('mass', M, 'abstol', tol, 'reltol', tol);

[xi, Y] = ode45(F, xisim, Y0, options);

%  Extract the results:

psi = Y(:,1);           %  rad
theta = Y(:,2);         %  rad
phi = Y(:,3);           %  rad

%  Plot the directed curve and animate the motion of the triad 
%  {d1,d2,d3} along the curve, where d3 = d1 x d2 = dr/dxi:

animate_directed_curve(psi, theta, phi, r0, dxi);