syms rp alpha beta theta psi z
A=cos(alpha)-cos(beta);
B=sin(alpha)-sin(beta);
C=(cos(beta)-1)/tan(beta)-(cos(alpha)-1)/tan(alpha);
R=A*(cos(theta)-cos(psi))+B*cos(theta)*sin(psi);
S=A*sin(theta)*sin(psi)-B*cos(theta)+C*cos(psi);
phi=atan(-R/S);
x=-rp*(cos(theta)*cos(phi)+sin(psi)*sin(theta)*sin(phi))*cos(alpha)-rp*(-cos(theta)*sin(phi)+sin(psi)*sin(theta)*cos(phi))*sin(alpha)+(rp/tan(alpha))*(cos(psi)*sin(phi)*(cos(alpha)-1)+cos(psi)*cos(phi)*sin(alpha));
y=-cos(psi)*sin(phi)*rp;
p=[x;y;z];
T=Rot('y',theta)*Rot('x',psi)*Rot('z',phi);
a1=T*[rp;0;0];
a2=T*[rp*cos(alpha);rp*sin(alpha);0];
a3=T*[rp*cos(beta);rp*sin(beta);0];
Con1=(p+a1)'*[0;1;0];
Con2=(p+a2)'*[-sin(alpha);cos(alpha);0];
Con3=(p+a3)'*[-sin(beta);cos(beta);0];
disp([Con1; Con2; Con3])
function output=Rot(axis,angle)
syms x y z
if axis==x
output=[1,0,0;0,cos(angle),-sin(angle);0,sin(angle),cos(angle)];
end
if axis==y
output=[cos(angle),0,sin(angle);0,1,0;-sin(angle),0,cos(angle)];
end
if axis==z
output=[cos(angle),-sin(angle),0;sin(angle),cos(angle),0;0,0,1];
end
end
Best Answer