refactor(pose_3d): move normalize_quaternion to geometry module

src/presa_mano/geometry.py

@@ -2,6 +2,8 @@
 
 from __future__ import annotations
 
+import math
+
 import numpy as np
 
 
@@ -60,6 +62,16 @@ def R_to_quat_xyzw(R: np.ndarray) -> np.ndarray:
     return np.array([x, y, z, w])
 
 
+def normalize_quaternion(q: list[float]) -> list[float]:
+    """Normalizza un quaternion a lunghezza unitaria. Solleva ValueError se
+    la norma e' troppo piccola per essere normalizzata in modo sensato
+    (es. quaternion zero, sotto 1e-6)."""
+    n = math.sqrt(sum(float(x)*float(x) for x in q))
+    if n < 1e-6:
+        raise ValueError(f"quaternion is degenerate (norm={n:.3e})")
+    return [float(x) / n for x in q]
+
+
 def make_T(R: np.ndarray, t: np.ndarray) -> np.ndarray:
     T = np.eye(4)
     T[:3, :3] = R