Homec4science
Diffusion MSM (command_line) Browse

msm/TOOLBOX_Kinematics/KINEMATICS_TOOL_four_sphere_intersect.m
74ce03b0ec87command_line

Last Change
Raw File

KINEMATICS_TOOL_four_sphere_intersect.m
View Options

function [estimated_GH, alpha] = KINEMATICS_TOOL_four_sphere_intersect(center1, center2, center3, center4, initial_configuration1, initial_configuration2, alpha0in)
%{
Function for defining the intersection of the four spheres centered at center1
to center4 with a common point as common_point (GH point) that defines the
radii of the spheres.
--------------------------------------------------------------------------
Syntax :
estimated_GH = KINEMATICS_TOOL_four_sphere_intersect(center1, center2, center3, center4, common_point)
--------------------------------------------------------------------------
Function Description :
This function finds the intersection of four spheres. It first uses the
function called "KINEMATICS_TOOL_two_sphere_intersect" to define the conic
section associated with two pairs of the spheres. For instance the two
spheres centered at center1 and center2 are treated first and the the other two
as the second pair. Then it finds the intersection of the the two resulted
conic sections. More precisely, the intersection of two rings in the space.
It does that by minimizing the distance between the two conic sections
(rings). Obviously, we will have two points that depending on the quality
of our input data can be coincide.
--------------------------------------------------------------------------
%}
% find the first conic section (intersection of spheres centered at center1 and center2)
[CS_point_in_T1, r1, H_PS2T1] = KINEMATICS_TOOL_two_sphere_intersect(center1, center2, initial_configuration1);
% find the second conic section (intersection of spheres centered at center1 and center2)
[CS_point_in_T2, r2, H_PS2T2] = KINEMATICS_TOOL_two_sphere_intersect(center3, center4, initial_configuration2);
% set the optimization options fminunc
%options = optimoptions(@fminunc,'Algorithm','quasi-newton');
% options = optimoptions(@fsolve,'Algorithm','Levenberg-Marquardt');
options = optimset('Display','iter');
% run the optimization
conic_section1 = [H_PS2T1(1:3,4), H_PS2T1(1:3,1), H_PS2T1(1:3,2), [r1 0 0]']; % conic section center, u1, v1, conic section radius
conic_section2 = [H_PS2T2(1:3,4), H_PS2T2(1:3,1), H_PS2T2(1:3,2), [r2 0 0]']; % conic section center, u1, v1, conic section radius
alpha0 = alpha0in; % Starting guess
[alpha,fval,exitflag,output] = fminsearch(@(alpha) objfun(conic_section1, conic_section2, alpha), alpha0, options);% fminunc
estimated_GH(1:3,1) = point_on_conic(conic_section1, alpha(1));
estimated_GH(1:3,2) = point_on_conic(conic_section2, alpha(2));
% % define the intersection line
% % 1) define the direction vector
% n_matrix = [w1 w2]';
% n = null(n_matrix); % or equivalently cross(n1, n2)
% % 2) define an arbitrary point of the intersection line
% x0 = inv(n_matrix'*n_matrix)*n_matrix'*[-d1; -d2] ;%+ null(n_matrix);
%
% % find the first intersection point (conic section one and the intersection line)
% % 1) intersection line parameter
% t1 = -dot((x0 - O_PS1), n)/norm(n)^2;
% % 2) define the intersection point one
% estimated_GH(1:3,1) = t1*n + x0;
%
%
%
% % find the second intersection point (conic section two and the intersection line)
% % 1) intersection line parameter
% t2 = -dot((x0 - O_PS2), n)/norm(n)^2;
% % 2) define the intersection point two
% estimated_GH(1:3,2) = t2*n + x0;
% -------------------------------------------------------------------------
% Plot the results
% -------------------------------------------------------------------------
%figure;
% plot the spheres
sphere_plot(center1, norm(initial_configuration1(:,1) - initial_configuration1(:,3)));
hold on
sphere_plot(center2, norm(initial_configuration1(:,2) - initial_configuration1(:,3)));
sphere_plot(center3, norm(initial_configuration2(:,1) - initial_configuration2(:,3)));
sphere_plot(center4, norm(initial_configuration2(:,2) - initial_configuration2(:,3)));
% plot the conic section 1
plot3(CS_point_in_T1(1,:),CS_point_in_T1(2,:),CS_point_in_T1(3,:),'o',...
'MarkerSize', 7, 'MarkerEdgeColor', [0 0 0], 'MarkerFaceColor', [0 0 0])
% plot the conic section 2
plot3(CS_point_in_T2(1,:),CS_point_in_T2(2,:),CS_point_in_T2(3,:),'o',...
'MarkerSize', 7, 'MarkerEdgeColor', [0 0 0], 'MarkerFaceColor', [0 0 0])
% plot the intersection point 1
% plot3(estimated_GH(1,1),estimated_GH(2,1),estimated_GH(3,1),'o',...
% 'MarkerSize', 12, 'MarkerEdgeColor', [0 0 1], 'MarkerFaceColor', [0 0 1])
% plot the intersection point 2
plot3(estimated_GH(1,2),estimated_GH(2,2),estimated_GH(3,2),'o',...
'MarkerSize', 12, 'MarkerEdgeColor', [1 0 0], 'MarkerFaceColor', [1 0 0])
plot3(center1(1,1),center1(2,1),center1(3,1),'o',...
'MarkerSize', 5, 'MarkerEdgeColor', [0 0 0], 'MarkerFaceColor', [0 1 0])
plot3(center2(1,1),center2(2,1),center2(3,1),'o',...
'MarkerSize', 5, 'MarkerEdgeColor', [0 0 0], 'MarkerFaceColor', [0 1 0])
plot3(center3(1,1),center3(2,1),center3(3,1),'o',...
'MarkerSize', 5, 'MarkerEdgeColor', [0 0 0], 'MarkerFaceColor', [0 1 0])
plot3(center4(1,1),center4(2,1),center4(3,1),'o',...
'MarkerSize', 5, 'MarkerEdgeColor', [0 0 0], 'MarkerFaceColor', [0 1 0])
%axis equal
return;
% -------------------------------------------------------------------------
% function for plotting
% -------------------------------------------------------------------------
function sphere_plot(center_value, radius_value)
theta = linspace(0,2*pi,40);
phi = linspace(0,pi,40);
[theta,phi] = meshgrid(theta,phi);
% quantify and plot the sphere centered at center_value
% r_value = norm(center_value - common_point_value);
r_value = radius_value;
x_value = r_value*sin(phi).*cos(theta) + center_value(1);
y_value = r_value*sin(phi).*sin(theta) + center_value(2);
z_value = r_value*cos(phi) + center_value(3);
sphere_plot = surf(x_value, y_value, z_value);
sphere_plot.EdgeColor = [0.7 0.7 0.7];
sphere_plot.FaceColor = [0.7 0.7 0.7];
sphere_plot.FaceAlpha = 0.1;
sphere_plot.EdgeAlpha = 0.3;
axis equal
return;
% -------------------------------------------------------------------------
% Cost function
% -------------------------------------------------------------------------
function f = objfun(conic_section1, conic_section2, alpha)
% center_value1 = conic_section1(1:3,1);
% radius_value1 = conic_section1(1,4);
% yvector_value1 = conic_section1(1:3,2);
% xvector_value1 = conic_section1(1:3,3);
%
% center_value2 = conic_section2(1:3,1);
% radius_value2 = conic_section2(1,4);
% yvector_value2 = conic_section2(1:3,2);
% xvector_value2 = conic_section2(1:3,3);
pp1 = point_on_conic(conic_section1, alpha(1));
pp2 = point_on_conic(conic_section2, alpha(2));
f = norm(pp1 - pp2)^2;
%f = pp1 - pp2;
return;
% -------------------------------------------------------------------------
% Conic section eval
% -------------------------------------------------------------------------
function pp = point_on_conic(conic_section, angle_id)
center_value = conic_section(1:3,1);
radius_value = conic_section(1,4);
yvector_value = conic_section(1:3,2);
xvector_value = conic_section(1:3,3);
pp = center_value + radius_value*cos(angle_id)*yvector_value + radius_value*sin(angle_id)*xvector_value;
return;
c4science · Help