112 lines
6.0 KiB
Python
112 lines
6.0 KiB
Python
"""Small, dependency-free geometry routines. Right-handed, Z-up, metres."""
|
|
import math
|
|
|
|
EPS = 1e-10
|
|
|
|
def add(a, b): return [a[i] + b[i] for i in range(3)]
|
|
def sub(a, b): return [a[i] - b[i] for i in range(3)]
|
|
def mul(a, s): return [v * s for v in a]
|
|
def dot(a, b): return sum(x*y for x, y in zip(a, b))
|
|
def cross(a, b): return [a[1]*b[2]-a[2]*b[1], a[2]*b[0]-a[0]*b[2], a[0]*b[1]-a[1]*b[0]]
|
|
def length(a): return math.sqrt(dot(a, a))
|
|
def unit(a):
|
|
d = length(a)
|
|
if d < EPS: raise ValueError("Cannot normalize a zero-length vector")
|
|
return mul(a, 1/d)
|
|
def identity(): return [[float(i == j) for j in range(4)] for i in range(4)]
|
|
def matmul(a, b): return [[sum(a[i][k]*b[k][j] for k in range(4)) for j in range(4)] for i in range(4)]
|
|
def point(m, p): return [sum(m[i][j]*p[j] for j in range(3)) + m[i][3] for i in range(3)]
|
|
def direction(m, p): return [sum(m[i][j]*p[j] for j in range(3)) for i in range(3)]
|
|
def determinant(m): return dot(m[0][:3], cross(m[1][:3], m[2][:3]))
|
|
def rotate(v, axis, angle):
|
|
c, s = math.cos(angle), math.sin(angle)
|
|
return add(add(mul(v, c), mul(cross(axis, v), s)), mul(axis, dot(axis, v)*(1-c)))
|
|
def transport(v, old_t, new_t):
|
|
axis = cross(old_t, new_t); sn = length(axis); cs = max(-1., min(1., dot(old_t, new_t)))
|
|
if sn < EPS:
|
|
if cs < 0: raise ValueError("Curve reverses direction by 180 degrees; refine or change the path")
|
|
return v[:]
|
|
return rotate(v, mul(axis, 1/sn), math.atan2(sn, cs))
|
|
def transform(translation=(0,0,0), rotation=(0,0,0), scale=(1,1,1)):
|
|
if isinstance(scale, (int, float)): scale = [scale]*3
|
|
if len(scale) != 3 or any(s <= 0 for s in scale): raise ValueError("Scale must have three positive components")
|
|
rx, ry, rz = [math.radians(v) for v in rotation]
|
|
cx,sx,cy,sy,cz,sz = math.cos(rx),math.sin(rx),math.cos(ry),math.sin(ry),math.cos(rz),math.sin(rz)
|
|
x = [[1,0,0,0],[0,cx,-sx,0],[0,sx,cx,0],[0,0,0,1]]
|
|
y = [[cy,0,sy,0],[0,1,0,0],[-sy,0,cy,0],[0,0,0,1]]
|
|
z = [[cz,-sz,0,0],[sz,cz,0,0],[0,0,1,0],[0,0,0,1]]
|
|
m = matmul(z,matmul(y,x))
|
|
for i in range(3):
|
|
for j in range(3): m[i][j] *= scale[j]
|
|
m[i][3] = translation[i]
|
|
return m
|
|
def frames(points, closed=False, up=(0,0,1), twist=0):
|
|
"""Rotation-minimizing frames, with distributed holonomy correction on closed paths."""
|
|
n = len(points)
|
|
if n < (3 if closed else 2): raise ValueError("Too few points for frames")
|
|
tangents = []
|
|
for i in range(n):
|
|
a = points[(i-1)%n] if closed or i else points[0]
|
|
b = points[(i+1)%n] if closed or i < n-1 else points[-1]
|
|
tangents.append(unit(sub(b,a)))
|
|
t0 = tangents[0]; right = cross(t0, unit(up))
|
|
if length(right) < EPS:
|
|
axis = min([[1,0,0],[0,1,0],[0,0,1]], key=lambda x: abs(dot(x,t0)))
|
|
right = cross(t0,axis)
|
|
rights = [unit(right)]
|
|
for i in range(1,n): rights.append(unit(transport(rights[-1],tangents[i-1],tangents[i])))
|
|
correction = 0
|
|
if closed:
|
|
if abs(twist/360-round(twist/360)) > 1e-8:
|
|
raise ValueError("Closed frames require twist_degrees to be a multiple of 360")
|
|
seam = transport(rights[-1], tangents[-1], tangents[0])
|
|
correction = math.atan2(dot(t0,cross(seam,rights[0])),dot(seam,rights[0]))
|
|
out = []
|
|
for i,(p,t,r) in enumerate(zip(points,tangents,rights)):
|
|
a = (correction+math.radians(twist))*i/(n if closed else n-1)
|
|
r = unit(rotate(r,t,a)); u = unit(cross(r,t))
|
|
out.append({"origin":p[:], "x":r, "y":t, "z":u})
|
|
return out
|
|
|
|
def triangulate_polygon(points):
|
|
"""Ear clipping in 2D; accepts either winding, rejects invalid/degenerate profiles."""
|
|
n = len(points)
|
|
if n < 3: raise ValueError("A section needs at least three vertices")
|
|
def orient(a,b,c): return (b[0]-a[0])*(c[1]-a[1])-(b[1]-a[1])*(c[0]-a[0])
|
|
area = sum(a[0]*b[1]-b[0]*a[1] for a,b in zip(points,points[1:]+points[:1]))*.5
|
|
if abs(area) < EPS: raise ValueError("Section has zero area")
|
|
# A simple polygon must not have crossings or repeated vertices.
|
|
for i in range(n):
|
|
for j in range(i+1,n):
|
|
if math.dist(points[i],points[j]) < EPS: raise ValueError("Section has duplicate vertices")
|
|
if j == i+1 or (i==0 and j==n-1): continue
|
|
a,b,c,d=points[i],points[(i+1)%n],points[j],points[(j+1)%n]
|
|
if orient(a,b,c)*orient(a,b,d)<-EPS and orient(c,d,a)*orient(c,d,b)<-EPS:
|
|
raise ValueError("Section self-intersects")
|
|
ids = list(range(n)) if area > 0 else list(reversed(range(n)))
|
|
triangles=[]
|
|
while len(ids)>3:
|
|
found=False
|
|
for k in range(len(ids)):
|
|
a,b,c=ids[k-1],ids[k],ids[(k+1)%len(ids)]
|
|
if orient(points[a],points[b],points[c])<=EPS: continue
|
|
inside=any(all(v>=-EPS for v in [orient(points[a],points[b],points[q]),orient(points[b],points[c],points[q]),orient(points[c],points[a],points[q])]) for q in ids if q not in (a,b,c))
|
|
if inside:continue
|
|
triangles.append([a,b,c]);ids.pop(k);found=True;break
|
|
if not found:raise ValueError("Section cannot be triangulated; remove collinear or crossing edges")
|
|
triangles.append(ids)
|
|
return triangles, area
|
|
|
|
def mesh_report(vertices, faces):
|
|
edges={}; directed={}; degenerate=[]; volume=0.
|
|
for i,f in enumerate(faces):
|
|
if len(f)!=3 or len(set(f))!=3 or any(not isinstance(x,int) or x<0 or x>=len(vertices) for x in f):
|
|
raise ValueError(f"Invalid triangle {i}: {f}")
|
|
a,b,c=[vertices[j] for j in f]
|
|
if length(cross(sub(b,a),sub(c,a)))<1e-10: degenerate.append(i)
|
|
volume+=dot(a,cross(b,c))/6
|
|
for x,y in zip(f,f[1:]+f[:1]):
|
|
key=tuple(sorted((x,y)));edges[key]=edges.get(key,0)+1
|
|
directed[key]=directed.get(key,0)+(1 if x<y else -1)
|
|
return {"vertices":len(vertices),"triangles":len(faces),"boundary_edges":sum(v==1 for v in edges.values()),"nonmanifold_edges":sum(v>2 for v in edges.values()),"inconsistent_edges":sum(edges[k]==2 and v!=0 for k,v in directed.items()),"degenerate_triangles":len(degenerate),"signed_volume":volume}
|