Minecraft End Portal Triangulation

triangulate.py

#!/usr/bin/env python3

import math

class Vector:
    def __init__(self, x, z, r):
        self.x, self.z, self.r = x, z, r

    def slope(r):
        return math.tan(r)

class Inequality:
    def __init__(self, rep, func):
        self.rep = rep
        self.func = func

    def within(self, x, z):
        return self.func(x, z)

    def __repr__(self):
        return self.rep

    def __str__(self):
        return self.rep

def mc_yaw_to_radian(mc_yaw: float) -> float:
    return math.radians( (mc_yaw + 90) % 360 )

def get_inequalities(vectors: list[Vector], accuracy: float):
    constr = []
    ep = 0.01
    for vec in vectors:
        # line formula:
        # (z-z0) - (x-x0)*m > 0
        m0 = Vector.slope(vec.r - accuracy)
        m1 = Vector.slope(vec.r + accuracy)

        x, z = vec.x, vec.z

        # if Q1 || Q4: x >= x0, vec m0 is >=0, vec m1 is <=0
        if vec.r <= math.pi/2 or vec.r >= 3/2*math.pi:
            # Lambdas use default values to capture outside variables in-time
            constr.append(Inequality(
                f"x \\ge {x}",
                lambda xt,zt,x=x,z=z,ep=ep: xt >= x - ep)
            )

            constr.append(Inequality(
                vec_to_desmos(x, z, m0, "\\ge"),
                lambda xt,zt,x=x,z=z,m0=m0,ep=ep: (zt-z)-(xt-x)*m0 >= -ep)
            )

            constr.append(Inequality(
                vec_to_desmos(x, z, m1, "\\le"),
                lambda xt,zt,x=x,z=z,m1=m1,ep=ep: (zt-z)-(xt-x)*m1 <= ep)
            )

        # else: x <= x0, vec m0 is <=0, vec m1 is >=0
        else:
            constr.append(Inequality(
                f"x \\le {x}",
                lambda xt,zt,x=x,z=z,ep=ep: xt <= x+ep)
            )

            constr.append(Inequality(
                vec_to_desmos(x, z, m0, "\\le"),
                lambda xt,zt,x=x,z=z,m0=m0,ep=ep: (zt-z)-(xt-x)*m0 <= ep)
            )

            constr.append(Inequality(
                vec_to_desmos(x, z, m1, "\\ge"),
                lambda xt,zt,x=x,z=z,m1=m1,ep=ep: (zt-z)-(xt-x)*m1 >= -ep)
            )

        # if Q1 || Q2: z >= z0
        # else: z <= z0
        if 0 <= vec.r <= math.pi:
            constr.append(Inequality(
                f"y \\ge {z}",
                lambda xt,zt,z=z,ep=ep: zt >= z - ep)
            )
        else:
            constr.append(Inequality(
                f"y \\le {z}",
                lambda xt,zt,z=z,ep=ep: zt <= z + ep)
            )

    return constr

def vec_to_desmos(x, z, m, op="="):
    return f"(y-{z})-{m}*(x-{x}){op}0"

def intersect(vec1, vec2):
    """Find the intersection between two vectors"""
    #   (z-z0) - (x-x0) * m0 = 0
    # -((z-z1) - (x-x1) * m1 = 0)
    # ___________________
    # z-z0 -z+z1 +m0(-x+x0) +m1(x-x1) = 0
    # x( m1 - m0 ) = m1(x1) - m0(x0) + z0 - z1
    # x = (m1(x1) - m0(x0) + z0 - z1) / (m1 - m0)
    m1 = Vector.slope(vec1.r)
    m2 = Vector.slope(vec2.r)

    x = (m2 * vec2.x - m1 * vec1.x + vec1.z - vec2.z) / (m2 - m1)
    # (z-z1) - (x-x1) * m1 = 0
    z = m1 * (x - vec1.x) + vec1.z
    # Returned point may be incorrect if vectors do not intersect at all
    return x, z

def solve(vectors: list[Vector], accuracy: float):
    """Solves the triangulation for a list of vectors and a margin of error"""

    inequalities = get_inequalities(vectors, accuracy)

    # Convert each vector into a vector interval.
    # Used for taking error into account.
    new_vectors = []
    for vec in vectors:
        vec_bk = Vector(vec.x, vec.z, vec.r - accuracy)
        vec_fw = Vector(vec.x, vec.z, vec.r + accuracy)
        new_vectors.append(vec_bk)
        new_vectors.append(vec_fw)
    vectors = new_vectors
    
    # Find all intersections.
    # Runtime: O(n^3)
    points = []
    for i in range(len(vectors)):
        for j in range(i + 1, len(vectors)):
            x, z = intersect(vectors[i], vectors[j])

            # Filter out intersection points that don't satisfy all linear inequalities.
            if all(ie.within(x, z) for ie in inequalities):
                points.append((x,z))

    return points

def distance(pt0, pt1):
    return ( (pt0[0] - pt1[0]) ** 2 + (pt0[1] - pt1[1]) ** 2 ) ** 0.5

def centroid(data):
    """Find the mean of a set of points"""
    xs, ys = zip(*data)
    mean_x = sum(xs) / len(xs)
    mean_y = sum(ys) / len(ys)
    radius = sum(distance((x, y), (mean_x, mean_y)) for x, y in data) / len(xs)

    return mean_x, mean_y, radius

if __name__ == "__main__":
    accuracy = float(input("Enter your accuracy in degrees: ±"))
    accuracy = math.radians(accuracy)
    print("Enter lines in the format of:")
    print("<x> <z> <yaw>")
    print("Press enter twice when you are finished.")

    vectors = []

    while True:
        line = input().strip()
        if not line:
            break

        x, z, yaw = line.split()
        x, z, yaw = float(x), float(z), float(yaw)
        vec = Vector(x, z, mc_yaw_to_radian(yaw))
        vectors.append(vec)

    points = solve(vectors, accuracy)

    x, z, radius = centroid(points)
    x, z, radius = int(x), int(z), int(radius)

    print(f"Stronghold: {x}, {z}")
    print(f"Dig a radius of {radius}")