//////////////////////////////////////////////////////////////////////
// Geometrically nonlinear model of an oscillating string.
// Run this file from within scilab by launching
//
// exec("nonlin_string.sci");
//
// You can switch to the linear model by changing
// use_nonlinear_model to %f.
//
// Be patient. The computation of the motion of the string takes
// some time. If nothing went wrong during the computation a graphics
// window should pop up and you should see the moving string.
//////////////////////////////////////////////////////////////////////
// Paramters:
// mass per length in kilograms
mass_per_length = 1;
// Gravitational acceleration in meters per seconds
gravity = [0;0;-9.81];
// stiffness is the force (in Newton) needed to double the length of the
// string
stiffness = 1;
// Tension-less length of the string:
string_length = 0.5;
//
damping = 1;
//


use_nonlinear_model = %t;

//////////////////////////////////////////////////////////////////////
// boundary conditions for the starting point ra and the end point rb
// of the string.
function [ra,rb] = bdc(t)
  ra = [0;0;0];
  rb = [1;0;0];
endfunction;

//////////////////////////////////////////////////////////////////////
// Initial conditions for the position of the string r0 and the velocity of
// the string v0 at the initial time t=0. Should be consistent with the
// boundary conditions!  The length parameter xi can take on values from 0
// to the tensionless length 'string-length'.
function [r0, v0] = ic(xi)
// Next if block manages multiple values of xi.
// This should not be changed.
  if length(xi) > 1 then
    r0 = []; v0 = [];
    for i=1:length(xi) do
      [r0xi, v0xi] = ic(xi(i));
      r0 = [ r0; r0xi ];
      v0 = [ v0; v0xi ];
    end;
    return;
  end;

  // Here, the user can specify his own initial conditions
  // for a single parameter value xi.
  rc = [0.5;1;0];
  [ra, rb] = bdc(0);
  r0 = ra + xi/string_length*((rc-ra)/0.5 + (xi/string_length-0.5)*(rb-2*rc+ra)/0.5);
  v0 = [0;0;0];
endfunction;


//////////////////////////////////////////////////////////////////////
// Force differentiated with respect to length:
function [df] = d_elastic_force(r,dr,d2r)
  if size(r,1) > 3 then
    df = [];
    for i=0:length(r)/3-1 do
      df1 = ...
	  d_elastic_force(r(3*i+1:3*i+3),dr(3*i+1:3*i+3),d2r(3*i+1:3*i+3));
      df = [df; df1];
    end;
    return;
  end;

  if use_nonlinear_model then
    ds = norm(dr);
    if ds > 1 then // Stretched string.
      df = stiffness*( (1.0-1.0/ds)*d2r + (dr'*d2r)*dr/ds**3 );
    else // ... pressed string?!
      df = zeros(3,1);
    end;
  else
    df = stiffness * d2r;    
  end;
endfunction;

//////////////////////////////////////////////////////////////////////
// Jacobian of the above Force function:
function [df_r, df_dr, df_d2r] = d_elastic_force_Jac(r,dr,d2r)
  if size(r,1) > 3 then
    df_r = [];
    df_dr = [];
    df_d2r = [];
    for i=0:length(r)/3-1 do
      [ df_r1, df_dr1, df_d2r1 ] = ...
	  d_elastic_force_Jac(r(3*i+1:3*i+3),dr(3*i+1:3*i+3),d2r(3*i+1:3*i+3));
      
      front = zeros(3,3*i);
      back =  zeros(3,3*(n-i-1));
      df_r = [df_r; front, df_r1, back ];
      df_dr = [ df_dr; front, df_dr1, back ];
      df_d2r = [ df_d2r; front, df_d2r1, back ];
    end;
    return;
  end;

  df_r = zeros(3,3);
  
  if use_nonlinear_model then
    ds = norm(dr);

    if ds > 1 then
      ds_dr = dr'/ds
      
      dr_times_d2r = dr'*d2r;
      
      df_dr = stiffness*( d2r*(ds_dr/ds**2) + ...
			  (dr_times_d2r*eye(3,3)+dr*d2r')/ds**3 - ...
			  (3*dr_times_d2r/ds**4) * dr*ds_dr );
      
      df_d2r = stiffness*( -eye(3,3)/ds + dr*dr'/ds**3 );
    else
      df_dr = zeros(3,3);
      df_d2r = zeros(3,3);
    end;
  else // linear model
    df_r = zeros(3,3);
    df_dr = zeros(3,3);
    df_d2r = stiffness*eye(3,3);    
  end;
endfunction;

//////////////////////////////////////////////////////////////////////
// Num of the inner discretisation points of the string:
n = 10;
// Time points at which the solution should be computed:
// wave velocity is
v_wave = sqrt(stiffness/mass_per_length);
T0 = string_length/v_wave;

t = 0:T0/100:10*T0;


//////////////////////////////////////////////////////////////////////
// Usually, the user shouldn't need to change anything beyond this line.

h = string_length / (n+1);

//////////////////////////////////////////////////////////////////////
// Spacial derivations:
function [dr, d2r] = space_der( t, r )
  [ra, rb] = bdc(t);
  h2 = 2*h;
  hsqr = h*h;

  // reshape r to a row matrix of coordinate columns
  // (for easier computation)
  r = matrix( r, 3, -1);
  
  dr = ( r(:,2) - ra )/h2;
  dr(:,n) = ( rb - r(:,n-1) )/h2;
  
  d2r = ( r(:,2) - 2*r(:,1) + ra )/hsqr;
  d2r(:,n) = ( rb - 2*r(:,n) + r(:,n-1) )/hsqr;
  
  for i=2:n-1 do
    dr(:,i) = ( r(:,i+1) - r(:,i-1) )/h2;
    d2r(:,i) = ( r(:,i+1) - 2*r(:,i) + r(:,i-1) )/hsqr;
  end;
  
  // transform dr and d2r to column matrices:
  dr = matrix(dr,-1,1);
  d2r = matrix(d2r,-1,1);
endfunction;

//////////////////////////////////////////////////////////////////////
// The Jacobian of space_der with respect to r:
function [dr_r, d2r_r] = space_der_Jac()
  h2 = 2*h;
  hsqr = h*h;
  n3 = 3*n;
  dr_r = zeros(n3,n3);
  d2r_dr = zeros(n3,n3);

  d2r_r(1:3,1:3) = -2*eye(3,3)/hsqr;
  
  for i=1:n-1 do
    // dr(:,i) = ( r(:,i+1) - r(:,i-1) )/h2;
    dr_r(3*(i-1)+(1:3), 3*i+(1:3)) = eye(3,3)/h2;
    dr_r(3*i+(1:3), 3*(i-1)+(1:3)) = -eye(3,3)/h2;
    // d2r(:,i) = ( r(:,i+1) - 2*r(:,i) + r(:,i-1) )/hsqr;
    d2r_r(3*(i-1)+(1:3), 3*i+(1:3)) = eye(3,3)/hsqr;
    d2r_r(3*i+(1:3), 3*(i-1)+(1:3)) = eye(3,3)/hsqr;
    d2r_r(3*i+(1:3), 3*i+(1:3)) = -2*eye(3,3)/hsqr;
  end;
endfunction;

//////////////////////////////////////////////////////////////////////
// State space system:
function [Sdot] = Sys(t,S)
  r = S(1:3*n);
  v = S(3*n+1:6*n);

  [ dr, d2r ] = space_der( t, r );
  
  g = [];
  for i=1:n do g = [g; gravity]; end;
  
  a = -damping*v + ( d_elastic_force( r, dr, d2r ) + g )/mass_per_length;
  
  Sdot = [ v, a ];
endfunction;

//////////////////////////////////////////////////////////////////////
// Jacobian of the state space system:
function [dSdot] = Sys_Jac(t,S)
  n3 = 3*n;
  r = S(1:n3);
  v = S(n3+1:6*n);

  
  [ dr, d2r ] = space_der( t, r );
  [df_r, df_dr, df_d2r] = d_elastic_force_Jac(r,dr,d2r);
  [ dr_r, d2r_r ] = space_der_Jac();
  
  da_r =  ( df_r + df_dr * dr_r + df_d2r * d2r_r  )/mass_per_length;
  
  da_v = -damping*eye(n3,n3);

  dSdot = [ zeros(n3,n3), eye(n3,n3);
	    da_r, da_v ];
endfunction;

//////////////////////////////////////////////////////////////////////
// Compute the initial conditions:

xi = h:h:string_length-h;

[r0,v0] = ic(xi);

S0 = [ r0; v0 ];

S = ode( "stiff", S0, 0, t, Sys, Sys_Jac);

//////////////////////////////////////////////////////////////////////
// Plotting the results:

Scol = matrix(S(1:3*n,:),3,-1);

turn_about_x = 40;
turn_about_z = 60;

for i = 1:length(t) do
  [ra, rb] = bdc(t(i));
  r = [ra, Scol(:,(i-1)*n+1:i*n), rb ];
  xbasc(0);
  param3d1(r(1,:)',r(2,:)',r(3,:)',turn_about_x,turn_about_z,"x@y@z",[3,0],[0,string_length,-1,1,-2,0]);
  xpause(40000);
end;


