beihang/public/trilateration3d.py

from __future__ import division
import json
import math
from json import encoder
import itertools

encoder.FLOAT_REPR = lambda o: format(o, '.2f')


class base_station(object):
    def __init__(self, lat, lon, dist):
        self.lat = lat
        self.lon = lon
        self.dist = dist


class point(object):
    def __init__(self, x, y, z):
        self.x = x
        self.y = y
        self.z = z


class circle(object):
    def __init__(self, point, radius):
        self.center = point
        self.radius = radius


class json_data(object):
    def __init__(self, circles, inner_points, center):
        self.circles = circles
        self.inner_points = inner_points
        self.center = center


def serialize_instance(obj):
    d = {}
    d.update(vars(obj))
    return d


def get_two_points_distance(p1, p2):
    distance = math.sqrt(pow((p1.x - p2.x), 2) + pow((p1.y - p2.y), 2) + pow((p1.z - p2.z), 2))
    return distance


def get_two_circles_intersecting_points(c1, c2, c3):
    p1 = c1.center
    p2 = c2.center
    p3 = c3.center
    r1 = c1.radius
    r2 = c2.radius
    r3 = c3.radius

    if r1 > 1000 or r2 > 1000 or r3 > 1000:
        return None
    else:
        v1 = (p2.x - p1.x, p2.y - p1.y, p2.z - p1.z)
        v2 = (p3.x - p1.x, p3.y - p1.y, p3.z - p1.z)
        cross_product = (v1[1] * v2[2] - v1[2] * v2[1], v1[2] * v2[0] - v1[0] * v2[2],
                         v1[0] * v2[1] - v1[1] * v2[0])
        if cross_product == (0, 0, 0):
            return None
        else:
            d12 = get_two_points_distance(p1, p2)
            d13 = get_two_points_distance(p1, p3)
            d23 = get_two_points_distance(p2, p3)

            if (r1 + r2) > d12 > math.fabs(r1 - r2) \
                    and (r1 + r3) > d13 > math.fabs(r1 - r3) \
                    and (r2 + r3) > d23 > math.fabs(r2 - r3):
                X21 = p2.x - p1.x
                X31 = p3.x - p1.x
                Y21 = p2.y - p1.y
                Y31 = p3.y - p1.y
                Z21 = p2.z - p1.z
                Z31 = p3.z - p1.z
                A1 = pow(r1, 2) - pow(p1.x, 2) - pow(p1.y, 2) - pow(p1.z, 2)
                A2 = pow(r2, 2) - pow(p2.x, 2) - pow(p2.y, 2) - pow(p2.z, 2)
                A3 = pow(r3, 2) - pow(p3.x, 2) - pow(p3.y, 2) - pow(p3.z, 2)
                A21 = -(A2 - A1) / 2
                A31 = -(A3 - A1) / 2
                D = X21 * Y31 - Y21 * X31
                B0 = (A21 * Y31 - A31 * Y21) / D
                B1 = (Y21 * Z31 - Y31 * Z21) / D
                C0 = (A31 * X21 - A21 * X31) / D
                C1 = (X31 * Z21 - X21 * Z31) / D
                E = (pow(B1, 2) + pow(C1, 2) + 1)
                F = (B1 * (B0 - p1.x) + C1 * (C0 - p1.y) - p1.z)
                G = pow((B0 - p1.x), 2) + pow((C0 - p1.y), 2) + pow(p1.z, 2) - pow(r1, 2)
                if 4 * pow(F, 2) - 4 * E * G < 0:
                    return None
                else:
                    z1 = (-F + math.sqrt(pow(F, 2) - E * G)) / E
                    x1 = B0 + B1 * z1
                    y1 = C0 + C1 * z1
                    z2 = (-F - math.sqrt(pow(F, 2) - E * G)) / E
                    x2 = B0 + B1 * z2
                    y2 = C0 + C1 * z2
                    return[point(x1, y1, z1), point(x2, y2, z2)]
            else:
                return None


def get_all_intersecting_points(circles):
    points = []
    num = len(circles)
    for i, j, k in itertools.combinations(range(num), 3):
        res = get_two_circles_intersecting_points(circles[i], circles[j], circles[k])
        if res:
            points.extend(res)
    return points


def is_contained_in_circles(point, circles):
    for i in range(len(circles)):
        # if (get_two_points_distance(point, circles[i].center) > (circles[i].radius)):
        if abs(get_two_points_distance(point, circles[i].center) - circles[i].radius) > 0.2:
            return False
    return True


def get_polygon_center(points):
    center = point(0, 0, 0)
    if len(points) == 0:
        return None
    else:
        num = len(points)
        for i in range(num):
            center.x += points[i].x
            center.y += points[i].y
            center.z += points[i].z
        center.x /= num
        center.y /= num
        center.z /= num
        return center


def trilateration3d(MapBSList, dataMap):
    circle_list = []
    for key, value in dataMap.items():
        center = point(MapBSList[key][0], MapBSList[key][1], MapBSList[key][2])
        bscircle = circle(center, value)
        circle_list.append(bscircle)
    inner_points = []
    for p in get_all_intersecting_points(circle_list):
        if is_contained_in_circles(p, circle_list):
            inner_points.append(p)
    if len(inner_points) > 0:
        center = get_polygon_center(inner_points)
    else:
        center = None
    return center


if __name__ == '__main__':

    p1 = point(8.43, 14.35, 11.5)
    p2 = point(5.59, -4.65, 1.5)
    p3 = point(7.526, 11.74, 1.5)
    p4 = point(0, 0, 1.5)

    c1 = circle(p1, 4.51)
    c2 = circle(p2, 17.25)
    c3 = circle(p3, 20.98)
    c4 = circle(p4, 19.64)

    circle_list = [c1, c2, c3, c4]

    inner_points = []
    a = get_all_intersecting_points(circle_list)
    for p in get_all_intersecting_points(circle_list):
        if is_contained_in_circles(p, circle_list):
            inner_points.append(p)

    center = get_polygon_center(inner_points)

    in_json = json_data([c1, c2, c3, c4], [p1, p2, p3, p4], center)

    out_json = json.dumps(in_json, sort_keys=True,
                          indent=4, default=serialize_instance)

    print(out_json)