function [position] = forward(thetas, geometry)
position = zeros(length(thetas),3);
thetas = (pi/180) * thetas;
l1 = geometry(1);
l2 = geometry(2);
l3 = geometry(3);
l4 = geometry(4);
m = [0 sqrt(3)/2 -sqrt(3)/2; -1 1/2 1/2; 0 0 0];
for i=1:length(thetas)
b = [(l3*cos(thetas(i,1)) + l1 - l2) * m(:,1) + [0 0 l3*sin(thetas(i,1))].'...
(l3*cos(thetas(i,2)) + l1 - l2) * m(:,2) + [0 0 l3*sin(thetas(i,2))].'...
(l3*cos(thetas(i,3)) + l1 - l2) * m(:,3) + [0 0 l3*sin(thetas(i,3))].'];
if norm(b(:,1)-b(:,2)) > 2*l2 | norm(b(:,2)-b(:,3)) > 2*l4 |...
norm(b(:,1)-b(:,3)) > 2*l4
disp('At least two of the shin spheres were found not to intersect.');
return
end
plane1 = [(norm(b(:,1))^2 - norm(b(:,2))^2)/2 ...
b(1,2)-b(1,1) b(2,2)-b(2,1) b(3,2)-b(3,1)];
plane2 = [(norm(b(:,1))^2 - norm(b(:,3))^2)/2 ...
b(1,3)-b(1,1) b(2,3)-b(2,1) b(3,3)-b(3,1)];
a = plucker(plane1, plane2);
%Swap the triplets to express the line formed by the intersection of the
%two planes in plucker coordinates
p = [a(4:6) a(1:3)];
alpha = p(5)/p(3) + b(1,1);
beta = b(2,1) - p(4)/p(3);
eqn = [(p(2)^2 + p(1)^2 + p(3)^2)/p(3)^2 ...
-((2*alpha*p(1) + 2*beta*p(2) + 2*b(3,1)*p(3))/p(3))...
(alpha^2 + beta^2 + b(3,1)^2 - l4^2)];
R = roots(eqn);
[val, ind] = max(abs(R));
position(i,3) = R(ind);
position(i,2) = (p(4) + p(2)*position(i,3))/p(3);
position(i,1) = (position(i,3)*p(1) - p(5))/p(3);
end
- 1
- 2
前往页