迁移到dhxdl
This commit is contained in:
王闯
@@ -0,0 +1,176 @@
|
||||
#!/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()
|
||||
Reference in New Issue
Block a user