gistfile1.txt

function trilaterate(p1, p2, p3, return_middle)
{

	function sqr(a)
	{
		return a * a;
	}
	
	function norm(a)
	{
		return Math.sqrt(sqr(a.x) + sqr(a.y) + sqr(a.z));
	}
	
	function dot(a, b)
	{
		return a.x * b.x + a.y * b.y + a.z * b.z;
	}
	
	function vector_subtract(a, b)
	{
		return {
			x: a.x - b.x,
			y: a.y - b.y,
			z: a.z - b.z
		};
	}
	
	function vector_add(a, b)
	{
		return {
			x: a.x + b.x,
			y: a.y + b.y,
			z: a.z + b.z
		};
	}
	
	function vector_divide(a, b)
	{
		return {
			x: a.x / b,
			y: a.y / b,
			z: a.z / b
		};
	}
	
	function vector_multiply(a, b)
	{
		return {
			x: a.x * b,
			y: a.y * b,
			z: a.z * b
		};
	}
	
	function vector_cross(a, b)
	{
		return {
			x: a.y * b.z - a.z * b.y,
			y: a.z * b.x - a.x * b.z,
			z: a.x * b.y - a.y * b.x
		};
	}
	
	var ex, ey, ez, i, j, d, a, x, y, z, b, p4;
	
	ex = vector_divide(vector_subtract(p2, p1), norm(vector_subtract(p2, p1)));
	
	i = dot(ex, vector_subtract(p3, p1));
	//i = exX*(p3x-p1x)+  exY*(p3y-p1y)+exZ*(p3Z-p1Z)    
	a = vector_subtract(vector_subtract(p3, p1), vector_multiply(ex, i));
	//aX=(p3x-p1x)-(exX*i) aY=(p3y-p1y)-(exY*i) aZ=(p3z-p1z)-(exZ*i)
	ey = vector_divide(a, norm(a));
	//aN=SQRT(aX^2+aY^2+aZ^2) eyX=aX/aN eyY=aY/aN eyZ=eyZ=aZ/aN 
	ez =  vector_cross(ex, ey);
	//ezX=exY*eyZ-exZ*eyY ezY=exZ*eyX-exX*eyZ ezZ=exX*eyY-exY*eyX
	d = norm(vector_subtract(p2, p1));
	//d=SQRT((p2x-p1x)^2+(p2y-p1y)^2+(p2z-p1z)^2)
	j = dot(ey, vector_subtract(p3, p1));
	//j = eyX*(p3x-p1x)+eyY*(p3y-p1y)+eyZ*(p3z-p1z)
	
	x = (sqr(p1.r) - sqr(p2.r) + sqr(d)) / (2 * d);
	//x=(p1r^2-p2r^2+d^2)/(2*d)
	y = (sqr(p1.r) - sqr(p3.r) + sqr(i) + sqr(j)) / (2 * j) - (i / j) * x;
	//y=(p1r^2-p3r^2+i^2+j^2)/(2*j)-(i/j)*x
	b = sqr(p1.r) - sqr(x) - sqr(y);
	//b = p1r^2-x^2-y^2
	// floating point math flaw in IEEE 754 standard
	// see https://github.com/gheja/trilateration.js/issues/2
	if (Math.abs(b) < 0.0000000001)
	{
		b = 0;
	}
	
	z = Math.sqrt(b);
	
	// no solution found
	if (isNaN(z))
	{
		return null;
	}
	
	a = vector_add(p1, vector_add(vector_multiply(ex, x), vector_multiply(ey, y)))
	//pX=p1x+(exX*x+eyX*y) pY=p1y+(exY*x+eyY*y) pZ=p1z+(exZ*x+eyZ*y)
	p4a = vector_add(a, vector_multiply(ez, z));
	//paX=pX+(ezX*z) paY=pY+(ezY*z) paZ=pZ+(ezZ*Z)
	p4b = vector_subtract(a, vector_multiply(ez, z));
	//pbX=pX-(ezX*z) pbY=pY-(ezY*z) pbZ=pZ-(ezZ*Z)
	if (z == 0 || return_middle)
	{
		return a;
	}
	else
	{
		return [ p4a, p4b ];
	}
}



it('First test', function() {
	var p1, p2, p3, p4;
	
	p1 = { x: 0, y: 0, z: 0, r: 100 };
	p2 = { x: 100, y: 0, z: 0, r: 100 };
	p3 = { x: 0, y: 100, z: 0, r: 100 };

	pCheck = {x:0, y:0, z: -10}
	
	p4 = trilaterate(p1, p2, p3);
	
	expect(p4[0].x).toEqual(50);
	expect(p4[0].y).toEqual(50);
	expect(p4[0].z).toEqual(70.71067811865476);
	
	expect(p4[1].x).toEqual(50);
	expect(p4[1].y).toEqual(50);
	expect(p4[1].z).toEqual(-70.71067811865476);

	const a=  norm(vector_subtract(p4[0], pCheck))
	//p4da=SQRT((paX-p4x)^2+(paY-p4Y)^2+(paZ-p4Z)^2)
	const b =  norm(vector_subtract(p4[1], pCheck))
	//p4db=SQRT((pbX-p4x)^2+(pbY-p4Y)^2+(pbZ-p4Z)^2)

	//g=p4da==p4r fx=(g)*paX+(not g)*pbX fy=(g)*paY+(not g)*pbY fz=(g)*paZ+(not g)*pbZ 

	console.log(a,b)

});