math for the matlab function is as follows:

function [Stroke1, Stroke2] = Desired_Lengths(theta, phi, x1, y1, z1, x2, y2, z2, xb1, yb1, zb1, xb2, yb2, zb2, Lk)

R_pitch = [1 0 0; 0 cos(theta) -sin(theta); 0 sin(theta) cos(theta)];
R_yaw = [cos(phi) 0 sin(phi); 0 1 0; -sin(phi) 0 cos(phi)];
A1 = [x1; y1; z1];
A2 = [x2; y2; z2];
B1 = [xb1; yb1; zb1];
B2 = [xb2; yb2; zb2];
A1_prime = R_yaw*R_pitch*A1;
A2_prime = R_yaw*R_pitch*A2;
L_1 = norm(A1_prime-B1);
L_2 = norm(A2_prime-B2);
Stroke1 = L_1 - Lk;
Stroke2 = L_2 - Lk;

свернуть