#!/usr/bin/env python3 """Build the virtual-centerline construction figure (fig_center.pdf). Three panels, following subsec:centerline of the paper: (a) clearance field of the free space (distance transform), the EVG-thin skeleton ridge extracted from it, and the coarse A* path on the skeleton graph from the entrance to the loading bay. (b) the local tangent--normal rectangular corridors of eq:local_corridor_tangent / eq:local_corridor_normal built around each resampled reference point, and the centerline returned by the FEM-pos QP smoother of eq:qp_problem, overlaid on the coarse path. (c) the discrete curvature profile before and after smoothing, obtained from the second difference ||d_i|| = ell^2 kappa_i + O(ell^4), against the bound kappa_max = 1 / R_min imposed by eq:qp_curvature. Construction illustration on a synthetic site -- not an experimental result. """ import numpy as np import matplotlib matplotlib.use("Agg") import matplotlib.pyplot as plt from matplotlib.patches import Polygon as MplPolygon, Rectangle import sitegeom as sg R_MIN = 12.0 KAP_MAX = 1.0 / R_MIN HALF_W = 1.6 # lateral corridor half-width used by qp_smooth S_HALF = 0.5 # tangential corridor half-length, for illustration def curvature(P): """Discrete curvature via second differences, on P's own spacing.""" seg = np.hypot(*np.diff(P, axis=0).T) ell = 0.5 * (seg[:-1] + seg[1:]) d2 = P[:-2] - 2.0 * P[1:-1] + P[2:] kap = np.hypot(d2[:, 0], d2[:, 1]) / ell ** 2 s = np.concatenate([[0.0], np.cumsum(seg)]) return s[1:-1], kap def main(): g = sg.build_centerline() xs, ys, inside, dist = g["xs"], g["ys"], g["inside"], g["dist"] skel, raw, center = g["skel"], g["raw"], g["center"] coarse = sg.resample(raw, 1.0) t_c, n_c = sg.tangent_normal(coarse) s_raw, k_raw = curvature(coarse) s_sm, k_sm = curvature(center) print("coarse |kappa| max %.4f -> R %.2f m" % (k_raw.max(), 1 / k_raw.max())) print("smooth |kappa| max %.4f -> R %.2f m" % (k_sm.max(), 1 / k_sm.max())) print("bound kappa_max %.4f -> R %.2f m" % (KAP_MAX, R_MIN)) print("smoothed violates bound: %s" % bool((k_sm > KAP_MAX + 1e-9).any())) print("coarse length %.2f m / smoothed length %.2f m" % (np.hypot(*np.diff(coarse, axis=0).T).sum(), np.hypot(*np.diff(center, axis=0).T).sum())) print("max lateral deviation from coarse %.2f m" % np.abs(((center - coarse) * n_c).sum(axis=1)).max()) plt.rcParams.update({ "font.family": "serif", "font.serif": ["Nimbus Roman", "Times New Roman", "DejaVu Serif"], "font.size": 7, "mathtext.fontset": "stix", "axes.linewidth": 0.5, "pdf.fonttype": 42, }) fig = plt.figure(figsize=(7.16, 1.95), constrained_layout=True) gs = fig.add_gridspec(1, 3, width_ratios=[1.0, 1.0, 0.72]) ax0, ax1, ax2 = (fig.add_subplot(gs[0]), fig.add_subplot(gs[1]), fig.add_subplot(gs[2])) # ---- (a) clearance field, skeleton, coarse A* -------------------------- dm = np.ma.array(dist, mask=~inside) im = ax0.pcolormesh(xs, ys, dm, cmap="Blues", shading="auto", rasterized=True, vmin=0.0, vmax=dist.max()) rr, cc = np.nonzero(skel) ax0.plot(xs[cc], ys[rr], ls="none", marker="s", ms=0.55, color="#4A4A4A", label="EVG skeleton", zorder=3) ax0.plot(raw[:, 0], raw[:, 1], color="#C44E0A", lw=0.9, label=r"coarse A$^\ast$", zorder=4) ax0.plot(*sg.ENTRANCE, marker=">", ms=3.2, color="#111111", zorder=5) ax0.plot(*sg.LOAD_POSE, marker="o", ms=3.0, color="#111111", zorder=5) ax0.annotate("entrance", sg.ENTRANCE, textcoords="offset points", xytext=(3, -6), fontsize=5.6) ax0.annotate("loading bay", sg.LOAD_POSE, textcoords="offset points", xytext=(-13, 4), fontsize=5.6) ax0.legend(loc="lower right", fontsize=5.2, frameon=False, handlelength=1.4, borderaxespad=0.1, labelspacing=0.25, markerscale=3.0) ax0.set_title("(a) clearance field and skeleton", fontsize=6.6, pad=2) cb = fig.colorbar(im, ax=ax0, fraction=0.040, pad=0.015) cb.set_ticks([0.0, dist.max()]) cb.set_ticklabels(["0", "%.0f m" % dist.max()]) cb.ax.tick_params(labelsize=5.2, width=0.4, length=1.8) cb.outline.set_linewidth(0.4) # ---- (b) local corridors and the smoothed centerline ------------------- ax1.add_patch(MplPolygon(sg.SITE, closed=True, facecolor="#F4F4F4", edgecolor="#585858", lw=0.7, zorder=1)) for i in range(0, len(coarse), 3): ang = np.degrees(np.arctan2(t_c[i, 1], t_c[i, 0])) r = Rectangle((-S_HALF, -HALF_W), 2 * S_HALF, 2 * HALF_W, facecolor="#BBD7EE", edgecolor="#4A87BE", lw=0.25, alpha=0.75, zorder=2) tr = (matplotlib.transforms.Affine2D() .rotate_deg(ang).translate(*coarse[i]) + ax1.transData) r.set_transform(tr) ax1.add_patch(r) ax1.plot(coarse[:, 0], coarse[:, 1], color="#C44E0A", lw=0.7, ls=(0, (2.0, 1.3)), label=r"coarse A$^\ast$", zorder=4) ax1.plot(center[:, 0], center[:, 1], color="#703A94", lw=1.2, label=r"$\Lambda_{\mathrm{center}}$ (QP)", zorder=5) # annotate one corridor with its four bounds i0 = len(coarse) // 2 p0 = coarse[i0] ax1.annotate(r"$s_i^\pm,\ l_i^\pm$", p0, textcoords="offset points", xytext=(6, 7), fontsize=5.8, arrowprops=dict(arrowstyle="-", lw=0.4, color="#333333", shrinkA=0, shrinkB=1)) ax1.legend(loc="lower right", fontsize=5.2, frameon=False, handlelength=1.5, borderaxespad=0.1, labelspacing=0.25) ax1.set_title("(b) local corridors and QP smoothing", fontsize=6.6, pad=2) for ax in (ax0, ax1): ax.set_aspect("equal") ax.set_xlim(sg.SITE[:, 0].min() - 1, sg.SITE[:, 0].max() + 1) ax.set_ylim(sg.SITE[:, 1].min() - 1, sg.SITE[:, 1].max() + 1) ax.set_xticks([]) ax.set_yticks([]) for sp in ax.spines.values(): sp.set_visible(False) # ---- (c) curvature profile -------------------------------------------- ax2.plot(s_raw, k_raw, color="#C44E0A", lw=0.7, ls=(0, (2.0, 1.3)), label=r"coarse A$^\ast$") ax2.plot(s_sm, k_sm, color="#703A94", lw=1.0, label=r"$\Lambda_{\mathrm{center}}$") ax2.axhline(KAP_MAX, color="#111111", lw=0.6, ls=(0, (4.0, 1.6))) ax2.annotate(r"$\kappa_{\max}=1/R_{\min}$", (s_sm[-1], KAP_MAX), textcoords="offset points", xytext=(-2, 3), fontsize=5.6, ha="right") ax2.set_xlabel("arc length $s$ (m)", fontsize=6.2, labelpad=1.5) ax2.set_ylabel(r"$\kappa$ (m$^{-1}$)", fontsize=6.2, labelpad=1.5) ax2.set_title("(c) curvature profile", fontsize=6.6, pad=2) ax2.tick_params(labelsize=5.6, width=0.4, length=2.0, pad=1.5) ax2.set_xlim(0, max(s_raw[-1], s_sm[-1])) ax2.set_ylim(0, max(k_raw.max(), KAP_MAX) * 1.18) ax2.legend(loc="upper left", fontsize=5.2, frameon=False, handlelength=1.5, borderaxespad=0.2, labelspacing=0.25) for sp in ("top", "right"): ax2.spines[sp].set_visible(False) fig.savefig("fig_center.pdf", dpi=600) print("wrote fig_center.pdf") if __name__ == "__main__": main()