Source code for pyrockfall._utils

"""
Utility functions for geometric and mesh operations in model3d.

Includes helpers for angles, mesh processing, and PLY scalar field reading.
"""

import struct
import math as m
import numpy as np
from scipy.linalg import lstsq
from scipy.spatial import cKDTree

from typing import Tuple, List, Callable
from numpy.typing import NDArray

[docs] def rot2AlignWithX(direction: np.ndarray) -> np.ndarray: """Return a 3x3 rotation matrix R such that v_rot = v @ R aligns `direction` to +X.""" d = np.asarray(direction, dtype=float) r = np.linalg.norm(d) if r == 0: raise ValueError("direction must be non-zero") d = d / r # angles as in your code phi = np.arccos(np.clip(d[2], -1.0, 1.0)) theta = np.arctan2(d[1], d[0]) c, s = np.cos(-theta), np.sin(-theta) Rz = np.array([[c, -s, 0.0], [s, c, 0.0], [0.0, 0.0, 1.0]]) b = np.pi * 0.5 - phi c, s = np.cos(b), np.sin(b) Ry = np.array([[ c, 0.0, -s], [0.0, 1.0, 0.0], [ s, 0.0, c]]) # Apply z-then-y as in your original sequence: v' = v @ (Rz @ Ry) return Rz @ Ry
[docs] def angleBetweenVectors(cc: np.ndarray, cn: np.ndarray) -> float: """ Calculate the angle between two vectors. Args: cc (numpy.ndarray): The first vector. cn (numpy.ndarray): The second vector. Returns: float: The angle between the two vectors in degrees. """ nvp = (cn.dot(cc))/(np.linalg.norm(cn)*np.linalg.norm(cc)) if nvp > 1: nvp = 1 elif nvp < -1: nvp = -1 angle_s = m.degrees(m.acos(nvp)) return angle_s
[docs] def triangleCentroids(coords, connect): """ Compute centroids for triangles defined by connectivity. Args: coords (np.ndarray): Vertex coordinates. connect (np.ndarray): Connectivity indices. Returns: np.ndarray: Centroids of each triangle. """ return np.mean(coords[connect], axis=1)
[docs] def getSubMesh(V: np.ndarray, T: np.ndarray, ids: np.ndarray) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: """ Extract a sub-mesh by keeping only the given triangles, dropping any vertex not referenced by them and remapping connectivity accordingly. Args: V: Vertex coordinates, shape (N, D). T: Triangle connectivity, shape (E, 3) (0-based indices into V). ids: Indices of triangles (rows of T) to keep. Returns: V_sub: Reindexed vertex coordinates, shape (N_sub, D). T_sub: Reindexed triangle connectivity, shape (len(ids), 3), indices into V_sub. vertex_ids: Original vertex indices kept, shape (N_sub,), such that ``V_sub == V[vertex_ids]``. """ V = np.asarray(V) T = np.asarray(T) ids = np.asarray(ids, dtype=np.int64) T_kept = T[ids] vertex_ids, inverse = np.unique(T_kept, return_inverse=True) V_sub = V[vertex_ids] T_sub = inverse.reshape(T_kept.shape).astype(T.dtype) return V_sub, T_sub, vertex_ids
[docs] def build_neighbours_mesh(triangles: np.ndarray) -> np.ndarray: """ Build triangle adjacency for a triangle mesh. Parameters ---------- triangles : (E, 3) int array (0-based) Each row is a face with three vertex indices. Returns ------- neighbours : (E, 3) int32 For each face, neighbour across the edge opposite local vertex 0, 1, 2. -1 indicates a boundary (no neighbouring face). """ tri = np.asarray(triangles, dtype=np.int64) if tri.ndim != 2 or tri.shape[1] != 3: raise ValueError("triangles must be (M,3)") M = tri.shape[0] N = np.int64(int(tri.max()) + 1) # Directed edges in local order: (v0->v1), (v1->v2), (v2->v0) u = tri[:, [0, 1, 2]].reshape(-1) # (E,) v = tri[:, [1, 2, 0]].reshape(-1) # (E,) E = u.size owner = np.repeat(np.arange(M, dtype=np.int64), 3) # triangle per edge # Directed keys and reversed keys key_fwd = u * N + v key_rev = v * N + u # Sort forward keys once, then lookup reversed keys via searchsorted order = np.argsort(key_fwd) key_sorted = key_fwd[order] pos = np.searchsorted(key_sorted, key_rev, side="left") # Only index key_sorted where pos is in-bounds inbounds = pos < E match = np.zeros_like(pos, dtype=bool) match[inbounds] = (key_sorted[pos[inbounds]] == key_rev[inbounds]) # Triangle that owns the matching reversed edge partner_owner = np.full(E, -1, dtype=np.int64) # Only read from 'order' and 'pos' where we actually have a match hit = inbounds & match partner_owner[hit] = owner[order[pos[hit]]] # Drop self-matches (degenerate tri like [0,0,0] yields self edges) valid = hit & (partner_owner != owner) # Fill neighbours (flattened edge order mirrors u/v construction) neigh_flat = np.full(E, -1, dtype=np.int64) neigh_flat[valid] = partner_owner[valid] neighbours = neigh_flat.reshape(M, 3).astype(np.int32, copy=False) return neighbours
[docs] def build_neighbours_polygon(elements: np.ndarray) -> np.ndarray: """ Build segment adjacency for a polygonal chain / polygon. Parameters ---------- elements : (E, 2) int array Each row is a segment with two vertex indices. Returns ------- neighbours : (E, 2) int32 neighbours[i, 0] = segment adjacent through elements[i, 0] neighbours[i, 1] = segment adjacent through elements[i, 1] -1 indicates a boundary node. Notes ----- This assumes a manifold 1D topology: each node is connected to at most two segments. """ elems = np.asarray(elements, dtype=np.int64) if elems.ndim != 2 or elems.shape[1] != 2: raise ValueError("elements must be of shape (E, 2)") if elems.size == 0: return np.empty((0, 2), dtype=np.int32) E = elems.shape[0] n_nodes = int(elems.max()) + 1 # Flatten node incidence: # seg_id[k] is incident to node_id[k] at local endpoint loc[k] (0 or 1) node_id = elems.reshape(-1) # (2E,) seg_id = np.repeat(np.arange(E, dtype=np.int64), 2) loc = np.tile(np.array([0, 1], dtype=np.int64), E) # Sort by node so equal nodes are grouped order = np.argsort(node_id) node_sorted = node_id[order] seg_sorted = seg_id[order] loc_sorted = loc[order] # Find groups of equal nodes start = np.flatnonzero(np.r_[True, node_sorted[1:] != node_sorted[:-1]]) end = np.r_[start[1:], len(node_sorted)] count = end - start # Result neighbours = np.full((E, 2), -1, dtype=np.int64) # Boundary nodes: only one incident segment mask1 = count == 1 if np.any(mask1): idx = start[mask1] neighbours[seg_sorted[idx], loc_sorted[idx]] = -1 # Regular nodes: exactly two incident segments mask2 = count == 2 if np.any(mask2): idx0 = start[mask2] idx1 = idx0 + 1 s0, l0 = seg_sorted[idx0], loc_sorted[idx0] s1, l1 = seg_sorted[idx1], loc_sorted[idx1] neighbours[s0, l0] = s1 neighbours[s1, l1] = s0 # Non-manifold nodes: more than two incident segments if np.any(count > 2): bad_nodes = node_sorted[start[count > 2]] raise ValueError( f"Non-manifold polygon: nodes {bad_nodes.tolist()} belong to more than two segments." ) return neighbours.astype(np.int32, copy=False)
[docs] def angle(vert): """ Calculate the rotation angle needed to align a set of 3D points with a reference axis. Args: vert (np.ndarray): Array of vertex coordinates. Returns: float: The rotation angle in degrees. """ # averaged normalised plane normal + coefficients of plane equation Z = coeff[0]*X + coeff[1]*Y + coeff[2] v = np.asarray(vert) A = np.c_[v[:,0], v[:,1], np.ones(v.shape[0])] coeff,_,_,_ = lstsq(A, v[:,2]) # type: ignore # coefficients # plane equation ax+by+cz+d = 0 a = coeff[0] b = coeff[1] c = -1 # d = coeff[2] n = [a,b,c] n = np.array(n)/np.linalg.norm(n) ## normal has to point in positive y-direction if (b<0): n = -n coeff = -coeff # project normal vector in x-y plane zDirection = n[2] n[2]=0 n = n/np.linalg.norm(n) n_ref = np.array([0,1,0]) # reference normal #compute angle a_rotate = np.sign(n[0])*angleBetweenVectors(n, n_ref) # special case: if zDirection<0: print('Add 180 deg rotation to wall') a_rotate=a_rotate-180 return a_rotate
[docs] def read_scalar_field_from_ply(filename): """ Read a scalar field from a PLY file. Args: filename (str): Path to the PLY file. Returns: np.ndarray: Scalar field as a numpy array. """ with open(filename, 'rb') as f: # Read the header reading_vertices = False num_float_properties = 0 num_double_properties = 0 num_uchar_properties = 0 line_format = '' scalar_field_index = 0 scalar_field_index_found = False while True: line = f.readline().decode('utf-8') if 'format ascii' in line: format = 'ascii' elif 'format binary' in line: format = 'binary' elif 'element vertex' in line: n_vertices = int(line.split()[-1]) reading_vertices = True elif 'element face' in line: reading_vertices = False elif 'end_header' in line: reading_vertices = False break if reading_vertices: if 'property' in line: if 'float' in line: num_float_properties += 1 line_format += 'f' elif 'double' in line: num_double_properties += 1 line_format += 'd' elif 'uchar' in line: num_uchar_properties += 1 line_format += 'B' if 'scalar' in line: scalar_field_index_found = True if not scalar_field_index_found: scalar_field_index += 1 if not scalar_field_index_found: return np.array([]) # Read the scalar field scalar_field = [] for n in range(n_vertices): # Each vertex is represented by three floats (x, y, z) and one float (scalar field) if format == 'binary': try: data = f.read(4*num_float_properties + 8*num_double_properties + num_uchar_properties) if not data: raise ValueError('Error in extracting scalar field from file.') values = struct.unpack(line_format, data) scalar_field.append(values[scalar_field_index]) except struct.error: raise ValueError('Error in extracting scalar field from file.') elif format == 'ascii': try: line = f.readline().decode('utf-8') values = list(map(float, line.split())) scalar_field.append(values[scalar_field_index]) except ValueError: raise ValueError('Error in extracting scalar field from file.') return np.array(scalar_field)
[docs] def uniqueMaterialList(seq): """ Unify objects from the input list while preserving their order. Generic identity-based deduplication; used both for lists of Material (Slope) and lists of Drag (Vegetation). Returns ------- table : Unique objects in first-appearance order. ids : (E,) int32 Per-element indices into the returned table. """ table = [] index = {} ids = np.empty(len(seq), dtype=np.int32) for i, mobj in enumerate(seq): key = id(mobj) # identity-based; change to value-based if you prefer j = index.get(key) if j is None: j = len(table) table.append(mobj) index[key] = j ids[i] = j return table, ids
[docs] def getTriPoints(points: np.ndarray, triangle: np.ndarray, samples: np.ndarray) -> np.ndarray: i0, i1, i2 = triangle[:, 0], triangle[:, 1], triangle[:, 2] if points.shape[2] == 1: # same coordinates for all samples v0 = points[:, i0, 0] # (3,K) v1 = points[:, i1, 0] v2 = points[:, i2, 0] else: # per-sample perturbed coordinates: gather pairwise (node,sample) v0 = points[:, i0, samples] # (3,K) v1 = points[:, i1, samples] v2 = points[:, i2, samples] return np.stack((v0, v1, v2), axis=1) # (3,3,K)
[docs] def isInsideTriangle(p: np.ndarray, points: np.ndarray, tol: float = 1e-12) -> np.ndarray: """ Edge-function inside test using cross products in 3D. Inputs: p : (3, S) points to test (columns are samples) points : (3, 3, S) or (3, 3, 1) triangle vertices per sample: v0 = points[:, 0, :], v1 = points[:, 1, :], v2 = points[:, 2, :] If points.shape[2] == 1, it's broadcast to S. tol : tolerance for inside test Returns: idx : (S,) int32 -1 if inside (or on edges within tol), 0 if outside “to the right” of edge v0->v1, 1 if outside “to the right” of edge v1->v2, 2 if outside “to the right” of edge v2->v0. Notes: - The triangle winding defines “right/left”. Winding is v0->v1->v2; the normal is n = (v1-v0)×(v2-v0). - We do not normalise n; signs are preserved and it’s faster. """ p = np.asarray(p, dtype=float) assert p.shape[0] == 3 num_samples = p.shape[1] points = np.asarray(points, dtype=float) if points.shape[:2] != (3, 3) or points.ndim != 3: raise ValueError("points must have shape (3, 3, S) or (3, 3, 1)") if points.shape[2] == 1: points = np.repeat(points, num_samples, axis=2) elif points.shape[2] != num_samples: raise ValueError("points.shape[2] must be 1 or S to match p") v0 = points[:, 0, :] # (3, S) v1 = points[:, 1, :] v2 = points[:, 2, :] # Triangle normal (not normalised) n = np.cross((v1 - v0).T, (v2 - v0).T).T # (3, S) # Edge vectors per the v0->v1->v2 winding e01 = v1 - v0 # edge 0 (v0->v1) e12 = v2 - v1 # edge 1 (v1->v2) e20 = v0 - v2 # edge 2 (v2->v0) # Vectors from edge anchors to query point w0 = p - v0 # for edge 0 anchor v0 w1 = p - v1 # for edge 1 anchor v1 w2 = p - v2 # for edge 2 anchor v2 # Signed edge tests: s = n · (edge × (p - anchor)) c0 = np.cross(e01.T, w0.T).T c1 = np.cross(e12.T, w1.T).T c2 = np.cross(e20.T, w2.T).T s0 = np.einsum('ij,ij->j', n, c0) # edge 0 (v0->v1) s1 = np.einsum('ij,ij->j', n, c1) # edge 1 (v1->v2) s2 = np.einsum('ij,ij->j', n, c2) # edge 2 (v2->v0) inside = (s0 >= -tol) & (s1 >= -tol) & (s2 >= -tol) # Choose the most-negative edge (the one to cross) edge = np.argmin(np.vstack((s0, s1, s2)), axis=0).astype(np.int32) edge[inside] = -1 return edge
[docs] def timeParabolaPlane( p: np.ndarray, v: np.ndarray, a: np.ndarray, points: np.ndarray, tol: float = 1e-12, t_min: float = 0.0, t_max: float = np.inf, ) -> np.ndarray: """ Vectorised intersection: x(t)=p + v t + 0.5 a t^2 with plane(points[:,0],points[:,1],points[:,2]). Inputs p, v, a : (3, s) points : (3, 3, s) or (3, 3, 1) tol : tolerance used in plane/roots checks t_min : lower time window (inclusive, with tolerance) t_max : upper time window (inclusive, with tolerance) Returns t_hit : (s,) time to hit in [t_min, t_max] (np.inf if none) Quadratic selection when both roots acceptable: choose min(root) if min(root) > t_min + tol, else choose max(root). """ p = np.asarray(p, dtype=float); v = np.asarray(v, dtype=float); a = np.asarray(a, dtype=float) assert p.shape[0] == v.shape[0] == a.shape[0] == 3 s = p.shape[1] points = np.asarray(points, dtype=float) if points.shape[:2] != (3, 3) or points.ndim != 3: raise ValueError("points must have shape (3, 3, s) or (3, 3, 1)") if points.shape[2] == 1: points = np.repeat(points, s, axis=2) elif points.shape[2] != s: raise ValueError("points.shape[2] must be 1 or s") p1 = points[:, 0, :] # (3, s) p2 = points[:, 1, :] p3 = points[:, 2, :] # plane n·x + d = 0 e0 = p2 - p1 e1 = p3 - p1 n = np.cross(e0.T, e1.T).T # (3, s) nn = np.linalg.norm(n, axis=0) valid_plane = nn > tol n[:, valid_plane] /= nn[valid_plane] d = -np.sum(n * p1, axis=0) # coefficients: alpha t^2 + beta t + gamma = 0 alpha = 0.5 * np.einsum('ij,ij->j', a, n) # (s,) beta = np.einsum('ij,ij->j', v, n) gamma = np.einsum('ij,ij->j', p, n) + d v_small = np.linalg.norm(v, axis=0) <= tol a_small = np.linalg.norm(a, axis=0) <= tol plane_tol = max(10.0 * tol, 1e-12) t_hit = np.full(s, np.inf, dtype=float) # helper: accept times within window (with tolerance) def _in_window(t): return (t >= (t_min - tol)) & (t <= (t_max + tol)) # ----- stationary: v≈0 & a≈0 ----- stat = valid_plane & v_small & a_small if np.any(stat): on_plane = np.abs(gamma) <= plane_tol # whole trajectory lies on plane → choose t_min if within window if np.isfinite(t_min): t_hit[stat & on_plane] = t_min # ----- linear: alpha≈0 & beta!=0 ----- lin = valid_plane & (np.abs(alpha) <= tol) & (np.abs(beta) > tol) if np.any(lin): t_lin = -gamma[lin] / beta[lin] acc = _in_window(t_lin) if np.any(acc): chosen = np.clip(t_lin[acc], t_min, t_max) t_hit[np.flatnonzero(lin)[acc]] = chosen # ----- constant/parallel (excluding stationary): alpha≈0 & beta≈0 ----- const = valid_plane & (np.abs(alpha) <= tol) & (np.abs(beta) <= tol) & ~stat if np.any(const): on_plane = np.abs(gamma[const]) <= plane_tol if np.any(on_plane): # intersects for all t → choose t_min if within window if np.isfinite(t_min): t_hit[np.flatnonzero(const)[on_plane]] = t_min # ----- quadratic: alpha != 0 ----- quad = valid_plane & (np.abs(alpha) > tol) if np.any(quad): a2 = alpha[quad] b = beta[quad] c = gamma[quad] disc = b*b - 4.0*a2*c real = disc >= -tol # allow tiny negative due to round-off if np.any(real): idx = np.flatnonzero(quad)[real] r = np.sqrt(np.maximum(disc[real], 0.0)) denom = 2.0 * a2[real] t1 = (-b[real] - r) / denom t2 = (-b[real] + r) / denom t_small = np.minimum(t1, t2) t_large = np.maximum(t1, t2) # Get larger root if element's normal points up t_final = np.where(n[-1] >= 0, t_large, t_small) acc = _in_window(t_final) t_hit[idx[acc]] = np.clip(t_final[acc], t_min, t_max) return t_hit
[docs] def timeRaySegment( p: np.ndarray, v: np.ndarray, points: np.ndarray, tol: float = 1e-12, t_min: float = 0.0, t_max: float = np.inf, ) -> np.ndarray: """ Vectorised time t such that p + v*t intersects segment [p1, p2]. Inputs p, v : (3, S) points : (3, 2, S) or (3, 2, 1) with points[:,0,:]=p1, points[:,1,:]=p2 tol : absolute tolerance t_min : lower time window (inclusive with tol) t_max : upper time window (inclusive with tol) Returns t_hit : (S,) time of intersection; np.inf if no intersection. Parallel rule: if line ∥ segment and p at t=0 lies on the *finite* segment, return 0 (then windowed/clipped); else inf. """ p = np.asarray(p, dtype=float); v = np.asarray(v, dtype=float) assert p.shape[0] == v.shape[0] == 3 s = p.shape[1] pts = np.asarray(points, dtype=float) if pts.ndim != 3 or pts.shape[:2] != (3, 2): raise ValueError("points must be (3, 2, S) or (3, 2, 1)") if pts.shape[2] == 1: pts = np.repeat(pts, s, axis=2) elif pts.shape[2] != s: raise ValueError("points.shape[2] must be 1 or S") p1 = pts[:, 0, :] # (3, S) p2 = pts[:, 1, :] e = p2 - p1 # (3, S) segment direction w = p1 - p # (3, S) from line point to seg start # Helper: time window predicate and clipper def in_window(t): return (t >= (t_min - tol)) & (t <= (t_max + tol)) def clamp_window(t): return np.clip(t, t_min, t_max) # Cross and triple products n = np.cross(v.T, e.T).T # (3, S) n = v × e n2 = np.sum(n * n, axis=0) # (S,) ||v×e||^2 # Parallel if ||v×e|| ~ 0 parallel = n2 <= tol t_hit = np.full(s, np.inf, dtype=float) # --- Non-parallel branch --- np_mask = ~parallel if np.any(np_mask): nm = n[:, np_mask] n2m = n2[np_mask] wm = w[:, np_mask] em = e[:, np_mask] pm = p[:, np_mask] vm = v[:, np_mask] # Coplanarity: (p1 - p) · (v × e) ~ 0 cop = np.abs(np.sum(wm * nm, axis=0)) <= tol if np.any(cop): idx = np.flatnonzero(np_mask)[cop] nm = nm[:, cop]; n2c = n2m[cop] wm2 = wm[:, cop]; em2 = em[:, cop]; vm2 = vm[:, cop] # t = ((w × e) · n) / ||n||^2 t_num = np.sum(np.cross(wm2.T, em2.T).T * nm, axis=0) t = t_num / n2c # u = ((w × v) · n) / ||n||^2 (segment parameter) u_num = np.sum(np.cross(wm2.T, vm2.T).T * nm, axis=0) u = u_num / n2c on_seg = (u >= -tol) & (u <= 1.0 + tol) ok_t = in_window(t) acc = on_seg & ok_t if np.any(acc): t_hit[idx[acc]] = clamp_window(t[acc]) # --- Parallel branch --- if np.any(parallel): pr = np.flatnonzero(parallel) # Colinear if (w × e) ~ 0 col = np.linalg.norm(np.cross(w[:, pr].T, e[:, pr].T), axis=1) <= tol if np.any(col): prc = pr[col] # Check if p (t=0) lies on the finite segment ee2 = np.sum(e[:, prc] * e[:, prc], axis=0) on_seg = np.zeros_like(ee2, dtype=bool) # Degenerate segment → treat as a point deg = ee2 <= tol if np.any(deg): on_seg[deg] = np.linalg.norm(p[:, prc][:, deg] - p1[:, prc][:, deg], axis=0) <= tol if np.any(~deg): u0 = np.sum((p[:, prc][:, ~deg] - p1[:, prc][:, ~deg]) * e[:, prc][:, ~deg], axis=0) / ee2[~deg] on_seg[~deg] = (u0 >= -tol) & (u0 <= 1.0 + tol) if np.any(on_seg): t0 = np.zeros(np.count_nonzero(on_seg)) ok0 = in_window(t0) if np.any(ok0): t_hit[prc[on_seg][ok0]] = clamp_window(t0[ok0]) return t_hit
[docs] def timeClosest( p: np.ndarray, # (3, S) v: np.ndarray, # (3, S) a: np.ndarray, # (3, S) pt: np.ndarray, # (3, S) or (3, 1) *, t_min: float = 0.0, t_max: float | None = None, tol: float = 1e-12 ) -> np.ndarray: """ Vectorised closest-approach time for uniformly accelerated motion. Returns ------- t_star : (S,) array Time that minimises ||p + v t + 0.5 a t^2 - pt|| within [t_min, t_max]. Notes ----- If a ≈ 0 => linear solution t* = -(r0·v)/(v·v) clamped to [t_min, t_max]. If also v ≈ 0 => returns t* = t_min. """ # Broadcast pt to (3, S) if pt.shape[1] == 1: pt = np.repeat(pt, p.shape[1], axis=1) r0 = p - pt # (3, S) rv = np.sum(r0 * v, axis=0) # (S,) vv = np.sum(v * v, axis=0) # (S,) ra = np.sum(r0 * a, axis=0) # (S,) va = np.sum(v * a, axis=0) # (S,) aa = np.sum(a * a, axis=0) # (S,) S = p.shape[1] t_star = np.empty(S, dtype=float) # Masks has_accel = aa > tol no_accel = ~has_accel # --- Case 1: a ≈ 0 (linear in t) if np.any(no_accel): mask = no_accel vv_m = vv[mask] rv_m = rv[mask] # If vv > 0: t* = -rv/vv; else (v≈0): fallback to boundary moving = vv_m > tol t_lin = np.full(vv_m.shape, t_min if t_max is None else np.clip(t_min, t_min, t_max)) t_lin[moving] = -rv_m[moving] / vv_m[moving] if t_max is None: t_lin = np.maximum(t_lin, t_min) else: t_lin = np.clip(t_lin, t_min, t_max) t_star[mask] = t_lin # --- Case 2: a != 0 (cubic in t) if np.any(has_accel): idx = np.flatnonzero(has_accel) c3 = 0.5 * aa[idx] c2 = 1.5 * va[idx] c1 = vv[idx] + ra[idx] c0 = rv[idx] # Monic polynomial: t^3 + b2 t^2 + b1 t + b0 = 0 b2 = c2 / c3 b1 = c1 / c3 b0 = c0 / c3 # Build batched companion matrices (S3, 3, 3) S3 = b0.shape[0] C = np.zeros((S3, 3, 3), dtype=float) C[:, 0, 2] = -b0 C[:, 1, 0] = 1.0 C[:, 1, 2] = -b1 C[:, 2, 1] = 1.0 C[:, 2, 2] = -b2 # Roots of cubic via eigenvalues of the companion matrix roots = np.linalg.eigvals(C) # (S3, 3) # Keep real roots roots_real = np.real(roots) roots_imag = np.abs(np.imag(roots)) real_mask = roots_imag < 1e-10 t_candidates = np.where(real_mask, roots_real, np.nan) # (S3, 3) # Append boundaries t_lo = np.full((S3, 1), t_min) if t_max is None or np.isinf(t_max): t_hi = None else: t_hi = np.full((S3, 1), t_max) # Clamp/cull candidate roots to window if t_max is None: t_candidates = np.where((t_candidates >= t_min - tol), t_candidates, np.nan) else: in_win = (t_candidates >= t_min - tol) & (t_candidates <= t_max + tol) t_candidates = np.where(in_win, t_candidates, np.nan) # Stack all candidates: real roots + t_min (+ t_max if finite) if t_hi is None: T = np.concatenate([t_candidates, t_lo], axis=1) # (S3, K) else: T = np.concatenate([t_candidates, t_lo, t_hi], axis=1) # Evaluate f(t) for all candidates and pick the argmin # p_eval: broadcast to (3, S3, K) t_eval = T # Replace NaNs with a sentinel (we’ll set their cost to +inf) valid = np.isfinite(t_eval) t_eval = np.where(valid, t_eval, 0.0) # r(t) = r0 + v t + 0.5 a t^2 r_eval = (r0[:, idx, None] + v[:, idx, None] * t_eval[None, :, :] + 0.5 * a[:, idx, None] * (t_eval[None, :, :] ** 2)) d2 = np.sum(r_eval * r_eval, axis=0) # (S3, K) d2 = np.where(valid, d2, np.inf) k_min = np.argmin(d2, axis=1) t_best = T[np.arange(S3), k_min] t_star[idx] = t_best # # Return also the minimum squared distances for convenience # r_best = r0 + v * t_star[None, :] + 0.5 * a * (t_star[None, :] ** 2) # d2_min = np.sum(r_best * r_best, axis=0) return t_star
[docs] def rotationAlign2x(vector: np.ndarray) -> np.ndarray: """Return R such that R @ vector == ex (to numerical tolerance).""" d = np.asarray(vector, dtype=float).reshape(3) n = np.linalg.norm(d) if n == 0.0: raise ValueError("direction cannot be zero.") d = d / n ex = np.array([1.0, 0.0, 0.0], dtype=float) c = float(np.dot(d, ex)) # cos(theta) if c > 1 - 1e-12: # Already aligned return np.eye(3) if c < -1 + 1e-12: # Opposite: rotate 180° about any axis ⟂ to ex (and d) # Choose a stable axis: prefer y unless d is ~y, then use z. cand = np.array([0.0, 1.0, 0.0]) if abs(np.dot(d, cand)) > 0.9: cand = np.array([0.0, 0.0, 1.0]) u = np.cross(d, cand) u /= np.linalg.norm(u) # unit axis K = np.array([[0, -u[2], u[1]], [u[2], 0, -u[0]], [-u[1], u[0], 0]], dtype=float) # Rodrigues with theta=pi → R = I + 2 K^2 (since sin(pi)=0, 1-cos(pi)=2) return np.eye(3) + 2.0 * (K @ K) # General case: Rodrigues rotating d to ex around k = d × ex k = np.cross(d, ex) s = np.linalg.norm(k) # sin(theta) > 0 here u = k / s # unit axis K = np.array([[0, -u[2], u[1]], [u[2], 0, -u[0]], [-u[1], u[0], 0]], dtype=float) # Standard Rodrigues: R = I + K*sinθ + K^2*(1-cosθ). # But sinθ = s, cosθ = c, and K is built with unit axis. return np.eye(3) + K * s + (K @ K) * (1.0 - c)
[docs] def tri_plane_intersections_yz_per_triangle( tri_coords: np.ndarray, # (E,3,3) → [[x,y,z] per vertex] xs: np.ndarray, # (E,) plane x = xs[i] per triangle eps: float = 1e-9, ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: """ For each triangle i, intersect it with the plane x=xs[i]. Returns exactly two intersections (y,z) and their edge ids (0,1,2). If a triangle does not intersect properly, outputs NaNs and -1s. Args: tri_coords : (E,3,3) float xs : (E,) float, one slice-x per triangle eps : float, tolerance Returns: Y : (E,2) float, y coords of intersections (NaN if not cut) Z : (E,2) float, z coords of intersections Eids: (E,2) int, edge ids (0,1,2) or -1 if invalid """ E = tri_coords.shape[0] A = tri_coords[:, 0, :] # (E,3) B = tri_coords[:, 1, :] C = tri_coords[:, 2, :] Ax, Ay, Az = A[:, 0], A[:, 1], A[:, 2] Bx, By, Bz = B[:, 0], B[:, 1], B[:, 2] Cx, Cy, Cz = C[:, 0], C[:, 1], C[:, 2] def edge_hits(Px, Py, Pz, Qx, Qy, Qz, edge_id): den = Qx - Px between = (np.minimum(Px, Qx) <= xs) & (xs <= np.maximum(Px, Qx)) nonpar = np.abs(den) > eps base = between & nonpar t = np.full(E, np.nan) t[base] = (xs[base] - Px[base]) / den[base] on = base & (t >= -eps) & (t <= 1.0 + eps) y = np.full(E, np.nan); z = np.full(E, np.nan); eid = np.full(E, -1, int) if np.any(on): tm = t[on] y[on] = Py[on] * (1.0 - tm) + Qy[on] * tm z[on] = Pz[on] * (1.0 - tm) + Qz[on] * tm eid[on] = edge_id return y, z, eid # Edge ids: 0:A->B, 1:B->C, 2:C->A y0, z0, e0 = edge_hits(Ax, Ay, Az, Bx, By, Bz, 0) y1, z1, e1 = edge_hits(Bx, By, Bz, Cx, Cy, Cz, 1) y2, z2, e2 = edge_hits(Cx, Cy, Cz, Ax, Ay, Az, 2) # Stack → (E,3) Y = np.stack([y0, y1, y2], axis=1) Z = np.stack([z0, z1, z2], axis=1) Eids = np.stack([e0, e1, e2], axis=1) # Output arrays Y_out = np.full((E, 2), np.nan) Z_out = np.full((E, 2), np.nan) E_out = np.full((E, 2), -1, int) # For each triangle, pick two valid intersections for i in range(E): hits = ~np.isnan(Y[i]) idx = np.flatnonzero(hits) if idx.size < 2: continue if idx.size > 2: # Degenerate: vertex on plane; pick two with largest Δy yk = Y[i, idx]; zk = Z[i, idx]; ek = Eids[i, idx] d01 = abs(yk[0] - yk[1]) d02 = abs(yk[0] - yk[2]) d12 = abs(yk[1] - yk[2]) if d01 >= d02 and d01 >= d12: idx = idx[[0,1]] elif d02 >= d01 and d02 >= d12: idx = idx[[0,2]] else: idx = idx[[1,2]] # sort by y (left→right) y_pair = Y[i, idx]; z_pair = Z[i, idx]; e_pair = Eids[i, idx] if y_pair[0] <= y_pair[1]: Y_out[i] = y_pair; Z_out[i] = z_pair; E_out[i] = e_pair else: Y_out[i] = y_pair[::-1]; Z_out[i] = z_pair[::-1]; E_out[i] = e_pair[::-1] return Y_out, Z_out, E_out
[docs] def pcWalk(p_init, p_end, slab_points, dlt_x, dlt_z): ''' Algorithm to walk from p_init to p_end through slab_points. slab_points: (N,2) array of (x,y) points in the slab dlt_x: (N,) array of local spacing between points dlt_z: float, global step size Returns: (M,2) array of (x,y) points along the path 1. Start at p_init 2. Move in the direction of p_end, but only as far as dlt_z 3. At each step, look for points in slab_points within dlt_z in the current direction and within dlt_x locally 4. If points are found, move towards the average position of those points, weighted by 1/distance^2 5. If no points are found, reset direction towards p_end 6. Stop when p_end is reached or no progress can be made ''' left_bound, bottom_bound = slab_points.min(axis=0) right_bound, top_bound = slab_points.max(axis=0) nodes = [np.array(p_init, copy=True)] cur = np.array(p_init, copy=True) def _plot_walk(): # import matplotlib.pyplot as plt # plt.scatter(slab_points[:,0], slab_points[:,1], s=0.5) # plt.plot(np.array(nodes)[:,0], np.array(nodes)[:,1], '-o', color='red', markersize=2) # plt.savefig('walk.png') # plt.close() pass def _reset(cur_point): dir_init = p_end - cur_point dir_init = dir_init / np.linalg.norm(dir_init) dir = np.array(dir_init, copy=True) prev_dir = np.repeat(dir_init[None, :], 10, axis=0) return dir, prev_dir if np.linalg.norm(p_end - cur) < dlt_z: return np.array([p_init, p_end], dtype=float) dir, prev_dir = _reset(cur) break_next = False while True: _plot_walk() if len(nodes) > slab_points.shape[0]: # print('Too many nodes, giving up') break # give up if np.linalg.norm(cur - p_end) < dlt_z: nodes += [np.array(p_end, copy=True)] break # reached the end # Get points within dlt_z in the current direction dlt = slab_points - cur dist = np.linalg.norm(dlt, axis=1) # Blend with direction to endpoint dist_global = np.linalg.norm(p_end - cur) dir_global = (p_end - cur) / dist_global search_dir = dir * 1/dlt_z + dir_global * 1/dist_global search_dir /= 1/dlt_z + 1/dist_global dlt_proj = np.sum(dlt * search_dir, axis=1) in_direction = dlt_proj > 0 within_reach = dist <= dlt_z # Calculate new direction if np.any(in_direction): new_dir_slab = np.average(dlt[in_direction], axis=0, weights=1/(dist[in_direction]**2 + 1e-12)) new_dir_slab /= np.linalg.norm(new_dir_slab) alpha_slab = max(np.dot(dir, new_dir_slab), 0.1) if np.any(in_direction & within_reach): new_dir_local = np.average(dlt[in_direction & within_reach], axis=0, weights=1/(dlt_x[in_direction & within_reach]**2 + 1e-12)) new_dir_local /= np.linalg.norm(new_dir_local) alpha_local = max(np.dot(dir, new_dir_local), 0.1) if alpha_local > alpha_slab: new_dir = new_dir_local else: new_dir = new_dir_slab alpha = min(alpha_local, alpha_slab) else: new_dir = new_dir_slab alpha = alpha_slab else: if break_next: break # give up break_next = True if np.linalg.norm(p_end - cur) < dlt_z: nodes += [np.array(p_end, copy=True)] break # reached the end dir, prev_dir = _reset(cur) continue if np.any(prev_dir @ new_dir < -0.75): # don't allow sharp turns: avoid loops break_next = True if break_next: break # give up dir, prev_dir = _reset(cur) continue # Advance prev_dir[1:, :] = prev_dir[:-1, :] prev_dir[0, :] = dir dir = new_dir if np.any(within_reach): dlt_seg = np.max(dist[within_reach]) else: dlt_seg = dlt_z if dlt_seg <= 1e-12 * dlt_z: # Only the current position itself is within reach (an isolated # point, e.g. a stray start point): take the nominal step rather # than standing still. dlt_seg = dlt_z cur += dir * dlt_seg * alpha cur[0] = np.clip(cur[0], left_bound, right_bound) cur[1] = np.clip(cur[1], bottom_bound, top_bound) nodes += [np.array(cur, copy=True)] break_next = False return np.array(nodes, dtype=float)
def _binned_median(key: np.ndarray, step: float, cols, min_count: int = 1, min_fraction: float = 0.0): """Median of each array in `cols` over bins of width `step` on `key`. Returns one array per column, ordered by increasing `key`. Empty bins are skipped, and so are bins with fewer values than `min_count` or than `min_fraction` times the median count of the occupied bins.""" k = np.floor((key - key.min()) / step).astype(np.int64) out = [] for c in cols: o = np.lexsort((c, k)) ks = k[o] starts = np.r_[0, 1 + np.flatnonzero(np.diff(ks))] cnt = np.diff(np.r_[starts, ks.size]) med = c[o][starts + cnt // 2] out.append(med[cnt >= max(min_count, min_fraction * np.median(cnt))]) return out def _arclength_on_polyline(points: np.ndarray, V: np.ndarray) -> np.ndarray: """Arclength, along polyline `V`, of the projection of each point onto it. The first and last segments are extended, so points beyond the ends get an arclength below zero or above the length of `V`.""" ab = np.diff(V, axis=0) L = np.maximum(np.hypot(ab[:, 0], ab[:, 1]), 1e-12) S0 = np.r_[0.0, np.cumsum(L)] best = np.full(points.shape[0], np.inf) arc = np.zeros(points.shape[0]) K = ab.shape[0] for i in range(K): q = points - V[i] t = np.clip(q @ ab[i] / L[i] ** 2, -np.inf if i == 0 else 0.0, np.inf if i == K - 1 else 1.0) r = q - t[:, None] * ab[i] d = np.hypot(r[:, 0], r[:, 1]) m = d < best best[m] = d[m] arc[m] = S0[i] + t[m] * L[i] return arc
[docs] def profile_from_slab( points: np.ndarray, step: float, axis: np.ndarray, *, refine: bool = True, min_count: int = 1, min_fraction: float = 0.1, ) -> np.ndarray: """ Profile through a slab of points, by interpolation instead of a walk. The slab is treated as a height field over a straight axis lying in the profile plane (for a wall: the direction up its face). Points are binned along that axis and each bin gives one node at the median offset from the axis, so a sparse layer of stray points does not move the profile and there is nothing to converge. Parameters ---------- points : (N, 2) array Slab points in the profile plane, e.g. (d, e). step : float Node spacing. axis : (2,) array Direction, in the profile plane, along which the surface is single-valued. refine : bool Second pass: re-bin the points along the arclength of the first profile and take the median position per bin. This evens out the node spacing where the surface is steep relative to `axis` (floor, bench edges). min_count : int Bins with fewer points give no node. min_fraction : float Bins with fewer points than this fraction of the median bin give no node either. This drops bins that hold only a few isolated stray points, e.g. above the crest. Returns ------- (M, 2) array Profile nodes ordered from the highest to the lowest second coordinate (crest to toe). Fewer than two rows if the slab is too sparse. Notes ----- A surface that folds back over `axis` (an overhang relative to it) cannot be represented; :func:`pcWalk` remains available for that case. """ P = np.asarray(points, dtype=float) if step <= 0: raise ValueError("step must be > 0.") if P.ndim != 2 or P.shape[1] != 2 or P.shape[0] == 0: return np.empty((0, 2), dtype=float) a = unit(np.asarray(axis, dtype=float).ravel()[:2]) normal = np.array([-a[1], a[0]]) t = P @ a n = P @ normal # Pass 1: height above the axis against position along it key = t tc, nc = _binned_median(t, 2.0 * step if refine else step, (t, n), min_count, min_fraction) nodes = np.outer(tc, a) + np.outer(nc, normal) # Pass 2: same again along the profile itself if refine and nodes.shape[0] >= 2: key = _arclength_on_polyline(P, nodes) x, y = _binned_median(key, step, (P[:, 0], P[:, 1]), min_count, min_fraction) nodes = np.column_stack([x, y]) # End nodes: a bin median sits about half a bin inside the ends of the slab. # Add the median of the outermost points at each end (a few of them, so that # an isolated stray point does not become the crest or the toe). if nodes.shape[0] >= 2: k = min(P.shape[0], max(3, P.shape[0] // 100)) part = np.argpartition(key, [k - 1, P.shape[0] - k]) first = np.median(P[part[:k]], axis=0) last = np.median(P[part[P.shape[0] - k:]], axis=0) if np.linalg.norm(first - nodes[0]) > 0.1 * step and (first - nodes[0]) @ (nodes[1] - nodes[0]) < 0: nodes = np.vstack([first, nodes]) if np.linalg.norm(last - nodes[-1]) > 0.1 * step and (last - nodes[-1]) @ (nodes[-2] - nodes[-1]) < 0: nodes = np.vstack([nodes, last]) if nodes.shape[0] >= 2 and nodes[0, 1] < nodes[-1, 1]: nodes = nodes[::-1] return np.ascontiguousarray(nodes)
[docs] def unit(v: np.ndarray, fallback: np.ndarray | None = None, eps: float = 1e-12) -> np.ndarray: v = np.asarray(v, dtype=float) n = np.linalg.norm(v) if n > eps: return v / n if fallback is not None: return unit(fallback, eps=eps) raise ValueError("Zero vector cannot be normalised.")
[docs] def normal2d(t: np.ndarray) -> np.ndarray: return np.array([-t[1], t[0]])
[docs] def limit_turn( e_new: np.ndarray, e_old: np.ndarray, max_turn_angle: float | None, ) -> np.ndarray: if max_turn_angle is None: return unit(e_new, e_old) e_new = unit(e_new, e_old) e_old = unit(e_old) cosang = np.clip(e_old @ e_new, -1.0, 1.0) angle = np.arccos(cosang) if angle <= max_turn_angle: return e_new cross = e_old[0] * e_new[1] - e_old[1] * e_new[0] sign = np.sign(cross) if cross != 0.0 else 1.0 c = np.cos(sign * max_turn_angle) s = np.sin(sign * max_turn_angle) R = np.array([[c, -s], [s, c]]) return R @ e_old
[docs] def weighted_centroid( xy: np.ndarray, centre: np.ndarray, power: float = 1.0, eps: float = 1e-6, ) -> np.ndarray: if power == 0.0: return xy.mean(axis=0) d = np.linalg.norm(xy - centre, axis=1) w = 1.0 / np.maximum(d, eps) ** power return np.average(xy, axis=0, weights=w)
[docs] def strip_indices_along_tangent( xy: np.ndarray, tree: cKDTree, centre: np.ndarray, tangent: np.ndarray, half_width: float, transverse_radius: float, ) -> np.ndarray: """ Select points in a finite strip. tangent: Marching direction. half_width: Half-thickness of the strip along the marching direction. transverse_radius: Search extent along the section direction, i.e. normal to tangent. """ tangent = unit(tangent) section_dir = normal2d(tangent) query_radius = np.hypot(half_width, transverse_radius) idx = np.asarray(tree.query_ball_point(centre, query_radius), dtype=int) if idx.size == 0: return idx rel = xy[idx] - centre s = rel @ tangent r = rel @ section_dir keep = (np.abs(s) <= half_width) & (np.abs(r) <= transverse_radius) return idx[keep]
[docs] def refine_next_node( xy: np.ndarray, tree: cKDTree, x_prev: np.ndarray, e_prev: np.ndarray, increment: float, transverse_radius: float, *, min_points: int = 20, max_iter: int = 20, tol: float = 1e-3, max_turn_angle: float | None = np.deg2rad(45.0), weight_power: float = 1.0, ) -> tuple[np.ndarray, np.ndarray, np.ndarray]: """ Find the next polyline node. The next node is constrained to be exactly `increment` away from x_prev. """ e_prev = unit(e_prev) q = x_prev + increment * e_prev idx_final = np.empty(0, dtype=int) for _ in range(max_iter): idx = strip_indices_along_tangent( xy=xy, tree=tree, centre=q, tangent=e_prev, half_width=0.5 * increment, transverse_radius=transverse_radius, ) if idx.size < min_points: break # Only use points ahead of the previous node. rel_prev = xy[idx] - x_prev ahead = rel_prev @ e_prev > 0.0 idx = idx[ahead] if idx.size < min_points: break centroid = weighted_centroid( xy[idx], centre=q, power=weight_power, ) v = centroid - x_prev if np.linalg.norm(v) < tol: break e_trial = unit(v, e_prev) e_trial = limit_turn(e_trial, e_prev, max_turn_angle) q_new = x_prev + increment * e_trial idx_final = idx if np.linalg.norm(q_new - q) < tol: q = q_new break q = q_new e_next = unit(q - x_prev, e_prev) return q, e_next, idx_final
[docs] def trace_nodes_from_seed( points: np.ndarray, increment: float, *, x0: np.ndarray, e0: np.ndarray, transverse_radius: float, min_points: int = 20, max_nodes: int | None = None, max_iter: int = 20, tol: float = 1e-3, max_turn_angle: float | None = np.deg2rad(45.0), weight_power: float = 1.0, min_new_fraction: float = 0.10, ) -> np.ndarray: points = np.asarray(points, dtype=float) xy = points[:, :2] tree = cKDTree(xy) x = np.asarray(x0, dtype=float).copy() e = unit(np.asarray(e0, dtype=float)) nodes = [x.copy()] used = np.zeros(points.shape[0], dtype=bool) while max_nodes is None or len(nodes) < max_nodes: idx_here = strip_indices_along_tangent( xy=xy, tree=tree, centre=x, tangent=e, half_width=0.5 * increment, transverse_radius=transverse_radius, ) if idx_here.size >= min_points: n_new = np.count_nonzero(~used[idx_here]) new_fraction = n_new / idx_here.size if new_fraction < min_new_fraction: break used[idx_here] = True x_next, e_next, idx_search = refine_next_node( xy=xy, tree=tree, x_prev=x, e_prev=e, increment=increment, transverse_radius=transverse_radius, min_points=min_points, max_iter=max_iter, tol=tol, max_turn_angle=max_turn_angle, weight_power=weight_power, ) if idx_search.size < min_points: break x = x_next e = e_next nodes.append(x.copy()) return np.asarray(nodes)
[docs] def trace_polyline_nodes( points: np.ndarray, increment: float, *, transverse_radius: float, buffer_steps: int = 5, initial_direction: tuple[float, float] = (1.0, 0.0), min_points: int = 20, max_nodes: int | None = None, max_iter: int = 20, tol: float = 1e-3, max_turn_angle: float | None = np.deg2rad(45.0), weight_power: float = 1.0, min_new_fraction: float = 0.10, ) -> np.ndarray: points = np.asarray(points, dtype=float) xy = points[:, :2] x_start = xy[np.argmin(xy[:, 0])] e_start = unit(np.asarray(initial_direction, dtype=float)) # First pass: deliberately allow bad initialisation. forward_nodes = trace_nodes_from_seed( points, increment, x0=x_start, e0=e_start, transverse_radius=transverse_radius, min_points=min_points, max_nodes=max_nodes, max_iter=max_iter, tol=tol, max_turn_angle=max_turn_angle, weight_power=weight_power, min_new_fraction=min_new_fraction, ) if forward_nodes.shape[0] <= buffer_steps + 1: raise RuntimeError( "Not enough nodes were traced to apply the requested buffer_steps." ) anchor = forward_nodes[buffer_steps] # Stable tangent at the anchor. e_anchor = unit(forward_nodes[buffer_steps + 1] - forward_nodes[buffer_steps]) # Walk backwards from the stable anchor. backward_nodes = trace_nodes_from_seed( points, increment, x0=anchor, e0=-e_anchor, transverse_radius=transverse_radius, min_points=min_points, max_nodes=max_nodes, max_iter=max_iter, tol=tol, max_turn_angle=max_turn_angle, weight_power=weight_power, min_new_fraction=min_new_fraction, ) # backward_nodes starts at anchor and goes backwards: # [anchor, b1, b2, ...] # Reverse it and remove duplicate anchor. backward_part = backward_nodes[::-1] # forward_nodes also starts at anchor. forward_part = forward_nodes[buffer_steps+1:] nodes = np.vstack([backward_part, forward_part]) return nodes
[docs] def profile_geometry_from_nodes(nodes: np.ndarray) -> tuple[np.ndarray, np.ndarray, np.ndarray]: """ Convert node polyline into profile centres and directions. Returns ------- centres : (N, 2) ndarray Profile locations, placed at segment midpoints. tangents : (N, 2) ndarray Marching tangent of each segment. section_dirs : (N, 2) ndarray XY direction of the 2D section, normal to the marching tangent. """ nodes = np.asarray(nodes, dtype=float) if nodes.shape[0] < 2: raise ValueError("At least two polyline nodes are required.") seg = nodes[1:] - nodes[:-1] length = np.linalg.norm(seg, axis=1) tangents = seg / length[:, None] centres = 0.5 * (nodes[:-1] + nodes[1:]) section_dirs = np.column_stack([-tangents[:, 1], tangents[:, 0]]) return centres, tangents, section_dirs
[docs] def assign_sections_from_nodes( points: np.ndarray, nodes: np.ndarray, *, transverse_radius: float, min_points: int = 20, ) -> tuple[list[np.ndarray], np.ndarray, np.ndarray, np.ndarray]: """ Stage 2. Assign points to profiles located at segment midpoints. Each section i owns points whose projected coordinate along the segment tangent lies between the two segment endpoints: abs((p - centre_i) . tangent_i) <= length_i / 2 The section direction is normal to the segment tangent. """ points = np.asarray(points, dtype=float) xy = points[:, :2] centres, tangents, section_dirs = profile_geometry_from_nodes(nodes) tree = cKDTree(xy) seg_lengths = np.linalg.norm(nodes[1:] - nodes[:-1], axis=1) section_ids: list[np.ndarray] = [] for c, t, L in zip(centres, tangents, seg_lengths): idx = strip_indices_along_tangent( xy=xy, tree=tree, centre=c, tangent=t, half_width=0.5 * L, transverse_radius=transverse_radius, ) if idx.size < min_points: idx = np.empty(0, dtype=int) section_ids.append(idx) return section_ids, centres, tangents, section_dirs
[docs] def brentq( f: Callable[[np.ndarray], np.ndarray], a: np.ndarray, b: np.ndarray, xtol: float = 1e-10, rtol: float = 4 * np.finfo(float).eps, ftol: float = 0.0, maxiter: int = 100, ) -> Tuple[np.ndarray, np.ndarray, np.ndarray]: """ Vectorised Brent root finder for many independent 1D brackets. Parameters ---------- f : callable Vectorised function. For input x with shape (N,), must return shape (N,). a, b : array_like Initial bracketing intervals. Must have the same shape. xtol : float, optional Absolute tolerance on x. rtol : float, optional Relative tolerance on x. ftol : float, optional Absolute tolerance on f(x). If |f(x)| <= ftol, root is accepted. maxiter : int, optional Maximum number of iterations. Returns ------- root : np.ndarray Estimated roots. Entries that never had a valid bracket are NaN. converged : np.ndarray of bool True where convergence was achieved. iters : np.ndarray of int Number of iterations performed for each problem. Notes ----- This is a batched version of Brent's method. Each root keeps its own state, but updates are performed with boolean masks. """ a = np.asarray(a, dtype=float).copy() b = np.asarray(b, dtype=float).copy() if a.shape != b.shape: raise ValueError("a and b must have the same shape") shape = a.shape n = a.size a = a.ravel() b = b.ravel() fa = np.asarray(f(a), dtype=float).ravel() fb = np.asarray(f(b), dtype=float).ravel() if fa.shape != a.shape or fb.shape != b.shape: raise ValueError("f(x) must return an array with the same shape as x") root = np.full(n, np.nan, dtype=float) converged = np.zeros(n, dtype=bool) iters = np.zeros(n, dtype=int) bracketed = fa * fb < 0.0 root[~bracketed] = b[~bracketed] converged[~bracketed] = True # Valid initial brackets only valid = np.isfinite(a) & np.isfinite(b) & np.isfinite(fa) & np.isfinite(fb) & bracketed if not np.any(valid): return root.reshape(shape), converged.reshape(shape), iters.reshape(shape) # Brent state c = a.copy() fc = fa.copy() d = np.empty_like(a) d[:] = np.nan mflag = np.ones(n, dtype=bool) # Ensure |f(a)| >= |f(b)| on valid entries swap = valid & (np.abs(fa) < np.abs(fb)) if np.any(swap): a[swap], b[swap] = b[swap].copy(), a[swap].copy() fa[swap], fb[swap] = fb[swap].copy(), fa[swap].copy() active = valid.copy() for k in range(1, maxiter + 1): if not np.any(active): break tol = xtol + rtol * np.abs(b) done = active & ((np.abs(fb) <= ftol) | (np.abs(b - a) <= tol)) if np.any(done): root[done] = np.where(fb[done] < 0, a[done], b[done]) converged[done] = True iters[done] = k - 1 active[done] = False if not np.any(active): break s = np.empty_like(a) # Inverse quadratic interpolation where possible iq = ( active & (fa != fc) & (fb != fc) ) if np.any(iq): ai, bi, ci = a[iq], b[iq], c[iq] fai, fbi, fci = fa[iq], fb[iq], fc[iq] s[iq] = ( ai * fbi * fci / ((fai - fbi) * (fai - fci)) + bi * fai * fci / ((fbi - fai) * (fbi - fci)) + ci * fai * fbi / ((fci - fai) * (fci - fbi)) ) # Secant otherwise sec = active & ~iq if np.any(sec): as_, bs_ = a[sec], b[sec] fas, fbs = fa[sec], fb[sec] s[sec] = bs_ - fbs * (bs_ - as_) / (fbs - fas) # Acceptance / fallback tests mid_lo = (3.0 * a + b) / 4.0 mid_hi = b between = ((s > np.minimum(mid_lo, mid_hi)) & (s < np.maximum(mid_lo, mid_hi))) cond1 = ~between cond2 = mflag & (np.abs(s - b) >= np.abs(b - c) / 2.0) cond3 = (~mflag) & (np.abs(s - b) >= np.abs(c - d) / 2.0) cond4 = mflag & (np.abs(b - c) < tol) cond5 = (~mflag) & (np.abs(c - d) < tol) use_bisect = active & (cond1 | cond2 | cond3 | cond4 | cond5) if np.any(use_bisect): s[use_bisect] = 0.5 * (a[use_bisect] + b[use_bisect]) mflag[use_bisect] = True accepted = active & ~use_bisect if np.any(accepted): mflag[accepted] = False fs = np.full_like(fa, np.nan) if np.any(active): fs[active] = np.asarray(f(s), dtype=float)[active] # If function evaluation produced invalid values, deactivate those cases bad_fs = active & ~np.isfinite(fs) if np.any(bad_fs): active[bad_fs] = False iters[bad_fs] = k still = active.copy() if not np.any(still): break d[still] = c[still] c[still] = b[still] fc[still] = fb[still] left = still & (fa * fs < 0.0) right = still & ~left if np.any(left): b[left] = s[left] fb[left] = fs[left] if np.any(right): a[right] = s[right] fa[right] = fs[right] swap = still & (np.abs(fa) < np.abs(fb)) if np.any(swap): a[swap], b[swap] = b[swap].copy(), a[swap].copy() fa[swap], fb[swap] = fb[swap].copy(), fa[swap].copy() iters[still] = k # Final convergence check after loop remaining = active & ((np.abs(fb) <= ftol) | (np.abs(b - a) <= (xtol + rtol * np.abs(b)))) if np.any(remaining): root[remaining] = np.where(fb[remaining] < 0, a[remaining], b[remaining]) converged[remaining] = True # For valid but unconverged cases, return current best estimate anyway unconverged_valid = valid & ~converged root[unconverged_valid] = b[unconverged_valid] return root.reshape(shape), converged.reshape(shape), iters.reshape(shape)
[docs] def rk4_step( dt: float| np.ndarray, x: np.ndarray, v: np.ndarray, accel, *args ) -> Tuple[np.ndarray, np.ndarray]: """ Vectorised RK4 step for many blocks at once. Parameters ---------- dt : float Time step. x : (3, n) ndarray Positions. v : (3, n) ndarray Velocities. accel : callable Function a = accel(x, v, *args), returning (3, n) acceleration. *args : Extra data passed to accel. Returns ------- x_new : (3, n) ndarray v_new : (3, n) ndarray """ k1x = v k1v = accel(x, v, *args) x2 = x + 0.5 * dt * k1x v2 = v + 0.5 * dt * k1v k2x = v2 k2v = accel(x2, v2, *args) x3 = x + 0.5 * dt * k2x v3 = v + 0.5 * dt * k2v k3x = v3 k3v = accel(x3, v3, *args) x4 = x + dt * k3x v4 = v + dt * k3v k4x = v4 k4v = accel(x4, v4, *args) x_new = x + (dt / 6.0) * (k1x + 2.0 * k2x + 2.0 * k3x + k4x) v_new = v + (dt / 6.0) * (k1v + 2.0 * k2v + 2.0 * k3v + k4v) return x_new, v_new
[docs] def getSubSamples(index: np.ndarray, *arrays: np.ndarray) -> tuple[np.ndarray, ...]: """Get sub-samples.""" return tuple(a if a.shape[-1] == 1 else a[...,index] for a in arrays)
[docs] def setSubSamples(index: np.ndarray, values: Tuple, *arrays: np.ndarray) -> None: """Set sub-samples back into arrays.""" for a, v in zip(arrays, values): if a.shape[-1] != 1: a[..., index] = v else: a[...] = v
[docs] def scatterSubSamples(index: np.ndarray, fill: float, *values: np.ndarray) -> tuple[np.ndarray, ...]: """Inverse of getSubSamples: scatter values selected by `index` (a boolean mask over the last axis) back into full-size arrays of shape value.shape[:-1] + index.shape, filling non-selected entries with `fill`. """ out = [] for v in values: full = np.full(v.shape[:-1] + index.shape, fill, dtype=v.dtype) full[..., index] = v out.append(full) return tuple(out)
[docs] def sampleNormals(position: np.ndarray, elementsIDs: np.ndarray, elementNormals: np.ndarray) -> np.ndarray: """Sample the normal vectors at the given positions.""" S = position.shape[1] hit = elementsIDs >= 0 ids_safe = np.where(hit, elementsIDs, 0) # avoid OOB for -1 # choose sample axis selector: 0 if element_norm has only one slice; else 0..S-1 sel = np.arange(S) if elementNormals.shape[2] == S else np.zeros(S, dtype=int) # vectorised gather: result is (D, S) norm_per_sample = elementNormals[:, ids_safe, sel].copy() # clear out the non-touching samples norm_per_sample[:, ~hit] = np.nan return norm_per_sample
[docs] def decompose( vector: np.ndarray, # (D, S) normal: np.ndarray, # (D, S) (not necessarily unit) eps: float = 1e-12, ) -> tuple[np.ndarray, np.ndarray]: """ Decompose `vector` into normal and tangential components w.r.t. `normal`. Args: vector: (D, S) vectors to decompose. normal: (D, S) normals; can be non-unit and vary per sample. eps: small number to guard division by zero. Returns: vn: (1, S) normal velocity (scalar) vn_vec = n * vn. vt: (D, S) tangential component in Cartesian coordinates. Notes: - Works for D = 2 or 3, S arbitrary. - If a normal has ~zero length, the output for that sample is: vn_vec = 0, vt_vec = vector, vn = 0, vt = ||vector||. """ # Scalar normal projection per sample (1, S), then expand to (D, S) vn_scalar = np.sum(vector * normal, axis=0) # (S,) # Tangential component (Cartesian) (D,S) - (D,S)*(S,) vt_vec = vector - normal * vn_scalar # (D, S) return vn_scalar, vt_vec