clc;
clear all;
close all;
rb=1;
R1=1.2;
h=0.4*R1;
ra=0.2;
k=0;
q4=0;
H=0.5;
for i=1:11
z=(i-1)*(0.1*h)+H;
for j=1:11
r2=(j-1)*(0.1*R1);
for k=1:13
phi=(k-1)*(30*pi/180);
x=r2*cos(phi);
y=r2*sin(phi);
r=[x y z];
rr=sqrt(x^2+y^2+z^2);
r1=((rr-q4)/rr)*[x
y
z];
r1r1=rr-q4;
alfa=atan(x/z);
beta=asin(y/r1r1);
beta1=0;
beta2=2*pi/3;
beta3=4*pi/3;
T=[cos(alfa) -sin(alfa)*sin(beta) sin(alfa)*cos(beta);0 cos(beta) sin(beta);-sin(alfa) -cos(alfa)*sin(beta) cos(alfa)*cos(beta)];
A=[((rr-q4)/rr)*x+ra*cos(alfa)*cos(beta1)-ra*sin(alfa)*sin(beta)*sin(beta1)-rb*cos(beta1);
((rr-q4)/rr)*y+ra*cos(beta)*sin(beta1)-rb*sin(beta1);
((rr-q4)/rr)*z-ra*sin(alfa)*cos(beta1)-ra*cos(alfa)*sin(beta)*sin(beta1)];
q1=sqrt(A'*A);
B=[((rr-q4)/rr)*x+ra*cos(alfa)*cos(beta2)-ra*sin(alfa)*sin(beta)*sin(beta2)-rb*cos(beta2);
((rr-q4)/rr)*y+ra*cos(beta)*sin(beta2)-rb*sin(beta2);
((rr-q4)/rr)*z-ra*sin(alfa)*cos(beta2)-ra*cos(alfa)*sin(beta)*sin(beta2)];
q2=sqrt(B'*B);
C=[((rr-q4)/rr)*x+ra*cos(alfa)*cos(beta3)-ra*sin(alfa)*sin(beta)*sin(beta3)-rb*cos(beta3);
((rr-q4)/rr)*y+ra*cos(beta)*sin(beta3)-rb*sin(beta3);
((rr-q4)/rr)*z-ra*sin(alfa)*cos(beta3)-ra*cos(alfa)*sin(beta)*sin(beta3)];
q3=sqrt(C'*C);

w4=[sin(alfa)*cos(beta) sin(beta) cos(alfa)*cos(beta)].';
beta1=0;
beta2=2*pi/3;
beta3=4*pi/3;
a01=ra*[cos(beta1) sin(beta1) 0]';
b1=rb*[cos(beta1) sin(beta1) 0]';
a02=ra*[cos(beta2) sin(beta2) 0]';
b2=rb*[cos(beta2) sin(beta2) 0]';
a03=ra*[cos(beta3) sin(beta3) 0]';
b3=rb*[cos(beta3) sin(beta3) 0]';
a1=T*a01;
a2=T*a02;
a3=T*a03;
w1=(r1+a1-b1)/q1;
w2=(r1+a2-b2)/q2;
w3=(r1+a3-b3)/q3;
A=[w1 w2 w3].';
B=[w1.'*w4*a1 w2.'*w4*a2 w3.'*w4*a3].';
J=A-B/r1r1;
K(j,k)=cond(J);

end
end
end
mesh(K)