129H - vectris.py

import math

def product(m0, m1):
    'Multiplying matrices (in columns)'

    m0 = [[v[i] for v in m0] for i in range(len(m0))]
    return [[dot(x,y) for x in m0] for y in m1]

def det(m):
    'Determinant of matrix m'

    if len(m) == 2:
        return m[0][0]*m[1][1] - m[0][1]*m[1][0]
    elif len(m) == 3:
        return sum([(m[0][i]*m[1][(i+1)%3]*m[2][(i+2)%3]) -
                    (m[0][i]*m[2][(i+1)%3]*m[1][(i+2)%3]) for i in range(3)])

def inverse(m):
    'Inverse of matrix m, described in column vectors'

    d = det(m)
    if len(m) == 2:
        n = [[m[1][1]/d, -m[0][1]/d], [-m[1][0]/d, m[0][0]/d]]
    elif len(m) == 3:
        n = []
        for i in range(3):
            j = [0,1,2]
            j = j[:i] + j[i+1:]
            n.append([(-1)**i*x/d for x in cross(m[j[0]], m[j[1]])])
    return n

def length(v):
    'Length of vector'

    return sum([i**2 for i in v])**0.5

def dot(v0, v1):
    'Dot product of two vectors'

    return sum([v0[i]*v1[i] for i in range(3)])

def angle(v0, v1):
    'Angle between two vectors'

    return math.acos(dot(v0,v1)/(length(v0)*length(v1)))*180/math.pi

def cross(v0, v1):
    'Cross product of two vectors'

    return [(-1)**i * det([v0[0:i]+v0[i+1:4], v1[0:i]+v1[i+1:4]]) for i in range(3)]

def plane_eq(*args):
    'Calculates the equation of the plane with the three points in it'

    if len(args) == 1:
        p0, p1, p2 = args[0]
    else:
        p0, p1, p2 = args
    v0 = [p1[i]-p0[i] for i in range(3)]
    v1 = [p2[i]-p1[i] for i in range(3)]

    normal = cross(v0, v1)
    if normal[0] == 0:
        if normal[1] == 0:
            normal = [0.0,0.0,1.0]
        else:
            normal = [i/normal[1] for i in normal]
    else:
        normal = [i/normal[0] for i in normal]
    return [normal, dot(p0, normal)]

def solve(v0, v1, t):
    'Calculates two variables s&t such that r*v0+s*v1=t'

    x = 0
    while True:
        y = [0,1,2]
        y = y[:x]+y[x+1:]
        m, n = y
        if v0[n]*v1[m]-v0[m]*v1[n] != 0:
            break
        else:
            x += 1
    r = (t[n]*v1[m]-t[m]*v1[n])/(v0[n]*v1[m]-v0[m]*v1[n])
    s = (t[n]*v0[m]-t[m]*v0[n])/(v0[n]*v1[m]-v0[m]*v1[n])
    return r, s

def intersect_lines(l0, l1):
    'Calculates whether two lines intersect'

    plane = plane_eq(l0[0], l0[1], [l0[0][i]+l1[0][i]-l1[1][i] for i in range(3)])
    if dot(plane[0], l0[0]) == plane[1]:
        a = [l0[1][i]-l0[0][i] for i in range(3)]
        b = [l1[0][i]-l1[1][i] for i in range(3)]
        c = [l1[0][i]-l0[0][i] for i in range(3)]
        r, s = solve(a, b, c)
        if r < 1 and s < 1:
            return True
    return False

def intersect_tris(t0, t1):
    'Caluculates whether two triangles intersect'

    p0 = plane_eq(t0)
    p1 = plane_eq(t1)
    if p0 == p1:
        # If the planes are the same
        a = [t0[1][i]-t0[0][i] for i in range(3)]
        b = [t0[2][i]-t0[1][i] for i in range(3)]
        for p in t1:
            r, s = solve(a, b, p)
            if r < 1 and s < r:
                return True
        a = [t1[1][i]-t1[0][i] for i in range(3)]
        b = [t1[2][i]-t1[1][i] for i in range(3)]
        for p in t0:
            r, s = solve(a, b, p)
            if r < 1 and s < r:
                return True

        for p in t0:
            if p in t1:
                return False

        l0 = t0[0:2]
        if intersect_lines(l0, t1[0:2]) or intersect_lines(l0, t1[1:3]):
            return True
    elif p0[0] == p1[0]:
        # If the normals are the same but the distances aren't
        # they are parallel and don't intersect
        return False
    else:
        # If they are not parallel
        # If a point is shared, then they do not intersect
        for p in t0:
            if p in t1:
                return False
        # Now we look at the dot products of the normal of the second plane with each point in the first
        # Since it changes linearly from point to point
        # If it goes from below the distance of the second plane to above
        # Then it intersects within the triangle
        vals0 = [dot(p, p1[0]) for p in t0]
        vals1 = [dot(p, p0[0]) for p in t1]

        below = [False, False]
        above = [False, False]
        for v in vals0:
            if v < p1[1]:
                below[0] = True
            elif v > p1[1]:
                above[0] = True
        for v in vals1:
            if v < p0[1]:
                below[1] = True
            elif v > p0[1]:
                above[0] = True

        for i in below + above:
            if not i:
                return False
        else:
            return True

def construct(p, v, tris):
    'For every tri, calculates the normal and which other tris it intersects with'

    tris2 = {}
    for t in tris:
        tri = (p[t[0]], p[t[1]], p[t[2]])
        v0 = [tri[1][i]-tri[0][i] for i in range(3)]
        v1 = [tri[2][i]-tri[1][i] for i in range(3)]
        normal = cross(v0, v1)
        if angle(v, normal) > 90:
            normal = [-i for i in normal]
        intersecting = []

        for t2 in tris2:
            if intersect_tris([p[i] for i in t2], tri):
                intersecting.append(t2)
                tris2[t2][1].append(t)

        tris2[t] = [normal, intersecting]
    return tris2

def find_sets(p,v):
    'Calculates the possible sets that can be formed in p'

    n = len(p)
    tris = []
    for i in range(n):
        for j in range(i+1, n):
            for k in range(j+1, n):
                tris.append((i,j,k))
    tris2 = construct(p, v, tris)

    sets = []
    size = 0
    possible_sets = []
    closest = 90
    closest_sets = []

    # We work through each tri
    # Seeing which sets it can be added to without intersecting any other tris
    for t in tris:
        new_sets = []
        for j in range(len(sets)):
            for k in sets[j]:
                if k in tris2[t][1]:
                    break
            else:
                sets[j] = sets[j] + [t]
                new_sets.append(sets[j])
        sets.append([t])
        new_sets.append([t])

        for s in new_sets:
            # Now we calculate the average normal and see what angle it forms with the target vector
            norms = [tris2[i][0] for i in s]
            average = [sum([norms[n][j] for n in range(len(s))]) for j in range(3)]
            a = angle(average, v)
            if a > 170:
                a = 180 - a
            if a < 10:
                l = len(s)
                if l > size:
                    size = l
                    possible_sets = [s]
                elif l == size:
                    possible_sets.append(s)

                if a < closest:
                    closest = a
                    closest_sets = [s]
                elif a == closest:
                    if l > len(closest_sets[0]):
                        closest_sets = [s]
                    elif l == len(closest_sets[0]):
                        closest_sets.append(s)
                break

    return size, possible_sets, closest, closest_sets

def main():
    raw = open('129H.txt').read().split('\n')
    points = [[float(i) for i in line.split()] for line in raw[1:-1]]
    vector = [float(i) for i in raw[-1].split()]

    size, possible_sets, closest, closest_sets = find_sets(points, vector)

    if possible_sets:
        print("Largest sized set:", size)
        print("Possible sets:", len(possible_sets))

        for s in possible_sets:
            print()
            for t in s:
                print(t, end = ': ')
                for p in t:
                    print(tuple(points[p]), end = ' ')
                print()

    if closest_sets:
        print()
        print("Closest set: {0:}°".format(closest))
        print("Possible sets:", len(closest_sets))

        for s in closest_sets:
            print()
            for t in s:
                print(t, end = ': ')
                for p in t:
                    print(tuple(points[p]), end = ' ')
                print()
    else:
        print("No valid result found")

if __name__ == '__main__':
    main()