BerGeo/h2/mbc.py

import statistics
from math import inf, isnan, sqrt
from typing import Set, List, Tuple
import util
from scipy.optimize import linprog
import scipy
import random

from util import Side, Point, gen_point, display, display_line_only, gen_circular_point, gen_weird_point


def sidedness(slope: float, intersection: float, p3: Point, flipper: callable, eps=0.0000001) -> Side:
    # finds where a point is in regards to a line
    if flipper(p3.y) - eps <= flipper(slope * p3.x + intersection) <= flipper(p3.y) + eps:
        return Side.ON
    elif p3.y > slope * p3.x + intersection:
        return Side.ABOVE
    return Side.BELOW


def solve_1dlp(c: float, constraints: List[Tuple[float, float]]):
    """
    :param c: c1
    :param constraints: [(ai, bi), ...]
    :return: x1
    """
    min_ = -10000
    max_ = 10000

    for constraint in constraints:
        (a, b) = constraint

        if a == 0:
            assert(b >= 0)

        if a > 0:
            max_ = min(b/a, max_)
        elif a < 0:
            min_ = max(b/a, min_)


    if c > 0:
        return min_
    else:
        return max_



assert solve_1dlp(1, [(-1, -2)]) == 2
assert solve_1dlp(1, [(-1, -2), (-1, -3)]) == 3
assert solve_1dlp(1, [(-1, -3), (-1, -2)]) == 3
assert solve_1dlp(-1, [(1, 3), (1, 2)]) == 2
assert solve_1dlp(1, [(-1, 3), (-1, 2)]) == -2

def new_solve_1dlp(c, constraints, idx):
    c1, c2 = c
    ((a1, a2), b) = constraints[idx]
    q, p = b / a2, a1 /a2

    interval = [-9999999, 9999999]

    for new_idx, ((lel_a1, lel_a2), lel_b) in enumerate(constraints):
        if not new_idx == idx:
            bj, aj = (lel_b - lel_a2 * q), (lel_a1 - lel_a2 * p)
            if aj < 0 and bj / aj > interval[0]:
                interval[0] = bj / aj
            elif aj > 0 and bj / aj < interval[1]:
                interval[1] = bj/aj

    c = -(c1 - c2 * p)
    if c < 0:
        return interval[0], q - (p * interval[0])
    elif c >= 0:
        return interval[1], q - (p * interval[1])



def new_solve_2dlp(c, constraints):
    c1, c2 = c
    x1 = -10000 if c1 > 0 else 10000
    x2 = -10000 if c2 > 0 else 10000

    for idx, ((a1, a2), b) in enumerate(constraints):
        if not (a1*x1 + a2*x2 <= b):
            x1,x2 = new_solve_1dlp(c, constraints[:idx+1], idx)
    return x1,x2



def solve_2dlp(c: Tuple[float, float], constraints: List[Tuple[Tuple[float, float], float]]):
    """
    :param c: (c1, c2)
    :param constraints: [(ai1, ai2, bi), ...]
    :return: x1, x2
    """
    c1, c2 = c
    x1 = -10000 if c1 > 0 else 10000
    x2 = -10000 if c2 > 0 else 10000

    #random.shuffle(constraints)


    for idx, ((a1, a2), b) in enumerate(constraints[1:]):

        #print("x1 and x2", x1, x2)


        #print("{} + {} <= {}".format(a1*x1, a2*x2, b))
        #print("pls",a1*x1 + a2*x2)

        #print("yes"*10) if isnan(a1*x1+a2*x2) else print("no"*10)

        if not (a1*x1 + a2*x2 <= b):
            constraint_for_1d = []

            new_obj = c[0] - ((c[1]*a1)/a2)

            for constraint in constraints[:idx]:
                (a_i1, a_i2), b_i = constraint

                a_prime = a_i1 - ((a_i2*a1)/a2)
                b_prime = b_i - ((a_i2*b)/a2)
                constraint_for_1d.append((a_prime, b_prime))

            #print("obj", new_obj)
            #print("const", constraint_for_1d)

            #print("lol",[cons[0] for cons in constraint_for_1d])
            #res = linprog([new_obj], [[cons[0]] for cons in constraint_for_1d], [[cons[1]] for cons in constraint_for_1d], bounds=[(None, None)])
            x1 = solve_1dlp(new_obj, constraint_for_1d)
            #x1 = res.x
            #print(res)
            x2 = ((b/a2) - (a1/a2)*x1)


    return x1, x2


def find_median(points):
    num_candidates = min(5, len(points))
    candidates = random.sample(points, num_candidates)

    candidates.sort(key=lambda p: p.x)
    median_i = num_candidates // 2
    median = ((candidates[median_i - 1].x + candidates[median_i].x)/2,
              (candidates[median_i - 1].y + candidates[median_i].y)/2)

    return median[0]

def mbc_ch(points: Set[Point], flipper: callable) -> Set[Point]:

    if len(points) < 2:
        return points

    # Find the point with median x-coordinate, and partition the points on this point
    med_x = find_median(points)
    med_x = statistics.median(p.x for p in points)
    #print(med_x)

    # Find left and right points in regards to median
    pl = {p for p in points if p.x < med_x}
    pr = {p for p in points if p.x >= med_x}
    #print("pl\n",pl)
    #print("pr\n",pr)

    c = [flipper(med_x), flipper(1)]
    A = [[flipper(-p.x), flipper(-1)] for p in points]
    b = [flipper(-p.y) for p in points]

    # Find the bridge over the vertical line in pm
    #slope, intercept = solve_2dlp((flipper(med_x), flipper(1)),
    #                              [((flipper(-p.x), flipper(-1)), flipper(-p.y)) for p in points])  # confirmed correct

    slope, intercept = new_solve_2dlp((flipper(med_x), flipper(1)),
                                      [((flipper(-p.x), flipper(-1)), flipper(-p.y)) for p in points])


    #print("slope, intercept:",slope, intercept)

    res = linprog(c, A, b, bounds=[[None, None], [None, None]], options={"tol": 0.01})
    #print("res0, res1:",res.x[0], res.x[1])

    #slope, intercept = res.x[0], res.x[1]

    #display_line_only(points, slope, intercept, [])

    # Find the two points which are on the line, should work
    #on = {p for p in points if sidedness(slope, intercept, p, flipper) == Side.ON}
    #print("On Points:",on)
    #left_point = min(on)
    #right_point = max(on)


    #dist_to_line = lambda p: abs(intercept + slope * p.x - p.y)/sqrt(1 + slope**2)
    #left_point = min(pl, key = dist_to_line)
    #right_point = min(pr, key=dist_to_line)

    #display_line_only(points, slope, intercept, [left_point, right_point])


    left_point = next(p for p in pl if sidedness(slope, intercept, p, flipper) == Side.ON)
    right_point = next(p for p in pr if sidedness(slope, intercept, p, flipper) == Side.ON)




    def find_med_point(points, med_x):
        for p in points:
            if med_x+0.001 >= p.x >= med_x-0.001:
                return {p}
        return {}

    #print("med point:",find_med_point(points, med_x))

    #display_line_only(points, slope, intercept, {left_point, right_point})

    # Prune the points between the two line points
    pl = {p for p in pl if p.x <= left_point.x}
    pr = {p for p in pr if p.x >= right_point.x}

    return set.union(mbc_ch(pl, flipper), {left_point, right_point}, mbc_ch(pr, flipper))


def mbc(points: Set[Point]) -> Set[Point]:
    return set.union(mbc_ch(points, lambda x: x), mbc_ch(points, lambda x: -x))


if __name__ == '__main__':
    # points = {gen_circular_point(1, 100, 50) for _ in range(200)}
    points = {gen_weird_point(1, 100) for _ in range(200)}

    #points = {Point(x=-33.11091053638924, y=38.88967778961347), Point(x=61.20269947424177, y=-78.96305419217254), Point(x=99.44053842147957, y=-89.11579172297581), Point(x=-92.40380889537532, y=84.33904351991652), Point(x=-90.63139185545595, y=-91.13793846505985)}

    points = {Point(x=5.2, y=9.7), Point(x=5.3, y=4.9), Point(x=3.3, y=3.6), Point(x=9.2, y=4.8), Point(x=9.7, y=5.7), Point(x=5.6, y=8.7)}

    upper_hull_points = mbc_ch(points, lambda x: x)
    lower_hull_points = mbc_ch(points, lambda x: -x)

    display(points, upper_hull_points.union(lower_hull_points))