pt2game/compile_navmesh.py

#!/usr/bin/python3
import lxml.etree, math, sys

_, inpath, outpath, basename = sys.argv

tree = lxml.etree.parse(inpath)

polys = []
edge2poly = {}

# Polygons must be convex to work correctly, but we don't verify that

def points_to_edges(points):
    points = points + [points[0]]
    return zip(points[:-1], points[1:])

def parse_object_polygon_points(object):
    polygon_data = object.find("polygon")
    basex = int(object.attrib["x"])
    basey = int(object.attrib["y"])
    if polygon_data is None:
        # It's a rectangle by default
        width = float(object.attrib["width"])
        height = float(object.attrib["height"])
        points = [(basex, basey), (basex + width, basey), (basex + width, basey + height), (basex, basey + height)]
    else:
        points = []
        for point in polygon_data.attrib["points"].split(" "):
            x,y=point.split(",")
            points.append((int(x)+basex, int(y)+basey))
    # check winding order
    ax,ay=points[0]
    bx,by=points[1]
    cx,cy=points[2]
    abx,aby=bx-ax,by-ay
    acx,acy=cx-ax,cy-ay
    cross = abx*acy-acx*aby
    # positive for clockwise winding order. Example clockwise winding order: (0,0), (1,0), (0,1)
    if cross < 0:
        points = points[::-1]
    return points

# Returns P,Q,R such that Px+Qy+R=0 for points on the edge, and >0 for points on the right (inside polygon for clockwise winding order)
def get_edge_equation(a, b):
    (ax,ay),(bx,by)=a,b
    # Line equation Px+Qy+R=0 from https://math.stackexchange.com/questions/422602/convert-two-points-to-line-eq-ax-by-c-0
    P,Q,R=ay-by,bx-ax,ax*by-bx*ay
    # Line equation >0 for points inside the triangle.
    # Normalize so that P^2+Q^2=1 so that the equation result tells us the distance. This can be used to find the closest triangle to a point.
    norm=1/math.sqrt(P*P+Q*Q)
    P *= norm
    Q *= norm
    R *= norm
    return P,Q,R

for object in tree.findall("objectgroup[@name='navmesh']/object"):
    points = parse_object_polygon_points(object)
    
    poly_id = len(polys)
    polys.append(points)
    
    for edge in points_to_edges(points):
        edge_id = tuple(sorted(edge))
        if edge_id not in edge2poly: edge2poly[edge_id] = []
        edge2poly[edge_id].append(poly_id)
        assert len(edge2poly[edge_id]) <= 2, "more than 2 polygons share edge "+str(edge_id)

pathfind_edges = [[] for _ in range(len(polys))]
for edgepolys in edge2poly.values():
    assert len(edgepolys) in (1,2)
    if len(edgepolys) == 1: continue
    p1,p2 = edgepolys
    pathfind_edges[p1].append(p2)
    pathfind_edges[p2].append(p1)

pathfind_matrix = [[255 for _1 in range(len(polys))] for _2 in range(len(polys))]
# pathfind_matrix[source][dest] is how to get to dest from source

for dest in range(len(polys)):
    # Find paths to poly by breadth-first search. We don't take the size of the polygon into account yet.
    openset = [dest]
    while len(openset)>0:
        cur, openset = openset[0], openset[1:]
        for adjacent in pathfind_edges[cur]:
            if adjacent != dest and pathfind_matrix[adjacent][dest] == 255:
                pathfind_matrix[adjacent][dest] = cur
                openset.append(adjacent)



out = open(outpath,"w")

out.write("#include \"compiled_structures.h\"\n")
out.write("#include \"objids.h\"\n")
out.write(f"static const struct navmesh_tri {basename}_triangles[{len(polys)}] = {{\n");
for poly_id,points in enumerate(polys):
    out.write("\t{\n")
    out.write("\t\t"+str(len(points))+",\n")
    out.write("\t\t(const struct navmesh_tri_edge[]){\n")
    for a,b in points_to_edges(points):
        P,Q,R = get_edge_equation(a,b)
        edge_id = tuple(sorted((a,b)))
        polys_on_edge = edge2poly[edge_id]
        assert len(polys_on_edge) in (1,2)
        assert poly_id in polys_on_edge
        if len(polys_on_edge) == 2:
            other_poly_id = [x for x in polys_on_edge if x!=poly_id][0]
        else:
            other_poly_id = -1
        
        (ax,ay),(bx,by)=a,b
        centx, centy = (ax+bx)/2, (ay+by)/2
        
        out.write(f"\t\t\t{{{P},{Q},{R},{other_poly_id},{{{int(centx)},{int(centy)}}}}},\n")
    out.write("\t\t},\n")
    out.write("\t\t(const struct navmesh_point[]){" + ",".join(f"{{{x},{y}}}" for x,y in points) + "},\n")
    out.write("\t},\n");
out.write("};\n")
out.write(f"static const unsigned char {basename}_pathfind[{len(polys)*len(polys)}] = {{\n")
for dest in range(len(polys)):
    out.write("\t")
    for source in range(len(polys)):
        out.write(str(pathfind_matrix[source][dest])+",")
    out.write("\n")
out.write("};\n")
out.write(f"extern const struct navmesh navmesh_{basename} = {{{len(polys)}, {basename}_triangles, {basename}_pathfind}};\n")

out.write("extern const struct level_predef_data predef_"+basename+" = {\n")
out.write("\t(const struct level_clickregion[]){\n") # clickregions start
for object in tree.findall("objectgroup[@name='clickable']/object"):
    out.write("\t\t{\n");
    points = parse_object_polygon_points(object)
    out.write("\t\t\t"+object.attrib["name"]+",\n")
    out.write("\t\t\t" + str(len(points)) + ",\n")
    out.write("\t\t\t(const struct level_clickregion_edge[]){\n")
    for a,b in points_to_edges(points):
        P,Q,R = get_edge_equation(a,b)
        out.write(f"\t\t\t\t{{{P},{Q},{R}}},\n")
    out.write("\t\t\t},\n")
    out.write("\t\t},\n");
out.write("\t\t{0}\n")
out.write("\t},\n") # clickregions end
out.write("};\n")