from sympy import Circle, Point, Line, sqrt
import numpy as np

def solve4Points(pointA, pointB, pointC, pointD, distA, distB, distC, distD):
    #build linear equation matrix
    matrix= []
    matrix.append([pointB.x-pointA.x, pointB.y-pointA.y, pointB.z-pointA.z])
    matrix.append([pointC.x-pointA.x, pointC.y-pointA.y, pointC.z-pointA.z])
    matrix.append([pointD.x-pointA.x, pointD.y-pointA.y, pointD.z-pointA.z])
    a = np.array(matrix, dtype='float')

    vect1 = -(distB**2-distA**2 + pointA.dot(pointA) - pointB.dot(pointB))/2
    vect2 = -(distC**2-distA**2 + pointA.dot(pointA) - pointC.dot(pointC))/2
    vect3 = -(distD**2-distA**2 + pointA.dot(pointA) - pointD.dot(pointD))/2
    vector = [vect1, vect2, vect3]
    b = np.array(vector, dtype='float')
    x = np.linalg.solve(a,b)

    return Point(x[0],x[1], x[2])

pointA = Point(1,2,3)
pointB = Point(5,2,5)
pointC = Point(2,5,2)
pointD = Point(0,0,0)

pointE = Point(8,8,8)
distA = pointE.distance(pointA)
distB = pointE.distance(pointB)
distC = pointE.distance(pointC)
distD = pointE.distance(pointD)
result = solve4Points(pointA,pointB,pointC,pointD,distA,distB,distC,distD)

if result.distance(pointE) == 0:
    print("it works!!!")