numkong 7.0.0

package/include/numkong/mesh.hpp

@@ -0,0 +1,762 @@
+/**
+ *  @brief C++ bindings for mesh-distance kernels.
+ *  @file include/numkong/mesh.hpp
+ *  @author Ash Vardanian
+ *  @date February 5, 2026
+ */
+#ifndef NK_MESH_HPP
+#define NK_MESH_HPP
+
+#include <cstdint>
+#include <type_traits>
+#include <utility>
+
+#include "numkong/mesh.h"
+
+#include "numkong/types.hpp"
+
+namespace ashvardanian::numkong {
+
+#pragma region - SVD Helpers for Scalar Fallbacks
+
+/** @brief 3x3 matrix determinant. */
+template <typename scalar_type_>
+scalar_type_ det3x3_(scalar_type_ const *m) {
+    return m[0] * (m[4] * m[8] - m[5] * m[7]) - m[1] * (m[3] * m[8] - m[5] * m[6]) + m[2] * (m[3] * m[7] - m[4] * m[6]);
+}
+
+/** @brief Conditional swap helper. */
+template <typename scalar_type_>
+void conditional_swap_(bool c, scalar_type_ *x, scalar_type_ *y) {
+    scalar_type_ temp = *x;
+    *x = c ? *y : *x;
+    *y = c ? temp : *y;
+}
+
+/** @brief Conditional negating swap helper. */
+template <typename scalar_type_>
+void conditional_negating_swap_(bool c, scalar_type_ *x, scalar_type_ *y) {
+    scalar_type_ neg_x = scalar_type_(0.0) - *x;
+    *x = c ? *y : *x;
+    *y = c ? neg_x : *y;
+}
+
+/** @brief Approximate Givens quaternion for Jacobi eigenanalysis. */
+template <typename scalar_type_>
+void approximate_givens_quaternion_(scalar_type_ a11, scalar_type_ a12, scalar_type_ a22, scalar_type_ *cos_half,
+                                    scalar_type_ *sin_half) {
+    constexpr scalar_type_ gamma_k = scalar_type_(5.828427124746190);  // gamma = (sqrt8 + 3)^2 / 4
+    constexpr scalar_type_ cstar_k = scalar_type_(0.9238795325112867); // cos(pi/8)
+    constexpr scalar_type_ sstar_k = scalar_type_(0.3826834323650898); // sin(pi/8)
+
+    *cos_half = scalar_type_(2.0) * (a11 - a22);
+    *sin_half = a12;
+    bool use_givens = gamma_k * (*sin_half) * (*sin_half) < (*cos_half) * (*cos_half);
+    scalar_type_ w = ((*cos_half) * (*cos_half) + (*sin_half) * (*sin_half)).rsqrt();
+    *cos_half = use_givens ? w * (*cos_half) : cstar_k;
+    *sin_half = use_givens ? w * (*sin_half) : sstar_k;
+}
+
+/** @brief Jacobi conjugation step for eigenanalysis. */
+template <typename scalar_type_>
+void jacobi_conjugation_(int idx_x, int idx_y, int idx_z, scalar_type_ *s11, scalar_type_ *s21, scalar_type_ *s22,
+                         scalar_type_ *s31, scalar_type_ *s32, scalar_type_ *s33, scalar_type_ *quat) {
+
+    scalar_type_ cos_half, sin_half;
+    approximate_givens_quaternion_(*s11, *s21, *s22, &cos_half, &sin_half);
+    scalar_type_ scale = cos_half * cos_half + sin_half * sin_half;
+    scalar_type_ cos_theta = (cos_half * cos_half - sin_half * sin_half) / scale;
+    scalar_type_ sin_theta = (scalar_type_(2.0) * sin_half * cos_half) / scale;
+    scalar_type_ s11_old = *s11, s21_old = *s21, s22_old = *s22;
+    scalar_type_ s31_old = *s31, s32_old = *s32, s33_old = *s33;
+
+    *s11 = cos_theta * (cos_theta * s11_old + sin_theta * s21_old) +
+           sin_theta * (cos_theta * s21_old + sin_theta * s22_old);
+    *s21 = cos_theta * ((scalar_type_(0.0) - sin_theta) * s11_old + cos_theta * s21_old) +
+           sin_theta * ((scalar_type_(0.0) - sin_theta) * s21_old + cos_theta * s22_old);
+    *s22 = (scalar_type_(0.0) - sin_theta) * ((scalar_type_(0.0) - sin_theta) * s11_old + cos_theta * s21_old) +
+           cos_theta * ((scalar_type_(0.0) - sin_theta) * s21_old + cos_theta * s22_old);
+    *s31 = cos_theta * s31_old + sin_theta * s32_old;
+    *s32 = (scalar_type_(0.0) - sin_theta) * s31_old + cos_theta * s32_old;
+    *s33 = s33_old;
+
+    // Update quaternion accumulator
+    scalar_type_ quat_temp[3];
+    quat_temp[0] = quat[0] * sin_half;
+    quat_temp[1] = quat[1] * sin_half;
+    quat_temp[2] = quat[2] * sin_half;
+    sin_half = sin_half * quat[3];
+    quat[0] = quat[0] * cos_half;
+    quat[1] = quat[1] * cos_half;
+    quat[2] = quat[2] * cos_half;
+    quat[3] = quat[3] * cos_half;
+    quat[idx_z] = quat[idx_z] + sin_half;
+    quat[3] = quat[3] - quat_temp[idx_z];
+    quat[idx_x] = quat[idx_x] + quat_temp[idx_y];
+    quat[idx_y] = quat[idx_y] - quat_temp[idx_x];
+    // Cyclic permutation of matrix elements
+    s11_old = *s22, s21_old = *s32, s22_old = *s33, s31_old = *s21, s32_old = *s31, s33_old = *s11;
+    *s11 = s11_old, *s21 = s21_old, *s22 = s22_old, *s31 = s31_old, *s32 = s32_old, *s33 = s33_old;
+}
+
+/** @brief Convert quaternion to 3x3 rotation matrix. */
+template <typename scalar_type_>
+void quaternion_to_mat3x3_(scalar_type_ const *quat, scalar_type_ *matrix) {
+    scalar_type_ w = quat[3], x = quat[0], y = quat[1], z = quat[2];
+    scalar_type_ q_xx = x * x, q_yy = y * y, q_zz = z * z;
+    scalar_type_ q_xz = x * z, q_xy = x * y, q_yz = y * z;
+    scalar_type_ q_wx = w * x, q_wy = w * y, q_wz = w * z;
+    matrix[0] = scalar_type_(1.0) - scalar_type_(2.0) * (q_yy + q_zz);
+    matrix[1] = scalar_type_(2.0) * (q_xy - q_wz);
+    matrix[2] = scalar_type_(2.0) * (q_xz + q_wy);
+    matrix[3] = scalar_type_(2.0) * (q_xy + q_wz);
+    matrix[4] = scalar_type_(1.0) - scalar_type_(2.0) * (q_xx + q_zz);
+    matrix[5] = scalar_type_(2.0) * (q_yz - q_wx);
+    matrix[6] = scalar_type_(2.0) * (q_xz - q_wy);
+    matrix[7] = scalar_type_(2.0) * (q_yz + q_wx);
+    matrix[8] = scalar_type_(1.0) - scalar_type_(2.0) * (q_xx + q_yy);
+}
+
+/** @brief Jacobi eigenanalysis for symmetric 3x3 matrix. */
+template <typename scalar_type_>
+void jacobi_eigenanalysis_(scalar_type_ *s11, scalar_type_ *s21, scalar_type_ *s22, scalar_type_ *s31,
+                           scalar_type_ *s32, scalar_type_ *s33, scalar_type_ *quat) {
+    quat[0] = scalar_type_(0.0);
+    quat[1] = scalar_type_(0.0);
+    quat[2] = scalar_type_(0.0);
+    quat[3] = scalar_type_(1.0);
+    // 16 iterations for better convergence
+    for (unsigned int iter = 0; iter < 16; iter++) {
+        jacobi_conjugation_(0, 1, 2, s11, s21, s22, s31, s32, s33, quat);
+        jacobi_conjugation_(1, 2, 0, s11, s21, s22, s31, s32, s33, quat);
+        jacobi_conjugation_(2, 0, 1, s11, s21, s22, s31, s32, s33, quat);
+    }
+    scalar_type_ norm = (quat[0] * quat[0] + quat[1] * quat[1] + quat[2] * quat[2] + quat[3] * quat[3]).rsqrt();
+    quat[0] = quat[0] * norm;
+    quat[1] = quat[1] * norm;
+    quat[2] = quat[2] * norm;
+    quat[3] = quat[3] * norm;
+}
+
+/** @brief QR Givens quaternion for QR decomposition. */
+template <typename scalar_type_>
+void qr_givens_quaternion_(scalar_type_ a1, scalar_type_ a2, scalar_type_ *cos_half, scalar_type_ *sin_half) {
+    constexpr scalar_type_ epsilon_k = scalar_type_(1e-12);
+
+    scalar_type_ a1_sq_plus_a2_sq = a1 * a1 + a2 * a2;
+    scalar_type_ rho = a1_sq_plus_a2_sq * a1_sq_plus_a2_sq.rsqrt();
+    rho = a1_sq_plus_a2_sq > epsilon_k ? rho : scalar_type_(0.0);
+    *sin_half = rho > epsilon_k ? a2 : scalar_type_(0.0);
+    scalar_type_ abs_a1 = a1 < scalar_type_(0.0) ? (scalar_type_(0.0) - a1) : a1;
+    scalar_type_ max_rho = rho > epsilon_k ? rho : epsilon_k;
+    *cos_half = abs_a1 + max_rho;
+    bool should_swap = a1 < scalar_type_(0.0);
+    conditional_swap_(should_swap, sin_half, cos_half);
+    scalar_type_ w = ((*cos_half) * (*cos_half) + (*sin_half) * (*sin_half)).rsqrt();
+    *cos_half = (*cos_half) * w;
+    *sin_half = (*sin_half) * w;
+}
+
+/** @brief Sort singular values in descending order. */
+template <typename scalar_type_>
+void sort_singular_values_(scalar_type_ *b, scalar_type_ *v) {
+    scalar_type_ rho1 = b[0] * b[0] + b[3] * b[3] + b[6] * b[6];
+    scalar_type_ rho2 = b[1] * b[1] + b[4] * b[4] + b[7] * b[7];
+    scalar_type_ rho3 = b[2] * b[2] + b[5] * b[5] + b[8] * b[8];
+    bool should_swap;
+    // Sort columns by descending singular value magnitude
+    should_swap = rho1 < rho2;
+    conditional_negating_swap_(should_swap, &b[0], &b[1]);
+    conditional_negating_swap_(should_swap, &v[0], &v[1]);
+    conditional_negating_swap_(should_swap, &b[3], &b[4]);
+    conditional_negating_swap_(should_swap, &v[3], &v[4]);
+    conditional_negating_swap_(should_swap, &b[6], &b[7]);
+    conditional_negating_swap_(should_swap, &v[6], &v[7]);
+    conditional_swap_(should_swap, &rho1, &rho2);
+    should_swap = rho1 < rho3;
+    conditional_negating_swap_(should_swap, &b[0], &b[2]);
+    conditional_negating_swap_(should_swap, &v[0], &v[2]);
+    conditional_negating_swap_(should_swap, &b[3], &b[5]);
+    conditional_negating_swap_(should_swap, &v[3], &v[5]);
+    conditional_negating_swap_(should_swap, &b[6], &b[8]);
+    conditional_negating_swap_(should_swap, &v[6], &v[8]);
+    conditional_swap_(should_swap, &rho1, &rho3);
+    should_swap = rho2 < rho3;
+    conditional_negating_swap_(should_swap, &b[1], &b[2]);
+    conditional_negating_swap_(should_swap, &v[1], &v[2]);
+    conditional_negating_swap_(should_swap, &b[4], &b[5]);
+    conditional_negating_swap_(should_swap, &v[4], &v[5]);
+    conditional_negating_swap_(should_swap, &b[7], &b[8]);
+    conditional_negating_swap_(should_swap, &v[7], &v[8]);
+}
+
+/** @brief QR decomposition of 3x3 matrix. */
+template <typename scalar_type_>
+void qr_decomposition_(scalar_type_ const *input, scalar_type_ *q, scalar_type_ *r) {
+    scalar_type_ cos_half_1, sin_half_1;
+    scalar_type_ cos_half_2, sin_half_2;
+    scalar_type_ cos_half_3, sin_half_3;
+    scalar_type_ cos_theta, sin_theta;
+    scalar_type_ rotation_temp[9], matrix_temp[9];
+    // First Givens rotation (zero input[3])
+    qr_givens_quaternion_(input[0], input[3], &cos_half_1, &sin_half_1);
+    cos_theta = scalar_type_(1.0) - scalar_type_(2.0) * sin_half_1 * sin_half_1;
+    sin_theta = scalar_type_(2.0) * cos_half_1 * sin_half_1;
+    rotation_temp[0] = cos_theta * input[0] + sin_theta * input[3];
+    rotation_temp[1] = cos_theta * input[1] + sin_theta * input[4];
+    rotation_temp[2] = cos_theta * input[2] + sin_theta * input[5];
+    rotation_temp[3] = (scalar_type_(0.0) - sin_theta) * input[0] + cos_theta * input[3];
+    rotation_temp[4] = (scalar_type_(0.0) - sin_theta) * input[1] + cos_theta * input[4];
+    rotation_temp[5] = (scalar_type_(0.0) - sin_theta) * input[2] + cos_theta * input[5];
+    rotation_temp[6] = input[6];
+    rotation_temp[7] = input[7];
+    rotation_temp[8] = input[8];
+    // Second Givens rotation (zero rotation_temp[6])
+    qr_givens_quaternion_(rotation_temp[0], rotation_temp[6], &cos_half_2, &sin_half_2);
+    cos_theta = scalar_type_(1.0) - scalar_type_(2.0) * sin_half_2 * sin_half_2;
+    sin_theta = scalar_type_(2.0) * cos_half_2 * sin_half_2;
+    matrix_temp[0] = cos_theta * rotation_temp[0] + sin_theta * rotation_temp[6];
+    matrix_temp[1] = cos_theta * rotation_temp[1] + sin_theta * rotation_temp[7];
+    matrix_temp[2] = cos_theta * rotation_temp[2] + sin_theta * rotation_temp[8];
+    matrix_temp[3] = rotation_temp[3];
+    matrix_temp[4] = rotation_temp[4];
+    matrix_temp[5] = rotation_temp[5];
+    matrix_temp[6] = (scalar_type_(0.0) - sin_theta) * rotation_temp[0] + cos_theta * rotation_temp[6];
+    matrix_temp[7] = (scalar_type_(0.0) - sin_theta) * rotation_temp[1] + cos_theta * rotation_temp[7];
+    matrix_temp[8] = (scalar_type_(0.0) - sin_theta) * rotation_temp[2] + cos_theta * rotation_temp[8];
+    // Third Givens rotation (zero matrix_temp[7])
+    qr_givens_quaternion_(matrix_temp[4], matrix_temp[7], &cos_half_3, &sin_half_3);
+    cos_theta = scalar_type_(1.0) - scalar_type_(2.0) * sin_half_3 * sin_half_3;
+    sin_theta = scalar_type_(2.0) * cos_half_3 * sin_half_3;
+    r[0] = matrix_temp[0];
+    r[1] = matrix_temp[1];
+    r[2] = matrix_temp[2];
+    r[3] = cos_theta * matrix_temp[3] + sin_theta * matrix_temp[6];
+    r[4] = cos_theta * matrix_temp[4] + sin_theta * matrix_temp[7];
+    r[5] = cos_theta * matrix_temp[5] + sin_theta * matrix_temp[8];
+    r[6] = (scalar_type_(0.0) - sin_theta) * matrix_temp[3] + cos_theta * matrix_temp[6];
+    r[7] = (scalar_type_(0.0) - sin_theta) * matrix_temp[4] + cos_theta * matrix_temp[7];
+    r[8] = (scalar_type_(0.0) - sin_theta) * matrix_temp[5] + cos_theta * matrix_temp[8];
+    // Construct Q = Q1 * Q2 * Q3 (closed-form expressions)
+    scalar_type_ sin_half_1_sq = sin_half_1 * sin_half_1;
+    scalar_type_ sin_half_2_sq = sin_half_2 * sin_half_2;
+    scalar_type_ sin_half_3_sq = sin_half_3 * sin_half_3;
+    q[0] = (scalar_type_(-1.0) + scalar_type_(2.0) * sin_half_1_sq) *
+           (scalar_type_(-1.0) + scalar_type_(2.0) * sin_half_2_sq);
+    q[1] = scalar_type_(4.0) * cos_half_2 * cos_half_3 * (scalar_type_(-1.0) + scalar_type_(2.0) * sin_half_1_sq) *
+               sin_half_2 * sin_half_3 +
+           scalar_type_(2.0) * cos_half_1 * sin_half_1 * (scalar_type_(-1.0) + scalar_type_(2.0) * sin_half_3_sq);
+    q[2] = scalar_type_(4.0) * cos_half_1 * cos_half_3 * sin_half_1 * sin_half_3 -
+           scalar_type_(2.0) * cos_half_2 * (scalar_type_(-1.0) + scalar_type_(2.0) * sin_half_1_sq) * sin_half_2 *
+               (scalar_type_(-1.0) + scalar_type_(2.0) * sin_half_3_sq);
+    q[3] = scalar_type_(2.0) * cos_half_1 * sin_half_1 * (scalar_type_(1.0) - scalar_type_(2.0) * sin_half_2_sq);
+    q[4] = scalar_type_(-8.0) * cos_half_1 * cos_half_2 * cos_half_3 * sin_half_1 * sin_half_2 * sin_half_3 +
+           (scalar_type_(-1.0) + scalar_type_(2.0) * sin_half_1_sq) *
+               (scalar_type_(-1.0) + scalar_type_(2.0) * sin_half_3_sq);
+    q[5] = scalar_type_(-2.0) * cos_half_3 * sin_half_3 +
+           scalar_type_(4.0) * sin_half_1 *
+               (cos_half_3 * sin_half_1 * sin_half_3 +
+                cos_half_1 * cos_half_2 * sin_half_2 * (scalar_type_(-1.0) + scalar_type_(2.0) * sin_half_3_sq));
+    q[6] = scalar_type_(2.0) * cos_half_2 * sin_half_2;
+    q[7] = scalar_type_(2.0) * cos_half_3 * (scalar_type_(1.0) - scalar_type_(2.0) * sin_half_2_sq) * sin_half_3;
+    q[8] = (scalar_type_(-1.0) + scalar_type_(2.0) * sin_half_2_sq) *
+           (scalar_type_(-1.0) + scalar_type_(2.0) * sin_half_3_sq);
+}
+
+/** @brief 3x3 SVD: A = U * S * Vt using McAdams algorithm. */
+template <typename scalar_type_>
+void svd3x3_(scalar_type_ const *a, scalar_type_ *svd_u, scalar_type_ *svd_s, scalar_type_ *svd_v) {
+    // Compute At * A (symmetric)
+    scalar_type_ ata[9];
+    ata[0] = a[0] * a[0] + a[3] * a[3] + a[6] * a[6];
+    ata[1] = a[0] * a[1] + a[3] * a[4] + a[6] * a[7];
+    ata[2] = a[0] * a[2] + a[3] * a[5] + a[6] * a[8];
+    ata[3] = ata[1];
+    ata[4] = a[1] * a[1] + a[4] * a[4] + a[7] * a[7];
+    ata[5] = a[1] * a[2] + a[4] * a[5] + a[7] * a[8];
+    ata[6] = ata[2];
+    ata[7] = ata[5];
+    ata[8] = a[2] * a[2] + a[5] * a[5] + a[8] * a[8];
+    // Jacobi eigenanalysis of At * A
+    scalar_type_ quat[4];
+    jacobi_eigenanalysis_(&ata[0], &ata[1], &ata[4], &ata[2], &ata[5], &ata[8], quat);
+    quaternion_to_mat3x3_(quat, svd_v);
+    // B = A * V
+    scalar_type_ product[9];
+    product[0] = a[0] * svd_v[0] + a[1] * svd_v[3] + a[2] * svd_v[6];
+    product[1] = a[0] * svd_v[1] + a[1] * svd_v[4] + a[2] * svd_v[7];
+    product[2] = a[0] * svd_v[2] + a[1] * svd_v[5] + a[2] * svd_v[8];
+    product[3] = a[3] * svd_v[0] + a[4] * svd_v[3] + a[5] * svd_v[6];
+    product[4] = a[3] * svd_v[1] + a[4] * svd_v[4] + a[5] * svd_v[7];
+    product[5] = a[3] * svd_v[2] + a[4] * svd_v[5] + a[5] * svd_v[8];
+    product[6] = a[6] * svd_v[0] + a[7] * svd_v[3] + a[8] * svd_v[6];
+    product[7] = a[6] * svd_v[1] + a[7] * svd_v[4] + a[8] * svd_v[7];
+    product[8] = a[6] * svd_v[2] + a[7] * svd_v[5] + a[8] * svd_v[8];
+    // Sort singular values and update V
+    sort_singular_values_(product, svd_v);
+    // Compute singular values from column norms of sorted B
+    scalar_type_ s1_sq = product[0] * product[0] + product[3] * product[3] + product[6] * product[6];
+    scalar_type_ s2_sq = product[1] * product[1] + product[4] * product[4] + product[7] * product[7];
+    scalar_type_ s3_sq = product[2] * product[2] + product[5] * product[5] + product[8] * product[8];
+    // QR decomposition: B = U * R
+    scalar_type_ qr_r[9];
+    qr_decomposition_(product, svd_u, qr_r);
+    // Store singular values in diagonal of svd_s
+    svd_s[0] = s1_sq.sqrt();
+    svd_s[1] = scalar_type_(0.0);
+    svd_s[2] = scalar_type_(0.0);
+    svd_s[3] = scalar_type_(0.0);
+    svd_s[4] = s2_sq.sqrt();
+    svd_s[5] = scalar_type_(0.0);
+    svd_s[6] = scalar_type_(0.0);
+    svd_s[7] = scalar_type_(0.0);
+    svd_s[8] = s3_sq.sqrt();
+}
+
+#pragma endregion - SVD Helpers for Scalar Fallbacks
+
+#pragma region - Mesh Alignment Kernels
+
+/**
+ *  @brief Root Mean Square Deviation between two 3D point clouds (no alignment)
+ *  @param[in] a,b Point clouds [d x 3] interleaved (x0,y0,z0, x1,y1,z1, ...)
+ *  @param[in] d Number of 3D points
+ *  @param[out] a_centroid,b_centroid Centroids (3 values each), can be nullptr
+ *  @param[out] rotation 3x3 rotation matrix (9 values), always identity, can be nullptr
+ *  @param[out] scale Scale factor, always 1.0, can be nullptr
+ *  @param[out] metric Output RMSD value
+ *
+ *  @tparam in_type_ Input point type (f32_t, f64_t, f16_t, bf16_t)
+ *  @tparam transform_type_ Type of centroids, rotation, and scale outputs
+ *  @tparam metric_type_ Type of the scalar fit metric output
+ *  @tparam allow_simd_ Enable SIMD kernel dispatch when `prefer_simd_k`
+ */
+template <typename in_type_, typename transform_type_ = typename in_type_::mesh_transform_t,
+          typename metric_type_ = typename in_type_::mesh_metric_t,
+          allow_simd_t allow_simd_ = prefer_simd_k>
+void rmsd(                                               //
+    in_type_ const *a, in_type_ const *b, std::size_t n, //
+    transform_type_ *a_centroid, transform_type_ *b_centroid, transform_type_ *rotation, transform_type_ *scale,
+    metric_type_ *metric) noexcept {
+    constexpr bool simd = allow_simd_ == prefer_simd_k &&
+                          std::is_same_v<transform_type_, typename in_type_::mesh_transform_t> &&
+                          std::is_same_v<metric_type_, typename in_type_::mesh_metric_t>;
+
+    if constexpr (std::is_same_v<in_type_, f64_t> && simd)
+        nk_rmsd_f64(&a->raw_, &b->raw_, n, &a_centroid->raw_, &b_centroid->raw_, &rotation->raw_, &scale->raw_,
+                    &metric->raw_);
+    else if constexpr (std::is_same_v<in_type_, f32_t> && simd)
+        nk_rmsd_f32(&a->raw_, &b->raw_, n, &a_centroid->raw_, &b_centroid->raw_, &rotation->raw_, &scale->raw_,
+                    &metric->raw_);
+    else if constexpr (std::is_same_v<in_type_, f16_t> && simd)
+        nk_rmsd_f16(&a->raw_, &b->raw_, n, &a_centroid->raw_, &b_centroid->raw_, &rotation->raw_, &scale->raw_,
+                    &metric->raw_);
+    else if constexpr (std::is_same_v<in_type_, bf16_t> && simd)
+        nk_rmsd_bf16(&a->raw_, &b->raw_, n, &a_centroid->raw_, &b_centroid->raw_, &rotation->raw_,
+                     scale ? &scale->raw_ : nullptr, &metric->raw_);
+    // Scalar fallback
+    else {
+        // Step 1: Compute centroids
+        metric_type_ sum_a_x {}, sum_a_y {}, sum_a_z {};
+        metric_type_ sum_b_x {}, sum_b_y {}, sum_b_z {};
+        metric_type_ val_a_x, val_a_y, val_a_z, val_b_x, val_b_y, val_b_z;
+
+        for (std::size_t i = 0; i < n; i++) {
+            val_a_x = metric_type_(a[i * 3 + 0]);
+            val_a_y = metric_type_(a[i * 3 + 1]);
+            val_a_z = metric_type_(a[i * 3 + 2]);
+            val_b_x = metric_type_(b[i * 3 + 0]);
+            val_b_y = metric_type_(b[i * 3 + 1]);
+            val_b_z = metric_type_(b[i * 3 + 2]);
+            sum_a_x = sum_a_x + val_a_x;
+            sum_a_y = sum_a_y + val_a_y;
+            sum_a_z = sum_a_z + val_a_z;
+            sum_b_x = sum_b_x + val_b_x;
+            sum_b_y = sum_b_y + val_b_y;
+            sum_b_z = sum_b_z + val_b_z;
+        }
+
+        metric_type_ inv_n = metric_type_(1.0) / metric_type_(static_cast<double>(n));
+        metric_type_ centroid_a_x = sum_a_x * inv_n;
+        metric_type_ centroid_a_y = sum_a_y * inv_n;
+        metric_type_ centroid_a_z = sum_a_z * inv_n;
+        metric_type_ centroid_b_x = sum_b_x * inv_n;
+        metric_type_ centroid_b_y = sum_b_y * inv_n;
+        metric_type_ centroid_b_z = sum_b_z * inv_n;
+
+        // Step 2: Store centroids if requested
+        if (a_centroid)
+            a_centroid[0] = transform_type_(centroid_a_x), a_centroid[1] = transform_type_(centroid_a_y),
+            a_centroid[2] = transform_type_(centroid_a_z);
+        if (b_centroid)
+            b_centroid[0] = transform_type_(centroid_b_x), b_centroid[1] = transform_type_(centroid_b_y),
+            b_centroid[2] = transform_type_(centroid_b_z);
+
+        // Step 3: RMSD uses identity rotation and scale=1.0
+        if (rotation) {
+            rotation[0] = transform_type_(1.0);
+            rotation[1] = transform_type_(0.0);
+            rotation[2] = transform_type_(0.0);
+            rotation[3] = transform_type_(0.0);
+            rotation[4] = transform_type_(1.0);
+            rotation[5] = transform_type_(0.0);
+            rotation[6] = transform_type_(0.0);
+            rotation[7] = transform_type_(0.0);
+            rotation[8] = transform_type_(1.0);
+        }
+        if (scale) *scale = transform_type_(1.0);
+
+        // Step 4: Compute RMSD between centered point clouds
+        metric_type_ sum_squared {};
+        for (std::size_t i = 0; i < n; i++) {
+            val_a_x = metric_type_(a[i * 3 + 0]);
+            val_a_y = metric_type_(a[i * 3 + 1]);
+            val_a_z = metric_type_(a[i * 3 + 2]);
+            val_b_x = metric_type_(b[i * 3 + 0]);
+            val_b_y = metric_type_(b[i * 3 + 1]);
+            val_b_z = metric_type_(b[i * 3 + 2]);
+            metric_type_ dx = (val_a_x - centroid_a_x) - (val_b_x - centroid_b_x);
+            metric_type_ dy = (val_a_y - centroid_a_y) - (val_b_y - centroid_b_y);
+            metric_type_ dz = (val_a_z - centroid_a_z) - (val_b_z - centroid_b_z);
+            sum_squared = sum_squared + dx * dx + dy * dy + dz * dz;
+        }
+
+        *metric = (sum_squared * inv_n).sqrt();
+    }
+}
+
+/**
+ *  @brief Kabsch algorithm: min ‖P − R × Q‖² over rotation R ∈ SO(3)
+ *  @param[in] a,b Point clouds [n x 3] interleaved (source and target)
+ *  @param[in] n Number of 3D points
+ *  @param[out] a_centroid,b_centroid Centroids (3 values each), can be nullptr
+ *  @param[out] rotation 3x3 rotation matrix (9 values, row-major), can be nullptr
+ *  @param[out] scale Scale factor, always 1.0 for Kabsch, can be nullptr
+ *  @param[out] metric Output RMSD after optimal rotation
+ *
+ *  @tparam in_type_ Input point type (f32_t, f64_t, f16_t, bf16_t)
+ *  @tparam transform_type_ Type of centroids, rotation, and scale outputs
+ *  @tparam metric_type_ Type of the scalar fit metric output
+ *  @tparam allow_simd_ Enable SIMD kernel dispatch when `prefer_simd_k`
+ */
+template <typename in_type_, typename transform_type_ = typename in_type_::mesh_transform_t,
+          typename metric_type_ = typename in_type_::mesh_metric_t,
+          allow_simd_t allow_simd_ = prefer_simd_k>
+void kabsch(                                             //
+    in_type_ const *a, in_type_ const *b, std::size_t n, //
+    transform_type_ *a_centroid, transform_type_ *b_centroid, transform_type_ *rotation, transform_type_ *scale,
+    metric_type_ *metric) noexcept {
+    constexpr bool simd = allow_simd_ == prefer_simd_k &&
+                          std::is_same_v<transform_type_, typename in_type_::mesh_transform_t> &&
+                          std::is_same_v<metric_type_, typename in_type_::mesh_metric_t>;
+
+    if constexpr (std::is_same_v<in_type_, f64_t> && simd)
+        nk_kabsch_f64(&a->raw_, &b->raw_, n, a_centroid ? &a_centroid->raw_ : nullptr, &b_centroid->raw_,
+                      &rotation->raw_, &scale->raw_, &metric->raw_);
+    else if constexpr (std::is_same_v<in_type_, f32_t> && simd)
+        nk_kabsch_f32(&a->raw_, &b->raw_, n, &a_centroid->raw_, &b_centroid->raw_, &rotation->raw_, &scale->raw_,
+                      &metric->raw_);
+    else if constexpr (std::is_same_v<in_type_, f16_t> && simd)
+        nk_kabsch_f16(&a->raw_, &b->raw_, n, &a_centroid->raw_, &b_centroid->raw_, &rotation->raw_, &scale->raw_,
+                      &metric->raw_);
+    else if constexpr (std::is_same_v<in_type_, bf16_t> && simd)
+        nk_kabsch_bf16(&a->raw_, &b->raw_, n, &a_centroid->raw_, &b_centroid->raw_, &rotation->raw_, &scale->raw_,
+                       &metric->raw_);
+    // Scalar fallback
+    else {
+        // Step 1: Compute centroids
+        metric_type_ sum_a_x {}, sum_a_y {}, sum_a_z {};
+        metric_type_ sum_b_x {}, sum_b_y {}, sum_b_z {};
+        metric_type_ val_a_x, val_a_y, val_a_z, val_b_x, val_b_y, val_b_z;
+
+        for (std::size_t i = 0; i < n; i++) {
+            val_a_x = metric_type_(a[i * 3 + 0]);
+            val_a_y = metric_type_(a[i * 3 + 1]);
+            val_a_z = metric_type_(a[i * 3 + 2]);
+            val_b_x = metric_type_(b[i * 3 + 0]);
+            val_b_y = metric_type_(b[i * 3 + 1]);
+            val_b_z = metric_type_(b[i * 3 + 2]);
+            sum_a_x = sum_a_x + val_a_x;
+            sum_a_y = sum_a_y + val_a_y;
+            sum_a_z = sum_a_z + val_a_z;
+            sum_b_x = sum_b_x + val_b_x;
+            sum_b_y = sum_b_y + val_b_y;
+            sum_b_z = sum_b_z + val_b_z;
+        }
+
+        metric_type_ inv_n = metric_type_(1.0) / metric_type_(static_cast<double>(n));
+        metric_type_ centroid_a_x = sum_a_x * inv_n;
+        metric_type_ centroid_a_y = sum_a_y * inv_n;
+        metric_type_ centroid_a_z = sum_a_z * inv_n;
+        metric_type_ centroid_b_x = sum_b_x * inv_n;
+        metric_type_ centroid_b_y = sum_b_y * inv_n;
+        metric_type_ centroid_b_z = sum_b_z * inv_n;
+
+        if (a_centroid)
+            a_centroid[0] = transform_type_(centroid_a_x), a_centroid[1] = transform_type_(centroid_a_y),
+            a_centroid[2] = transform_type_(centroid_a_z);
+
+        if (b_centroid)
+            b_centroid[0] = transform_type_(centroid_b_x), b_centroid[1] = transform_type_(centroid_b_y),
+            b_centroid[2] = transform_type_(centroid_b_z);
+
+        // Step 2: Build 3x3 covariance matrix H = (A - A_bar)^T x (B - B_bar)
+        metric_type_ cross_covariance[9] = {};
+        for (std::size_t i = 0; i < n; i++) {
+            val_a_x = metric_type_(a[i * 3 + 0]) - centroid_a_x;
+            val_a_y = metric_type_(a[i * 3 + 1]) - centroid_a_y;
+            val_a_z = metric_type_(a[i * 3 + 2]) - centroid_a_z;
+            val_b_x = metric_type_(b[i * 3 + 0]) - centroid_b_x;
+            val_b_y = metric_type_(b[i * 3 + 1]) - centroid_b_y;
+            val_b_z = metric_type_(b[i * 3 + 2]) - centroid_b_z;
+            cross_covariance[0] = cross_covariance[0] + val_a_x * val_b_x;
+            cross_covariance[1] = cross_covariance[1] + val_a_x * val_b_y;
+            cross_covariance[2] = cross_covariance[2] + val_a_x * val_b_z;
+            cross_covariance[3] = cross_covariance[3] + val_a_y * val_b_x;
+            cross_covariance[4] = cross_covariance[4] + val_a_y * val_b_y;
+            cross_covariance[5] = cross_covariance[5] + val_a_y * val_b_z;
+            cross_covariance[6] = cross_covariance[6] + val_a_z * val_b_x;
+            cross_covariance[7] = cross_covariance[7] + val_a_z * val_b_y;
+            cross_covariance[8] = cross_covariance[8] + val_a_z * val_b_z;
+        }
+
+        // Step 3: SVD of H = U * S * Vt
+        metric_type_ svd_u[9], svd_s[9], svd_v[9];
+        svd3x3_(cross_covariance, svd_u, svd_s, svd_v);
+
+        // Step 4: R = V * Ut
+        metric_type_ rotation_matrix[9];
+        rotation_matrix[0] = svd_v[0] * svd_u[0] + svd_v[1] * svd_u[1] + svd_v[2] * svd_u[2];
+        rotation_matrix[1] = svd_v[0] * svd_u[3] + svd_v[1] * svd_u[4] + svd_v[2] * svd_u[5];
+        rotation_matrix[2] = svd_v[0] * svd_u[6] + svd_v[1] * svd_u[7] + svd_v[2] * svd_u[8];
+        rotation_matrix[3] = svd_v[3] * svd_u[0] + svd_v[4] * svd_u[1] + svd_v[5] * svd_u[2];
+        rotation_matrix[4] = svd_v[3] * svd_u[3] + svd_v[4] * svd_u[4] + svd_v[5] * svd_u[5];
+        rotation_matrix[5] = svd_v[3] * svd_u[6] + svd_v[4] * svd_u[7] + svd_v[5] * svd_u[8];
+        rotation_matrix[6] = svd_v[6] * svd_u[0] + svd_v[7] * svd_u[1] + svd_v[8] * svd_u[2];
+        rotation_matrix[7] = svd_v[6] * svd_u[3] + svd_v[7] * svd_u[4] + svd_v[8] * svd_u[5];
+        rotation_matrix[8] = svd_v[6] * svd_u[6] + svd_v[7] * svd_u[7] + svd_v[8] * svd_u[8];
+
+        // Handle reflection: if det(R) < 0, negate third column of V and recompute R
+        metric_type_ rotation_det = det3x3_(rotation_matrix);
+        if (rotation_det < metric_type_(0.0)) {
+            svd_v[2] = metric_type_(0.0) - svd_v[2];
+            svd_v[5] = metric_type_(0.0) - svd_v[5];
+            svd_v[8] = metric_type_(0.0) - svd_v[8];
+            rotation_matrix[0] = svd_v[0] * svd_u[0] + svd_v[1] * svd_u[1] + svd_v[2] * svd_u[2];
+            rotation_matrix[1] = svd_v[0] * svd_u[3] + svd_v[1] * svd_u[4] + svd_v[2] * svd_u[5];
+            rotation_matrix[2] = svd_v[0] * svd_u[6] + svd_v[1] * svd_u[7] + svd_v[2] * svd_u[8];
+            rotation_matrix[3] = svd_v[3] * svd_u[0] + svd_v[4] * svd_u[1] + svd_v[5] * svd_u[2];
+            rotation_matrix[4] = svd_v[3] * svd_u[3] + svd_v[4] * svd_u[4] + svd_v[5] * svd_u[5];
+            rotation_matrix[5] = svd_v[3] * svd_u[6] + svd_v[4] * svd_u[7] + svd_v[5] * svd_u[8];
+            rotation_matrix[6] = svd_v[6] * svd_u[0] + svd_v[7] * svd_u[1] + svd_v[8] * svd_u[2];
+            rotation_matrix[7] = svd_v[6] * svd_u[3] + svd_v[7] * svd_u[4] + svd_v[8] * svd_u[5];
+            rotation_matrix[8] = svd_v[6] * svd_u[6] + svd_v[7] * svd_u[7] + svd_v[8] * svd_u[8];
+        }
+
+        // Output rotation matrix and scale=1.0
+        if (rotation) {
+            for (unsigned int j = 0; j < 9; j++) rotation[j] = transform_type_(rotation_matrix[j]);
+        }
+        if (scale) *scale = transform_type_(1.0);
+
+        // Step 5: Compute RMSD after rotation
+        metric_type_ sum_squared {};
+        for (std::size_t i = 0; i < n; i++) {
+            metric_type_ point_a[3], point_b[3], rotated_point_a[3];
+            point_a[0] = metric_type_(a[i * 3 + 0]) - centroid_a_x;
+            point_a[1] = metric_type_(a[i * 3 + 1]) - centroid_a_y;
+            point_a[2] = metric_type_(a[i * 3 + 2]) - centroid_a_z;
+            point_b[0] = metric_type_(b[i * 3 + 0]) - centroid_b_x;
+            point_b[1] = metric_type_(b[i * 3 + 1]) - centroid_b_y;
+            point_b[2] = metric_type_(b[i * 3 + 2]) - centroid_b_z;
+            rotated_point_a[0] = rotation_matrix[0] * point_a[0] + rotation_matrix[1] * point_a[1] +
+                                 rotation_matrix[2] * point_a[2];
+            rotated_point_a[1] = rotation_matrix[3] * point_a[0] + rotation_matrix[4] * point_a[1] +
+                                 rotation_matrix[5] * point_a[2];
+            rotated_point_a[2] = rotation_matrix[6] * point_a[0] + rotation_matrix[7] * point_a[1] +
+                                 rotation_matrix[8] * point_a[2];
+            metric_type_ dx = rotated_point_a[0] - point_b[0];
+            metric_type_ dy = rotated_point_a[1] - point_b[1];
+            metric_type_ dz = rotated_point_a[2] - point_b[2];
+            sum_squared = sum_squared + dx * dx + dy * dy + dz * dz;
+        }
+
+        *metric = (sum_squared * inv_n).sqrt();
+    }
+}
+
+/**
+ *  @brief Umeyama algorithm: min ‖P − s × R × Q‖² over R ∈ SO(3), s ∈ ℝ⁺
+ *  @param[in] a,b Point clouds [n x 3] interleaved (source and target)
+ *  @param[in] n Number of 3D points
+ *  @param[out] a_centroid,b_centroid Centroids (3 values each), can be nullptr
+ *  @param[out] rotation 3x3 rotation matrix (9 values, row-major), can be nullptr
+ *  @param[out] scale Uniform scale factor, can be nullptr
+ *  @param[out] metric Output RMSD after optimal transformation
+ *
+ *  @tparam in_type_ Input point type (f32_t, f64_t, f16_t, bf16_t)
+ *  @tparam transform_type_ Type of centroids, rotation, and scale outputs
+ *  @tparam metric_type_ Type of the scalar fit metric output
+ *  @tparam allow_simd_ Enable SIMD kernel dispatch when `prefer_simd_k`
+ */
+template <typename in_type_, typename transform_type_ = typename in_type_::mesh_transform_t,
+          typename metric_type_ = typename in_type_::mesh_metric_t, allow_simd_t allow_simd_ = prefer_simd_k>
+void umeyama(in_type_ const *a, in_type_ const *b, std::size_t n, transform_type_ *a_centroid,
+             transform_type_ *b_centroid, transform_type_ *rotation, transform_type_ *scale,
+             metric_type_ *metric) noexcept {
+    constexpr bool simd = allow_simd_ == prefer_simd_k &&
+                          std::is_same_v<transform_type_, typename in_type_::mesh_transform_t> &&
+                          std::is_same_v<metric_type_, typename in_type_::mesh_metric_t>;
+
+    if constexpr (std::is_same_v<in_type_, f64_t> && simd)
+        nk_umeyama_f64(&a->raw_, &b->raw_, n, &a_centroid->raw_, &b_centroid->raw_, &rotation->raw_, &scale->raw_,
+                       &metric->raw_);
+    else if constexpr (std::is_same_v<in_type_, f32_t> && simd)
+        nk_umeyama_f32(&a->raw_, &b->raw_, n, &a_centroid->raw_, &b_centroid->raw_, &rotation->raw_, &scale->raw_,
+                       &metric->raw_);
+    else if constexpr (std::is_same_v<in_type_, f16_t> && simd)
+        nk_umeyama_f16(&a->raw_, &b->raw_, n, &a_centroid->raw_, &b_centroid->raw_, &rotation->raw_, &scale->raw_,
+                       &metric->raw_);
+    else if constexpr (std::is_same_v<in_type_, bf16_t> && simd)
+        nk_umeyama_bf16(&a->raw_, &b->raw_, n, &a_centroid->raw_, &b_centroid->raw_, &rotation->raw_, &scale->raw_,
+                        &metric->raw_);
+    // Scalar fallback
+    else {
+        // Step 1: Compute centroids
+        metric_type_ sum_a_x {}, sum_a_y {}, sum_a_z {};
+        metric_type_ sum_b_x {}, sum_b_y {}, sum_b_z {};
+        metric_type_ val_a_x, val_a_y, val_a_z, val_b_x, val_b_y, val_b_z;
+
+        for (std::size_t i = 0; i < n; i++) {
+            val_a_x = metric_type_(a[i * 3 + 0]);
+            val_a_y = metric_type_(a[i * 3 + 1]);
+            val_a_z = metric_type_(a[i * 3 + 2]);
+            val_b_x = metric_type_(b[i * 3 + 0]);
+            val_b_y = metric_type_(b[i * 3 + 1]);
+            val_b_z = metric_type_(b[i * 3 + 2]);
+            sum_a_x = sum_a_x + val_a_x;
+            sum_a_y = sum_a_y + val_a_y;
+            sum_a_z = sum_a_z + val_a_z;
+            sum_b_x = sum_b_x + val_b_x;
+            sum_b_y = sum_b_y + val_b_y;
+            sum_b_z = sum_b_z + val_b_z;
+        }
+
+        metric_type_ inv_n = metric_type_(1.0) / metric_type_(static_cast<double>(n));
+        metric_type_ centroid_a_x = sum_a_x * inv_n;
+        metric_type_ centroid_a_y = sum_a_y * inv_n;
+        metric_type_ centroid_a_z = sum_a_z * inv_n;
+        metric_type_ centroid_b_x = sum_b_x * inv_n;
+        metric_type_ centroid_b_y = sum_b_y * inv_n;
+        metric_type_ centroid_b_z = sum_b_z * inv_n;
+
+        if (a_centroid) {
+            a_centroid[0] = transform_type_(centroid_a_x);
+            a_centroid[1] = transform_type_(centroid_a_y);
+            a_centroid[2] = transform_type_(centroid_a_z);
+        }
+        if (b_centroid) {
+            b_centroid[0] = transform_type_(centroid_b_x);
+            b_centroid[1] = transform_type_(centroid_b_y);
+            b_centroid[2] = transform_type_(centroid_b_z);
+        }
+
+        // Step 2: Build covariance matrix H and compute variance of A
+        metric_type_ cross_covariance[9] = {};
+        metric_type_ variance_a {};
+        for (std::size_t i = 0; i < n; i++) {
+            val_a_x = metric_type_(a[i * 3 + 0]) - centroid_a_x;
+            val_a_y = metric_type_(a[i * 3 + 1]) - centroid_a_y;
+            val_a_z = metric_type_(a[i * 3 + 2]) - centroid_a_z;
+            val_b_x = metric_type_(b[i * 3 + 0]) - centroid_b_x;
+            val_b_y = metric_type_(b[i * 3 + 1]) - centroid_b_y;
+            val_b_z = metric_type_(b[i * 3 + 2]) - centroid_b_z;
+            variance_a = variance_a + val_a_x * val_a_x + val_a_y * val_a_y + val_a_z * val_a_z;
+            cross_covariance[0] = cross_covariance[0] + val_a_x * val_b_x;
+            cross_covariance[1] = cross_covariance[1] + val_a_x * val_b_y;
+            cross_covariance[2] = cross_covariance[2] + val_a_x * val_b_z;
+            cross_covariance[3] = cross_covariance[3] + val_a_y * val_b_x;
+            cross_covariance[4] = cross_covariance[4] + val_a_y * val_b_y;
+            cross_covariance[5] = cross_covariance[5] + val_a_y * val_b_z;
+            cross_covariance[6] = cross_covariance[6] + val_a_z * val_b_x;
+            cross_covariance[7] = cross_covariance[7] + val_a_z * val_b_y;
+            cross_covariance[8] = cross_covariance[8] + val_a_z * val_b_z;
+        }
+        variance_a = variance_a * inv_n;
+
+        // Step 3: SVD of H = U * S * Vt
+        metric_type_ svd_u[9], svd_s[9], svd_v[9];
+        svd3x3_(cross_covariance, svd_u, svd_s, svd_v);
+
+        // Step 4: R = V * Ut
+        metric_type_ rotation_matrix[9];
+        rotation_matrix[0] = svd_v[0] * svd_u[0] + svd_v[1] * svd_u[1] + svd_v[2] * svd_u[2];
+        rotation_matrix[1] = svd_v[0] * svd_u[3] + svd_v[1] * svd_u[4] + svd_v[2] * svd_u[5];
+        rotation_matrix[2] = svd_v[0] * svd_u[6] + svd_v[1] * svd_u[7] + svd_v[2] * svd_u[8];
+        rotation_matrix[3] = svd_v[3] * svd_u[0] + svd_v[4] * svd_u[1] + svd_v[5] * svd_u[2];
+        rotation_matrix[4] = svd_v[3] * svd_u[3] + svd_v[4] * svd_u[4] + svd_v[5] * svd_u[5];
+        rotation_matrix[5] = svd_v[3] * svd_u[6] + svd_v[4] * svd_u[7] + svd_v[5] * svd_u[8];
+        rotation_matrix[6] = svd_v[6] * svd_u[0] + svd_v[7] * svd_u[1] + svd_v[8] * svd_u[2];
+        rotation_matrix[7] = svd_v[6] * svd_u[3] + svd_v[7] * svd_u[4] + svd_v[8] * svd_u[5];
+        rotation_matrix[8] = svd_v[6] * svd_u[6] + svd_v[7] * svd_u[7] + svd_v[8] * svd_u[8];
+
+        // Handle reflection and compute scale: c = trace(D*S) / variance_a
+        // D = diag(1, 1, det(R)), svd_s contains singular values on diagonal
+        metric_type_ rotation_det = det3x3_(rotation_matrix);
+        metric_type_ sign_det = rotation_det < metric_type_(0.0) ? metric_type_(-1.0) : metric_type_(1.0);
+        metric_type_ trace_scaled_s = svd_s[0] + svd_s[4] + sign_det * svd_s[8];
+        metric_type_ scale_factor = trace_scaled_s / (metric_type_(static_cast<double>(n)) * variance_a);
+
+        if (scale) *scale = transform_type_(scale_factor);
+
+        if (rotation_det < metric_type_(0.0)) {
+            svd_v[2] = metric_type_(0.0) - svd_v[2];
+            svd_v[5] = metric_type_(0.0) - svd_v[5];
+            svd_v[8] = metric_type_(0.0) - svd_v[8];
+            rotation_matrix[0] = svd_v[0] * svd_u[0] + svd_v[1] * svd_u[1] + svd_v[2] * svd_u[2];
+            rotation_matrix[1] = svd_v[0] * svd_u[3] + svd_v[1] * svd_u[4] + svd_v[2] * svd_u[5];
+            rotation_matrix[2] = svd_v[0] * svd_u[6] + svd_v[1] * svd_u[7] + svd_v[2] * svd_u[8];
+            rotation_matrix[3] = svd_v[3] * svd_u[0] + svd_v[4] * svd_u[1] + svd_v[5] * svd_u[2];
+            rotation_matrix[4] = svd_v[3] * svd_u[3] + svd_v[4] * svd_u[4] + svd_v[5] * svd_u[5];
+            rotation_matrix[5] = svd_v[3] * svd_u[6] + svd_v[4] * svd_u[7] + svd_v[5] * svd_u[8];
+            rotation_matrix[6] = svd_v[6] * svd_u[0] + svd_v[7] * svd_u[1] + svd_v[8] * svd_u[2];
+            rotation_matrix[7] = svd_v[6] * svd_u[3] + svd_v[7] * svd_u[4] + svd_v[8] * svd_u[5];
+            rotation_matrix[8] = svd_v[6] * svd_u[6] + svd_v[7] * svd_u[7] + svd_v[8] * svd_u[8];
+        }
+
+        // Output rotation matrix
+        if (rotation) {
+            for (unsigned int j = 0; j < 9; j++) rotation[j] = transform_type_(rotation_matrix[j]);
+        }
+
+        // Step 5: Compute RMSD after similarity transform: ||c * R * a - b||
+        metric_type_ sum_squared {};
+        for (std::size_t i = 0; i < n; i++) {
+            metric_type_ point_a[3], point_b[3], rotated_point_a[3];
+            point_a[0] = metric_type_(a[i * 3 + 0]) - centroid_a_x;
+            point_a[1] = metric_type_(a[i * 3 + 1]) - centroid_a_y;
+            point_a[2] = metric_type_(a[i * 3 + 2]) - centroid_a_z;
+            point_b[0] = metric_type_(b[i * 3 + 0]) - centroid_b_x;
+            point_b[1] = metric_type_(b[i * 3 + 1]) - centroid_b_y;
+            point_b[2] = metric_type_(b[i * 3 + 2]) - centroid_b_z;
+            rotated_point_a[0] = scale_factor * (rotation_matrix[0] * point_a[0] + rotation_matrix[1] * point_a[1] +
+                                                 rotation_matrix[2] * point_a[2]);
+            rotated_point_a[1] = scale_factor * (rotation_matrix[3] * point_a[0] + rotation_matrix[4] * point_a[1] +
+                                                 rotation_matrix[5] * point_a[2]);
+            rotated_point_a[2] = scale_factor * (rotation_matrix[6] * point_a[0] + rotation_matrix[7] * point_a[1] +
+                                                 rotation_matrix[8] * point_a[2]);
+            metric_type_ dx = rotated_point_a[0] - point_b[0];
+            metric_type_ dy = rotated_point_a[1] - point_b[1];
+            metric_type_ dz = rotated_point_a[2] - point_b[2];
+            sum_squared = sum_squared + dx * dx + dy * dy + dz * dz;
+        }
+
+        *metric = (sum_squared * inv_n).sqrt();
+    }
+}
+
+#pragma endregion - Mesh Alignment Kernels
+
+} // namespace ashvardanian::numkong
+
+#endif // NK_MESH_HPP