# `Monty.Frameworks.Utils.SpatialArithmetics`
[🔗](https://github.com/d-led/ex.tbp.monty/blob/v0.1.0/packages/ex_tbp_monty_core/lib/monty/frameworks/utils/spatial_arithmetics.ex#L1)

Vector and rotation arithmetic on pose vectors.

Mirrors `tbp/monty/frameworks/utils/spatial_arithmetics.py`. Everything here
answers one of two questions: *how far apart are these directions*, or *what
does this rotation do to these directions*.

## What a pose is

A pose vector set is three row vectors — the surface normal and the two
principal curvature directions — held as a 3×3 list of lists, in Monty's
`[normal, curvature_1, curvature_2]` order. Features carry the same thing
flattened to nine numbers, so the batch helpers take rows of nine.

## Two divergences from the Python, both deliberate

  * **Quaternions are rotations.** Python returns `[x, y, z, w]` arrays from
    scipy's `as_quat()`, while Monty's own `QuaternionWXYZ` and its configs
    write `w` first. Rather than pass an array whose meaning depends on which
    library produced it, the converters return a `Monty.Geometry.Rotation`,
    which has exactly one spelling.
  * **An undefined angle is `:infinity`.** Python returns `np.inf`; there is
    no float infinity in the BEAM, and `:infinity` sorts above every number,
    so `get_angle_beefed_up/2 < tolerance` still reads as false — the answer
    callers want. Trying to do arithmetic on it raises, which is better than
    quietly propagating a NaN.

# `angle`

```elixir
@type angle() :: float() | :infinity
```

# `matrix`

```elixir
@type matrix() :: [[float()]]
```

# `vector`

```elixir
@type vector() :: [float()]
```

# `add`

```elixir
@spec add([number()], [number()]) :: vector()
```

Adds two 3-vectors.

# `align_multiple_orthonormal_vectors_as_matrices`

```elixir
@spec align_multiple_orthonormal_vectors_as_matrices([matrix()], matrix()) :: [
  matrix()
]
```

The matrix that turns each of `frames` into `target`, one per frame.

Monty asks for these rather than for rotations (`as_scipy=False` in
`align_multiple_orthonormal_vectors`), and that is the form a learning module
keeps: a stored node frame is only *approximately* orthonormal — YCB's surface
normals and curvature directions are unit to a few parts in a thousand and
perpendicular to a few hundredths — so the product is only approximately a
rotation. Rounding it to the nearest rotation instead, as a rotation object
does, moves the pose by hundredths of a radian: the displacement it rotates
drifts by millimetres a step, and the pose evidence drifts with it.

# `align_orthonormal_vectors`

```elixir
@spec align_orthonormal_vectors(matrix(), matrix()) ::
  {Monty.Geometry.Rotation.t(), float()}
```

The rotation that turns the frame `m1` into the frame `m2`, and how badly it
fails to.

Rows are vectors, so the alignment rotation is `m1ᵀ · m2` — `m2`'s rows
expressed in `m1`'s basis. The error is how far applying that rotation's
inverse to `m1` lands from `m2`: ~0 for a right-handed pair, and large for a
mirrored one, which is why Monty kept the number around.

# `align_orthonormal_vectors_as_matrix`

```elixir
@spec align_orthonormal_vectors_as_matrix(matrix(), matrix()) :: matrix()
```

The rotation *matrix* that turns the frame `m1` into the frame `m2`, with no
rotation object in the way.

A mirrored pair has no matrix that turns one into the other; the matrix here
is the reflection itself, which is why `align_orthonormal_vectors/2` measures
its error against the nearest proper rotation instead.

# `apply_rf_transform_to_points`

```elixir
@spec apply_rf_transform_to_points(
  [vector()],
  %{required(String.t()) =&gt; term()},
  vector(),
  vector(),
  Monty.Geometry.Rotation.t(),
  number()
) :: {[vector()], %{required(String.t()) =&gt; term()}}
```

Transforms observed points into the model's reference frame.

`locations` are sensor positions in the body frame, `features` the features
observed there. The two reference points — where that spot sits in the model
and where it sits in the body — are the same physical point seen twice, so
centring on one, rotating, and shifting to the other is the whole transform.
`object_rotation` expresses how the model must be rotated to match the
observations, hence its inverse here.

`object_scale` is accepted and ignored, as in Monty.

# `batch_rotate_pose_vectors`

```elixir
@spec batch_rotate_pose_vectors(Monty.Geometry.SurfacePoseVectors.t(), [matrix()]) ::
  [matrix()]
```

Applies each of `rotations` to the same pose vector set.

Returns one rotated 3×3 set per rotation, in the rotations' order.

# `check_orthonormal`

```elixir
@spec check_orthonormal(matrix()) :: boolean()
```

Whether a matrix is orthonormal: mutually perpendicular unit rows.

Both halves come from `transpose(m) · m` being the identity — off-diagonal
entries measure non-perpendicularity, diagonal ones measure row length. Monty
allows a mean absolute error of 0.01, generous enough to accept a curvature
estimate that is only roughly orthogonal.

# `cross`

```elixir
@spec cross([number()], [number()]) :: vector()
```

The cross product of two 3-vectors.

# `dot`

```elixir
@spec dot([number()], [number()]) :: number()
```

The dot product of two 3-vectors.

# `euler_to_rotations`

```elixir
@spec euler_to_rotations([vector()], keyword()) :: [Monty.Geometry.Rotation.t()]
```

Converts a list of Euler angles, in degrees, to rotations.

# `get_angle`

```elixir
@spec get_angle([number()], [number()]) :: float()
```

The angle between two *unit* vectors, in radians.

Assumes unit input for speed — the surface normals and curvature directions
this is used on already are. Use `get_angle_between/2` for vectors of any
length, or `get_angle_beefed_up/2` when either side may be undefined.

# `get_angle_beefed_up`

```elixir
@spec get_angle_beefed_up([number()] | nil, [number()] | nil) :: angle()
```

The angle between two vectors, in radians, or `:infinity` when either is
undefined (not a vector) or has no direction (the zero vector).

An undefined direction is similar to nothing, which is the point: a hypothesis
with no curvature direction should be excluded from a comparison, not win it
by looking maximally unlike its competitors along one particular axis.

# `get_angle_between`

```elixir
@spec get_angle_between([number()], [number()]) :: float()
```

The angle between two vectors of any length, in radians.

The magnitudes cancel inside `atan2`, so no normalisation is needed. A zero
vector contains no direction, and 0.0 is the answer Monty documents for it.

# `get_angles_for_all_hypotheses`

```elixir
@spec get_angles_for_all_hypotheses([[vector()]], [vector()]) :: [[float()]]
```

The angles between each hypothesis' neighbours and its query vector.

`hypothesis_features` is one list of neighbour vectors per hypothesis,
`query_features` one vector per hypothesis; the result is one list of angles
per hypothesis, in the neighbours' order.

# `get_more_directions_in_plane`

```elixir
@spec get_more_directions_in_plane(matrix(), pos_integer()) :: [matrix()]
```

Unit vectors evenly spaced in the plane orthogonal to `frame`'s first vector.

`frame` holds the normal and the two curvature directions; `count` directions
are sampled by turning the first curvature direction towards the second. This
is how a flat surface, whose curvature directions are arbitrary, still offers
Monty a spread of candidate poses.

# `get_right_hand_angle`

```elixir
@spec get_right_hand_angle([number()], [number()], [number()]) :: float()
```

The angle from `v1` to `v2`, in radians, signed by `surface_normal`.

Both directions live in the plane, so the sign is what "clockwise seen from
the normal" means — the sign is the whole reason to prefer this over
`get_angle/2`, which cannot tell a turn from its mirror image.

# `get_unique_rotations`

```elixir
@spec get_unique_rotations([[Monty.Geometry.Rotation.t()]], float(), keyword()) ::
  {[vector()], [Monty.Geometry.Rotation.t()]}
```

The distinct rotations among `poses`, as `{euler_poses, rotations}`.

`poses` is a list of paths, each a list of rotations; a rotation is kept the
first time it appears within `similarity_th` radians of everything kept so
far. The Euler angles come back rounded to 3 decimals and wrapped into
`[0, 360)`, because they are used as human-readable labels.

`:reverse?` (default `true`) returns the rotations that undo each kept pose,
matching Monty's default.

# `inverse`

```elixir
@spec inverse(matrix()) :: matrix() | :singular
```

The inverse of a 3×3 matrix, or `:singular` when it has none.

Cofactor expansion, which in three dimensions is both exact and shorter than
any iterative method.

# `machine_epsilon`

```elixir
@spec machine_epsilon() :: float()
```

The gap between 1.0 and the next float, as `sys.float_info.epsilon`.

# `magnitude`

```elixir
@spec magnitude([number()]) :: float()
```

The length of a 3-vector.

# `non_singular_mat`

```elixir
@spec non_singular_mat(matrix()) :: boolean()
```

Whether a matrix can be inverted.

Monty asks numpy for the condition number — how much an input error can be
amplified by inverting — and accepts anything below `1/ε`. Without LAPACK the
estimate here is the 1-norm condition number `‖a‖₁ · ‖a⁻¹‖₁`, a small factor
away from numpy's 2-norm figure and sharing the property that matters: a
singular matrix has no inverse, so both are infinite.

# `norm_1`

```elixir
@spec norm_1(matrix()) :: float()
```

The 1-norm of a matrix: its largest absolute column sum.

# `normalize`

```elixir
@spec normalize([number()], float()) :: vector()
```

A unit vector in the direction of `v`.

Raises when `v` is shorter than `epsilon`: there is no direction to return for
the zero vector, and returning zero would put a non-unit vector into a pose
that later arithmetic assumes is unit.

# `parallel?`

```elixir
@spec parallel?([number()], [number()], float()) :: boolean()
```

Whether two *unit* vectors point along the same line, in either direction.

`tolerance` bounds `1 - |cos θ|`, so it reads as "the angle is within
`sqrt(2 · tolerance)` of 0 or π" — and, unlike comparing a dot product to 1,
it treats both ends of the line alike.

Monty calls this `is_parallel`; Elixir reserves the `is_` prefix for guards,
so the name is the one thing here that changed shape.

# `pose_is_new`

```elixir
@spec pose_is_new([Monty.Geometry.Rotation.t()], Monty.Geometry.Rotation.t(), float()) ::
  boolean()
```

Whether `new_pose` differs from every pose in `all_poses` by at least
`similarity_th` radians.

# `project_onto_tangent_plane`

```elixir
@spec project_onto_tangent_plane([number()], [number()]) :: vector()
```

The part of `v` lying in the plane with normal `n`.

`v` minus its projection onto `n`, so a vector parallel to the normal collapses
to zero and a vector already in the plane is unchanged.

# `rotate_multiple_pose_vectors`

```elixir
@spec rotate_multiple_pose_vectors([vector()], Monty.Geometry.Rotation.t()) :: [
  vector()
]
```

Rotates rows of flattened pose vectors — nine numbers, three vectors each.

The rows keep their order and their length; only the directions change.

# `rotate_pose_vectors`

```elixir
@spec rotate_pose_vectors(
  Monty.Geometry.SurfacePoseVectors.t() | Monty.Geometry.GazeDirection.t(),
  Monty.Geometry.Rotation.t()
) :: Monty.Geometry.SurfacePoseVectors.t()
```

Rotates a pose vector set by one rotation.

A gaze direction is where to look, not a surface frame, so rotating it is
meaningless; it raises rather than returning something that looks like a pose.

# `rotation_matrices_to_rotations`

```elixir
@spec rotation_matrices_to_rotations([matrix()], keyword()) :: [
  Monty.Geometry.Rotation.t()
]
```

Converts a list of rotation matrices to rotations.

# `rotations_to_rotations`

```elixir
@spec rotations_to_rotations([vector() | matrix()], keyword()) :: [
  Monty.Geometry.Rotation.t()
]
```

Converts rotations to `Monty.Geometry.Rotation`s.

Euler angles (a flat three-list) are what the feature LM produces, rotation
matrices (a list of three rows) what the evidence LM produces; the shape says
which, exactly as `ndim` does in Python. `invert: true` returns the rotations
that undo the given ones.

# `scale`

```elixir
@spec scale([number()], number()) :: vector()
```

Multiplies a 3-vector by a scalar.

# `subtract`

```elixir
@spec subtract([number()], [number()]) :: vector()
```

Subtracts two 3-vectors.

---

*Consult [api-reference.md](api-reference.md) for complete listing*