Trilateration in 3d ie. computing the location of the intersection points of 3 spheres

trilateration-3d.py

import numpy

def norm2(X):
	return numpy.sqrt(numpy.sum(X ** 2))

def trilateration_3d(A, B, C, rA, rB, rC):
	# Find the two intersection points of three spheres
	# The routine returns D, W, h_sqr, and the two solutions are D - sqrt(h_sqr) * W and D + sqrt(h_sqr) * W
	# The points are at a distance 2 * sqrt(h_sqr) from each other, and both on the line of origin D and direction W
	# If h_sqr is negative, there's no solutions to the problem

	AB, AC = B - A, C - A
	nAB, nAC = norm2(AB), norm2(AC)
	U, V = AB / nAB, AC / nAC
	dUV = numpy.dot(U, V)
	W = numpy.cross(U, V)
	W /= norm2(W)

	M = numpy.array([[1, dUV], [dUV, 1]])
	K = numpy.array([(rA ** 2 + nAB ** 2 - rB ** 2) / (2 * nAB), (rA ** 2 + nAC ** 2 - rC ** 2) / (2 * nAC)])

	x, y = numpy.linalg.solve(M, K)
	h_sqr = rA ** 2 - (x ** 2 + y ** 2 + 2 * dUV * x * y)

	return x * U + y * V + A, W, h_sqr