Robotics operators#

The robotics-specialized small-operator families — spatial 6-D ops (cross products, coordinate transforms, and the 10-parameter inertia: rigid-body dynamics), the SO(3)/SE(3)/quaternion Lie family with pose-error metrics (manifold states, their derivatives, and goal costs), projection/cone/augmented-Lagrangian scalars (constrained trajectory optimization), geometry distance primitives (sphere-decomposed collision checking), the 3x3 estimation kit (eig3/svd3/closest_rotation: ICP, alignment, re-orthonormalization), and the sampling-planner L1 additions (softmax/logsumexp/argmax/argmin + _fast twins).

Read the conventions first: Robotics operators & conventions pins the angular-first spatial ordering, the linear-first SE(3) tangent blocks, the QuatLayout storage tag, column-major storage, and the small-angle branch policy. Every array-shaped op below also exists as glass::warp:: and glass::thread:: (same serial core, one tier per problem-packing granularity); scalar ops are tier-free.

Spatial 6-D cross products#

Functions

template<typename T, bool TRAILING_SYNC = true> void motion_cross(const T *v, T *M)#

Spatial motion cross-product matrix: M = motion_cross(v) (6x6, “crm”).

M·x = v ×ₘ x for spatial motion vectors, M = [[ωₓ, 0],[v_linₓ, ωₓ]] with v = [ω; v_lin] (angular-first) and column-major storage. Prefer the fused motion_cross_mul when only the product is needed.

Template Parameters:

T – Scalar type (e.g. float, double).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

v – Spatial motion vector (6 elements, [ω; v_lin]).
M – Output 6x6 matrix (36 elements, column-major; no aliasing).

template<typename T, int AXIS = -1, bool HAS_BETA = false, bool TRAILING_SYNC = true> void motion_cross_mul(T alpha, const T *v, const T *x, T beta, T *y)#

Fused spatial motion cross apply: y = alpha·(v ×ₘ x) + beta·y.

Never materializes the 6x6 — each output row is its 2-4 term formula (the hot inner op of RNEA/ABA velocity sweeps; ~70 call sites in a typical generated dynamics suite). With compile-time AXIS in 0..5 the multiply specializes to the cardinal basis column x = e_AXIS (the revolute/ prismatic motion-subspace fast path; x is ignored and may be nullptr). beta is only read when HAS_BETA (BLAS beta==0 semantics via beta_blend); the (alpha, beta) pair replaces the _peq/_scaled variant explosion. Equivalent to gemv(motion_cross(v), x) — tested against exactly that composition.

Template Parameters:

T – Scalar type.
AXIS – −1 for a dense x (default), or 0..5 for x = e_AXIS.
HAS_BETA – Read/accumulate into y (default false = overwrite).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

alpha – Scalar on the product.
v – Spatial motion vector (6 elements, [ω; v_lin]).
x – Right operand (6 elements; ignored when AXIS >= 0).
beta – Scalar on the existing y (read only when HAS_BETA).
y – Output spatial vector (6 elements; no aliasing with v/x).

template<typename T, bool TRAILING_SYNC = true> void force_cross(const T *v, T *M)#

Spatial force cross-product matrix: M = force_cross(v) (6x6, “crf”/”fx”).

The dual cross for spatial FORCE vectors: force_cross(v) = −motion_cross(v)ᵀ = [[ωₓ, v_linₓ],[0, ωₓ]]. Column-major; always writes all 36 entries (no pre-zeroed-destination variant). Prefer the fused force_cross_mul.

Template Parameters:

T, TRAILING_SYNC – See motion_cross.

Parameters:

v – Spatial motion vector (6 elements, [ω; v_lin]).
M – Output 6x6 matrix (36 elements, column-major; no aliasing).

template<typename T, bool HAS_BETA = false, bool TRAILING_SYNC = true> void force_cross_mul(T alpha, const T *v, const T *f, T beta, T *y)#

Fused spatial force cross apply: y = alpha·(v ×* f) + beta·y.

Row formulas of force_cross(v)·f, fused (no 6x6 materialized). Same (alpha, beta)/beta_blend semantics as motion_cross_mul. Tested against the gemv(force_cross(v), f) composition.

Template Parameters:

T, HAS_BETA, TRAILING_SYNC – See motion_cross_mul.

Parameters:

alpha – Scalar on the product.
v – Spatial motion vector (6 elements, [ω; v_lin]).
f – Spatial force vector (6 elements, [n; f_lin]).
beta – Scalar on the existing y (read only when HAS_BETA).
y – Output spatial force vector (6 elements; no aliasing).

template<typename T, bool TRAILING_SYNC = true> void force_cross_dual(const T *f, T *M)#

Operand-swapped force cross matrix: M = force_cross_dual(f) (6x6, “icrf”).

DEFINED by the identity force_cross(v)·f == force_cross_dual(f)·v for all v — it swaps which operand becomes the matrix, the rearrangement inertia-gradient kernels need (∂/∂q of v ×* (I·v) terms). Column-major.

Template Parameters:

T, TRAILING_SYNC – See motion_cross.

Parameters:

f – Spatial force vector (6 elements, [n; f_lin]).
M – Output 6x6 matrix (36 elements, column-major; no aliasing).

template<typename T> void motion_cross(const T *v, T *M)#

Single-warp motion cross matrix.

Single-thread motion cross matrix.

See glass::motion_cross.

template<typename T, int AXIS = -1, bool HAS_BETA = false> void motion_cross_mul(T alpha, const T *v, const T *x, T beta, T *y)#

Single-warp fused motion cross apply.

Single-thread fused motion cross apply (fully unrolled).

See glass::motion_cross_mul.

template<typename T> void force_cross(const T *v, T *M)#

Single-warp force cross matrix.

Single-thread force cross matrix.

See glass::force_cross.

template<typename T, bool HAS_BETA = false> void force_cross_mul(T alpha, const T *v, const T *f, T beta, T *y)#

Single-warp fused force cross apply.

Single-thread fused force cross apply (fully unrolled).

See glass::force_cross_mul.

template<typename T> void force_cross_dual(const T *f, T *M)#

Single-warp operand-swapped force cross matrix.

Single-thread operand-swapped force cross matrix.

See glass::force_cross_dual.

namespace spatial_detail#

Functions

template<typename T> T motion_cross_entry(uint32_t r, uint32_t c, const T *v)#

template<typename T> T motion_cross_mul_row(uint32_t r, const T *v, const T *x)#

template<typename T> T force_cross_entry(uint32_t r, uint32_t c, const T *v)#

template<typename T> T force_cross_mul_row(uint32_t r, const T *v, const T *f)#

template<typename T> T force_cross_dual_entry(uint32_t r, uint32_t c, const T *f)#

namespace warp

namespace thread

Spatial coordinate transforms#

Functions

template<typename T, bool TRAILING_SYNC = true> void motion_transform(const T *E, const T *r, T *X)#

Spatial motion transform matrix: X = [[E, 0], [−E·[r]ₓ, E]] (6x6).

Materializes Featherstone’s ᴮX_A from the (E, r) pair (see the file header for the frame convention). Column-major. Prefer the fused motion_transform_mul when only a product is needed.

Template Parameters:

T – Scalar type (e.g. float, double).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

E – 3x3 rotation A→B (9 elements, column-major).
r – Origin of B expressed in A (3 elements).
X – Output 6x6 transform (36 elements, column-major; no aliasing).

template<typename T, bool TRAILING_SYNC = true> void force_transform(const T *E, const T *r, T *X)#

Spatial force transform matrix: X* = X⁻ᵀ = [[E, −E·[r]ₓ], [0, E]] (6x6).

The dual transform for spatial FORCE vectors. Column-major. Prefer the fused force_transform_mul.

Template Parameters:

T, TRAILING_SYNC – See motion_transform.

Parameters:

E, r – The transform pair (see motion_transform).
X – Output 6x6 transform (36 elements, column-major; no aliasing).

template<typename T, bool INVERSE = false, bool HAS_BETA = false, bool TRAILING_SYNC = true> void motion_transform_mul(T alpha, const T *E, const T *r, const T *v, T beta, T *y)#

Fused spatial motion transform apply: y = alpha·X·v + beta·y (or X⁻¹·v when INVERSE).

Never materializes the 6x6: X·v = [E·ω; E·(v_lin − r×ω)], X⁻¹·v = [Eᵀ·ω; Eᵀ·v_lin + r×(Eᵀ·ω)]. The forward form is the RNEA/ABA velocity down-sweep (v_child = X·v_parent); the inverse recovers parent-frame coordinates. beta is only read when HAS_BETA (BLAS beta==0 semantics via beta_blend). Tested against the gemv(motion_transform(E, r), v) composition.

Template Parameters:

T – Scalar type.
INVERSE – Apply X⁻¹ instead of X (default false).
HAS_BETA – Read/accumulate into y (default false = overwrite).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

alpha – Scalar on the product.
E, r – The transform pair (see motion_transform).
v – Spatial motion vector (6 elements, [ω; v_lin]).
beta – Scalar on the existing y (read only when HAS_BETA).
y – Output spatial motion vector (6 elements; no aliasing).

template<typename T, bool INVERSE = false, bool HAS_BETA = false, bool TRAILING_SYNC = true> void force_transform_mul(T alpha, const T *E, const T *r, const T *f, T beta, T *y)#

Fused spatial force transform apply: y = alpha·X*·f + beta·y (or X*⁻¹·f = Xᵀ·f when INVERSE).

X*·f = [E·(n − r×f_lin); E·f_lin], Xᵀ·f = [Eᵀ·n + r×(Eᵀ·f_lin); Eᵀ·f_lin]. The INVERSE form is THE RNEA back-pass (f_parent += Xᵀ·f_child — use HAS_BETA = true, beta = 1). Same (alpha, beta) semantics as motion_transform_mul. Tested against the gemv(force_transform(E, r), f) composition.

Template Parameters:

T, INVERSE, HAS_BETA, TRAILING_SYNC – See motion_transform_mul.

Parameters:

alpha – Scalar on the product.
E, r – The transform pair (see motion_transform).
f – Spatial force vector (6 elements, [n; f_lin]).
beta – Scalar on the existing y (read only when HAS_BETA).
y – Output spatial force vector (6 elements; no aliasing).

template<typename T> void motion_transform(const T *E, const T *r, T *X)#

Single-warp motion transform matrix.

Single-thread motion transform matrix.

See glass::motion_transform.

template<typename T> void force_transform(const T *E, const T *r, T *X)#

Single-warp force transform matrix.

Single-thread force transform matrix.

See glass::force_transform.

template<typename T, bool INVERSE = false, bool HAS_BETA = false> void motion_transform_mul(T alpha, const T *E, const T *r, const T *v, T beta, T *y)#

Single-warp fused motion transform apply.

Single-thread fused motion transform apply.

See glass::motion_transform_mul.

template<typename T, bool INVERSE = false, bool HAS_BETA = false> void force_transform_mul(T alpha, const T *E, const T *r, const T *f, T beta, T *y)#

Single-warp fused force transform apply.

Single-thread fused force transform apply.

See glass::force_transform_mul.

namespace spatial_detail

Functions

template<typename T> void rot_apply(const T *E, const T *x, T *y)#

template<typename T> void rot_apply_t(const T *E, const T *x, T *y)#

template<typename T> void cross3(const T *a, const T *b, T *c)#

template<typename T> T xform_skew_entry(uint32_t i, uint32_t j, const T *E, const T *r)#

template<typename T, bool FORCE> T xform_entry(uint32_t i, uint32_t j, const T *E, const T *r)#

template<typename T, bool INVERSE> void motion_transform_mul_core(const T *E, const T *r, const T *v, T *y)#

template<typename T, bool INVERSE> void force_transform_mul_core(const T *E, const T *r, const T *f, T *y)#

template<typename T, uint32_t N, bool HAS_BETA> void blend_out(uint32_t rank, uint32_t size, T alpha, const T *tmp, T beta, T *y)#

namespace warp

namespace thread

Spatial inertia (10 parameters)#

Functions

template<typename T, bool TRAILING_SYNC = true> void spatial_inertia(const T *pi, T *M)#

Spatial inertia matrix from the 10 standard parameters (6x6).

M = [[I_O, [h]ₓ], [[h]ₓᵀ, m·1₃]] with pi = [m, h(3), I_O(6)] (see the file header for the packing and the Pinocchio-ordering caveat). Column-major; symmetric by construction. Prefer the fused spatial_inertia_mul when only a product is needed.

Template Parameters:

T – Scalar type (e.g. float, double).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

pi – The 10 inertial parameters [m, h(3), Ixx, Ixy, Ixz, Iyy, Iyz, Izz].
M – Output 6x6 spatial inertia (36 elements, column-major; no aliasing).

template<typename T, bool HAS_BETA = false, bool TRAILING_SYNC = true> void spatial_inertia_mul(T alpha, const T *pi, const T *v, T beta, T *f)#

Fused spatial inertia apply: f = alpha·I(pi)·v + beta·f.

I(pi)·v = [I_O·ω + h × v_lin ; m·v_lin − h × ω] — momentum from motion (RNEA’s I·v and I·a terms) straight from the parameter 10-vector; the 6x6 never materializes. beta is only read when HAS_BETA (BLAS beta==0 semantics via beta_blend). Tested against the gemv(spatial_inertia(pi), v) composition.

Template Parameters:

T – Scalar type.
HAS_BETA – Read/accumulate into f (default false = overwrite).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

alpha – Scalar on the product.
pi – The 10 inertial parameters (see spatial_inertia).
v – Spatial motion vector (6 elements, [ω; v_lin]).
beta – Scalar on the existing f (read only when HAS_BETA).
f – Output spatial force vector (6 elements; no aliasing).

template<typename T> void spatial_inertia(const T *pi, T *M)#

Single-warp spatial inertia matrix.

Single-thread spatial inertia matrix.

See glass::spatial_inertia.

template<typename T, bool HAS_BETA = false> void spatial_inertia_mul(T alpha, const T *pi, const T *v, T beta, T *f)#

Single-warp fused spatial inertia apply.

Single-thread fused spatial inertia apply.

See glass::spatial_inertia_mul.

namespace spatial_detail

Functions

template<typename T> T inertia_entry(uint32_t i, uint32_t j, const T *pi)#

template<typename T> void spatial_inertia_mul_core(const T *pi, const T *v, T *f)#

namespace warp

namespace thread

Quaternions#

Enums

enum class QuatLayout#

Compile-time quaternion storage layout tag.

xyzw — vector part first, scalar last (Eigen / NVIDIA Warp / cuRobo / GRiD storage order; the GLASS default). wxyz — scalar first (MuJoCo / Ceres / GTSAM / ROS geometry_msgs order). Both are HAMILTON quaternions; the tag only moves where each component is stored.

Values:

enumerator xyzw#

enumerator wxyz#

Functions

template<typename T, QuatLayout L = QuatLayout::xyzw, bool TRAILING_SYNC = true> void quat_mul(const T *a, const T *b, T *out)#

Hamilton quaternion product: out = a ⊗ b.

Composition of rotations: R(out) = R(a)·R(b) (apply b first in the body frame of a). Layout via the L tag (default xyzw); both operands and the result share one layout. out must not alias a/b at block/warp scope. NumPy equivalent (xyzw, scipy): (Rotation.from_quat(a) * Rotation.from_quat(b)).as_quat().

Template Parameters:

T – Scalar type (e.g. float, double).
L – Quaternion storage layout (default QuatLayout::xyzw).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

a – Left quaternion (4 elements).
b – Right quaternion (4 elements).
out – Result quaternion (4 elements; no aliasing).

template<typename T, QuatLayout L = QuatLayout::xyzw, bool TRAILING_SYNC = true> void quat_conj(const T *a, T *out)#

Quaternion conjugate (= inverse for unit quaternions): out = a*.

Negates the vector part. NumPy equivalent (xyzw): [-x, -y, -z, w].

Template Parameters:

T, L, TRAILING_SYNC – See quat_mul.

Parameters:

a – Input quaternion (4 elements).
out – Conjugated quaternion (4 elements; no aliasing at block/warp scope).

template<typename T, QuatLayout L = QuatLayout::xyzw, bool CANONICAL = false, bool TRAILING_SYNC = true> void quat_normalize(const T *q, T *out)#

Normalize a quaternion to unit length: out = q / |q|.

With CANONICAL = true the result is also flipped onto the w >= 0 half of the double cover (q and -q encode the same rotation) — useful before comparing or interpolating quaternions. In-place (out == q) is safe at thread:: scope only.

Template Parameters:

T, L, TRAILING_SYNC – See quat_mul.
CANONICAL – Also canonicalize the sign to w >= 0 (default false).

Parameters:

q – Input quaternion (4 elements, nonzero).
out – Unit quaternion (4 elements).

template<typename T, QuatLayout L = QuatLayout::xyzw, bool TRAILING_SYNC = true> void quat_exp(const T *phi, T *out)#

Quaternion exponential of a rotation vector: out = exp([φ/2]).

φ is the FULL rotation vector (axis · angle, radians); the half-angle is taken internally: out = [sin(|φ|/2)·φ̂ ; cos(|φ|/2)], with the sin(θ)/θ Taylor head below θ = 1e-12 so the derivative is exact through φ = 0. NumPy equivalent (xyzw): Rotation.from_rotvec(phi).as_quat().

Template Parameters:

T, L, TRAILING_SYNC – See quat_mul.

Parameters:

phi – Rotation vector (3 elements).
out – Unit quaternion (4 elements; no aliasing at block/warp scope).

template<typename T, QuatLayout L = QuatLayout::xyzw, bool TRAILING_SYNC = true> void quat_log(const T *q, T *phi)#

Quaternion logarithm: the rotation vector φ of a unit quaternion.

The inverse of quat_exp on the canonical branch |φ| ≤ π: φ = 2·log(q) with the double cover folded (q and −q return the SAME shortest-path vector). Routed through 2·atan2(|v|, w) — stable across the whole range including θ near π — with a series head below |v| = 1e-8 keeping the map smooth through the identity. q must be unit length. NumPy equivalent (xyzw): Rotation.from_quat(q).as_rotvec().

Template Parameters:

T, L, TRAILING_SYNC – See quat_mul.

Parameters:

q – Unit quaternion (4 elements).
phi – Output rotation vector (3 elements; no aliasing at block/warp scope).

template<typename T, QuatLayout L = QuatLayout::xyzw, bool TRAILING_SYNC = true> void quat_rotate(const T *q, const T *p, T *out)#

Rotate a 3-vector by a unit quaternion: out = R(q)·p.

Uses the cross-product form p + 2w(v×p) + 2v×(v×p) (no 3x3 materialized; 18 mul + 12 add vs 15 mul + 15 add through the matrix, but with no scratch). q must be unit length. NumPy equivalent (xyzw): Rotation.from_quat(q).apply(p).

Template Parameters:

T, L, TRAILING_SYNC – See quat_mul.

Parameters:

q – Unit quaternion (4 elements).
p – Input vector (3 elements).
out – Rotated vector (3 elements; no aliasing at block/warp scope).

template<typename T, QuatLayout L = QuatLayout::xyzw, uint32_t LDA = 3, bool TRAILING_SYNC = true> void quat_to_rot(const T *q, T *R)#

Rotation matrix (3x3 column-major) from a unit quaternion.

LDA is the destination’s leading dimension (row stride between columns; default 3 = contiguous). LDA = 4 writes the rotation block of a column-major 4x4 homogeneous transform IN PLACE — only the nine rotation entries are touched (the gemv/gemm ROW_STRIDE pattern extended to the Lie corner). NumPy equivalent (xyzw): Rotation.from_quat(q).as_matrix() (flatten Fortran-order for the column-major array).

Template Parameters:

T, L, TRAILING_SYNC – See quat_mul.
LDA – Destination leading dimension (default 3).

Parameters:

q – Unit quaternion (4 elements).
R – Output rotation (column-major, leading dimension LDA; no aliasing).

template<typename T, QuatLayout L = QuatLayout::xyzw, uint32_t LDA = 3, bool TRAILING_SYNC = true> void rot_to_quat(const T *R, T *q)#

Unit quaternion from a rotation matrix (Shepperd max-pivot extraction).

Branches on the largest of the trace and the three diagonal entries so the divisor is never small — numerically safe for every rotation including the θ = π family. Result is canonicalized to w >= 0. R is column-major with leading dimension LDA (default 3 = contiguous; LDA = 4 reads the rotation block of a column-major 4x4 homogeneous transform in place — no repack). NumPy equivalent (xyzw): Rotation.from_matrix(R).as_quat() (up to the double-cover sign).

Template Parameters:

T, L, TRAILING_SYNC – See quat_mul.
LDA – Source leading dimension (default 3).

Parameters:

R – Input rotation matrix (column-major, leading dimension LDA).
q – Output unit quaternion (4 elements; no aliasing at block/warp scope).

template<typename T, QuatLayout L = QuatLayout::xyzw, bool TRAILING_SYNC = true> void quat_to_basis(const T *q, T *u, T *v, T *w)#

Normalize a quaternion and return the three rotation-matrix columns.

u/v/w receive columns 0/1/2 of R(q/|q|) — the body frame’s three axes in the parent frame. The explicit normalize makes this safe on raw stored quaternions (e.g. parsed poses). Collision-geometry / frame-setup helper.

Template Parameters:

T, L, TRAILING_SYNC – See quat_mul.

Parameters:

q – Quaternion (4 elements; NOT required to be unit).
u – Output column 0 (3 elements).
v – Output column 1 (3 elements).
w – Output column 2 (3 elements).

template<typename T, QuatLayout L = QuatLayout::xyzw, bool TRAILING_SYNC = true> void quat_retract(const T *q, const T *phi, T *q_new)#

SO(3) retract on the quaternion chart: q_new = normalize(q ⊗ exp([φ/2])).

The manifold update q ⊞ φ (body-frame tangent φ, radians): one integrator step of a spherical joint / orientation state under angular velocity is quat_retract(q, ω·dt, q_new). The trailing renormalize keeps repeated retracts on the unit sphere (drift-free). NumPy equivalent (xyzw): (Rotation.from_quat(q) * Rotation.from_rotvec(phi)).as_quat().

Template Parameters:

T, L, TRAILING_SYNC – See quat_mul.

Parameters:

q – Current unit quaternion (4 elements).
phi – Tangent step — full rotation vector (3 elements).
q_new – Updated unit quaternion (4 elements; no aliasing at block/warp scope).

template<typename T, QuatLayout L = QuatLayout::xyzw> void quat_mul(const T *a, const T *b, T *out)#

Single-warp out = a ⊗ b.

Single-thread out = a ⊗ b.

See glass::quat_mul.

template<typename T, QuatLayout L = QuatLayout::xyzw> void quat_conj(const T *a, T *out)#

Single-warp conjugate.

Single-thread conjugate.

See glass::quat_conj.

template<typename T, QuatLayout L = QuatLayout::xyzw, bool CANONICAL = false> void quat_normalize(const T *q, T *out)#

Single-warp normalize.

Single-thread normalize (optional w>=0 canonicalization).

See glass::quat_normalize.

template<typename T, QuatLayout L = QuatLayout::xyzw> void quat_exp(const T *phi, T *out)#

Single-warp exp([φ/2]).

Single-thread exp([φ/2]).

See glass::quat_exp.

template<typename T, QuatLayout L = QuatLayout::xyzw> void quat_log(const T *q, T *phi)#

Single-warp quaternion logarithm.

Single-thread quaternion logarithm.

See glass::quat_log.

template<typename T, QuatLayout L = QuatLayout::xyzw> void quat_rotate(const T *q, const T *p, T *out)#

Single-warp R(q)·p.

Single-thread R(q)·p.

See glass::quat_rotate.

template<typename T, QuatLayout L = QuatLayout::xyzw, uint32_t LDA = 3> void quat_to_rot(const T *q, T *R)#

Single-warp quaternion → column-major 3x3 (LDA-strided).

Single-thread quaternion → column-major 3x3 (LDA-strided).

See glass::quat_to_rot.

template<typename T, QuatLayout L = QuatLayout::xyzw, uint32_t LDA = 3> void rot_to_quat(const T *R, T *q)#

Single-warp 3x3 (LDA-strided) → quaternion (Shepperd).

Single-thread column-major 3x3 (LDA-strided) → quaternion (Shepperd).

See glass::rot_to_quat.

template<typename T, QuatLayout L = QuatLayout::xyzw> void quat_to_basis(const T *q, T *u, T *v, T *w)#

Single-warp normalize + rotation columns.

Single-thread normalize + rotation columns.

See glass::quat_to_basis.

template<typename T, QuatLayout L = QuatLayout::xyzw> void quat_retract(const T *q, const T *phi, T *q_new)#

Single-warp SO(3) quaternion retract.

Single-thread SO(3) quaternion retract.

See glass::quat_retract.

namespace lie_detail#

Functions

template<typename T, QuatLayout L> void quat_mul_core(const T *a, const T *b, T *out)#

template<typename T, QuatLayout L> void quat_exp_core(const T *phi, T *out)#

template<typename T, QuatLayout L, bool CANONICAL> void quat_normalize_core(const T *q, T *out)#

template<typename T, QuatLayout L> void quat_rotate_core(const T *q, const T *p, T *out)#

template<typename T, QuatLayout L> void quat_to_rot_core(const T *q, T *R)#

template<typename T, QuatLayout L, uint32_t LDA = 3> void rot_to_quat_core(const T *R, T *q)#

template<typename T, QuatLayout L> void quat_retract_core(const T *q, const T *phi, T *q_new)#

template<typename T, QuatLayout L> void quat_log_core(const T *q, T *phi)#

template<typename T, uint32_t N> void copy_out(uint32_t rank, uint32_t size, const T *tmp, T *out)#

template<typename T, uint32_t LDA> void copy_out_mat3(uint32_t rank, uint32_t size, const T *tmp, T *out)#

template<> struct layout<QuatLayout::wxyz>#

Public Static Attributes

static constexpr uint32_t X = 1#

static constexpr uint32_t Y = 2#

static constexpr uint32_t Z = 3#

static constexpr uint32_t W = 0#

template<> struct layout<QuatLayout::xyzw>#

Public Static Attributes

static constexpr uint32_t X = 0#

static constexpr uint32_t Y = 1#

static constexpr uint32_t Z = 2#

static constexpr uint32_t W = 3#

namespace warp

namespace thread

Pose errors#

Enums

enum class ErrorFrame#

Compile-time frame tag for the pose-error family.

LOCAL (default) — the error resolved in the body frame of q_des (log(q_des⁻¹ ⊗ q); pair with body-frame Jacobians). WORLD — the same from-desired-to-current step resolved in the world frame (log(q ⊗ q_des⁻¹); pair with world-frame geometric Jacobians). Field precedent: pinocchio ReferenceFrame::{LOCAL, WORLD}.

Values:

enumerator LOCAL#

enumerator WORLD#

Functions

template<typename T, QuatLayout L = QuatLayout::xyzw, ErrorFrame F = ErrorFrame::LOCAL, bool TRAILING_SYNC = true> void quat_error(const T *q, const T *q_des, T *e)#

Shortest-path rotation error, frame-tagged: LOCAL (default) e = log(q_des⁻¹ ⊗ q); WORLD e = log(q ⊗ q_des⁻¹).

Both frames are the tangent step FROM q_des TO q (|e| ≤ π always, double cover folded): LOCAL satisfies quat_retract(q_des, e) == q and pairs with body-frame Jacobians (exact tangent Jacobian Jr(e)⁻¹, so3_right_jacobian_inv); WORLD satisfies quat_mul(quat_exp(e), q_des) == q, equals R(q_des)·e_LOCAL, and pairs with world-frame geometric Jacobians (IK / visual servoing / task-space control). Swapping the arguments negates the error in either frame. Both inputs must be unit. NumPy equivalents (xyzw, scipy): LOCAL (Rotation.from_quat(q_des).inv() * Rotation.from_quat(q)).as_rotvec(); WORLD (Rotation.from_quat(q) * Rotation.from_quat(q_des).inv()).as_rotvec().

Template Parameters:

T – Scalar type (e.g. float, double).
L – Quaternion storage layout (default QuatLayout::xyzw).
F – Error frame (default ErrorFrame::LOCAL).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

q – Current unit quaternion (4 elements).
q_des – Desired unit quaternion (4 elements).
e – Output rotation-error vector (3 elements; no aliasing).

template<typename T, QuatLayout L = QuatLayout::xyzw, ErrorFrame F = ErrorFrame::LOCAL, bool TRAILING_SYNC = true> void pose_error(const T *pose, const T *pose_des, T *e)#

Decoupled 6-D pose error: e = [p − p_des ; quat_error(q, q_des)].

Poses are [p(3); q(4)] (the se3_retract layout); the error is linear-first, matching the SE(3)-tangent [ρ; φ] block order — but it is the R³ × SO(3) product error every GPU goal cost minimizes, NOT the coupled SE(3) log (see the file header). Exact tangent Jacobian: identity on the translation block, Jr(e_rot)⁻¹ on the rotation block.

Template Parameters:

T, L, F, TRAILING_SYNC – See quat_error (the frame tag applies to the rotation block; the translation difference is world-frame in both).

Parameters:

pose – Current pose [p; q] (7 elements, unit q).
pose_des – Desired pose [p; q] (7 elements, unit q).
e – Output error (6 elements, [dp; drot]; no aliasing).

template<typename T, QuatLayout L = QuatLayout::xyzw> T quat_angle(const T *q, const T *q_des)#

Geodesic angle between two orientations: θ = |quat_error(q, q_des)|.

Computed as 2·atan2(|vec(q_des⁻¹ ⊗ q)|, |w(q_des⁻¹ ⊗ q)|) — stable across the whole range (no acos precision cliff near θ = 0), double cover folded, θ ∈ [0, π]. FRAME-INVARIANT: the LOCAL and WORLD errors are the same vector resolved in different frames, so their magnitude — this angle — is identical (no ErrorFrame tag by design). Both inputs must be unit. Scalar, tier-free. NumPy equivalent (xyzw, scipy): (Rotation.from_quat(q_des).inv() * Rotation.from_quat(q)).magnitude().

Template Parameters:

T – Scalar type.
L – Quaternion storage layout (default QuatLayout::xyzw).

Parameters:

q – Current unit quaternion (4 elements).
q_des – Desired unit quaternion (4 elements).

Returns:

The geodesic angle (radians).

template<typename T, QuatLayout L = QuatLayout::xyzw, ErrorFrame F = ErrorFrame::LOCAL> void quat_error(const T *q, const T *q_des, T *e)#

Single-warp frame-tagged rotation error.

Single-thread frame-tagged rotation error.

See glass::quat_error.

template<typename T, QuatLayout L = QuatLayout::xyzw, ErrorFrame F = ErrorFrame::LOCAL> void pose_error(const T *pose, const T *pose_des, T *e)#

Single-warp decoupled 6-D pose error.

Single-thread decoupled 6-D pose error.

See glass::pose_error.

namespace lie_detail

Functions

template<typename T, QuatLayout L, ErrorFrame F = ErrorFrame::LOCAL> void quat_error_core(const T *q, const T *q_des, T *e)#

template<typename T, QuatLayout L, ErrorFrame F = ErrorFrame::LOCAL> void pose_error_core(const T *pose, const T *pose_des, T *e)#

namespace warp

namespace thread

SO(3): exp / log / Jacobians#

Functions

template<typename T, bool TRAILING_SYNC = true> void skew(const T *v, T *S)#

Hat map: S = [v]ₓ (3x3 column-major skew-symmetric matrix).

S·x = v × x. The SO(3) building block under every op in this family. NumPy equivalent: np.array([[0,-v[2],v[1]],[v[2],0,-v[0]],[-v[1],v[0],0]]) (flatten Fortran-order for the column-major array).

Template Parameters:

T – Scalar type (e.g. float, double).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

v – Input 3-vector.
S – Output 3x3 skew matrix (9 elements, column-major; no aliasing).

template<typename T, bool TRAILING_SYNC = true> void so3_exp(const T *phi, T *R)#

SO(3) exponential (Rodrigues): R = exp([φ]ₓ).

φ is the full rotation vector (axis·angle). Taylor heads below θ = 1e-8 keep the map smooth through the identity. NumPy equivalent: Rotation.from_rotvec(phi).as_matrix() (Fortran-order flatten).

Template Parameters:

T, TRAILING_SYNC – See skew.

Parameters:

phi – Rotation vector (3 elements).
R – Output 3x3 rotation (9 elements, column-major; no aliasing).

template<typename T, bool TRAILING_SYNC = true> void so3_log(const T *R, T *phi)#

SO(3) logarithm: φ = log(R), canonical branch |φ| ≤ π.

Implemented through the Shepperd quaternion extraction plus φ = 2·atan2(|v|, w)·v̂ — numerically stable across the WHOLE range, including θ near π where the textbook θ/(2 sinθ)·vee(R−Rᵀ) formula loses the axis. Inverse of so3_exp (round-trip exact to floating tolerance for |φ| < π; at exactly θ = π the axis sign is the canonical-cover choice). NumPy equivalent: Rotation.from_matrix(R).as_rotvec().

Template Parameters:

T, TRAILING_SYNC – See skew.

Parameters:

R – Input 3x3 rotation (9 elements, column-major).
phi – Output rotation vector (3 elements; no aliasing at block/warp scope).

template<typename T, bool TRAILING_SYNC = true> void so3_right_jacobian(const T *phi, T *J)#

SO(3) right Jacobian: Jr(φ) = I − b·[φ]ₓ + c·[φ]ₓ².

b = (1−cosθ)/θ², c = (θ−sinθ)/θ³. Maps additive tangent perturbations to the manifold: exp(φ+δ) ≈ exp(φ)·exp(Jr(φ)·δ) — the object every on-manifold optimizer and covariance propagation needs. Identity: Jr(φ) = Jl(−φ).

Template Parameters:

T, TRAILING_SYNC – See skew.

Parameters:

phi – Rotation vector (3 elements).
J – Output 3x3 Jacobian (9 elements, column-major; no aliasing).

template<typename T, bool TRAILING_SYNC = true> void so3_right_jacobian_inv(const T *phi, T *J)#

Inverse right Jacobian: Jr(φ)⁻¹ = I + [φ]ₓ/2 + e·[φ]ₓ².

e = 1/θ² − cot(θ/2)/(2θ) (half-angle form — finite at θ = π, exactly 1/π²; series 1/12 + θ²/720 + … below θ = 1e-4). Jr⁻¹ pulls manifold differences back to the tangent (IMU preintegration, on-manifold GN steps). Genuinely singular only at θ = 2π, outside the canonical |φ| ≤ π range.

Template Parameters:

T, TRAILING_SYNC – See skew.

Parameters:

phi – Rotation vector (3 elements).
J – Output 3x3 inverse Jacobian (9 elements, column-major; no aliasing).

template<typename T, bool TRAILING_SYNC = true> void so3_left_jacobian(const T *phi, T *J)#

SO(3) left Jacobian: Jl(φ) = Jr(−φ) = I + b·[φ]ₓ + c·[φ]ₓ².

Also the SE(3) translation “V matrix”: exp_se3(ρ,φ) translates by Jl(φ)·ρ — some codebases (including the GRiD emitters this is promoted from) carry it under that name; it is the SAME matrix.

Template Parameters:

T, TRAILING_SYNC – See skew.

Parameters:

phi – Rotation vector (3 elements).
J – Output 3x3 Jacobian (9 elements, column-major; no aliasing).

template<typename T, bool TRAILING_SYNC = true> void so3_left_jacobian_inv(const T *phi, T *J)#

Inverse left Jacobian: Jl(φ)⁻¹ = I − [φ]ₓ/2 + e·[φ]ₓ².

Same e coefficient (and the same θ = 2π caveat) as so3_right_jacobian_inv; Jl⁻¹(φ) = Jr⁻¹(−φ).

Template Parameters:

T, TRAILING_SYNC – See skew.

Parameters:

phi – Rotation vector (3 elements).
J – Output 3x3 inverse Jacobian (9 elements, column-major; no aliasing).

template<typename T> void skew(const T *v, T *S)#

Single-warp hat map.

Single-thread hat map.

See glass::skew.

template<typename T> void so3_exp(const T *phi, T *R)#

Single-warp Rodrigues exponential.

Single-thread Rodrigues exponential.

See glass::so3_exp.

template<typename T> void so3_log(const T *R, T *phi)#

Single-warp SO(3) log (canonical branch).

Single-thread SO(3) log (canonical branch).

See glass::so3_log.

template<typename T> void so3_right_jacobian(const T *phi, T *J)#

Single-warp right Jacobian.

Single-thread right Jacobian.

See glass::so3_right_jacobian.

template<typename T> void so3_right_jacobian_inv(const T *phi, T *J)#

Single-warp inverse right Jacobian.

Single-thread inverse right Jacobian.

See glass::so3_right_jacobian_inv.

template<typename T> void so3_left_jacobian(const T *phi, T *J)#

Single-warp left Jacobian (SE(3) “V matrix”).

Single-thread left Jacobian (SE(3) “V matrix”).

See glass::so3_left_jacobian.

template<typename T> void so3_left_jacobian_inv(const T *phi, T *J)#

Single-warp inverse left Jacobian.

Single-thread inverse left Jacobian.

See glass::so3_left_jacobian_inv.

namespace lie_detail

Functions

template<typename T> void skew_core(const T *v, T *S)#

template<typename T> void mat3_mul_core(const T *A, const T *B, T *C)#

template<typename T> void mat3_vec_core(const T *A, const T *v, T *out)#

template<typename T> void rodrigues_core(const T *phi, T ca, T cb, T *M)#

template<typename T> void rodrigues_coefs(T theta, T &a, T &b, T &c)#

template<typename T> T vec3_norm(const T *v)#

template<typename T> void so3_exp_core(const T *phi, T *R)#

template<typename T> void so3_right_jacobian_core(const T *phi, T *J)#

template<typename T> void so3_left_jacobian_core(const T *phi, T *J)#

template<typename T> T inv_jacobian_coef(T theta)#

template<typename T> void so3_right_jacobian_inv_core(const T *phi, T *J)#

template<typename T> void so3_left_jacobian_inv_core(const T *phi, T *J)#

template<typename T> void so3_log_core(const T *R, T *phi)#

namespace warp

namespace thread

SE(3): retract + derivatives#

Functions

template<typename T, bool TRAILING_SYNC = true> void se3_Q_block(const T *rho, const T *phi, T *Q)#

Barfoot Q coupling block of the SE(3) exponential derivative (3x3).

The off-diagonal block of the SE(3) right Jacobian [[Jr(φ), Q(ρ,φ)],[0, Jr(φ)]]; Pinocchio sign convention (validated against pin.dIntegrate ARG1 in the GRiD arc this is promoted from). Series form below θ = 1e-4.

Template Parameters:

T – Scalar type (e.g. float, double).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

rho – Linear tangent (3 elements).
phi – Angular tangent (3 elements).
Q – Output 3x3 block (9 elements, column-major; no aliasing).

template<typename T, QuatLayout L = QuatLayout::xyzw, bool TRAILING_SYNC = true> void se3_retract(const T *pose, const T *rho, const T *phi, T *pose_new)#

SE(3) retract on a [p(3); quat(4)] pose block: pose_new = pose ⊞ (ρ, φ).

The free-flyer manifold update (one floating-base integrator step is se3_retract(pose, v_lin·dt, ω·dt, pose_new) with a BODY-frame twist): orientation q_new = normalize(q ⊗ exp([φ/2])), position p_new = p + R(q)·(Jl(φ)·ρ). Matches Pinocchio’s integrate on the free-flyer joint. Joint-space tails (revolute q += v·dt) are a plain vector add — compose with glass::axpy, they are not this op’s job.

Template Parameters:

T – Scalar type.
L – Quaternion layout inside the pose block (default xyzw — the Pinocchio nq layout [x,y,z, qx,qy,qz,qw]).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

pose – Input pose block (7 elements: position then quaternion).
rho – Linear tangent step (3 elements, body frame).
phi – Angular tangent step (3 elements, body frame).
pose_new – Output pose block (7 elements; no aliasing at block/warp scope).

template<typename T, QuatLayout L = QuatLayout::xyzw, bool TRAILING_SYNC = true> void se3_difference(const T *pose_from, const T *pose_to, T *rho, T *phi)#

SE(3) difference (boxminus): the tangent [ρ; φ] with se3_retract(pose_from, ρ, φ) == pose_to — the exact inverse of the retract, equal to Pinocchio difference(model, q_from, q_to) on the free-flyer block.

Canonical branch |φ| ≤ π.

φ = log(q_from⁻¹ ⊗ q_to), ρ = Jl(φ)⁻¹ · R(q_from)ᵀ · (p_to − p_from) (body-frame tangent, linear-first — the same conventions as the retract).

Template Parameters:

T – Scalar type.
L – Quaternion layout inside the pose blocks (default xyzw).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

pose_from – Base pose block (7 elements: position then quaternion).
pose_to – Target pose block (7 elements).
rho – Output linear tangent (3 elements; no aliasing at block scope).
phi – Output angular tangent (3 elements; no aliasing at block scope).

template<typename T, bool TRAILING_SYNC = true> void se3_retract_jacobian_q(const T *rho, const T *phi, T *J)#

6x6 derivative of the SE(3) retract w.r.t.

the BASE POSE: J = Ad_{exp(−[ρ;φ])} (Pinocchio dIntegrate ARG0).

Column-major, tangent ordered [ρ; φ] (linear-first): block form [[R⁻, [−Jl(−φ)ρ]ₓ·R⁻],[0, R⁻]] with R⁻ = exp(−[φ]ₓ).

Template Parameters:

T, TRAILING_SYNC – See se3_Q_block.

Parameters:

rho – Linear tangent (3 elements).
phi – Angular tangent (3 elements).
J – Output 6x6 block (36 elements, column-major; no aliasing).

template<typename T, bool TRAILING_SYNC = true> void se3_retract_jacobian_v(const T *rho, const T *phi, T *J)#

6x6 derivative of the SE(3) retract w.r.t.

the TANGENT: the SE(3) right Jacobian J = [[Jr(φ), Q(ρ,φ)],[0, Jr(φ)]] (Pinocchio dIntegrate ARG1).

Column-major, tangent ordered [ρ; φ] (linear-first).

Template Parameters:

T, TRAILING_SYNC – See se3_Q_block.

Parameters:

rho – Linear tangent (3 elements).
phi – Angular tangent (3 elements).
J – Output 6x6 block (36 elements, column-major; no aliasing).

template<typename T, bool IS_Q, bool TRAILING_SYNC = true> void se3_retract_hessian(const T *rho, const T *phi, T *J2)#

6x6x6 second derivative of the SE(3) retract: J2[k·36 + c·6 + r] = ∂J(r,c)/∂w_k, w = [ρ; φ].

The closed-form SE(3)-exp second derivative (structured chain rule on the Rodrigues/Q coefficient series; small-angle series below θ = 0.2), for IS_Q = true differentiating the base-pose block (se3_retract_jacobian_q) and for IS_Q = false the tangent block (se3_retract_jacobian_v). Computed in DOUBLE internally regardless of T (tiny once-per-step block; keeps float32 kernels matching float64 oracles — the validated design this is promoted from, mpmath-complex-step ground truth ≈1e-14). Layout: six stacked column-major 6x6 slices, one per tangent direction k.

Parallelism: the block/warp tiers stride the 6 directions over the active threads (each direction’s slice is computed serially by one thread).

Template Parameters:

T – Scalar type of the interface (output cast from double).
IS_Q – Differentiate the ARG0 (base-pose) block instead of ARG1.
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

rho – Linear tangent (3 elements).
phi – Angular tangent (3 elements).
J2 – Output tensor (216 elements; no aliasing).

template<typename T> void se3_Q_block(const T *rho, const T *phi, T *Q)#

Single-warp Barfoot Q block.

Single-thread Barfoot Q block.

See glass::se3_Q_block.

template<typename T, QuatLayout L = QuatLayout::xyzw> void se3_retract(const T *pose, const T *rho, const T *phi, T *pose_new)#

Single-warp SE(3) retract.

Single-thread SE(3) retract.

See glass::se3_retract.

template<typename T, QuatLayout L = QuatLayout::xyzw> void se3_difference(const T *pose_from, const T *pose_to, T *rho, T *phi)#

Single-warp SE(3) difference (boxminus).

Single-thread SE(3) difference (boxminus).

See glass::se3_difference.

template<typename T> void se3_retract_jacobian_q(const T *rho, const T *phi, T *J)#

Single-warp retract Jacobian w.r.t.

Single-thread retract Jacobian w.r.t.

the base pose. See glass::se3_retract_jacobian_q.

template<typename T> void se3_retract_jacobian_v(const T *rho, const T *phi, T *J)#

Single-warp retract Jacobian w.r.t.

Single-thread retract Jacobian w.r.t.

the tangent. See glass::se3_retract_jacobian_v.

template<typename T, bool IS_Q> void se3_retract_hessian(const T *rho, const T *phi, T *J2)#

Single-warp retract Hessian (double internals).

Single-thread retract Hessian (double internals).

See glass::se3_retract_hessian.

namespace lie_detail

Functions

template<typename T> void se3_Q_block_core(const T *rho, const T *phi, T *Q)#

template<typename T, QuatLayout L> void se3_retract_core(const T *pose, const T *rho, const T *phi, T *out)#

template<typename T, QuatLayout L> void se3_difference_core(const T *pose_from, const T *pose_to, T *rho, T *phi)#

template<typename T> void se3_retract_jacobian_q_core(const T *rho, const T *phi, T *J)#

template<typename T> void se3_retract_jacobian_v_core(const T *rho, const T *phi, T *J)#

void se3_d2_coefs(double t, double c[5], double dc[5])#

void se3_d2_dJr(const double phi[3], double t, const double c[5], const double dc[5], int k, double out[9])#

void se3_d2_dRinv(const double phi[3], double t, const double c[5], const double dc[5], int k, double out[9])#

void se3_d2_dQ(const double rho[3], const double phi[3], double t, const double c[5], const double dc[5], int kk, double out[9])#

void se3_d2_doff(const double rho[3], const double phi[3], double t, const double c[5], const double dc[5], int kk, double out[9])#

template<typename T, bool IS_Q> void se3_retract_hessian_slice(const double rho[3], const double phi[3], double t, const double c[5], const double dc[5], int k, T *J2_slice)#

namespace warp

namespace thread

Planar angles#

Functions

template<typename T> T angle_wrap(T x)#

Wrap an angle to the principal branch (−π, π].

Scalar, tier-free (see the header note). NumPy equivalent: np.arctan2(np.sin(x), np.cos(x)) — but computed by shifting, cheaper and exact for inputs already in range.

Template Parameters:: T – Scalar type (e.g. float, double).
Parameters:: x – Angle in radians (any magnitude).
Returns:: The equivalent angle in (−π, π].

template<typename T> T angle_diff(T a, T b)#

Shortest signed angular difference a − b, wrapped to (−π, π].

The SO(2) “boxminus”: the smallest-magnitude rotation taking b to a (positive = counterclockwise). Scalar, tier-free.

Template Parameters:

T – Scalar type.

Parameters:

a – First angle (radians).
b – Second angle (radians).

Returns:

angle_wrap(a − b).

template<typename T> T angle_lerp(T a, T b, T t)#

Wraparound-aware linear interpolation between two angles.

Interpolates along the SHORTEST arc: angle_lerp(a, b, t) = a + t·(b ⊖ a) wrapped back to (−π, π] — so interpolating 179° → −179° passes through 180°, not 0°. Scalar, tier-free.

Template Parameters:

T – Scalar type.

Parameters:

a – Start angle (radians).
b – End angle (radians).
t – Interpolation parameter (0 → a, 1 → b).

Returns:

The interpolated angle in (−π, π].

template<typename T> T clamp_unit(T v)#

Clamp to [−1, 1] — the acos/asin domain guard.

Dot products of nominally-unit vectors drift past ±1 in floating point; feeding acos a value like 1 + 1e-7 returns NaN. Clamp first: acos(clamp_unit(dot)). Scalar, tier-free.

Template Parameters:: T – Scalar type.
Parameters:: v – Input value.
Returns:: v clamped to [−1, 1].

namespace lie_detail

Functions

template<typename T> T pi()#

Cones & projections#

Functions

template<typename T> T soc_tail_norm(const T *g, int32_t m)#

Norm of the cone tail: ‖g[1:m]‖.

Serial per-caller scalar (tier-free — see the header note). The m ≤ ~16 regime these ops serve makes a serial loop the right shape.

Template Parameters:

T – Scalar type (e.g. float, double).

Parameters:

g – Cone-space vector (m elements; row 0 is the axis).
m – Vector length (axis + m−1 tail rows).

Returns:

sqrt(Σ_{i>=1} g[i]²).

template<typename T> T soc_violation(const T *g, int32_t m)#

Second-order-cone violation: max(0, ‖g[1:]‖ − g[0]).

The margin metric (0 inside the cone). NOT the Euclidean distance to K — outside both K and its polar the distance divides this by √2. Scalar, tier-free.

Template Parameters:

T – Scalar type.

Parameters:

g – Cone-space vector (m elements).
m – Vector length.

Returns:

The violation (>= 0).

template<typename T, bool TRAILING_SYNC = true> void soc_project(const T *w, T *p, int32_t m)#

Euclidean projection onto the second-order cone: p = Π_K(w).

K = {g : ‖g[1:m]‖ ≤ g[0]}. Idempotent; fixes points of K; maps the polar cone to 0; on the intermediate region returns the boundary point ((w0+r)/2)·(1, w̄/r). w and p MAY alias at thread:: scope (GATO’s calling pattern); at block/warp scope they must not (strided writers race a reader). m = 1 degenerates to max(0, ·) — the scalar hinge.

Template Parameters:

T – Scalar type.
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

w – Input vector (m elements).
p – Output projection (m elements).
m – Vector length.

template<typename T> T al_soc_value(const T *g, const T *lam, T rho, int32_t m)#

Conic PHR augmented-Lagrangian value: φ = (‖Π_K(λ − ρg)‖² − ‖λ‖²) / (2ρ).

The exact conic generalization of the hinge PHR term (m = 1 reduces to the g >= 0 hinge). C¹ in g; ∇_g φ = −Π_K(λ − ρg); GN Hessian = ρ·JΠ on the active set. Computed WITHOUT materializing the projection: ‖Π_K(w)‖² follows from the case split (inside: ‖w‖²; polar: 0; boundary: 2α² with α = (w0+r)/2). Scalar, tier-free.

Template Parameters:

T – Scalar type.

Parameters:

g – Constraint values in cone space (m elements).
lam – Multiplier estimate (m elements).
rho – Penalty parameter (> 0).
m – Vector length.

Returns:

The AL value contribution.

template<typename T> void soc_project(const T *w, T *p, int32_t m)#

Single-warp SOC projection.

Single-thread SOC projection (aliasing-safe).

See glass::soc_project.

namespace proj_detail#

namespace warp

namespace thread

Interval / AL / barrier scalars#

Functions

template<typename T> T interval_violation(T g, T lo, T hi)#

Interval violation: max(0, g − hi) + max(0, lo − g).

The true (unsigned) constraint violation of lo <= g <= hi; 0 when feasible. Scalar, tier-free.

Template Parameters:

T – Scalar type (e.g. float, double).

Parameters:

g – Constraint value.
lo – Lower bound (may be −inf).
hi – Upper bound (may be +inf).

Returns:

The violation (>= 0).

template<typename T> bool al_is_eq_row(T lo, T hi)#

Is this interval row an equality (lo == hi, finite)?

Scalar, tier-free. Equality rows carry a signed multiplier in the lam_hi slot of the interval AL ops.

template<typename T> T al_hinge_value(T c, T lam, T rho, T sigma)#

One hinge side’s PHR augmented-Lagrangian value (optionally elastic).

For the signed constraint c (feasible when c <= 0) with multiplier lam >= 0 and penalty rho: φ = (max(0, λ + ρc)² − λ²)/(2ρ), so dφ/dc = max(0, λ + ρc) and the GN Hessian is ρ on the active set. With sigma > 0 (L1 elastic slack weight) the activation saturates at sigma: beyond it φ = σc − (σ−λ)²/(2ρ) (linear; C¹ at the seam). sigma <= 0 keeps the exact hard path. Scalar, tier-free.

Template Parameters:

T – Scalar type.

Parameters:

c – Signed constraint value (g − hi, or lo − g).
lam – Multiplier (>= 0).
rho – Penalty parameter (> 0).
sigma – Elastic saturation weight (<= 0 disables softening).

Returns:

The AL value contribution.

template<typename T> T al_interval_value(T g, T lo, T hi, T lam_hi, T lam_lo, T rho, T sigma)#

PHR augmented-Lagrangian VALUE of an interval row lo <= g <= hi.

Splits into independent hinge sides (lam_hi for g <= hi, lam_lo for g >= lo); infinite bounds contribute 0; lo == hi rows are always-active equalities (φ = λc + ρc²/2, signed multiplier in lam_hi, elastic saturation symmetric at ±sigma). Scalar, tier-free. The matching outer multiplier update is λ ← max(0, λ + ρc) (equalities unclamped, elastic capped at sigma) on the ACCEPTED iterate.

Template Parameters:

T – Scalar type.

Parameters:

g – Constraint value.
lo, hi – Interval bounds (either may be infinite).
lam_hi – Upper-side multiplier (equality rows: the signed multiplier).
lam_lo – Lower-side multiplier (unused on equality rows).
rho – Penalty parameter (> 0).
sigma – Elastic saturation weight (<= 0 disables softening).

Returns:

The AL value contribution.

template<typename T> void al_interval_grad_hess(T g, T lo, T hi, T lam_hi, T lam_lo, T rho, T sigma, T &gr, T &h)#

PHR augmented-Lagrangian GRADIENT and GN HESSIAN of an interval row.

gr = dφ/dg, h = d²φ/dg² (GN: ρ per active side, 0 in the elastic saturation region). Same row semantics as al_interval_value. Scalar, tier-free.

Template Parameters:

T – Scalar type.

Parameters:

g, lo, hi, lam_hi, lam_lo, rho, sigma – See al_interval_value.
gr – Output gradient.
h – Output GN Hessian.

template<typename T> T relaxed_barrier_value(T d, T mu, T delta)#

C² relaxed log-barrier VALUE on a bound distance d.

B(d) = −μ·log(d) for d > δ; the C² quadratic extension −μ·(log δ − 3/2 + 2d/δ − d²/(2δ²)) for d <= δ — defined for ALL d (including infeasible d <= 0), Hessian bounded by μ/δ². Chain the sign of dd/dg at the call site (+1 lower bound, −1 upper). Scalar, tier-free.

Template Parameters:

T – Scalar type.

Parameters:

d – Distance to the bound (g − lo or hi − g).
mu – Barrier weight (> 0).
delta – Relaxation threshold (> 0).

Returns:

The barrier value.

template<typename T> T relaxed_barrier_grad(T d, T mu, T delta)#

Relaxed log-barrier derivative dB/dd.

See relaxed_barrier_value.

template<typename T> T relaxed_barrier_hess(T d, T mu, T delta)#

Relaxed log-barrier second derivative d²B/dd² (sign-free in the bound direction).

See relaxed_barrier_value.

template<typename T> T relaxed_barrier_interval_value(T g, T lo, T hi, T mu, T delta)#

Two-sided relaxed-barrier VALUE on g ∈ [lo, hi] (infinite bound → 0).

Scalar, tier-free. See relaxed_barrier_value for the one-sided form.

template<typename T> T relaxed_barrier_interval_grad(T g, T lo, T hi, T mu, T delta)#

Two-sided relaxed-barrier GRADIENT dB/dg.

See relaxed_barrier_interval_value.

template<typename T> T relaxed_barrier_interval_hess(T g, T lo, T hi, T mu, T delta)#

Two-sided relaxed-barrier HESSIAN d²B/dg².

See relaxed_barrier_interval_value.

template<typename T> T smooth_hinge(T d, T eta)#

Smooth hinge on a signed distance (collision-cost activation).

The CHOMP/cuRobo η-metric: for signed distance d (positive = clear) and activation width eta > 0, d <= 0 : −d + η/2 (linear in penetration) 0 < d < η : (d−η)²/(2η) (quadratic taper) d >= η : 0 (inactive) C¹ at both seams. Compose with sphere_*_dist for collision costs. Scalar, tier-free.

Template Parameters:

T – Scalar type.

Parameters:

d – Signed distance (positive = clear of contact).
eta – Activation width (> 0).

Returns:

The activation cost (>= 0).

template<typename T> T smooth_hinge_grad(T d, T eta)#

Smooth-hinge derivative d(cost)/dd.

See smooth_hinge.

template<typename T> T log_cosh(T x)#

Smooth absolute value: log(cosh(x)) (overflow-safe).

The classic C-infinity |x| surrogate of pose/tracking costs (cuRobo’s smooth pose-distance metric): quadratic (x²/2) near zero, asymptotically |x| − log 2. Evaluated as |x| + log1p(exp(−2|x|)) − log 2, which never overflows (naive log(cosh(x)) dies at |x| ≈ 89 in f32). For a scaled width use log_cosh(x/s)·s. Scalar, tier-free.

Template Parameters:: T – Scalar type (e.g. float, double).
Parameters:: x – Input value.
Returns:: log(cosh(x)) (>= 0).

template<typename T> T log_cosh_grad(T x)#

log_cosh derivative: tanh(x).

See log_cosh.

Geometry distances#

Functions

template<typename T> T sphere_sphere_dist(const T *c1, T r1, const T *c2, T r2)#

Sphere-sphere signed distance: ‖c1 − c2‖ − (r1 + r2).

Positive = separated, negative = penetrating. Scalar, tier-free.

Template Parameters:

T – Scalar type (e.g. float, double).

Parameters:

c1 – Center of sphere 1 (3 elements).
r1 – Radius of sphere 1.
c2 – Center of sphere 2 (3 elements).
r2 – Radius of sphere 2.

Returns:

The signed distance.

template<typename T> T sphere_sphere_dist(const T *c1, T r1, const T *c2, T r2, T *grad)#

Sphere-sphere signed distance with the gradient w.r.t.

c1.

grad = ∂d/∂c1 = (c1 − c2)/‖c1 − c2‖ (the unit separating direction; the gradient w.r.t. c2 is its negation). Coincident centers (‖·‖ below 1e-12) return a zero gradient — the caller sees the (measure-zero) degenerate case explicitly rather than a NaN. Scalar, tier-free.

Template Parameters:

T – Scalar type.

Parameters:

c1, r1, c2, r2 – See sphere_sphere_dist.
grad – Output ∂d/∂c1 (3 elements).

Returns:

The signed distance.

template<typename T> T sphere_box_dist(const T *c, T r, const T *half, T *grad)#

Sphere-box signed distance (box frame) with the gradient w.r.t.

the center.

The box is axis-aligned in ITS OWN frame, centered at the origin with half extents half[3]; transform the sphere center into the box frame first for an OBB (quat_rotate with the inverse box pose — the cuRobo pattern). Uses the canonical box SDF: with q_i = |c_i| − half_i, d_box = ‖max(q, 0)‖ + min(max_i q_i, 0); the sphere distance is d_box − r. Negative = penetrating. grad = ∂d/∂c: the normalized outward offset outside the box, the (sign-carrying) max-penetration face normal inside — the direction to move the CENTER to increase clearance. Scalar, tier-free.

Template Parameters:

T – Scalar type.

Parameters:

c – Sphere center IN THE BOX FRAME (3 elements).
r – Sphere radius.
half – Box half extents (3 elements, > 0).
grad – Output ∂d/∂c (3 elements; box frame).

Returns:

The signed distance.

template<typename T> T sphere_box_dist(const T *c, T r, const T *half)#

Sphere-box signed distance (box frame), distance only.

See the gradient overload for semantics.

Template Parameters:

T – Scalar type.

Parameters:

c – Sphere center IN THE BOX FRAME (3 elements).
r – Sphere radius.
half – Box half extents (3 elements, > 0).

Returns:

The signed distance.

template<typename T, QuatLayout L = QuatLayout::xyzw> void transform_sphere(const T *q, const T *p, const T *sph, T *out)#

Rigid-transform a sphere: rotate + translate the center, keep the radius.

sph = [cx, cy, cz, r] (the standard packed GPU collision-sphere layout); out[0:3] = R(q)·c + p, out[3] = r. q must be unit. Scalar, tier-free; out may alias sph.

Template Parameters:

T – Scalar type.
L – Quaternion layout of q (default xyzw).

Parameters:

q – Unit rotation quaternion (4 elements).
p – Translation (3 elements).
sph – Input sphere [c(3); r] (4 elements).
out – Output sphere [c(3); r] (4 elements; aliasing allowed).

Functions

template<typename T> void frame_from_vector(const T *n, T *t, T *b)#

Right-handed orthonormal tangent basis from a unit vector (branchless).

Given UNIT n, writes t and b with {t, b, n} orthonormal and right-handed (t × b = n). Duff et al. (JCGT 2017) construction: exact orthonormality to rounding for every input direction, no branch, no normalization — continuity breaks only across the single n_z sign change (unavoidable: no globally continuous tangent field exists on the sphere). n must be normalized (‖n‖ = 1); the basis quality degrades smoothly with ‖n‖ error. Scalar, tier-free.

Template Parameters:

T – Scalar type (e.g. float, double).

Parameters:

n – Unit vector (3 elements).
t – Output tangent (3 elements).
b – Output bitangent (3 elements).

Functions

template<typename T> T segment_segment_closest(const T *p1, const T *q1, const T *p2, const T *q2, T &s, T &t, T *c1, T *c2)#

Closest points between segments [p1, q1] and [p2, q2].

Returns the SQUARED distance between the closest points and writes the parameters s, t ∈ [0, 1] plus the points themselves (c1 = p1 + s·(q1−p1), c2 = p2 + t·(q2−p2)). Degenerate (point-like) segments and near-parallel pairs are handled via the 1e-12-scaled guards (Ericson §5.1.9); parallel overlapping segments return one valid minimizing pair. Capsule-capsule signed distance is sqrt(result) − (r1 + r2). Scalar, tier-free.

Template Parameters:

T – Scalar type (e.g. float, double).

Parameters:

p1, q1 – Endpoints of segment 1 (3 elements each).
p2, q2 – Endpoints of segment 2 (3 elements each).
s – Output parameter on segment 1 (in [0, 1]).
t – Output parameter on segment 2 (in [0, 1]).
c1 – Output closest point on segment 1 (3 elements).
c2 – Output closest point on segment 2 (3 elements).

Returns:

The squared distance ‖c1 − c2‖².

3x3 estimation kit#

Functions

template<typename T, bool TRAILING_SYNC = true> void eig3(const T *A, T *W, T *V)#

Symmetric 3x3 eigendecomposition: A = V·diag(W)·Vᵀ, W ASCENDING.

Fixed-sweep serial cyclic Jacobi (eigh_sweeps<T>() sweeps — the 3x3 sibling of glass::eigh; deterministic, bit-identical across thread counts and runs). A must be symmetric; only its stored values are read (no symmetrization). NumPy equivalent: W, V = np.linalg.eigh(A) (up to eigenvector signs).

Template Parameters:

T – Scalar type (e.g. float, double).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

A – Input symmetric 3x3 (9 elements, column-major).
W – Output eigenvalues, ascending (3 elements).
V – Output eigenvectors (9 elements, column-major; column k ↔ W[k]).

template<typename T, bool TRAILING_SYNC = true> void svd3(const T *A, T *U, T *S, T *V)#

General 3x3 SVD: A = U·diag(S)·Vᵀ, S DESCENDING (σ₁ ≥ σ₂ ≥ σ₃ ≥ 0).

Routed through eig3(AᵀA) with guarded left-vector recovery and deterministic Gram-Schmidt / cross-product completion for rank-deficient inputs (see the file header; note the √ε small-σ precision floor of the AᵀA squaring — prefer f64 for tight small-σ needs). U/V are orthogonal but may have det −1 (standard SVD semantics); closest_rotation applies the proper-rotation det fix. NumPy equivalent: U, S, Vt = np.linalg.svd(A) (up to paired column signs).

Template Parameters:

T, TRAILING_SYNC – See eig3.

Parameters:

A – Input 3x3 (9 elements, column-major; any matrix).
U – Output left singular vectors (9 elements, column-major).
S – Output singular values, descending (3 elements).
V – Output right singular vectors (9 elements, column-major, NOT transposed).

template<typename T, bool TRAILING_SYNC = true> void closest_rotation(const T *A, T *R)#

Closest proper rotation to a 3x3 matrix (Frobenius sense): R = U·diag(1, 1, det(U)·det(V))·Vᵀ from svd3(A).

ONE op, three classic jobs:

Re-orthonormalize a drifted rotation matrix (A ≈ R after integration).
The polar decomposition’s rotation factor (A = R·S, S symmetric PSD when det(A) ≥ 0).
The Kabsch / Wahba / Umeyama best-fit rotation: feed the cross covariance M = Σ b_i·a_iᵀ (centered correspondences, as glass::ger accumulations) and R = argmin_R Σ‖b_i − R·a_i‖². The det fix guarantees R ∈ SO(3) (det +1) for ANY input including det(A) < 0. Unique when σ₂ + σ₃ > 0 (i.e. rank ≥ 2 and not the degenerate reflection tie); the deterministic completion picks a fixed representative otherwise.

Template Parameters:

T, TRAILING_SYNC – See eig3.

Parameters:

A – Input 3x3 (9 elements, column-major).
R – Output rotation (9 elements, column-major, det +1; no aliasing).

template<typename T> void eig3(const T *A, T *W, T *V)#

Single-warp symmetric 3x3 eigendecomposition.

Single-thread symmetric 3x3 eigendecomposition.

See glass::eig3.

template<typename T> void svd3(const T *A, T *U, T *S, T *V)#

Single-warp 3x3 SVD.

Single-thread 3x3 SVD.

See glass::svd3.

template<typename T> void closest_rotation(const T *A, T *R)#

Single-warp closest proper rotation.

Single-thread closest proper rotation.

See glass::closest_rotation.

namespace est_detail#

Functions

template<typename T> T det3(const T *A)#

template<typename T> void eig3_core(const T *A, T *W, T *V)#

template<typename T> T dot3(const T *a, const T *b)#

template<typename T> void perp3(const T *u, T *out)#

template<typename T> void svd3_core(const T *A, T *U, T *S, T *V)#

template<typename T> void closest_rotation_core(const T *A, T *R)#

namespace warp

namespace thread

Fused Gauss-Newton / LM step#

Functions

template<typename T, uint32_t N> constexpr std::size_t gn_step_scratch_bytes()#

Scratch size in bytes for warp::gn_step’s normal-matrix buffer.

Template Parameters:

T – Scalar type.
N – Parameter count (columns of J).

Returns:

Bytes to allocate for s_A.

template<typename T, uint32_t M, uint32_t N, bool REGULARIZE = true, bool CHECK = true, bool REG_DIAG = true> void gn_step(const T *J, const T *r, T lambda, T *dq, T *s_A, int *s_fail = nullptr)#

Single-warp fused GN/LM step: form and solve (JᵀJ + lambda·shift) dq = Jᵀ r.

One 32-lane warp: s_A = JᵀJ (warp::syrk, lanes spread the N² accumulations), dq = Jᵀr (warp::gemv<TRANSPOSE>), then the flagged warp::posv shifts the diagonal (Marquardt lambda·diag(A) when REG_DIAG, else Levenberg lambda·I), factors, solves in place, and reports a non-PD pivot through s_fail when CHECK. On return dq holds the step, s_A holds the Cholesky factor of the shifted matrix (clobbered scratch). lambda = 0 with CHECK = true is plain Gauss-Newton with a rank guard. Full 32 lanes required; independent warps may run distinct problems concurrently.

Template Parameters:

T – Scalar type (use double for ill-conditioned J).
M – Rows of J (residual length; M >= N for a full-rank system).
N – Columns of J (parameter count; A is N x N).
REGULARIZE – Apply the lambda shift (default true; false compiles it out).
CHECK – Report a non-PD pivot via s_fail (default true).
REG_DIAG – Shift by lambda·diag(JᵀJ) (Marquardt, default true) instead of lambda·I (Levenberg).

Parameters:

J – Input Jacobian (M x N, column-major; read-only).
r – Input residual (M elements; read-only).
lambda – Damping factor (ignored when !REGULARIZE).
dq – Output step (N elements; warp-shared memory).
s_A – Scratch for the N x N normal matrix (warp-shared, N*N elements — see gn_step_scratch_bytes); clobbered.
s_fail – Non-PD flag when CHECK (set to 1 on failure, else 0).

namespace warp

Sampling reductions (L1)#

Functions

template<typename T> constexpr std::size_t softmax_scratch_bytes(uint32_t n)#

Shared-scratch size in bytes for softmax / logsumexp.

Both stage the scaled inputs / exponentials through an n-element buffer that the in-place reduction trees then consume.

Template Parameters:: T – Scalar type.
Parameters:: n – Number of elements.
Returns:: Bytes to allocate for s_scratch.

template<typename T, bool TRAILING_SYNC = true> void softmax(uint32_t n, T alpha, const T *x, T *y, T *s_scratch)#

Max-shifted softmax: y_i = exp(αx_i − M) / Σ_j exp(αx_j − M), M = max_j(αx_j).

Σ y = 1; α = −λ gives the MPPI path-integral weight update on a cost vector. Shift-invariant and overflow-safe by the max subtraction. y MAY alias x (same-index elementwise writes); s_scratch must be a distinct n-element buffer. Thread-count invariant (fixed-order reduction trees). NumPy equivalent: scipy.special.softmax(alpha * x).

Template Parameters:

T – Scalar type (e.g. float, double).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

n – Number of elements.
alpha – Scale on the inputs (temperature; negate for costs).
x – Input vector of length n.
y – Output weights of length n (may alias x).
s_scratch – Shared scratch of softmax_scratch_bytes<T>(n) bytes.

template<typename T, bool TRAILING_SYNC = true> void logsumexp(uint32_t n, T alpha, const T *x, T *out, T *s_scratch)#

Stable log-sum-exp: out[0] = M + log(Σ_j exp(αx_j − M)), M = max_j(αx_j).

The log-partition of softmax(α·x) — free energy in path-integral control, the stable normalizer everywhere else. Thread-count invariant; ends on the trailing sync so out[0] is block-visible. NumPy equivalent: scipy.special.logsumexp(alpha * x).

Template Parameters:

T, TRAILING_SYNC – See softmax.

Parameters:

n – Number of elements.
alpha – Scale on the inputs.
x – Input vector of length n.
out – Output: out[0] receives the log-sum-exp.
s_scratch – Shared scratch of softmax_scratch_bytes<T>(n) bytes.

template<typename T> void softmax(uint32_t n, T alpha, const T *x, T *y)#

Single-warp max-shifted softmax (no scratch).

Single-thread max-shifted softmax (no scratch).

See glass::softmax.

One full 32-lane warp; lanes stride the vector, the max and the sum fold through xor-butterfly shuffles (every lane holds the result — no shared memory, no __syncthreads). y may alias x. Full 32 lanes required.

See glass::softmax.

template<typename T> T logsumexp(uint32_t n, T alpha, const T *x)#

Single-warp stable log-sum-exp (register return on every lane).

Single-thread stable log-sum-exp (register return).

See glass::logsumexp. Full 32 lanes required.

See glass::logsumexp.

namespace softmax_detail#

Functions

template<typename T> T butterfly_max(T v)#

template<typename T> T butterfly_sum(T v)#

namespace warp

namespace thread

Functions

template<typename T> constexpr std::size_t argreduce_scratch_bytes(uint32_t block_threads)#

Shared-scratch size in bytes for argmax / argmin.

One signed key (T) plus one index (uint32_t) per thread — the same layout (and size) as iamax_scratch_bytes.

Template Parameters:: T – Scalar type.
Parameters:: block_threads – Number of threads in the launching block.
Returns:: Bytes to allocate for s_scratch.

template<typename T, bool TRAILING_SYNC = true> void argmax(uint32_t n, const T *x, uint32_t *out, T *s_scratch)#

Index of the maximum element (signed), into out[0].

np.argmax semantics with the LOWER-index tie-break at every combine step (thread-count invariant) and NaN skipped (see the header note; all-NaN → 0). Non-destructive. NumPy equivalent: int(np.argmax(x)) (NaN-free inputs).

Template Parameters:

T – Scalar type (e.g. float, double).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

n – Number of elements.
x – Read-only input vector of length n.
out – Output: out[0] receives the argmax index.
s_scratch – Shared scratch of argreduce_scratch_bytes<T>(blockDim) bytes.

template<typename T, bool TRAILING_SYNC = true> void argmax(uint32_t n, const T *x, uint32_t *out, T *out_val, T *s_scratch)#

argmax also returning the maximum value in out_val[0].

NumPy equivalents: out[0] = int(np.argmax(x)), out_val[0] = np.max(x) (NaN-free inputs; all-NaN returns x[0]).

Template Parameters:

T, TRAILING_SYNC – See argmax.

Parameters:

n, x, s_scratch – See argmax.
out – Output index slot.
out_val – Output value slot.

template<typename T, bool TRAILING_SYNC = true> void argmin(uint32_t n, const T *x, uint32_t *out, T *s_scratch)#

Index of the minimum element (signed), into out[0].

np.argmin semantics; tie-break, NaN policy, and invariance as argmax.

Template Parameters:: T, TRAILING_SYNC – See argmax.
Parameters:: n, x, out, s_scratch – See argmax.

template<typename T, bool TRAILING_SYNC = true> void argmin(uint32_t n, const T *x, uint32_t *out, T *out_val, T *s_scratch)#

argmin also returning the minimum value in out_val[0].

NumPy equivalents: out[0] = int(np.argmin(x)), out_val[0] = np.min(x).

Template Parameters:: T, TRAILING_SYNC – See argmax.
Parameters:: n, x, out, out_val, s_scratch – See the value-returning argmax.

template<typename T> constexpr std::size_t argreduce_fast_scratch_bytes(uint32_t block_threads)#

Shared-scratch size in bytes for argmax_fast / argmin_fast.

One (key, index) slot per warp — the same layout (and size) as iamax_fast_scratch_bytes.

Template Parameters:: T – Scalar type.
Parameters:: block_threads – Number of threads in the launching block.
Returns:: Bytes to allocate for the _fast scratch.

template<typename T, bool TRAILING_SYNC = true> void argmax_fast(uint32_t n, const T *x, uint32_t *out, T *s_scratch)#

argmax, warp-shuffle variant (in-register warp folds + per-warp scratch combine — the iamax_fast strategy, signed).

Bit-identical result to argmax (same combine, same tie-break/NaN policy); fewer scratch bytes and no serial thread-0 fold — the wide-block fast path. Scratch via argreduce_fast_scratch_bytes<T>(blockDim). REQUIRES a full-warp block size (a multiple of 32): the shuffle folds use the full 0xffffffff mask (the iamax_fast contract) — use the default argmax for partial-warp blocks.

Template Parameters:

T – Scalar type (e.g. float, double).
TRAILING_SYNC – Emit a trailing __syncthreads() (default true).

Parameters:

n – Number of elements.
x – Read-only input vector of length n.
out – Output: out[0] receives the argmax index.
s_scratch – Shared scratch of argreduce_fast_scratch_bytes<T>(blockDim) bytes.

template<typename T, bool TRAILING_SYNC = true> void argmax_fast(uint32_t n, const T *x, uint32_t *out, T *out_val, T *s_scratch)#

argmax_fast also returning the maximum value in out_val[0].

Template Parameters:

T, TRAILING_SYNC – See argmax_fast.

Parameters:

n, x, s_scratch – See argmax_fast.
out – Output index slot.
out_val – Output value slot.

template<typename T, bool TRAILING_SYNC = true> void argmin_fast(uint32_t n, const T *x, uint32_t *out, T *s_scratch)#

argmin, warp-shuffle variant.

See argmax_fast.

Template Parameters:: T, TRAILING_SYNC – See argmax_fast.
Parameters:: n, x, out, s_scratch – See argmax_fast.

template<typename T, bool TRAILING_SYNC = true> void argmin_fast(uint32_t n, const T *x, uint32_t *out, T *out_val, T *s_scratch)#

argmin_fast also returning the minimum value in out_val[0].

Template Parameters:: T, TRAILING_SYNC – See argmax_fast.
Parameters:: n, x, out, out_val, s_scratch – See the value-returning argmax_fast.

template<typename T> uint32_t argmax_pair(T key, uint32_t idx)#

Warp argmax over PER-LANE register (key, index) pairs; winning index returned on every lane.

The register entry point for “each lane evaluated its own candidate” patterns (warp-packed samplers, coarse-search sweeps, best-rollout picks): no array staging, no scratch — the pair folds through __shfl_down_sync with the SAME lower-index tie-break and NaN-skip combine as every other argreduction, so results are lane/order independent. Pass idx = UINT32_MAX from lanes with no candidate (they never win); if ALL lanes are empty the sentinel UINT32_MAX is returned. Full 32-lane warp required (mask 0xffffffff).

Template Parameters:

T – Scalar type (e.g. float, double).

Parameters:

key – This lane’s candidate key.
idx – This lane’s candidate index (UINT32_MAX = empty lane).

Returns:

The winning index, identical on every lane.

template<typename T> uint32_t argmax_pair(T key, uint32_t idx, T &win_key)#

argmax_pair also returning the winning key on every lane.

Template Parameters:

T – Scalar type.

Parameters:

key, idx – See argmax_pair.
win_key – Out: the winning key (valid on every lane; unspecified when all lanes are empty).

Returns:

The winning index, identical on every lane.

template<typename T> uint32_t argmin_pair(T key, uint32_t idx)#

Warp argmin over PER-LANE register (key, index) pairs.

See argmax_pair (same mechanism, minimum direction).

Template Parameters:: T – Scalar type.
Parameters:: key, idx – See argmax_pair.
Returns:: The winning index, identical on every lane.

template<typename T> uint32_t argmin_pair(T key, uint32_t idx, T &win_key)#

argmin_pair also returning the winning key on every lane.

Template Parameters:: T – Scalar type.
Parameters:: key, idx, win_key – See the value-returning argmax_pair.
Returns:: The winning index, identical on every lane.

template<typename T> uint32_t argmax(uint32_t n, const T *x)#

Single-warp argmax, index returned on every lane (register broadcast, no scratch).

Single-thread argmax (register return).

Full 32 lanes required. See glass::argmax and the warp::iamax notes (same mechanism, signed).

See glass::argmax.

template<typename T> uint32_t argmin(uint32_t n, const T *x)#

Single-warp argmin, index returned on every lane.

Single-thread argmin (register return).

Full 32 lanes required. See glass::argmin.

See glass::argmin.

namespace argreduce_detail#

Functions

template<typename T, bool MINIMUM> void combine(T &key, uint32_t &idx, T ckey, uint32_t cidx)#

template<typename T, bool MINIMUM, bool TRAILING_SYNC, typename Key = IdKey> void argreduce(uint32_t n, const T *x, uint32_t *out, T *out_val, T *s_scratch)#

template<typename T, bool MINIMUM, bool TRAILING_SYNC, typename Key = IdKey> void argreduce_fast(uint32_t n, const T *x, uint32_t *out, T *out_val, T *s_scratch)#

template<typename T, bool MINIMUM, typename Key = IdKey> uint32_t argreduce_warp(uint32_t n, const T *x)#

template<typename T, bool MINIMUM, typename Key = IdKey> uint32_t argreduce_serial(uint32_t n, const T *x)#

template<typename T, bool MINIMUM> uint32_t argreduce_pair(T key, uint32_t idx, T *win_key)#

struct AbsKey#

Public Functions

template<typename T> inline T operator()(T v) const#

template<typename T> inline T empty(const T*) const#

struct IdKey#

Public Functions

template<typename T> inline T operator()(T v) const#

template<typename T> inline T empty(const T *x) const#

namespace warp

namespace thread