177 lines
7.4 KiB
Python
177 lines
7.4 KiB
Python
#!/usr/bin/env python3
|
|
"""Compute exact coordinates for the convex-decomposition schematic.
|
|
|
|
Follows supplement.tex S-IV verbatim:
|
|
|
|
eq:sup_ellipsoid_quadratic E = { p | (p-d)^T (C C^T)^{-1} (p-d) <= 1 },
|
|
o_i not in int(E)
|
|
eq:sup_halfspace_normal atil_i = (C C^T)^{-1} (o_i - d), a_i = atil/|atil|,
|
|
b_i = a_i^T o_i
|
|
|
|
Construction: d is the segment midpoint, the major axis is the segment
|
|
direction with semi-axis equal to the segment half-length, and the minor
|
|
semi-axis is shrunk until the boundary first touches an obstacle point.
|
|
Redundant obstacles are pruned greedily: after each half-space is emitted,
|
|
every obstacle it already excludes is dropped.
|
|
|
|
Prints TikZ-ready numbers; nothing is drawn here.
|
|
"""
|
|
import numpy as np
|
|
|
|
# --- segment (figure units = cm in the TikZ picture) ------------------------
|
|
P0 = np.array([0.0, 0.0])
|
|
P1 = np.array([5.2, 0.0])
|
|
D = 0.5 * (P0 + P1)
|
|
A_AX = 0.5 * np.linalg.norm(P1 - P0) # semi-major, along the segment
|
|
|
|
# --- obstacle points (site-boundary / ore-pile samples) --------------------
|
|
OBST = np.array([
|
|
[0.30, 1.42], [1.35, 1.05], [2.55, 1.28], [3.75, 1.62], [4.85, 1.35],
|
|
[0.65, -1.30], [1.90, -1.62], [3.10, -1.12], [4.35, -1.45], [5.35, -1.02],
|
|
[-0.55, 0.85], [5.75, 0.62],
|
|
# points beyond the segment ends: they do not constrain the minor axis
|
|
# (|u| > 1) but they do cap the polytope longitudinally
|
|
[-1.05, 0.12], [6.35, -0.18],
|
|
])
|
|
|
|
|
|
def main():
|
|
# ---- minor semi-axis: shrink until the boundary touches an obstacle ----
|
|
u = (OBST[:, 0] - D[0]) / A_AX
|
|
live = np.abs(u) < 1.0 - 1e-9
|
|
b_req = np.abs(OBST[live, 1]) / np.sqrt(1.0 - u[live] ** 2)
|
|
B_AX = float(b_req.min())
|
|
touch = int(np.nonzero(live)[0][int(np.argmin(b_req))])
|
|
print("semi-axes: a = %.4f, b = %.4f center d = (%.3f, %.3f)"
|
|
% (A_AX, B_AX, D[0], D[1]))
|
|
print("touching obstacle index %d at (%.3f, %.3f)"
|
|
% (touch, OBST[touch, 0], OBST[touch, 1]))
|
|
|
|
C = np.diag([A_AX, B_AX])
|
|
Minv = np.linalg.inv(C @ C.T)
|
|
|
|
def qform(P):
|
|
dv = P - D
|
|
return np.einsum("ij,jk,ik->i", dv, Minv, dv)
|
|
|
|
q = qform(OBST)
|
|
print("min obstacle quadratic form = %.6f (must be >= 1)" % q.min())
|
|
assert q.min() >= 1.0 - 1e-9, "eq:sup_ellipsoid_quadratic violated"
|
|
# the segment must lie inside the ellipsoid
|
|
seg = P0 + np.linspace(0, 1, 201)[:, None] * (P1 - P0)
|
|
print("max segment quadratic form = %.6f (must be <= 1)" % qform(seg).max())
|
|
assert qform(seg).max() <= 1.0 + 1e-9
|
|
|
|
# ---- greedy half-space generation with redundancy pruning --------------
|
|
remaining = list(range(len(OBST)))
|
|
planes = []
|
|
while remaining:
|
|
# process the obstacle closest in the ellipsoid metric first
|
|
qi = qform(OBST[remaining])
|
|
k = remaining[int(np.argmin(qi))]
|
|
o = OBST[k]
|
|
at = Minv @ (o - D)
|
|
a = at / np.linalg.norm(at)
|
|
b = float(a @ o)
|
|
planes.append((k, a, b))
|
|
# the ellipsoid (hence the segment) must be on the safe side
|
|
sup = float(a @ D + np.sqrt((a @ (C @ C.T)) @ a))
|
|
assert sup <= b + 1e-9, "ellipsoid crosses the half-space"
|
|
remaining = [j for j in remaining if float(a @ OBST[j]) < b - 1e-9]
|
|
|
|
print("\n%d half-spaces retained out of %d obstacles"
|
|
% (len(planes), len(OBST)))
|
|
for k, a, b in planes:
|
|
print(" o_%-2d (%6.3f,%6.3f) a = (%7.4f,%7.4f) b = %7.4f"
|
|
% (k, OBST[k, 0], OBST[k, 1], a[0], a[1], b))
|
|
|
|
# ---- polytope vertices, for drawing ------------------------------------
|
|
A = np.array([p[1] for p in planes])
|
|
bb = np.array([p[2] for p in planes])
|
|
verts = []
|
|
for i in range(len(planes)):
|
|
for j in range(i + 1, len(planes)):
|
|
M = np.array([A[i], A[j]])
|
|
if abs(np.linalg.det(M)) < 1e-9:
|
|
continue
|
|
v = np.linalg.solve(M, [bb[i], bb[j]])
|
|
if np.all(A @ v <= bb + 1e-7):
|
|
verts.append(v)
|
|
V = np.array(verts)
|
|
ctr = V.mean(axis=0)
|
|
V = V[np.argsort(np.arctan2(V[:, 1] - ctr[1], V[:, 0] - ctr[0]))]
|
|
print("\npolytope P: %d vertices" % len(V))
|
|
print(" tikz path: " + " -- ".join("(%.3f,%.3f)" % (x, y) for x, y in V)
|
|
+ " -- cycle")
|
|
|
|
# ---- ellipse as a TikZ primitive ---------------------------------------
|
|
print("\n tikz ellipse: (%.3f,%.3f) ellipse [x radius=%.4f, "
|
|
"y radius=%.4f]" % (D[0], D[1], A_AX, B_AX))
|
|
print(" tikz obstacles: " + " ".join("(%.2f,%.2f)" % (x, y)
|
|
for x, y in OBST))
|
|
kept = " ".join("o%d" % p[0] for p in planes)
|
|
print(" retained: " + kept)
|
|
|
|
# ---- support points: where each half-space touches the ellipsoid -------
|
|
# The boundary point whose outward normal is parallel to a_i is
|
|
# p = d + (C C^T) a / sqrt(a^T (C C^T) a).
|
|
M = C @ C.T
|
|
print("\n ellipsoid support points (tangency anchors of a_i):")
|
|
for k, a, b in planes:
|
|
p = D + (M @ a) / np.sqrt(a @ M @ a)
|
|
assert abs(qform(p[None, :])[0] - 1.0) < 1e-9, "anchor off the boundary"
|
|
print(" o_%-2d: anchor (%.3f,%.3f) gap to o_i = %.3f"
|
|
% (k, p[0], p[1], float(a @ OBST[k]) - float(a @ p)))
|
|
|
|
# ---- pruned (redundant) obstacles, drawn hollow in the figure ----------
|
|
kept_idx = {p[0] for p in planes}
|
|
pruned = [i for i in range(len(OBST)) if i not in kept_idx]
|
|
print("\n pruned as redundant: " +
|
|
" ".join("(%.2f,%.2f)" % (OBST[i, 0], OBST[i, 1]) for i in pruned))
|
|
print(" retained points: " +
|
|
" ".join("(%.2f,%.2f)" % (OBST[p[0], 0], OBST[p[0], 1])
|
|
for p in planes))
|
|
|
|
# ---- initial sphere before the minor-axis shrink ------------------------
|
|
inside_sphere = [i for i in range(len(OBST))
|
|
if np.linalg.norm(OBST[i] - D) < A_AX]
|
|
print("\n initial sphere radius %.3f encloses %d obstacle(s)"
|
|
% (A_AX, len(inside_sphere)))
|
|
|
|
# ---- ready-to-paste TikZ for the half-space boundaries -----------------
|
|
# Each line a^T p = b is drawn through a*b along t = (-a_y, a_x) and left
|
|
# for the picture's own \clip to trim.
|
|
print("\n TikZ half-space boundaries (draw long, clip in the picture):")
|
|
for k, a, b in planes:
|
|
p_on = a * b
|
|
t = np.array([-a[1], a[0]])
|
|
q0, q1 = p_on - 9.0 * t, p_on + 9.0 * t
|
|
print(" \\draw[hsp] (%.3f,%.3f) -- (%.3f,%.3f); %% o_%d"
|
|
% (q0[0], q0[1], q1[0], q1[1], k))
|
|
|
|
# ---- half-space boundary segments clipped to the drawing box ----------
|
|
print("\n half-space boundary lines (clipped to x in [-1.2, 6.4]):")
|
|
for k, a, b in planes:
|
|
# parameterize the line a^T p = b
|
|
t = np.array([-a[1], a[0]])
|
|
p_on = a * b # closest point of the line to the origin
|
|
ts = []
|
|
for xlim in (-1.2, 6.4):
|
|
if abs(t[0]) > 1e-9:
|
|
ts.append((xlim - p_on[0]) / t[0])
|
|
for ylim in (-2.3, 2.3):
|
|
if abs(t[1]) > 1e-9:
|
|
ts.append((ylim - p_on[1]) / t[1])
|
|
pts = [p_on + tv * t for tv in ts]
|
|
pts = [p for p in pts
|
|
if -1.2 - 1e-6 <= p[0] <= 6.4 + 1e-6
|
|
and -2.3 - 1e-6 <= p[1] <= 2.3 + 1e-6]
|
|
if len(pts) >= 2:
|
|
pts = sorted(pts, key=lambda p: (p[0], p[1]))
|
|
print(" o_%-2d: (%.3f,%.3f) -- (%.3f,%.3f)"
|
|
% (k, pts[0][0], pts[0][1], pts[-1][0], pts[-1][1]))
|
|
|
|
|
|
if __name__ == "__main__":
|
|
main()
|