geometry

geometry — stateless R^n / SO(3) primitives

Purpose

The codebase’s shared toolbox of stateless array/rotation primitives: casting & shape
coercion, safe normalization, SO(3) log/exp, frame construction, skew (cross/vee),
interpolation, and saturation. No state, no config, no I/O — pure functions everyone imports.

Role in the system

• The support layer beneath every GNC module: breve_controller, com_controller,
  base_controller, the ee_guidance / base_guidance / com_guidance guidance tower,
  and robot all pull casts (as_col3, cross), normalization (flat_unit), and frame math
  (so3_log, matrix_from_xz) from here.
• The full helper catalog (what to reach for before writing a new one) lives in
  GNC/equations/helpers.md; this page is the why.
• Sibling math primitives: quaternions (scalar-last quaternion algebra), curvature
  (polyline field sampling).

Inputs / Outputs

• In: raw arrays / lists / None of any shape; rotation matrices; axis*angle 3-vectors.
• Out: shape-normalized float arrays (None-safe), SO(3) matrices, so(3) 3-vectors, angles.
• Module constant: EPS = 1e-6 (the project-wide small-norm / small-angle threshold).

Key functions (load-bearing)

• safe_normalize — normalize along last axis; zero inputs return zeros + flag (no raise) — utils/geometry.py:90
• as_flat / as_col3 / as_mat3 — None-safe shape coercion, float cast — :33 / :139 / :46
• flatten_any — ravel preserving dtype (the bool/int-safe counterpart to as_flat) — :41
• so3_log — SO(3)→so(3) log map, three numerical branches — :292
• so3_exp — so(3)→SO(3) Rodrigues exp map — :343
• cross / vee — skew w → wˣ and its inverse — :282 / :289
• matrix_from_xz / matrix_from_vector / rotation_aligned_frame — SO(3) frame construction — :205 / :225 / :246
• frame_ang_vel — world-frame ω from two frames via so3_log — :271
• saturate — clip a vector’s norm, direction preserved — :156
• finite_check — raise on non-finite with a located preview — :180
• block6 / block9 — block-diagonal assembly for the reduced dynamics — :63 / :54

Footguns

The cast helpers silently convert to float

as_flat / as_mat3 / as_col (and as_flat3 / as_col3) cast to float. For bool/int signals
(e.g. coverage flags, index logs) use flatten_any instead — it preserves dtype. All cast
helpers are None-safe. (utils/INSIGHTS.md §geometry.py [footgun])

safe_normalize warns-and-zeros; it does not raise

A zero-norm input returns zeros and sets flag=True (and prints a whodunnit caller trace),
rather than raising. Callers that need to know must pass return_flag=True and check it —
a silently-zeroed unit vector will otherwise propagate. (utils/INSIGHTS.md [footgun])

matrix_from_vector flips its up-reference on collinearity

When the forward direction is collinear with the default up=[0,0,1], it falls back to
up=[0,1,0]. A caller supplying a custom up_ref must still handle that collinearity itself
— the fallback only guards the default. (utils/INSIGHTS.md [footgun])

Pseudocode (so3_log — the three branches)

θ = arccos(clip((tr R − 1)/2, −1, 1))
if θ < 1e-7:          return 0.5 · vee(R − Rᵀ)          # small-angle
if π − θ < 1e-6:      axis = unit(strongest column of R+I); return θ · axis   # near-π
else:                 return θ · vee((R − Rᵀ) / (2 sin θ))                    # generic

Each branch dodges a real failure mode of the others (vanishing sin θ near 0 and π).
(utils/INSIGHTS.md §geometry.py [math])

Related

quaternions · curvature · robot · breve_controller · ee_guidance · terminology