def lineIntersect3DPoints(PA,PB):
    import numpy as np
    '''
    Find intersection point of lines in 3D space, in the least squares sense.
    PA :          Nx3-matrix containing starting point of N lines
    PB :          Nx3-matrix containing end point of N lines
    P_Intersect : Best intersection point of the N lines, in least squares sense.
    distances   : Distances from intersection point to the input lines
    From https://uk.mathworks.com/matlabcentral/fileexchange/37192-intersection-point-of-lines-in-3d-space
    '''

    Si = PB-PA      #Describe N lines as vectors
    Si_root = np.sqrt(np.sum(np.square(Si),axis=1))*np.ones(shape=(1,3),dtype=np.uint8)


    ni = np.divide(Si,  Si_root)        #normalise vectors
    nx,ny,nz = ni[:,0],ni[:,1],ni[:,2]

    SXX = np.sum((nx**2)-1)
    SYY = np.sum((ny**2)-1)
    SZZ = np.sum((nz**2)-1)
    SXY = np.sum(nx*ny)
    SXZ = np.sum(nx*nz)
    SYZ = np.sum(ny*nz)
    S = np.array([[SXX,SXY,SXZ],[SXY,SYY,SYZ],[SXZ,SYZ,SZZ]])

    CX  = np.sum(    PA[:,0]*((nx**2)-1)  +  PA[:,1]*(nx*ny)         +   PA[:,2]*(nx*nz)         )
    CY  = np.sum(    PA[:,0]*(nx*ny)      +  PA[:,1]*((ny**2)-1)     +   PA[:,2]*(ny*nz)         )
    CZ  = np.sum(    PA[:,0]*(nx*nz)      +  PA[:,1]*(ny*nz)         +   PA[:,2]*((nz**2)-1)     )
    C   = np.array([[CX],[CY],[CZ]]) 

    P_intersect = (np.linalg.lstsq(S,C)).H

    N = PA.shape()[0]
    distances = np.zeros(shape=(N,1))
    for i in np.arange(N):
        ui = (  (P_intersect-PA[i,:])*(Si[i,:]).H   ) / (   Si[i,:]*(Si[i,:]).H     )
        distances[i] = np.linalg.norm(P_intersect-PA[i,:]-ui*Si[i,:])

    return P_intersect,distances

def lineIntersectionNormVec3D(ni):
    import numpy as np
    '''
    Find intersection point of lines in 3D space, in the least squares sense.
    ni:          Nx3-matrix containing normalised vectors in x,y,z format
    P_Intersect : Best intersection point of the N lines, in least squares sense.
    From https://uk.mathworks.com/matlabcentral/fileexchange/37192-intersection-point-of-lines-in-3d-space
    '''
    
    
    nx,ny,nz = ni[:,0],ni[:,1],ni[:,2]

    SXX = np.sum((nx**2)-1)
    SYY = np.sum((ny**2)-1)
    SZZ = np.sum((nz**2)-1)
    SXY = np.sum(nx*ny)
    SXZ = np.sum(nx*nz)
    SYZ = np.sum(ny*nz)
    S = np.array([[SXX,SXY,SXZ],[SXY,SYY,SYZ],[SXZ,SYZ,SZZ]])

    CX  = np.sum(    nx*((nx**2)-1)  +  ny*(nx*ny)         +   nz*(nx*nz)         )
    CY  = np.sum(    nx*(nx*ny)      +  ny*((ny**2)-1)     +   nz*(ny*nz)         )
    CZ  = np.sum(    nx*(nx*nz)      +  ny*(ny*nz)         +   nz*((nz**2)-1)     )
    C   = np.array([[CX],[CY],[CZ]]) 

    P_intersect = np.array(np.linalg.lstsq(S,C)[0]).conj().T
    

    N = ni.shape[0]
    distances = np.zeros(shape=(N,1))
    for i in np.arange(N):
        ui = (  (P_intersect-ni[i,:]) * (ni[i,:]).conj().T   ) / (   ni[i,:]*(ni[i,:]).conj().T     )
        distances[i] = np.linalg.norm(P_intersect-ni[i,:]-ui*ni[i,:])

    return P_intersect,distances

def johannesLSintersect(norms):
    import numpy as np
    """
    P0 and P1 are NxD arrays defining N lines.
    D is the dimension of the space. This function 
    returns the least squares intersection of the N
    lines from the system given by eq. 13 in 
    http://cal.cs.illinois.edu/~johannes/research/LS_line_intersect.pdf.
    """
    # generate all line direction vectors 
    # n = (P1-P0)/np.linalg.norm(P1-P0,axis=1)[:,np.newaxis] # normalized
    

    # generate the array of all projectors 
    projs = np.eye(norms.shape[1]) - norms[:,:,np.newaxis]*norms[:,np.newaxis]  # I - n*n.T
    # see fig. 1 

    # generate R matrix and q vector
    R = projs.sum(axis=0)
    q = (projs @ (norms*2)[:,:,np.newaxis]).sum(axis=0)

    # solve the least squares problem for the 
    # intersection point p: Rp = q
    p = np.linalg.lstsq(R,q,rcond=None)[0]

    return p


