function [] = bht_g_kinematics(fish,filename)
% BHT_G_KINEMATICS
%
% Generate a M-File by which the articulated bodies' kinematics is handled
% in the computations.
%
% Syntax
%
% BHT_G_KINEMATICS(fish,filename)
%
% Description
%
% The function is ran by BHT_DATA_COMPILE. It generates the M-file
% 'filename' by which the articulated bodies' kinematics is handled in
% the computations. The articulated bodies' structure is described by
% the user in the DAT-File, next compiled by the function
% BHT_DATA_COMPILE and eventually stored in the structure array
% variable 'fish'.
%
% The M-File filename is called by the functions BHT_KINE_CHECK,
% BHT_TRAJECT_COMPUTE and BHT_ENERGY.
%
% See also BHT_DATA_COMPILE
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
% %
% 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 %
% %
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%##########################################################################
% Preallocation
%+++++++++++++++++++++++
nb_fishes = numel(fish);
nb_tot_solids = 0;
nb_tot_controls = 0;
nb_freedom = 3*nb_fishes;
nb_solids_cum = 0;
nb_controls_cum = 0;
%+++++++++++++++++++++++
for k = 1:nb_fishes
nb_tot_solids = nb_tot_solids + numel(fish{k});
nb_tot_controls = nb_tot_controls + numel(fish{k})-1;
end
%##########################################################################
% Writting function filename.m
nom_fichier = [filename, '.m'];
fid = fopen(nom_fichier, 'w');
fprintf(fid, 'function [q,dpq,dcq,dppqdp,dpcqdc,dccqdc] = %s(p,dp,c,dc)\n',filename);
fprintf(fid,'%% initialization of q, dpq, dcq, dppqdp, dpcqdc, dccqdc \n');
fprintf(fid,'\t q = zeros(%d,1);\n',3*nb_tot_solids);
fprintf(fid,'\t dpq = zeros(%d,%d);\n',3*nb_tot_solids,nb_freedom);
fprintf(fid,'\t dcq = zeros(%d,%d);\n',3*nb_tot_solids,nb_tot_controls);
fprintf(fid,'\t dppqdp = zeros(%d,%d);\n',3*nb_tot_solids,nb_freedom);
fprintf(fid,'\t dpcqdc = zeros(%d,%d);\n',3*nb_tot_solids,nb_freedom);
fprintf(fid,'\t dccqdc = zeros(%d,%d);\n',3*nb_tot_solids,nb_tot_controls);
fprintf(fid,'\n');
fprintf(fid,'%% turning real controls into complex numbers\n');
fprintf(fid,'c_complex = exp(i*c);\n');
fprintf(fid,'p_complex = complex(p(1:3:end),p(2:3:end));\n');
fprintf(fid,'\n');
%==========================================================================
for j=1:nb_fishes
fprintf(fid,'%%===================================================\n');
fprintf(fid,'%%============== PROCEEDING FISH %d ==================\n',j);
fprintf(fid,'%%===================================================\n');
SA=fish{j};
nb_solids= numel(SA);
nb_controls = nb_solids-1;
if nb_solids > 1
fprintf(fid,'q_complex = zeros(%d,1);\n',nb_solids);
end
%==========================================================================
%==========================================================================
%===================== DETERMINING POSITIONS ===========================
%==========================================================================
%==========================================================================
fprintf(fid,'%% ======================================================\n');
fprintf(fid,'%% DETERMINING POSITIONS AND ORIENTATIONS\n');
fprintf(fid,'%% ======================================================\n');
fprintf(fid,'\t%% Proceeding solid %d ====================\n',1);
if nb_solids > 1;
fprintf(fid,'\t q_complex(1) = p_complex(%d);\n',j);
end
fprintf(fid,'\t q(%d:%d) = p(%d:%d);\n',...
3*nb_solids_cum+1,3*nb_solids_cum+3,...
3*(j-1)+1,3*j);
fprintf(fid,'\n');
%==========================================================================
art=zeros(nb_solids,1);
art_m=zeros(nb_solids,1);
art_angle=zeros(nb_solids,1);
art_loc=zeros(nb_solids,1);
art_loc_m=zeros(nb_solids,1);
art_loc_angle=zeros(nb_solids,1);
%==========================================================================
if nb_solids>1
for k=2:nb_solids
%k_cum=nb_solids_cum+k;
fprintf(fid,'\t%% Proceeding solid %d ====================\n',k);
%recherche du pere
num_f_loc = SA(k).num_father;
num_f = num_f_loc +nb_solids_cum;
% complex data for the articulations ====================
art(k) = complex(SA(k).hinge_coord(1),SA(k).hinge_coord(2));
art_m(k) = norm(art(k));
art_angle(k) = angle(art(k));
art_loc(k) = complex(SA(k).local_hinge(1),SA(k).local_hinge(2));
art_loc_m(k) = norm(art_loc(k));
art_loc_angle(k) = angle(art_loc(k));
% computation of the coordinates ========================
fprintf(fid,'\t%% Computation of the coordinates\n');
fprintf(fid,['\t\t q_complex(%d) = q_complex(%d)+(%-24.16e+i*%-24.16e)...\n\t\t\t',...
'*exp(i*q(%d))*(%-24.16e+c_complex(%d)*%-24.16e);\n'],...
k,num_f_loc,cos(art_angle(k)),sin(art_angle(k)),...
3*num_f,art_m(k),nb_controls_cum+k-1,art_loc_m(k));
% build real valued q ===================================
num = 3*(nb_solids_cum+k-1) + (1:3);
fprintf(fid,'\t\t q(%d) = real(q_complex(%d));\n',num(1),k);
fprintf(fid,'\t\t q(%d) = imag(q_complex(%d));\n',num(2),k);
fprintf(fid,'\t\t q(%d) = -%-24.16e+%-24.16e+pi+c(%d)+q(%d);\n',...
num(3),art_loc_angle(k),art_angle(k),nb_controls_cum+k-1,3*num_f);
fprintf(fid,'\n');
end
end
%==========================================================================
%==========================================================================
%========================= PROCEEDING dpq ==============================
%==========================================================================
%==========================================================================
fprintf(fid,'%% ======================================================\n');
fprintf(fid,'%% PROCEEDING dpq\n');
fprintf(fid,'%% ======================================================\n');
fprintf(fid,'\n');
fprintf(fid,'\t%% Proceeding solid %d ====================\n',1);
fprintf(fid,'\t dpq(%d+(1:3),%d+(1:3)) = eye(3);\n',...
3*nb_solids_cum,3*(j-1));
fprintf(fid,'\n');
%==========================================================================
if nb_solids>=1
for k=2:nb_solids
k_cum=nb_solids_cum+k;
fprintf(fid,'\t%% Proceeding solid %d ====================\n',k);
%recherche du pere
num_f_loc = SA(k).num_father;
num_f = num_f_loc + nb_solids_cum;
%Computation of the coordinates du solide
fprintf(fid,'\t%% Computation of the coordinates\n');
fprintf(fid,['\t\t dpq(%d+(1:3),%d+(1:3)) = dpq(%d+(1:3),%d+(1:3))...\n',...
'\t\t +[real(i*(q_complex(%d)-q_complex(%d)))*dpq(%d,%d+(1:3));...\n',...
'\t\t imag(i*(q_complex(%d)-q_complex(%d)))*dpq(%d,%d+(1:3));...\n',...
'\t\t zeros(1,3)];\n'],...
3*(k_cum-1),3*(j-1),3*(num_f-1),3*(j-1),...
k,num_f_loc,3*num_f,3*(j-1),...
k,num_f_loc,3*num_f,3*(j-1));
fprintf(fid,'\n');
end
end
%==========================================================================
%==========================================================================
%===================== PROCEEDING dcq ================================
%==========================================================================
%==========================================================================
if nb_solids>1
fprintf(fid,'%% ======================================================\n');
fprintf(fid,'%% Proceeding dcq\n');
fprintf(fid,'%% ======================================================\n');
fprintf(fid,'\n');
%======================================================================
for k=2:nb_solids
k_cum=nb_solids_cum+k;
fprintf(fid,'\t%% Proceeding solid %d ====================\n',k);
%recherche du pere
num_f_loc = SA(k).num_father;
num_f = num_f_loc + nb_solids_cum;
%Computation of the coordinates du solide
fprintf(fid,'\t%% Computation of the coordinates\n');
fprintf(fid,['\t\t dcq(%d+(1:3),%d:%d) = dcq(%d+(1:3),%d:%d)...\n',...
'\t\t +[real(i*(q_complex(%d)-q_complex(%d)))*dcq(%d,%d:%d);...\n',...
'\t\t imag(i*(q_complex(%d)-q_complex(%d)))*dcq(%d,%d:%d);...\n',...
'\t\t zeros(1,%d)];\n'],...
3*(k_cum-1),nb_controls_cum+1,nb_controls_cum+nb_solids-1,3*(num_f-1),nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
k,num_f_loc,3*num_f,nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
k,num_f_loc,3*num_f,nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
nb_controls);
fprintf(fid,['\t\t dcq(%d+(1:3),%d) = dcq(%d+(1:3),%d)...\n',...
'\t\t\t+[real(i*exp(i*q(%d))*c_complex(%d)...\n',...
'\t\t\t\t*(%-24.16e+i*%-24.16e)...\n',...
'\t\t\t\t\t*%-24.16e);...\n',...
'\t\t\t imag(i*exp(i*q(%d))*c_complex(%d)...\n',...
'\t\t\t\t*(%-24.16e+i*%-24.16e)...\n',...
'\t\t\t\t\t*%-24.16e);...\n',...
'\t\t\t 1];\n'],...
3*(k_cum-1),nb_controls_cum+k-1,3*(k_cum-1),nb_controls_cum+k-1,...
3*num_f,nb_controls_cum+k-1,...
cos(art_angle(k)),sin(art_angle(k)),...
art_loc_m(k),...
3*num_f,nb_controls_cum+k-1,...
cos(art_angle(k)),sin(art_angle(k)),...
art_loc_m(k));
fprintf(fid,'\n');
end
end
%==========================================================================
%==========================================================================
%========================= PROCEEDING dppqdp ==========================
%==========================================================================
%==========================================================================
fprintf(fid,'%% ======================================================\n');
fprintf(fid,'%% Proceeding dppqdp\n');
fprintf(fid,'%% ======================================================\n');
fprintf(fid,'\n');
%==========================================================================
if nb_solids>1
for k=2:nb_solids
k_cum=nb_solids_cum+k;
fprintf(fid,'\t%% Proceeding solid %d ====================\n',k);
%recherche du pere
num_f_loc = SA(k).num_father;
num_f = num_f_loc + nb_solids_cum;
%Computation of the coordinates du solide
fprintf(fid,'\t%% Computation of the coordinates\n');
fprintf(fid,['\t\t dppqdp(%d+(1:3),%d+(1:3)) = dppqdp(%d+(1:3),%d+(1:3))...\n',...
'\t\t +[real(q_complex(%d)-q_complex(%d))*(i*dppqdp(%d,%d+(1:3))-(dpq(%d,%d+(1:3))*dp(%d+(1:3)))*dpq(%d,%d+(1:3)));...\n',...
'\t\t imag(q_complex(%d)-q_complex(%d))*(i*dppqdp(%d,%d+(1:3))-(dpq(%d,%d+(1:3))*dp(%d+(1:3)))*dpq(%d,%d+(1:3)));...\n',...
'\t\t zeros(1,3)];\n'],...
3*(k_cum-1),3*(j-1),3*(num_f-1),3*(j-1),...
k,num_f_loc,3*num_f,3*(j-1),3*num_f,3*(j-1),3*(j-1),3*num_f,3*(j-1),...
k,num_f_loc,3*num_f,3*(j-1),3*num_f,3*(j-1),3*(j-1),3*num_f,3*(j-1));
fprintf(fid,'\n');
end
end
%==========================================================================
%==========================================================================
%======================== PROCEEDING dpcqdc ===========================
%==========================================================================
%==========================================================================
fprintf(fid,'%% ======================================================\n');
fprintf(fid,'%% Proceeding dpcqdc\n');
fprintf(fid,'%% ======================================================\n');
fprintf(fid,'\n');
%==========================================================================
if nb_solids>1
for k=2:nb_solids
k_cum=nb_solids_cum+k;
fprintf(fid,'\t%% Proceeding solid %d ====================\n',k);
%recherche du pere
num_f_loc = SA(k).num_father;
num_f = num_f_loc + nb_solids_cum;
%Computation of the coordinates du solide
fprintf(fid,'\t%% Computation of the coordinates\n');
fprintf(fid,['\t\t dpcqdc(%d+(1:3),%d+(1:3)) = dpcqdc(%d+(1:3),%d+(1:3))...\n',...
'\t\t +[real(q_complex(%d)-q_complex(%d))*(i*dpcqdc(%d,%d+(1:3))-(dcq(%d,%d:%d)*dc(%d:%d))*dpq(%d,%d+(1:3)));...\n',...
'\t\t imag(q_complex(%d)-q_complex(%d))*(i*dpcqdc(%d,%d+(1:3))-(dcq(%d,%d:%d)*dc(%d:%d))*dpq(%d,%d+(1:3)));...\n',...
'\t\t zeros(1,3)]...\n',...
'\t\t -[real(exp(i*q(%d))*c_complex(%d)...\n',...
'\t\t\t *(%-24.16e+i*%-24.16e)...\n'...
'\t\t\t\t *%-24.16e)*dc(%d)*dpq(%d,%d+(1:3));...\n',...
'\t\t imag(exp(i*q(%d))*c_complex(%d)...\n',...
'\t\t *(%-24.16e+i*%-24.16e)...\n',...
'\t\t\t\t *%-24.16e)*dc(%d)*dpq(%d,%d+(1:3));...\n',...
'\t\t zeros(1,3)];\n'],...
3*(k_cum-1),3*(j-1),3*(num_f-1),3*(j-1),...
k,num_f_loc,3*num_f,3*(j-1),3*num_f,...
nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
3*num_f,3*(j-1),...
k,num_f_loc,3*num_f,3*(j-1),3*num_f,...
nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
3*num_f,3*(j-1),...
3*num_f,nb_controls_cum+k-1,...
cos(art_angle(k)),sin(art_angle(k)),art_loc_m(k),nb_controls_cum+k-1,3*num_f,3*(j-1),...
3*num_f,nb_controls_cum+k-1,...
cos(art_angle(k)),sin(art_angle(k)),art_loc_m(k),nb_controls_cum+k-1,3*num_f,3*(j-1));
fprintf(fid,'\n');
end
end
%==========================================================================
%==========================================================================
%========================= PROCEEDING dccqdc ============================
%==========================================================================
%==========================================================================
fprintf(fid,'%% ======================================================\n');
fprintf(fid,'%% Proceeding dccqdc\n');
fprintf(fid,'%% ======================================================\n');
fprintf(fid,'\n');
%==========================================================================
if nb_solids>1
for k=2:nb_solids
k_cum=nb_solids_cum+k;
fprintf(fid,'\t%% Proceeding solid %d ====================\n',k);
%recherche du pere
num_f_loc = SA(k).num_father;
num_f = num_f_loc + nb_solids_cum;
%Computation of the coordinates du solide
fprintf(fid,'\t%% Computation of the coordinates\n');
fprintf(fid,['\t\t dccqdc(%d+(1:3),%d:%d) = dccqdc(%d+(1:3),%d:%d)...\n'...
'\t\t\t-[real(q_complex(%d)-q_complex(%d))*(dcq(%d,%d:%d)*dc(%d:%d))*dcq(%d,%d:%d);...\n',...
'\t\t\t imag(q_complex(%d)-q_complex(%d))*(dcq(%d,%d:%d)*dc(%d:%d))*dcq(%d,%d:%d);...\n',...
'\t\t\t zeros(1,%d)]...\n',...
'\t\t\t-[real(exp(i*q(%d))*c_complex(%d)...\n',...
'\t\t\t\t*(%-24.16e+i*%-24.16e)...\n',...
'\t\t\t\t\t*%-24.16e)*dc(%d)*dcq(%d,%d:%d);...\n',...
'\t\t\t imag(exp(i*q(%d))*c_complex(%d)...\n',...
'\t\t\t\t*(%-24.16e+i*%-24.16e)...\n',...
'\t\t\t\t\t*%-24.16e)*dc(%d)*dcq(%d,%d:%d);...\n',...
'\t\t\t zeros(1,%d)];\n'],...
3*(k_cum-1),nb_controls_cum+1,nb_controls_cum+nb_solids-1,3*(num_f-1),nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
k,num_f_loc,3*num_f,nb_controls_cum+1,nb_controls_cum+nb_solids-1,nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
3*num_f,nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
k,num_f_loc,3*num_f,nb_controls_cum+1,nb_controls_cum+nb_solids-1,nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
3*num_f,nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
nb_controls,...
3*num_f,nb_controls_cum+k-1,...
cos(art_angle(k)),sin(art_angle(k)),...
art_loc_m(k),nb_controls_cum+k-1,3*num_f,nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
3*num_f,nb_controls_cum+k-1,...
cos(art_angle(k)),sin(art_angle(k)),...
art_loc_m(k),nb_controls_cum+k-1,3*num_f,nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
nb_controls);
fprintf(fid,['\t\t dccqdc(%d+(1:3),%d) = dccqdc(%d+(1:3),%d)...\n',...
'\t\t\t-[real(exp(i*q(%d))*c_complex(%d)...\n',...
'\t\t\t\t*(%-24.16e+i*%-24.16e)...\n',...
'\t\t\t\t\t*%-24.16e)*(dc(%d)+dcq(%d,%d:%d)*dc(%d:%d));...\n',...
'\t\t\t imag(exp(i*q(%d))*c_complex(%d)...\n',...
'\t\t\t\t*(%-24.16e+i*%-24.16e)...\n',...
'\t\t\t\t\t*%-24.16e)*(dc(%d)+dcq(%d,%d:%d)*dc(%d:%d));...\n',...
'\t\t\t 0];\n'],...
3*(k_cum-1),nb_controls_cum+k-1,3*(k_cum-1),nb_controls_cum+k-1,...
3*num_f,nb_controls_cum+k-1,...
cos(art_angle(k)),sin(art_angle(k)),...
art_loc_m(k),nb_controls_cum+k-1,3*num_f,...
nb_controls_cum+1,nb_controls_cum+nb_solids-1,nb_controls_cum+1,nb_controls_cum+nb_solids-1,...
3*num_f,nb_controls_cum+k-1,...
cos(art_angle(k)),sin(art_angle(k)),...
art_loc_m(k),nb_controls_cum+k-1,3*num_f,...
nb_controls_cum+1,nb_controls_cum+nb_solids-1,nb_controls_cum+1,nb_controls_cum+nb_solids-1);
fprintf(fid,'\n');
end
end
nb_solids_cum = nb_solids_cum + nb_solids;
nb_controls_cum = nb_controls_cum + nb_solids-1;
end
fclose(fid);
