function [T,p,dp,varargout] = bht_traject_compute1(varargin)
% BHT_TRAJECT_COMPUTE
%
% Compute articulated bodies' trajectories and return their positions and
% velocities at each time step.
%
% Syntax
%
% [T,p,dp] =
% BHT_TRAJECT_COMPUTE('DataFilename',filename1,'ControlsFilename',filename2)
%
% [...,q,dq,Q,dQ,JJ] =
% BHT_TRAJECT_COMPUTE('DataFilename',filename1,'ControlsFilename',filename2)
%
% [...] = BHT_TRAJECT_COMPUTE(...,options)
%
% Description
%
% The function returns the time under the form of a linearly spaced
% vector T. The second output variable p is a matrix whose rows are the
% degrees of freedom of the bodies' first link (coordinates of its
% center of mass and its orientation) and whose columns correspond to
% the time steps ; dp are the related velocities. The function provides
% also other ways of locating the bodies:
%
% o q is a matrix whose rows are the degrees of freedom of the
% solids and dq the related velocities.
% o Q is a matrix whose rows are the coordinates of the center
% of mass and the orientation of the bodies; dQ being the related
% velocities.
%
% The function returns also JJ, a row vector whose coordinates are the
% momenta of inertia of the articulated bodies at each time step.
% Indeed, the momenta of inertia of the bodies depend on the bodies'
% shapes and evolve in time.
%
% The input variable finelame1 is the filename of the DAT-File and
% filename2 is the filename of the controls M-File. If this file
% requires a parameter, use the option
%
% [...] = BHT_TRAJECT_COMPUTE(...,'ControlsParameters',cont_parameter)
%
% where cont_parameter is a parameters matrix. The function
% BHT_TRAJECT_COMPUTE can be ran only after BHT_DATA_COMPILE having
% exited successfully for it requires the MAT-File generated and saved
% on disk during the compilation of the DAT-File. The output variables
% T,p,dp,q,dq,Q,dQ,JJ and input variables filename1, filename2 and
% cont_parameter are automatically saved on disk at the end of the
% computations in the MAT-File filename1_results. This saving can be
% disabled by adding an optional field to the command line:
%
% [...] = BHT_TRAJECT_COMPUTE(...,'SaveResults','off')
%
% You can specify the relative tolerance of the ODE solver (the ODE
% solver is the MATLAB ODE solver ODE113) by adding the option 'reltol'
% as follows:
%
% [...] = BHT_TRAJECT_COMPUTE(...,'RelTol',reltol)
%
% When this option is omitted, the default value used is reltol = 1e-4.
% Two subfunctions are contained in BHT_TRAJECT_COMPUTE: BHT_EULER_RHS
% which returns the right hand side of the main ODE and BHT_MNYSTROM
% which solves the integral equations with Nystrm's method.
%
% See also BHT_DATA_COMPILE, BHT_MNYSTROM
%
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% BIOHYDRODYNAMICS MATLAB TOOLBOX %
% %
% A. MUNNIER and B. PINCON %
% %
% alexandre.munnier@iecn.u-nancy.fr bruno.pincon@iecn.u-nancy.fr %
% http://www.iecn.u-nancy.fr/~munnier http://www.iecn.u-nancy.fr/~pincon %
% %
% INSTITUT ELIE CARTAN, NANCY 1 %
% http://www.iecn.u-nancy.fr/ %
% %
% INRIA Lorraine, Projet CORIDA %
% http://www.iecn.u-nancy.fr/~corida/ %
% %
% %
% %
% August 15th 2008 %
% %
% GNU GPL v3 licence %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%==========================================================================
warning off Matlab:dividebyzero; % turn off dividebyzero warning
%==========================================================================
%##########################################################################
% PART I
% reading the options from varargin
%##########################################################################
% default values
cont_parameters = [];
save_results = 1;
rel_tol = 1e-4;
no_filename = 1;
no_contfilename = 1;
%==========================================================================
% reading options
%================
nb_varargin = numel(varargin);
kk = 1;
while kk <= nb_varargin
if ~ischar(varargin{kk})
error(['Unknown option ',num2str(varargin{kk})]) %#ok<ST2NM>
end
%------------------------------------------------------------------
switch lower(varargin{kk})
%--------------------------------------------------------------
case 'datafilename'
if ischar(varargin{kk+1})
filename = varargin{kk+1};
exten = regexp(filename,'\w*\.(\w*)', 'tokens');
no_filename = 0;
if isempty(exten)
filename = [filename,'.dat']; %#ok<AGROW>
end
else
error('Unexpected expression for option DataFilename.')
end
kk = kk+2;
%--------------------------------------------------------------
case 'controlsfilename'
if ischar(varargin{kk+1})
cont_filename = varargin{kk+1};
no_contfilename = 0;
else
error('Unexpected expression for option ControlsFilename.')
end
kk = kk+2;
%----------------------------------------------------------
case 'controlsparameters'
if isfloat(varargin{kk+1})
cont_parameters = varargin{kk+1};
else
error('Unexpected value for option ControlsParameters. Must be a matrix.')
end
kk = kk+2;
%-------------
case 'saveresults'
if ischar(varargin{kk+1})
if strcmpi(varargin{kk+1},'off')
save_results = 0;
elseif strcmpi(varargin{kk+1},'on')
save_results = 1;
else
error('Unexpected value for option SaveResults. Must be ''on'' or ''off''.')
end
else
error('Unexpected value for option SaveResults. Must be ''on'' or ''off''.')
end
kk = kk+2;
%----------------------------------------------------------
case 'reltol'
if isvector(varargin{kk+1})
if length(varargin{kk+1}) == 1
rel_tol = varargin{kk+1};
else
error('Unexpected value for option RelTol. Must be a real positive number.')
end
else
error('Unexpected value for option RelTol. Must be a real positive number.')
end
kk = kk+2;
%----------------------------------------------------------
otherwise
error(['Unknown option ''',varargin{kk},''''])
end
end
% =========================================================================
% DataFilename is missing
if no_filename
filename = input('Enter data file''s name (hit Return to browse): ','s');
if isempty(filename);
filename = uigetfile('*.dat','Select data file');
end
end
% =========================================================================
% ControlsFilename is missing
if no_contfilename
cont_filename = input('Enter controls file''s name (hit Return to browse): ','s');
if isempty(cont_filename);
cont_filename = uigetfile('*.m','Select controls m file');
cont_filename = regexp(cont_filename,'(\w*)\.', 'tokens');
cont_filename = cont_filename{1}{1};
end
end
% building data kinematic filename ========================================
base_filename = regexp(filename,'(\w*)(?:\.)?', 'tokens');
base_filename = base_filename{1}{1};
kine_filename = [base_filename,'_kine'];
%##########################################################################
% PART II
% loading data compiled by datacompil.m
%##########################################################################
filename1 = [base_filename,'_data'];
filename2 = [base_filename,'_results']; % will be used later on
load(filename1,'gene_data','discret_data','bound_data','ode_data',...
'phys_data','fish');
%##########################################################################
% PART III
% initializing variables
%##########################################################################
% to display progress time
rel_time1 =0;
% initial positions and velocities of the fishes ==========================
p0 = ode_data.initial_positions;
dp0 = ode_data.initial_velocities;
Tmax = ode_data.Tmax;
dT = ode_data.dT;
% physical data (densities and mass matrix) ===============================
rhos = phys_data.rhos;
rhof = phys_data.rhof;
Ms = phys_data.Ms;
% degrees of freedom of the system, nb of solids, fishes, boundaries,
% collisions ==============================================================
deg_lib = 3*gene_data.nb_fishes;
nb_solids = gene_data.nb_solids;
nb_boundaries = gene_data.nb_boundaries;
if gene_data.collisions
alpha = gene_data.alpha;
epsilon = gene_data.epsilon;
end
% preallocating variables used later on ===================================
g = cell(nb_solids,1);
gg = cell(nb_solids,1);
G = cell(nb_solids,3);
Mf = zeros(3*nb_solids,3*nb_solids);
% ------------------------
S1 = zeros(3*nb_solids,1);
S2 = zeros(3*nb_solids,1);
S3 = zeros(3*nb_solids,1);
S4 = zeros(3*nb_solids,1);
B1 = zeros(3*nb_solids,1);
%-------------------------
X = cell(1,nb_boundaries);
N = cell(1,nb_boundaries);
for kk = nb_solids+1:nb_boundaries
N{kk} = bound_data(kk).N;
X{kk} = bound_data(kk).X;
end
%--------------------------
n = zeros(1,nb_boundaries);
m = zeros(1,nb_boundaries);
dt = zeros(1,nb_boundaries);
for kk = 1:nb_boundaries
n(kk) = bound_data(kk).nbr_modes;
m(kk) = bound_data(kk).nbr_points;
dt(kk) = bound_data(kk).dt;
end
mtot = sum(m);
mc = [0,cumsum(m)];
A = zeros(mtot,mtot);
b = zeros(mtot,3*(nb_solids));
phi = cell(nb_solids,3);
toto = cell(nb_solids,3);
%-----------------------
disp('Computations in progress...')
disp('0%------20%-------40%-------60%-------80%------100%')
%-----------------------
%##########################################################################
% PART IV
% solving the ode
%##########################################################################
tic
options = odeset('reltol',rel_tol);
[T,P] = ode113(@euler_rhs,[0:dT:Tmax],[dp0;p0],options);%#ok<NBRAK> %,'options');
%##########################################################################
% PART V
% saving the results
%##########################################################################
dp = P(:,1:deg_lib)';
p = P(:,deg_lib+1:2*deg_lib)';
%==========================================================================
% q, dq are the coordinates and velocities of the solids
nb_time_step = length(T);
q = zeros(3*nb_solids,nb_time_step);
dq = zeros(3*nb_solids,nb_time_step);
for compt = 1:nb_time_step
t = T(compt);
pp = p(:,compt);
dpp = dp(:,compt);
[c,dc] = feval(cont_filename,t,cont_parameters);
[q(:,compt),dpq,dcq] = feval(kine_filename,pp,dpp,c,dc);
dq(:,compt) = dpq*dpp+dcq*dc;
end
varargout{1} = q;
varargout{2} = dq;
% =========================================================================
% Q, dQ are the coordinates and velocities of the fishes
array_fishes_solids = gene_data.array_fishes_solids;
Ms = phys_data.Ms;
mm = phys_data.fishes_mass;
[Q,dQ,JJ] = bht_gravity_center(array_fishes_solids,Ms,mm,dT,q,dq);
varargout{3} = Q;
varargout{4} = dQ;
varargout{5} = JJ;
%---------------------------------------------------------
fprintf('\n\n')
disp(['Computation completed in ',num2str(toc),' seconds'])
%---------------------------------------------------------
if save_results
save(filename2,'cont_filename','cont_parameters','T','p','dp',...
'q','dq',...
'Q','dQ','JJ');
disp(['Solution saved in file ''',filename2,'.mat'''])
end
%##########################################################################
% Appendix I
% function euler_rhs
%##########################################################################
function [dP] = euler_rhs(t,P)
% displaying simulation's time ====================================
%disp(['t = ',num2str(t)]);
rel_time = floor((t/Tmax)*50);
string = repmat('|',1,rel_time-rel_time1);
rel_time1 = max(rel_time,rel_time1);
fprintf(1,string)
% separating positions (= pp) and velocities ( = dpp) =============
dpp = P(1:deg_lib);
pp = P(deg_lib+1:2*deg_lib);
% computing controls and kinematic ================================
[c,dc,dttc] = feval(cont_filename,t,cont_parameters);
[qq,dpq,dcq,dppqdp,dpcqdc,dccqdc] = feval(kine_filename,pp,dpp,c,dc);
qq = reshape(qq,3,nb_solids);
% moving solids ===================================================
for k = 1:nb_solids
h = qq(1:2,k);
theta = qq(3,k);
R = [cos(theta),-sin(theta);...
sin(theta), cos(theta)];
X{k} = R*bound_data(k).X + h*ones(1,bound_data(k).nbr_points);
N{k} = R*bound_data(k).N;
end
% computing potentials using nystrom's method =====================
[uu,duu] = mnistrom(X,N);
% computing boundary conditions ===================================
for k = 1:nb_solids
g{k} = [N{k}(1,:)./bound_data(k).dl;...
N{k}(2,:)./bound_data(k).dl;...
bound_data(k).flux];
gg{k} = bound_data(k).fflux;
G{k,1} = [bound_data(k).K.*(g{k}(2,:).^2-g{k}(1,:).^2);...
-2*bound_data(k).K.*g{k}(1,:).*g{k}(2,:);...
-bound_data(k).K.*(g{k}(2,:).*gg{k}+g{k}(3,:).*g{k}(1,:))-g{k}(2,:)];
G{k,2} = [-2*bound_data(k).K.*g{k}(1,:).*g{k}(2,:);...
bound_data(k).K.*(g{k}(1,:).^2-g{k}(2,:).^2);...
bound_data(k).K.*(gg{k}.*g{k}(1,:)-g{k}(3,:).*g{k}(2,:))+g{k}(1,:)];
G{k,3} = [-bound_data(k).K.*(g{k}(2,:).*gg{k}+g{k}(3,:).*g{k}(1,:))-g{k}(2,:);...
bound_data(k).K.*(g{k}(1,:).*gg{k}-g{k}(2,:).*g{k}(3,:))+g{k}(1,:);...
bound_data(k).K.*(gg{k}.^2-g{k}(3,:).^2)+gg{k}];
end
% building added mass matrix ======================================
for s=1:nb_solids
for l=1:3
for j=1:nb_solids
for k=1:3
Mf(3*(s-1)+l,3*(j-1)+k) =...
0.5*rhof*(bound_data(s).dt*...
sum(bound_data(s).dl.*g{s}(l,:).*uu{s}(:,3*(j-1)+k)')...
+ bound_data(j).dt*...
sum(bound_data(j).dl.*g{j}(k,:).*uu{j}(:,3*(s-1)+l)'));
end
end
end
end
% Mass matrix of the system =======================================
M = Ms + Mf;
M1 = dpq'*M*dpq; % reduced mass matrix
% =================================================================
% computing other terms of the ode ================================
W = dpq*dpp+dcq*dc;
W1 = reshape(W,3,nb_solids);
Z = dppqdp*dpp+2*dpcqdc*dpp+dccqdc*dc+dcq*dttc;
%==================================================================
% term <dqM,W>W-1/2W^t<dqM,.>W of the ode =========================
for s = 1:nb_solids
for l = 1:3
S1p = 0;
S2p = 0;
for k = 1:nb_solids
S1p = S1p+bound_data(k).dt*...
sum(bound_data(k).dl.*duu{k}(:,3*(s-1)+l)'.*(duu{k}*W)'.*(W1(:,k)'*g{k}));
S2p = S2p+bound_data(k).dt*...
sum(bound_data(k).dl.*uu{k}(:,3*(s-1)+l)'.*...
(W1(:,k)'*(G{k,1}*W1(1,k)+G{k,2}*W1(2,k)+G{k,3}*W1(3,k))));
end;
S1(3*(s-1)+l) = S1p;
S2(3*(s-1)+l) = S2p;
S3(3*(s-1)+l) = bound_data(s).dt*...
sum(bound_data(s).dl.*g{s}(l,:).*((duu{s}*W).^2)');
S4(3*(s-1)+l) = bound_data(s).dt*...
sum(bound_data(s).dl.*g{s}(l,:).*((W1(:,s)'*g{s}).^2));
% buoyant force ==========================================
B1(3*(s-1)+l) = 9.80665*(rhos(s)-rhof)*bound_data(s).dt*...
sum(bound_data(s).dl.*g{s}(l,:).*X{s}(2,:));
end;
end;
%==================================================================
% collisions
V = zeros(1,3*nb_solids);
if gene_data.collisions
for ki = 1:nb_solids
W2=zeros(3,nb_solids);
for kj = 1:nb_boundaries
if kj ~= ki
dx1 = repmat(X{kj}(1,:),m(ki),1) - repmat((X{ki}(1,:))',1,m(kj));
dx2 = repmat(X{kj}(2,:),m(ki),1) - repmat((X{ki}(2,:))',1,m(kj));
dist2 = dx1.^2 + dx2.^2;
big_mat = (bound_data(ki).dl'*bound_data(kj).dl).*(dist2.^(-alpha)) ...
.*(2*alpha*( dx1.*repmat((N{ki}(1,:)./bound_data(ki).dl)',1,m(kj))...
+ dx2.*repmat((N{ki}(2,:)./bound_data(ki).dl)',1,m(kj)))./dist2 ...
+ repmat(bound_data(ki).K',1,m(kj)) );
for k = 1:3
W2(k,kj) = dt(ki)*dt(kj)*sum(sum(big_mat.*repmat(g{ki}(k,:)',1,m(kj))));
end
end
for k = 1:3
V(3*(ki-1)+k) = epsilon^alpha*sum(W2(k,:));
end
end
end
end
V1 = dpq'*V';
%==================================================================
% right hand side term
F1 = rhof*dpq'*(-S1+0.5*S3+S2+0.5*S4);
F2 = dpq'*M*Z;
B = dpq'*B1;
%++++++++++++++
F = F1+F2-B+V1;
%++++++++++++++
%=========================================
dP = [-M1\F;dpp];
%=========================================
end
%##########################################################################
% Appendix II
% function mnistrom_new
%##########################################################################
function [uu,duu] = mnistrom(X,N)
% this function computes the potentials and its tangential
% derivative on the boundaries of the fluid
for j = 1:nb_solids
phi{j,1} = N{j}(1,:);
phi{j,2} = N{j}(2,:);
phi{j,3} = bound_data(j).flux.*bound_data(j).dl;
for k = 1:3
toto{j,k} = real(ifft(fft(phi{j,k})./[1,1:n(j),n(j):-1:1]));
end
end
for ki = 1:nb_boundaries
for kj = 1:nb_boundaries
% computing the matricial bloc A(ki,kj)
dx1 = repmat(X{kj}(1,:),m(ki),1) - repmat((X{ki}(1,:))',1,m(kj));
dx2 = repmat(X{kj}(2,:),m(ki),1) - repmat((X{ki}(2,:))',1,m(kj));
dist2 = dx1.^2 + dx2.^2;
A(mc(ki)+1:mc(ki+1),mc(kj)+1:mc(kj+1)) = dt(kj)*(dx1.*repmat(N{kj}(1,:),m(ki),1)...
+ dx2.*repmat(N{kj}(2,:),m(ki),1))./dist2;
if ki == kj
% the diagonal bloc A(ki,kj) must be recomputed as
% follows :
id_diag = sub2ind([mtot,mtot],mc(ki)+1:mc(ki+1),mc(ki)+1:mc(ki+1));
A(id_diag) = -pi - 0.5*dt(ki)*bound_data(ki).K.*bound_data(ki).dl;
end
% second right hand side
if kj <= nb_solids
if kj ~= ki
J0 = 3*(kj-1);
for j=1:3
b(mc(ki)+1:mc(ki+1),J0+j) = 0.5*dt(kj)*log(dist2)*phi{kj,j}';
end
else % ki == kj the kernel has to be splited into two parts
temp = abs(sqrt(dist2)./(2*sin(0.5*(repmat(bound_data(ki).t,m(ki),1)...
-repmat(bound_data(ki).t',1,m(ki))))));
id_diag = sub2ind([m(ki),m(ki)],1:m(ki),1:m(ki));
temp(id_diag) = bound_data(ki).dl;
temp = log( exp(0.5)*temp );
J0 = 3*(ki-1);
for j=1:3
b(mc(ki)+1:mc(ki+1),J0+j) = dt(ki)* (temp * phi{ki,j}') - pi*toto{ki,j}';
end
end
end
end
end
% computation of the potentials
if gene_data.pb_type == 1;
nA = size(A,1);
A(1:end-1,:) = A(1:end-1,:) - ones(nA-1,1)*A(end,:);
b(1:end-1,:) = b(1:end-1,:) - ones(nA-1,1)*b(end,:);
A(end,:) = discret_data.last_line;
b(end,:) = 0;
end;
u = A\b;
%------------------------------
uu = cell(nb_solids,1);
duu = cell(nb_solids,1);
%==============================
% computation of the tangiential derivative
for j=1:nb_solids
uu{j}=u(mc(j)+1:mc(j+1),:);
duu{j}=ifft(i*([0:n(j),-n(j):-1]'*...
ones(1,3*nb_solids)).*fft(uu{j}))./(bound_data(j).dl'*ones(1,3*nb_solids));
end
end
%##########################################################################
end
