Source code for pyrockfall._geometry

import numpy as np
from scipy.stats import norm
from typing import Callable, Optional, Tuple, Union, Sequence
from numpy.typing import NDArray

from ._utils import build_neighbours_polygon, brentq

[docs] class Geometry: def __init__( self, nodes: NDArray[np.floating], nodes_std: Optional[NDArray[np.floating]] = None, elements: Optional[Union[Sequence[int], NDArray[np.integer]]] = None, attributes: Optional[Union[Sequence[int], NDArray[np.integer]]] = None, neighbours: Optional[NDArray[np.integer]] = None, ): # --- store nodes internally as (D, N) --- nodes = np.asarray(nodes, dtype=float) if nodes.ndim != 2: raise ValueError("`nodes` must have shape (N, D).") self._nodes = nodes.T # (D, N) # --- set elements --- if elements is None: self._elements = np.column_stack([np.arange(self._nodes.shape[1] - 1), np.arange(1, self._nodes.shape[1])]) else: self._elements = np.asarray(elements, dtype=int) # --- set attributes --- if attributes is None: self._attributes = attributes else: self._attributes = np.asarray(attributes, dtype=int) # --- consistency checks --- if self._elements.max() >= self._nodes.shape[1]: raise ValueError("Element connectivity references non-existent node index.") if self._attributes is not None: if len(self._elements) != len(self._attributes): raise ValueError("Number of elements and number of materials must match.") # --- stochastic node perturbation (optional, defaults to zero) --- if nodes_std is None: self._nodes_std = np.zeros_like(self._nodes) else: arr = np.asarray(nodes_std, dtype=float) if arr.shape != nodes.shape: raise ValueError("nodes_std must match nodes shape.") self._nodes_std = arr.T # Neighbours: store if provided, else compute on demand self._neighbours: Optional[np.ndarray] = None if neighbours is not None: self._neighbours = np.asarray(neighbours, dtype=np.int32) self._check_neighbours() elif elements is None: E = self._elements.shape[0] neighbours = np.column_stack(( np.arange(-1, E - 1), np.arange(1, E + 1) )) neighbours[-1, 1] = -1 def _check_neighbours(self) -> None: """Checks neighbours.""" nb = self._neighbours if nb is None: raise ValueError("`neighbours` is None.") if nb.shape != (self._elements.shape[0], self._nodes.shape[1]): raise ValueError( f"`neighbours` must have shape ({self._elements.shape[0]}, {self._nodes.shape[1]}); got {nb.shape}." ) if nb.size and ( np.any(nb < -1) or np.any(nb >= self._elements.shape[0]) ): raise ValueError("`neighbours` contains out-of-range element indices.") # -------------------------- # Neighbours # -------------------------- @property def neighbours(self) -> np.ndarray: """np.ndarray of shape (E, D): Directed-edge neighbours per element. Convention: for element ``[v0, v1]``, ``neighbours[e, 0]`` is before ``v0``, and ``neighbours[e, 1]`` is after ``v1``. """ if self._neighbours is None: self._neighbours = build_neighbours_polygon(self._elements.astype(np.int32)) return self._neighbours @neighbours.setter def neighbours(self, value: np.ndarray) -> None: """Sets the neighbours array.""" self._neighbours = np.asarray(value, dtype=np.int32) self._check_neighbours() # ------------------------------------------------------------------ # Properties # ------------------------------------------------------------------ @property def nodes(self) -> np.ndarray: """np.ndarray of shape (N, D): Node coordinates (always returned as (N, D)).""" return self._nodes.T @property def elements(self) -> np.ndarray: """np.ndarray of shape (E, M): Element connectivity.""" return self._elements @property def attributes(self) -> np.ndarray | None: """np.ndarray of shape (E,): Element attributes.""" return self._attributes @attributes.setter def attributes(self, attrs: np.ndarray) -> None: if len(self._elements) != len(attrs): raise ValueError("Number of elements and number of materials must match.") self._attributes = attrs @property def hasUncertainty(self) -> bool: return np.any(self._nodes_std != 0.0).astype(bool) @property def nodes_std(self) -> np.ndarray: """np.ndarray of shape (D, N): Standard deviations for stochastic perturbation.""" return self._nodes_std.T @property def numRandomVariables(self) -> int: """Number of random variables (zero if no standard deviations).""" if np.all(self._nodes_std == 0.0): return 0 else: return self._nodes_std.size @property def floor(self) -> float: """Lowest vertical coordinate across all slope nodes.""" return float(np.min(self.nodes[:, -1])) # ------------------------------------------------------------------ # Methods # ------------------------------------------------------------------
[docs] def rvs(self, S: int = 1, *, random_state=None) -> np.ndarray: """ Vectorised sampling of nodes. Inputs: self.nodes : (D, M) float — mean coordinates self.nodes_std : (D, M) float — per-node std dev Returns: (D, M, 1) if all std == 0, else (D, M, S) """ mu = np.asarray(self.nodes, dtype=float) sd = np.asarray(self.nodes_std, dtype=float) if mu.shape != sd.shape or mu.ndim != 2: raise ValueError("`nodes` and `nodes_std` must both be (D, M).") if np.all(sd == 0): return mu[..., None] # (D, M, 1) # Broadcast to (D, M, S) without copying muS = mu[..., None] sdS = sd[..., None] samples = norm.rvs( loc=muS, scale=sdS, size=(mu.shape[0], mu.shape[1], S), random_state=random_state ) return np.asarray(samples, dtype=float)
[docs] def ppf(self, q) -> np.ndarray: """ Vectorised inverse-CDF for nodes. q can be: - (S,) : same percentiles for every node → output (D, M, S) - (D, M, S) : per-node percentiles → output (D, M, S) Returns: (D, M, 1) if all std == 0, else (D, M, S) """ mu = np.asarray(self.nodes, dtype=float) sd = np.asarray(self.nodes_std, dtype=float) if mu.shape != sd.shape or mu.ndim != 2: raise ValueError("`nodes` and `nodes_std` must both be (D, M).") if np.all(sd == 0): return mu[..., None] # deterministic: ignore q q = np.asarray(q, dtype=float) if q.ndim == 1: # (S,) → (1,1,S) so it broadcasts with (D,M,1) q = q[None, None, :] elif q.ndim == 3: if q.shape[:2] != mu.shape: raise ValueError(f"`q` leading shape must be (D, M)={mu.shape} for 3D input.") else: raise ValueError("`q` must be shape (S,) or (D, M, S).") mu3 = mu[..., None] # (D, M, 1) sd3 = sd[..., None] # (D, M, 1) # norm.ppf(..., scale=0) yields NaN (scipy computes -inf * 0 for the # support bounds), so substitute a safe placeholder scale for # deterministic (std == 0) entries and overwrite them with the exact # node coordinate afterward, mirroring how rvs() already handles a # mix of zero and nonzero std correctly. safe_sd3 = np.where(sd3 == 0, 1.0, sd3) out = norm.ppf(q, loc=mu3, scale=safe_sd3) # full broadcast to (D, M, S) out = np.where(sd3 == 0, mu3, out) # If numerically constant across S (can happen with tiny std), compress if out.shape[2] > 1 and np.allclose(out, out[..., :1], rtol=0, atol=0): return out[..., :1] return out.astype(float, copy=False)
# ------------------------------------------------------------------ # Contiguity checks and combination # ------------------------------------------------------------------ def _contiguity_ok(self, other: "Geometry", atol: float = 1e-8) -> bool: """ Check if two profiles are contiguous (last node of self equals first node of other). Parameters ---------- other : Geometry Another geometry to check. atol : float Absolute tolerance for coordinates comparison. Returns ------- bool True if contiguous, False otherwise. """ return np.allclose(self.nodes[-1, :], other.nodes[0, :], atol=atol) def __add__(self, other: "Geometry") -> "Geometry": """ Combine two contiguous profiles into a new one. Raises ------ TypeError If `other` is not a Geometry. ValueError If profiles are not contiguous. """ if not isinstance(other, Geometry): raise TypeError(f"Unsupported operand type for +: 'Geometry' and '{type(other).__name__}'") if not self._contiguity_ok(other): raise ValueError("Geometries must be contiguous to add.") if not ( (self.attributes is None and other.attributes is None) or (self.attributes is not None and other.attributes is not None) ): raise ValueError("Cannot add geometries if one has attributes and the other doesn't.") offset = self._nodes.shape[1] - 1 new_nodes = np.concatenate([self._nodes, other._nodes[:, 1:]], axis=1) new_nodes_std = np.concatenate([self._nodes_std, other._nodes_std[:, 1:]], axis=1) new_elements = np.vstack([self._elements, other._elements + offset]) new_attributes = np.vstack([self._attributes, other._attributes]) if (self._attributes is not None) and (other._attributes is not None) else None return Geometry( nodes=new_nodes.T, nodes_std=new_nodes_std.T, elements=new_elements, attributes=new_attributes ) def __iadd__(self, other: "Geometry") -> "Geometry": """In-place version of :meth:`__add__`.""" if not isinstance(other, Geometry): raise TypeError(f"Unsupported operand type for +: 'Geometry' and '{type(other).__name__}'") if not self._contiguity_ok(other): raise ValueError("Geometries must be contiguous to add.") if not ( (self.attributes is None and other.attributes is None) or (self.attributes is not None and other.attributes is not None) ): raise ValueError("Cannot add geometries if one has attributes and the other doesn't.") offset = self._nodes.shape[1] - 1 self._nodes = np.concatenate([self._nodes, other._nodes[:, 1:]], axis=1) self._nodes_std = np.concatenate([self._nodes_std, other._nodes_std[:, 1:]], axis=1) self._elements = np.vstack([self._elements, other._elements + offset]) self._attributes = np.vstack([self._attributes, other._attributes]) if (self._attributes is not None) and (other._attributes is not None) else None return self def __repr__(self) -> str: return ( f"Geometry(num_nodes={self._nodes.shape[1]}, " f"num_elements={len(self._elements)})" )
[docs] def exitTime( self, p: np.ndarray, # (2, S) v: np.ndarray, # (2, S) elem_id: np.ndarray, # (S,) *, samples: Optional[np.ndarray] = None, # None or (2, M, 1) or (2, M, S) tol: float = 1e-12, ) -> Tuple[np.ndarray, np.ndarray]: """ 2D: time to exit current polyline segment along constant velocity. Returns (S,)-shaped arrays: next_el, t_exit """ elems = self.elements.astype(np.int32) # (E,2) S = p.shape[1] if elem_id.shape != (S,): raise ValueError("elem_id must have shape (S,) matching p,v.") # node indices per sample i_idx = elems[elem_id, 0] # (S,) j_idx = elems[elem_id, 1] # (S,) pts = self.getSamples(samples, S=S) # (2, M, S) ar = np.arange(S, dtype=np.int32) Ni = pts[:, i_idx, ar] # pairwise gather Nj = pts[:, j_idx, ar] # segment vectors & metrics Dvec = Nj - Ni # (2,S) L2 = np.sum(Dvec * Dvec, axis=0) # (S,) L = np.sqrt(L2) ok = L2 > tol d_hat = np.zeros_like(Dvec) d_hat[:, ok] = Dvec[:, ok] / L[ok] # param s along segment [0,1] from i→j s = np.zeros(S, dtype=float) s[ok] = np.sum((p[:, ok] - Ni[:, ok]) * Dvec[:, ok], axis=0) / L2[ok] s = np.clip(s, 0.0, 1.0) # signed speed along segment (+ toward j, − toward i) v_par = np.zeros(S, dtype=float) v_par[ok] = np.sum(v[:, ok] * d_hat[:, ok], axis=0) speed = np.abs(v_par) moving = ok & (speed > tol) toward_j = v_par > 0.0 end_node = np.where(toward_j, j_idx, i_idx).astype(np.int32) # remaining distance to endpoint dist_rem = np.empty(S, dtype=float) dist_rem[toward_j] = (1.0 - s[toward_j]) * L[toward_j] dist_rem[~toward_j] = s[~toward_j] * L[~toward_j] t_exit = np.full(S, np.nan, dtype=float) t_exit[moving] = np.where(dist_rem[moving] <= tol, 0.0, dist_rem[moving] / speed[moving]) # guess next element by ±1; verify it shares the exit node guess = np.where(toward_j, elem_id + 1, elem_id - 1).astype(np.int32) next_el = np.full(S, -1, dtype=np.int32) in_range = (guess >= 0) & (guess < elems.shape[0]) if np.any(in_range): a = elems[guess[in_range], 0] b = elems[guess[in_range], 1] shares = (a == end_node[in_range]) | (b == end_node[in_range]) idx = np.flatnonzero(in_range) acc = idx[shares] next_el[acc] = guess[acc] # rare fallback: dumb search for those where ±1 failed need = moving & (next_el == -1) if np.any(need): for k in np.flatnonzero(need): nd = int(end_node[k]) # all segments that touch node nd cand = np.flatnonzero((elems[:, 0] == nd) | (elems[:, 1] == nd)) cand = cand[cand != elem_id[k]] if cand.size: # pick the most aligned with v[:,k] nodes2 = pts[:, :, k] # (2,M) other = np.where(elems[cand, 0] == nd, elems[cand, 1], elems[cand, 0]) vecs = nodes2[:, other] - nodes2[:, nd][:, None] # (2,Nc) nrm = np.linalg.norm(vecs, axis=0) valid = nrm > tol if np.any(valid): dirs = vecs[:, valid] / nrm[valid][None, :] dots = dirs.T @ v[:, k] next_el[k] = cand[valid][np.argmax(dots)] return next_el, t_exit
[docs] def intersectParabolaMatrix( self, p: np.ndarray, # (2, S) v: np.ndarray, # (2, S) a: np.ndarray, # (2, S) elem_id: Optional[np.ndarray] = None, # kept for API symmetry *, samples: Optional[np.ndarray] = None, # (2, M, 1) or (2, M, S) tol: float = 1e-12, ) -> np.ndarray: """ Candidate parabola/polyline support-line intersection times. Returns a matrix with shape (M-1, S). Entries are np.inf where the parabola has no finite intersection with the segment's supporting line. Use feasibleImpactTime to filter by segment bounds and choose one hit per sample. """ p = np.asarray(p, dtype=float) v = np.asarray(v, dtype=float) a = np.asarray(a, dtype=float) if p.shape != v.shape or p.shape != a.shape or p.ndim != 2 or p.shape[0] != 2: raise ValueError("`p`, `v`, `a` must each be shape (2, S) with matching S.") S = p.shape[1] pts = self.getSamples(samples, S=S) # (2, M, S) A = pts[:, :-1, :] # (2, M-1, S) B = pts[:, 1:, :] # (2, M-1, S) E = B - A # (2, M-1, S) len2 = np.sum(E * E, axis=0) # (M-1, S) valid_seg = len2 > tol M1, S1 = len2.shape # Perpendicular vector to segment n_perp_x = -E[1] # (M-1, S) n_perp_y = E[0] # (M-1, S) # Quadratic coefficients for n_perp · (x(t) - A) = 0 # Broadcasting (M-1,S) * (S,) works: the (S,) broadcasts along columns. Acoef = (n_perp_x * a[0] + n_perp_y * a[1]) * 0.5 # (M-1, S) Bcoef = (n_perp_x * v[0] + n_perp_y * v[1]) # (M-1, S) Ccoef = (n_perp_x * (p[0][None, :] - A[0]) + n_perp_y * (p[1][None, :] - A[1])) # (M-1, S) # Candidate times (init to +inf) t = np.full((M1, S1), np.inf) # Quadratic roots where |A| > tol mask_quad = np.abs(Acoef) > tol if np.any(mask_quad): Ny = n_perp_y[mask_quad] Aq = Acoef[mask_quad] Bq = Bcoef[mask_quad] Cq = Ccoef[mask_quad] disc = Bq * Bq - 4.0 * Aq * Cq ok = disc >= -tol if np.any(ok): disc = np.maximum(disc, 0.0) sdisc = np.sqrt(disc) t1_vals = (-Bq - sdisc) / (2.0 * Aq) t2_vals = (-Bq + sdisc) / (2.0 * Aq) t_min_root = np.minimum(t1_vals, t2_vals) t_max_root = np.maximum(t1_vals, t2_vals) # Where element's normal points up get t_max t_root = np.where(Ny >= 0, t_max_root, t_min_root) t_root[~ok] = np.inf t[mask_quad] = t_root # Linear where ~quad and |B| > tol: B t + C = 0 mask_lin = (~mask_quad) & (np.abs(Bcoef) > tol) if np.any(mask_lin): t[mask_lin] = -Ccoef[mask_lin] / Bcoef[mask_lin] v2 = np.sum(v * v, axis=0) a2 = np.sum(a * a, axis=0) stationary = (v2 <= tol) & (a2 <= tol) stationary_on_line = ( stationary[None, :] & valid_seg & ~mask_quad & ~mask_lin & (np.abs(Ccoef) <= tol) ) t[stationary_on_line] = 0.0 return t
[docs] def feasibleImpactTime( self, t: np.ndarray, # (M-1, S) p: np.ndarray, # (2, S) v: np.ndarray, # (2, S) a: np.ndarray, # (2, S) *, samples: Optional[np.ndarray] = None, # (2, M, 1) or (2, M, S) t_min: float = 0.0, t_max: float = np.inf, tol: float = 1e-12, position: Optional[Callable[[np.ndarray], np.ndarray]] = None, ) -> Tuple[np.ndarray, np.ndarray]: """ Filter candidate impact times and choose the earliest feasible hit. The candidate matrix is tested against segment bounds, the time window, and then reduced to one segment/time pair per sample. """ t = np.asarray(t, dtype=float) p = np.asarray(p, dtype=float) v = np.asarray(v, dtype=float) a = np.asarray(a, dtype=float) if p.shape != v.shape or p.shape != a.shape or p.ndim != 2 or p.shape[0] != 2: raise ValueError("`p`, `v`, `a` must each be shape (2, S) with matching S.") if t.ndim != 2 or t.shape[1] != p.shape[1]: raise ValueError("`t` must have shape (M-1, S).") S = p.shape[1] pts = self.getSamples(samples, S=S) # (2, M, S) A = pts[:, :-1, :] # (2, M-1, S) B = pts[:, 1:, :] # (2, M-1, S) E = B - A # (2, M-1, S) len2 = np.sum(E * E, axis=0) # (M-1, S) valid_seg = len2 > tol if t.shape != len2.shape: raise ValueError(f"`t` must have shape {len2.shape}; got {t.shape}.") if position is None: def position(tarr: np.ndarray) -> np.ndarray: tt = tarr[None, :, :] return p[:, None, :] + v[:, None, :] * tt + 0.5 * a[:, None, :] * (tt * tt) tmask = np.isfinite(t) if np.any(tmask): safe_t = np.where(tmask, t, 0.0) x = position(safe_t) if x.shape != (2, *t.shape): raise ValueError("`position(t)` must return shape (2, M-1, S).") w = x - A len2_safe = np.where(valid_seg, len2, np.inf) with np.errstate(divide="ignore", invalid="ignore"): u = np.sum(E * w, axis=0) / len2_safe u_ok = (u >= -tol) & (u <= 1.0 + tol) & valid_seg & tmask & np.isfinite(u) t = np.where(u_ok, t, np.inf) else: t = np.full_like(t, np.inf) # Time window filter def _window(tarr: np.ndarray, dlt: float) -> np.ndarray: # close to min if np.isfinite(t_min): tarr = np.where(tarr < t_min - dlt, np.inf, tarr) # close to max if np.isfinite(t_max): tarr = np.where(tarr > t_max + dlt, np.inf, tarr) return tarr t_s = _window(t, -tol) # shrink window by tol if np.any(np.isfinite(t_s)): t = t_s else: t_e = _window(t, tol) # expand window by tol if np.isfinite(t_min): t = np.where(np.abs(t_e - t_min) < tol, t_min, t_e) else: t = t_e # Choose earliest candidate per sample seg_best = np.argmin(t, axis=0).astype(int) t_best = t[seg_best, np.arange(S)] # shape (S,) # Write back, handling no-hit nohit = ~np.isfinite(t_best) seg_best[nohit] = -1 t_best[nohit] = np.nan return seg_best, t_best
[docs] def intersectFloor( self, p: np.ndarray, # (D, S) v: np.ndarray, # (D, S) a: np.ndarray, # (D, S) floor: float, *, t_min: float = 0.0, tol: float = 1e-12, ) -> np.ndarray: """ Smallest t >= t_min at which the parabola (p, v, a) reaches the floor: p[-1] + v[-1] t + 0.5 a[-1] t^2 = floor. Returns an (S,) array of times, np.inf where no such time exists. """ p = np.asarray(p, dtype=float) v = np.asarray(v, dtype=float) a = np.asarray(a, dtype=float) py, vy, ay = p[-1], v[-1], a[-1] c = py - floor b = vy aq = 0.5 * ay t = np.full(p.shape[1], np.inf) mask_quad = np.abs(aq) > tol if np.any(mask_quad): Aq, Bq, Cq = aq[mask_quad], b[mask_quad], c[mask_quad] disc = Bq * Bq - 4.0 * Aq * Cq ok = disc >= -tol disc = np.maximum(disc, 0.0) sdisc = np.sqrt(disc) t1 = (-Bq - sdisc) / (2.0 * Aq) t2 = (-Bq + sdisc) / (2.0 * Aq) lo, hi = np.minimum(t1, t2), np.maximum(t1, t2) cand = np.where(lo >= t_min - tol, lo, hi) cand = np.where((cand >= t_min - tol) & ok, cand, np.inf) t[mask_quad] = cand mask_lin = (~mask_quad) & (np.abs(b) > tol) if np.any(mask_lin): tl = -c[mask_lin] / b[mask_lin] t[mask_lin] = np.where(tl >= t_min - tol, tl, np.inf) stationary_on_floor = (~mask_quad) & (~mask_lin) & (np.abs(c) <= tol) t[stationary_on_floor] = max(t_min, 0.0) return t
[docs] def intersectParabola( self, p: np.ndarray, # (2, S) v: np.ndarray, # (2, S) a: np.ndarray, # (2, S) elem_id: np.ndarray, # (S,) *, samples: Optional[np.ndarray] = None, # (2, M, 1) or (2, M, S) t_min: float = 0.0, t_max: float = np.inf, tol: float = 1e-12, ) -> Tuple[np.ndarray, np.ndarray]: """ Vectorised 2D intersection between parabolic trajectories and a polyline. Returns (seg_id, t) with shapes (S,), (S,). seg_id=-1 and t is non-finite for no hit. """ t = self.intersectParabolaMatrix( p, v, a, elem_id, samples=samples, tol=tol, ) return self.feasibleImpactTime( t, p, v, a, samples=samples, t_min=t_min, t_max=t_max, tol=tol, )
[docs] def intersectDamped( self, p: np.ndarray, # (2, S) v: np.ndarray, # (2, S) a: np.ndarray, # (2, S), constant acceleration, e.g. [[0],[-g]] damping: np.ndarray, # (S,), damping coefficients elem_id: np.ndarray, *, samples: Optional[np.ndarray] = None, t_min: float = 0.0, t_max: float = np.inf, t_ref: Optional[np.ndarray] = None, tol: float = 1e-12, max_expand: int = 32, maxiter: int = 100, ) -> Tuple[np.ndarray, np.ndarray]: """ Vectorised 2D intersection between damped trajectories and a polyline. Solves, for every segment m and trajectory sample s, n_m · (r_s(t) - A_m) = 0 where r'(t) = v(t) v'(t) = a - damping * v Returns ------- seg_id, t : Tuple[np.ndarray, np.ndarray] Shapes (S,), (S,). seg_id=-1 and t=nan where no valid segment intersection was found. """ # ------------------------------------------------------------------ # Linear damping trajectory: # # v'(t) = a - D v # # v(t) = a/D + (v0 - a/D) exp(-Dt) # # r(t) = p0 + a/D * t # + (v0 - a/D) / D * (1 - exp(-Dt)) # ------------------------------------------------------------------ p = np.asarray(p, dtype=float) v = np.asarray(v, dtype=float) a = np.asarray(a, dtype=float) if p.shape != v.shape or p.shape != a.shape or p.ndim != 2 or p.shape[0] != 2: raise ValueError("`p`, `v`, and `a` must have shape (2, S).") S = p.shape[1] D = np.asarray(damping, dtype=float) if D.shape != (S,): raise ValueError(f"`damping` must be a scalar or have shape (S,)={(S,)}.") if np.any(D <= 0.0): raise ValueError("`damping` must be positive for damped motion.") pts = self.getSamples(samples, S=S) # (2, M, S) A = pts[:, :-1, :] # (2, M-1, S) B = pts[:, 1:, :] # (2, M-1, S) E = B - A # (2, M-1, S) len2 = np.sum(E * E, axis=0) # (M-1, S) valid_seg = len2 > tol M1, S1 = len2.shape def r_damped(t: np.ndarray) -> np.ndarray: """ t shape: (M-1, S) returns X shape: (2, M-1, S) """ tt = t[None, :, :] # (1, M-1, S) Dv = D[None, None, :] e = np.exp(-Dv * tt) drift = a[:, None, :] / Dv transient = (v[:, None, :] - drift) / Dv return p[:, None, :] + drift * tt + transient * (1.0 - e) # Segment-line residual nx = -E[1] # (M-1, S) ny = E[0] # (M-1, S) def f_mat(t: np.ndarray) -> np.ndarray: X = r_damped(t) return nx * (X[0] - A[0]) + ny * (X[1] - A[1]) # brentq expects flat arrays shape = (M1, S1) def f_flat(x: np.ndarray) -> np.ndarray: return f_mat(x.reshape(shape)).ravel() # ------------------------------------------------------------------ # Reference time # ------------------------------------------------------------------ if t_ref is None: t_ref = self.intersectParabolaMatrix( p, v, a, elem_id, samples=samples, tol=tol, ) else: t_ref = np.asarray(t_ref, dtype=float) if t_ref.shape == (S,): t_ref = np.broadcast_to(t_ref[None, :], shape).copy() elif t_ref.shape != shape: raise ValueError(f"`t_ref` must have shape (S,) or {shape}.") finite_ref = np.isfinite(t_ref) & (t_ref > tol) if np.any(finite_ref): dt0 = np.nanmedian(t_ref[finite_ref]) else: dt0 = 1.0 t_step = np.where(finite_ref, t_ref, dt0) t_step = np.maximum(t_step, tol) # ------------------------------------------------------------------ # Bracket construction # ------------------------------------------------------------------ t_l = np.full(shape, t_min, dtype=float) t_u = t_l + t_step if np.isfinite(t_max): t_u = np.minimum(t_u, t_max) f_l = f_mat(t_l) f_u = f_mat(t_u) active = valid_seg & np.isfinite(f_l) & np.isfinite(f_u) # Exact roots at lower/upper bounds root_at_l = active & (np.abs(f_l) <= tol) root_at_u = active & (np.abs(f_u) <= tol) bracketed = active & (f_l * f_u < 0.0) for _ in range(max_expand): bad = active & ~root_at_l & ~root_at_u & ~bracketed if np.isfinite(t_max): bad &= t_u < t_max if not np.any(bad): break t_l = np.where(bad, t_u, t_l) f_l = np.where(bad, f_u, f_l) t_u_new = t_u + t_step t_step = np.where(bad, 2.0 * t_step, t_step) if np.isfinite(t_max): t_u_new = np.minimum(t_u_new, t_max) t_u = np.where(bad, t_u_new, t_u) f_u = np.where(bad, f_mat(t_u), f_u) root_at_l = active & (np.abs(f_l) <= tol) root_at_u = active & (np.abs(f_u) <= tol) bracketed = active & (f_l * f_u < 0.0) # ------------------------------------------------------------------ # Brent solve # ------------------------------------------------------------------ t = np.full(shape, np.inf, dtype=float) t[root_at_l] = t_l[root_at_l] t[root_at_u] = t_u[root_at_u] solve = bracketed & ~root_at_l & ~root_at_u if np.any(solve): aa = np.where(solve, t_l, np.nan) bb = np.where(solve, t_u, np.nan) roots, converged, _ = brentq( f_flat, aa, bb, xtol=tol, ftol=tol, maxiter=maxiter, ) t = np.where(solve & converged & np.isfinite(roots), roots, t) return self.feasibleImpactTime( t, p, v, a, samples=samples, t_min=t_min, t_max=t_max, tol=tol, position=r_damped, )
def _dim(self) -> int: """Return geometry dimension D from self.nodes shape (N, D).""" pts = np.asarray(self.nodes) # (N, D) if pts.ndim != 2: raise ValueError("`self.nodes` must be 2D with shape (N, D).") return int(pts.shape[1])
[docs] def getSamples( self, samples: Optional[np.ndarray], *, S: Optional[int] = None, dtype: np.dtype | type = float, ) -> np.ndarray: """ Return samples with shape (D, M, S). If `samples` is None, derive from self.points. - If samples is None -> use self.points.T[:, :, None] -> (D, M, 1) - If last axis is 1 and S is provided -> repeat along last axis to (D, M, S) - If last axis is not in {1, S} -> error """ D = self._dim() if samples is None: # self.nodes is (M, D); want (D, M, 1) pts = np.asarray(self.nodes, dtype=dtype, order="C") out = pts.T[:, :, None] else: out = np.asarray(samples, dtype=dtype, order="C") if out.ndim != 3 or out.shape[0] != D: raise ValueError(f"`samples` must have shape (D={D}, M, 1 or S). Got {out.shape}.") if S is not None: if out.shape[2] == 1: # broadcast single realisation to all S samples out = np.repeat(out, repeats=S, axis=2) elif out.shape[2] != S: raise ValueError(f"`samples.shape[2]` must be 1 or S={S}. Got {out.shape[2]}.") return np.ascontiguousarray(out)
def _find_vertical_global( self, p: np.ndarray, direction: int, samples: np.ndarray, ) -> tuple[np.ndarray, np.ndarray]: """ Global vertical search over all elements for each sample. Parameters ---------- p : (2, S) float Query points. direction : int -1 under, 0 both, 1 above samples : (2, M, S) float Slope node coordinates. Returns ------- elem_id : (S,) int point : (2, S) float """ p = np.asarray(p, dtype=float) samples = np.asarray(samples, dtype=float) S = p.shape[1] E = self._elements.shape[0] x = p[0] # (S,) y = p[1] # (S,) ei = self._elements[:, 0] ej = self._elements[:, 1] xi = samples[0, ei, :] # (E, S) yi = samples[1, ei, :] xj = samples[0, ej, :] yj = samples[1, ej, :] xmin = np.minimum(xi, xj) xmax = np.maximum(xi, xj) spans_x = (xmin <= x[None, :]) & (x[None, :] <= xmax) dx = xj - xi t = np.divide( x[None, :] - xi, dx, out=np.full((E, S), 0.5, dtype=float), where=np.abs(dx) > 0.0, ) y_seg = yi + t * (yj - yi) if direction == -1: valid = spans_x & (y_seg <= y[None, :]) dist = np.where(valid, y[None, :] - y_seg, np.inf) elif direction == 1: valid = spans_x & (y_seg >= y[None, :]) dist = np.where(valid, y_seg - y[None, :], np.inf) else: # direction == 0 valid = spans_x dist = np.where(valid, np.abs(y_seg - y[None, :]), np.inf) idx = np.argmin(dist, axis=0) # (S,) best = dist[idx, np.arange(S)] # (S,) found = np.isfinite(best) out_elem = np.full(S, -1, dtype=np.int64) out_elem[found] = idx[found] out_point = np.full((2, S), np.nan, dtype=float) out_point[0, found] = x[found] out_point[1, found] = y_seg[idx[found], np.arange(S)[found]] return out_elem, out_point def _find_vertical_from_elem( self, p: np.ndarray, element_id: np.ndarray, neighbour_check: int, direction: int, samples: np.ndarray, ) -> tuple[np.ndarray, np.ndarray]: """ Local vertical search from seed elements, with limited neighbour traversal. Parameters ---------- p : (2, S) float Query points. element_id : (S,) int Seed elements. Must all be >= 0. neighbour_check : int Maximum number of neighbour traversals. direction : int -1 under, 0 both, 1 above samples : (2, M, S) float Slope node coordinates. Returns ------- elem_id : (S,) int Found element ids, or -1 where unresolved locally. point : (2, S) float Projection points, or nan where unresolved. """ p = np.asarray(p, dtype=float) curr = np.asarray(element_id, dtype=np.int64).copy() samples = np.asarray(samples, dtype=float) S = p.shape[1] x = p[0] y = p[1] out_elem = np.full(S, -1, dtype=np.int64) out_point = np.full((2, S), np.nan, dtype=float) active = curr >= 0 # number of segment evaluations = initial + neighbour_check moves for _ in range(neighbour_check + 1): work = np.where(active)[0] if work.size == 0: break e = curr[work] ni = self._elements[e, 0] nj = self._elements[e, 1] xi = samples[0, ni, work] yi = samples[1, ni, work] xj = samples[0, nj, work] yj = samples[1, nj, work] xmin = np.minimum(xi, xj) xmax = np.maximum(xi, xj) left = x[work] < xmin right = x[work] > xmax inside = ~(left | right) # evaluate segments that span x if np.any(inside): idx = work[inside] e_in = curr[idx] ni = self._elements[e_in, 0] nj = self._elements[e_in, 1] xi = samples[0, ni, idx] yi = samples[1, ni, idx] xj = samples[0, nj, idx] yj = samples[1, nj, idx] dx = xj - xi t = np.divide( x[idx] - xi, dx, out=np.full(idx.shape, 0.5, dtype=float), where=np.abs(dx) > 0.0, ) y_seg = yi + t * (yj - yi) if direction == -1: ok = y_seg <= y[idx] elif direction == 1: ok = y_seg >= y[idx] else: # direction == 0 ok = np.ones(idx.shape, dtype=bool) if np.any(ok): idx_ok = idx[ok] out_elem[idx_ok] = curr[idx_ok] out_point[0, idx_ok] = x[idx_ok] out_point[1, idx_ok] = y_seg[ok] # whether accepted or rejected, local search ends here active[idx] = False # move left/right for unresolved active samples move_mask = active[work] if np.any(move_mask): idx_move = work[move_mask] e_move = curr[idx_move] ni = self._elements[e_move, 0] nj = self._elements[e_move, 1] xi = samples[0, ni, idx_move] xj = samples[0, nj, idx_move] xmin = np.minimum(xi, xj) xmax = np.maximum(xi, xj) left = x[idx_move] < xmin right = x[idx_move] > xmax if np.any(left): idx_left = idx_move[left] curr[idx_left] = self.neighbours[curr[idx_left], 0] if np.any(right): idx_right = idx_move[right] curr[idx_right] = self.neighbours[curr[idx_right], 1] active[idx_move] = curr[idx_move] >= 0 return out_elem, out_point
[docs] def verticalProjection( self, p: np.ndarray, element_id: np.ndarray, direction: int = 0, neighbour_check: int = 1, samples: Optional[np.ndarray] = None, ) -> tuple[np.ndarray, np.ndarray]: """ Find, for each sample, the closest segment vertically aligned with p. Parameters ---------- p : (2, S) float Query points. element_id : (S,) int Seed element(s). If -1, performs a global search. direction : int Search direction: -1 -> only under 0 -> both 1 -> only above neighbour_check : int Maximum number of neighbour traversals before giving up and letting the caller trigger global search. samples : (2, M, 1) or (2, M, S) float, optional Slope node coordinates. Returns ------- elem_id : (S,) int Segment id for each sample, or -1 if none. point : (2, S) float Vertical projection point on that segment, or nan if none. """ if direction not in (-1, 0, 1): raise ValueError("direction must be -1, 0, or 1") p = np.asarray(p, dtype=float) element_id = np.asarray(element_id, dtype=np.int64) if p.ndim != 2 or p.shape[0] != 2: raise ValueError("p must have shape (2, S)") if element_id.shape != (p.shape[1],): raise ValueError("element_id must have shape (S,)") S = p.shape[1] pts = self.getSamples(samples, S=S) # (2, M, S) out_element_id = np.full(S, -1, dtype=np.int64) out_point = np.full((2, S), np.nan, dtype=float) seeded = element_id > -1 if np.any(seeded): e_loc, p_loc = self._find_vertical_from_elem( p[:, seeded], element_id[seeded], neighbour_check=neighbour_check, direction=direction, samples=pts[:, :, seeded], ) out_element_id[seeded] = e_loc out_point[:, seeded] = p_loc unresolved = out_element_id == -1 if np.any(unresolved): e_glob, p_glob = self._find_vertical_global( p[:, unresolved], direction=direction, samples=pts[:, :, unresolved], ) out_element_id[unresolved] = e_glob out_point[:, unresolved] = p_glob return out_element_id, out_point