Поиск соседей (точек, фигур)

#! /usr/bin/env python3
#-*- coding:utf-8 -*-
 
from xml.etree import ElementTree as ET
from math import sqrt
 
def get_points_from_xml_file(xml_file):
    """ function after parse file.xml returns an array of points of the form:
        [[(y1,x1)z1],[(y2,x2)z2]...[...n]] where z is a height """
    points = []
    cache = []
    with open(xml_file, 'r', encoding='utf-8') as file:
        tree = ET.parse(file)
    ROOT = tree.getroot()
    CgPoints = ROOT[-1]
    for point in CgPoints:
        cache.append(point.text)
    for str_point in cache:
        str_point = list(str_point.split())
        point = []
        for fl in str_point:
            fl = float(fl)
            point.append(fl)
        a = tuple(point[:2])  # get tuple with (y,x)
        b = point[-1]         # get height z
        points.append([a,b])  # adding to array with all points [(y,x),z]
    return points
 
def get_vectors(query_point, points):
    """функция получает точку запроса, т.е. x == крайней левой, и y == самый нижний по границе x
       и возвращает все вектора от точки запроса ко всем остальным
        имеющимся точкам за исключением точки запроса
        ... теперь ... функция возвращает четырехугольники"""
 
    points.remove(query_point)
    vectors = []
    quad = []
    x1 = query_point[0][1] #для тестовых [0] точек а для реальных надо заглянуть глубже в кортеж x1 = query_point[0][1]
    y1 = query_point[0][0] #y1 = query_point[0][0]
    for point in points:
        x2 = point[0][1] #x2 = point[0][1]
        y2 = point[0][0] #y2 = point[0][0]
 
        if point[1] >= query_point[1]:
            #это услови для проверки чтобы лучи векторов летели строго вверх и в право
            #тоесть если х точки запроса меньше икса искомой точки в которую летит луч
            #значит она находиться правее, либо правее и выше точки запроса"""
            length_of_the_vector = sqrt((x1-x2)**2 + (y1-y2)**2)
            vectors.append([query_point, point, length_of_the_vector])
    vectors = sorted(vectors, key=lambda vector: vector[-1]) # все массивы с длинами векторов отсортированные по длинам векторов
    first_3_vectors = vectors[:3]
    quad.append(query_point)
    for point in first_3_vectors:
        quad.append(point[1])
    if len(quad) == 4:
        return quad, points
    else:
        return None
 
#
xml_file = '2Cubes.xml'
points = get_points_from_xml_file(xml_file)
 
 
 
quads = []
while points:
    i = min(points)
    v = get_vectors(i, points)
    if v is not None:
        quad = v[0]
        points = v[1]
        quads.append(quad)
    else:
        continue
 
for i,e  in enumerate(quads):
    print(i, e)