beacon trilateration

trilateration

- (CGPoint)getCoordinateWith

	beaconA:(CGPoint)a
	beaconB:(CGPoint)b
	beaconC:(CGPoint)c

	distanceA:(CGFloat)dA 
	distanceB:(CGFloat)dB
	distanceC:(CGFloat)dC {

	    CGFloat W, Z, x, y, y2;
	    
	    W = dA * dA - dB * dB - a.x * a.x - a.y * a.y + b.x * b.x + b.y * b.y;
	    Z = dB * dB - dC * dC - b.x * b.x - b.y * b.y + c.x * c.x + c.y * c.y;
	    x = ( W * ( c.y - b.y ) - Z * ( b.y - a.y ) ) / ( 2 * ( ( b.x - a.x ) * ( c.y - b.y ) - ( c.x - b.x ) * ( b.y - a.y ) ) );
	    y = ( W - 2 * x * ( b.x - a.x ) ) / ( 2 * ( b.y - a.y ) );

	    // y2 is a second measure of y to mitigate errors

	    y2 = ( Z - 2 * x * ( c.x - b.x ) ) / ( 2 * ( c.y - b.y ) );
	    y = ( y + y2 ) / 2;

    return CGPointMake(x, y);

}