Trilateration in 2d ie. computing the location of the intersection points of 2 circles

trilateration-2d.py

import numpy

def norm2_sqr(X):
	return numpy.sum(X ** 2)

def perp(X):
	return numpy.array((-X[1], X[0]))

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

	AB = B - A
	AB_norm_sqr = norm2_sqr(AB)
	AB_norm = numpy.sqrt(AB_norm_sqr)
	U = AB / AB_norm

	k = (rA ** 2 - rB ** 2 + AB_norm_sqr) / (2 * AB_norm)

	return A + k * U, perp(U), rA ** 2 - k ** 2