import numpy as np
import numpy.linalg as alg

A = np.array([[5,1,2],[1,5,4],[2,4,6]])

Bc = [np.array([1,0,0]),np.array([0,1,0]),np.array([0,0,1])]

def ps(U,V) :
    return np.dot(U.T,np.dot(A,V))

def ortho(B) :
    U,V,W = B
    U1 = U/(ps(U,U)**0.5)
    V2 = V - ps(U1,V)*U1
    U2 = V2/(ps(V2,V2)**0.5)
    V3 = W - ps(U1,W)*U1 - ps(U2,W)*U2
    U3 = V3/(ps(V3,V3)**0.5)
    return [U1,U2,U3]
    
U1,U2,U3 = ortho(Bc)
print(U1,U2,U3)

B2 = [np.array([1,1,1]),np.array([1,-1,0]),np.array([1,0,0])]
V1,V2,V3 = ortho(B2)

U1 = U1.reshape(3,1)  # pb de forme des vecteurs !
U2 = U2.reshape(3,1)
U3 = U3.reshape(3,1)
M1 = np.dot(U1,U1.T) + np.dot(U2,U2.T) + np.dot(U3,U3.T)
V1 = V1.reshape(3,1)
V2 = V2.reshape(3,1)
V3 = V3.reshape(3,1)
M2 = np.dot(V1,V1.T) + np.dot(V2,V2.T) + np.dot(V3,V3.T)
print("AM1=",np.dot(A,M1))
print("AM2=",np.dot(A,M2))

x1 = np.dot(U1.T,U1) + np.dot(U2.T,U2) + np.dot(U3.T,U3)
x2 = np.dot(V1.T,V1) + np.dot(V2.T,V2) + np.dot(V3.T,V3)
print(x1,x2)
print("Tr(A^{-1})=",np.trace(alg.inv(A)))