function [p4a,p4b]=triangu3D(p1,p2,p3,l1,l2,l3);
// [p4a,p4b]=triangu3D(p1,p2,p3,l1,l2,l3);
//
// Given are three points in 3D, specified by the local vectors p1, p2 and p3
// Now the points p4a and p4b should be calculated, which have distance l1 to point p1, l2 to point p2 and l3 to point p3
// Instead of solving a nonlinear equation system, an algorithm based upon linear algebra is used.

TXTMSG=[];
flag0=0;
p4a=[];
p4b=[];

n12=p2-p1;                         // Vector from p1 to p2 (normal vector of plane 12)
n13=p3-p1;                         // Vector from p1 to p3 (normal vector of plane 13)
S12=sqrt(sum((p2-p1) .^2));        // distance between point p1 and p2
S13=sqrt(sum((p3-p1) .^2));        // distance between point p1 and p3
S23=sqrt(sum((p3-p2) .^2));        // distance between point p3 and p2

if l1+l2<S12,
	TXTMSG=[TXTMSG;"l1 + l2 < distance point p1-p2"];
	flag0=1;
end
if l1+l3<S13,
	TXTMSG=[TXTMSG;"l1 + l3 < distance point p1-p3"];
	flag0=1;
end
if l2+l3<S23,
	TXTMSG=[TXTMSG;"l2 + l3 < distance point p2-p3"];
	flag0=1;
end

k12=0.5 .*(1 +((l1 ./S12) .^2) -((l2 ./S12) .^2));  // Determine position of intersection of plane 12 with line p1-p2
u12=p1 + k12 .*n12;                                 // Point of intersection Plane p12 with line p1-p2

k13=0.5 .*(1 +((l1 ./S13) .^2) -((l3 ./S13) .^2));  // Determine position of intersection of plane 13 with line p1-p3
u13=p1 + k13 .*n13;                                 // Point of intersection Plane p13 with line p1-p3

d12=n12*(u12.');                   // Scalar value of plane 12 equation in normal form
d13=n13*(u13.');                   // Scalar value of plane 13 equation in normal form

rg=[(n12(2)*n13(3)-n12(3)*n13(2)), -(n12(1)*n13(3)-n12(3)*n13(1)), (n12(1)*n13(2)-n12(2)*n13(1))];
if rg==[0,0,0],
	TXTMSG=[TXTMSG;"No triangle basis"];
	flag0=1;
end

A=[n12;n13];
B=[d12;d13];
rho=(A\B).';                       // intersection line of planes 1-2 and 1-3: rho + lambda * rg
                                   // On this intersection line, the points p4 with distance l(i) to points p(i) can be found
								   // Now solving for lambda1 and lambda2
u=rho - p1;

z1=u*(rg.')/sum(rg .^2);
z2=(sum(u .^2) - (l1 .^2)) ./sum(rg .^2);
qval=(z1 .^2) - z2;
if qval<0;
	TXTMSG=[TXTMSG;" No solution possible"];
	flag0=1;
end
lam1=-z1 + sqrt( (z1 .^2) - z2);
lam2=-z1 - sqrt( (z1 .^2) - z2);

p4a=rho + lam1 .*rg;
p4b=rho + lam2 .*rg;

if flag0~=0,
	p4a=[];
	p4b=[];
	disp(TXTMSG);
end

endfunction;

