numkong 7.5.0 → 7.6.0

package/include/numkong/mesh/v128relaxed.h

@@ -101,11 +101,6 @@ NK_INTERNAL nk_f64_t nk_reduce_stable_f64x2_v128relaxed_(v128_t values_f64x2) {
     return sum + compensation;
 }
 
-NK_INTERNAL void nk_rotation_from_svd_f64_v128relaxed_(nk_f64_t const *svd_u, nk_f64_t const *svd_v,
-                                                       nk_f64_t *rotation) {
-    nk_rotation_from_svd_f64_serial_(svd_u, svd_v, rotation);
-}
-
 NK_INTERNAL void nk_accumulate_square_f64x2_v128relaxed_(v128_t *sum_f64x2, v128_t *compensation_f64x2,
                                                          v128_t values_f64x2) {
     v128_t product_f64x2 = wasm_f64x2_mul(values_f64x2, values_f64x2);
@@ -124,7 +119,7 @@ NK_INTERNAL void nk_centroid_and_cross_covariance_f32_v128relaxed_(         //
     nk_f32_t const *a, nk_f32_t const *b, nk_size_t n,                      //
     nk_f64_t *centroid_a_x, nk_f64_t *centroid_a_y, nk_f64_t *centroid_a_z, //
     nk_f64_t *centroid_b_x, nk_f64_t *centroid_b_y, nk_f64_t *centroid_b_z, //
-    nk_f64_t h[9]) {
+    nk_f64_t cross_covariance[9], nk_f64_t *centered_norm_squared_a, nk_f64_t *centered_norm_squared_b) {
     v128_t zero_f64x2 = wasm_f64x2_splat(0.0);
     v128_t sum_a_x_low_f64x2 = zero_f64x2, sum_a_x_high_f64x2 = zero_f64x2;
     v128_t sum_a_y_low_f64x2 = zero_f64x2, sum_a_y_high_f64x2 = zero_f64x2;
@@ -141,6 +136,8 @@ NK_INTERNAL void nk_centroid_and_cross_covariance_f32_v128relaxed_(         //
     v128_t cross_20_low_f64x2 = zero_f64x2, cross_20_high_f64x2 = zero_f64x2;
     v128_t cross_21_low_f64x2 = zero_f64x2, cross_21_high_f64x2 = zero_f64x2;
     v128_t cross_22_low_f64x2 = zero_f64x2, cross_22_high_f64x2 = zero_f64x2;
+    v128_t norm_squared_a_low_f64x2 = zero_f64x2, norm_squared_a_high_f64x2 = zero_f64x2;
+    v128_t norm_squared_b_low_f64x2 = zero_f64x2, norm_squared_b_high_f64x2 = zero_f64x2;
     nk_size_t index = 0;
 
     for (; index + 4 <= n; index += 4) {
@@ -192,6 +189,19 @@ NK_INTERNAL void nk_centroid_and_cross_covariance_f32_v128relaxed_(         //
         cross_21_high_f64x2 = wasm_f64x2_relaxed_madd(a_z_high_f64x2, b_y_high_f64x2, cross_21_high_f64x2);
         cross_22_low_f64x2 = wasm_f64x2_relaxed_madd(a_z_low_f64x2, b_z_low_f64x2, cross_22_low_f64x2),
         cross_22_high_f64x2 = wasm_f64x2_relaxed_madd(a_z_high_f64x2, b_z_high_f64x2, cross_22_high_f64x2);
+
+        norm_squared_a_low_f64x2 = wasm_f64x2_relaxed_madd(a_x_low_f64x2, a_x_low_f64x2, norm_squared_a_low_f64x2);
+        norm_squared_a_high_f64x2 = wasm_f64x2_relaxed_madd(a_x_high_f64x2, a_x_high_f64x2, norm_squared_a_high_f64x2);
+        norm_squared_a_low_f64x2 = wasm_f64x2_relaxed_madd(a_y_low_f64x2, a_y_low_f64x2, norm_squared_a_low_f64x2);
+        norm_squared_a_high_f64x2 = wasm_f64x2_relaxed_madd(a_y_high_f64x2, a_y_high_f64x2, norm_squared_a_high_f64x2);
+        norm_squared_a_low_f64x2 = wasm_f64x2_relaxed_madd(a_z_low_f64x2, a_z_low_f64x2, norm_squared_a_low_f64x2);
+        norm_squared_a_high_f64x2 = wasm_f64x2_relaxed_madd(a_z_high_f64x2, a_z_high_f64x2, norm_squared_a_high_f64x2);
+        norm_squared_b_low_f64x2 = wasm_f64x2_relaxed_madd(b_x_low_f64x2, b_x_low_f64x2, norm_squared_b_low_f64x2);
+        norm_squared_b_high_f64x2 = wasm_f64x2_relaxed_madd(b_x_high_f64x2, b_x_high_f64x2, norm_squared_b_high_f64x2);
+        norm_squared_b_low_f64x2 = wasm_f64x2_relaxed_madd(b_y_low_f64x2, b_y_low_f64x2, norm_squared_b_low_f64x2);
+        norm_squared_b_high_f64x2 = wasm_f64x2_relaxed_madd(b_y_high_f64x2, b_y_high_f64x2, norm_squared_b_high_f64x2);
+        norm_squared_b_low_f64x2 = wasm_f64x2_relaxed_madd(b_z_low_f64x2, b_z_low_f64x2, norm_squared_b_low_f64x2);
+        norm_squared_b_high_f64x2 = wasm_f64x2_relaxed_madd(b_z_high_f64x2, b_z_high_f64x2, norm_squared_b_high_f64x2);
     }
 
     nk_f64_t sum_a_x = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_a_x_low_f64x2, sum_a_x_high_f64x2));
@@ -209,6 +219,10 @@ NK_INTERNAL void nk_centroid_and_cross_covariance_f32_v128relaxed_(         //
     nk_f64_t cross_20 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_20_low_f64x2, cross_20_high_f64x2));
     nk_f64_t cross_21 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_21_low_f64x2, cross_21_high_f64x2));
     nk_f64_t cross_22 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_22_low_f64x2, cross_22_high_f64x2));
+    nk_f64_t norm_squared_a_sum = nk_hsum_f64x2_v128relaxed_(
+        wasm_f64x2_add(norm_squared_a_low_f64x2, norm_squared_a_high_f64x2));
+    nk_f64_t norm_squared_b_sum = nk_hsum_f64x2_v128relaxed_(
+        wasm_f64x2_add(norm_squared_b_low_f64x2, norm_squared_b_high_f64x2));
 
     for (; index < n; ++index) {
         nk_f64_t a_x = a[index * 3 + 0], a_y = a[index * 3 + 1], a_z = a[index * 3 + 2];
@@ -218,6 +232,8 @@ NK_INTERNAL void nk_centroid_and_cross_covariance_f32_v128relaxed_(         //
         cross_00 += a_x * b_x, cross_01 += a_x * b_y, cross_02 += a_x * b_z;
         cross_10 += a_y * b_x, cross_11 += a_y * b_y, cross_12 += a_y * b_z;
         cross_20 += a_z * b_x, cross_21 += a_z * b_y, cross_22 += a_z * b_z;
+        norm_squared_a_sum += a_x * a_x + a_y * a_y + a_z * a_z;
+        norm_squared_b_sum += b_x * b_x + b_y * b_y + b_z * b_z;
     }
 
     nk_f64_t inv_points_count = 1.0 / (nk_f64_t)n;
@@ -227,22 +243,31 @@ NK_INTERNAL void nk_centroid_and_cross_covariance_f32_v128relaxed_(         //
     *centroid_b_z = sum_b_z * inv_points_count;
 
     nk_f64_t n_f64 = (nk_f64_t)n;
-    h[0] = cross_00 - n_f64 * (*centroid_a_x) * (*centroid_b_x),
-    h[1] = cross_01 - n_f64 * (*centroid_a_x) * (*centroid_b_y),
-    h[2] = cross_02 - n_f64 * (*centroid_a_x) * (*centroid_b_z);
-    h[3] = cross_10 - n_f64 * (*centroid_a_y) * (*centroid_b_x),
-    h[4] = cross_11 - n_f64 * (*centroid_a_y) * (*centroid_b_y),
-    h[5] = cross_12 - n_f64 * (*centroid_a_y) * (*centroid_b_z);
-    h[6] = cross_20 - n_f64 * (*centroid_a_z) * (*centroid_b_x),
-    h[7] = cross_21 - n_f64 * (*centroid_a_z) * (*centroid_b_y),
-    h[8] = cross_22 - n_f64 * (*centroid_a_z) * (*centroid_b_z);
+    cross_covariance[0] = cross_00 - n_f64 * (*centroid_a_x) * (*centroid_b_x),
+    cross_covariance[1] = cross_01 - n_f64 * (*centroid_a_x) * (*centroid_b_y),
+    cross_covariance[2] = cross_02 - n_f64 * (*centroid_a_x) * (*centroid_b_z);
+    cross_covariance[3] = cross_10 - n_f64 * (*centroid_a_y) * (*centroid_b_x),
+    cross_covariance[4] = cross_11 - n_f64 * (*centroid_a_y) * (*centroid_b_y),
+    cross_covariance[5] = cross_12 - n_f64 * (*centroid_a_y) * (*centroid_b_z);
+    cross_covariance[6] = cross_20 - n_f64 * (*centroid_a_z) * (*centroid_b_x),
+    cross_covariance[7] = cross_21 - n_f64 * (*centroid_a_z) * (*centroid_b_y),
+    cross_covariance[8] = cross_22 - n_f64 * (*centroid_a_z) * (*centroid_b_z);
+
+    *centered_norm_squared_a = norm_squared_a_sum -
+                               n_f64 * ((*centroid_a_x) * (*centroid_a_x) + (*centroid_a_y) * (*centroid_a_y) +
+                                        (*centroid_a_z) * (*centroid_a_z));
+    *centered_norm_squared_b = norm_squared_b_sum -
+                               n_f64 * ((*centroid_b_x) * (*centroid_b_x) + (*centroid_b_y) * (*centroid_b_y) +
+                                        (*centroid_b_z) * (*centroid_b_z));
+    if (*centered_norm_squared_a < 0.0) *centered_norm_squared_a = 0.0;
+    if (*centered_norm_squared_b < 0.0) *centered_norm_squared_b = 0.0;
 }
 
 NK_INTERNAL void nk_centroid_and_cross_covariance_and_variance_f32_v128relaxed_( //
     nk_f32_t const *a, nk_f32_t const *b, nk_size_t n,                           //
     nk_f64_t *centroid_a_x, nk_f64_t *centroid_a_y, nk_f64_t *centroid_a_z,      //
     nk_f64_t *centroid_b_x, nk_f64_t *centroid_b_y, nk_f64_t *centroid_b_z,      //
-    nk_f64_t h[9], nk_f64_t *variance_a) {
+    nk_f64_t cross_covariance[9], nk_f64_t *centered_norm_squared_a, nk_f64_t *centered_norm_squared_b) {
     v128_t zero_f64x2 = wasm_f64x2_splat(0.0);
     v128_t sum_a_x_low_f64x2 = zero_f64x2, sum_a_x_high_f64x2 = zero_f64x2;
     v128_t sum_a_y_low_f64x2 = zero_f64x2, sum_a_y_high_f64x2 = zero_f64x2;
@@ -250,6 +275,7 @@ NK_INTERNAL void nk_centroid_and_cross_covariance_and_variance_f32_v128relaxed_(
     v128_t sum_b_x_low_f64x2 = zero_f64x2, sum_b_x_high_f64x2 = zero_f64x2;
     v128_t sum_b_y_low_f64x2 = zero_f64x2, sum_b_y_high_f64x2 = zero_f64x2;
     v128_t sum_b_z_low_f64x2 = zero_f64x2, sum_b_z_high_f64x2 = zero_f64x2;
+    v128_t norm_squared_b_low_f64x2 = zero_f64x2, norm_squared_b_high_f64x2 = zero_f64x2;
     v128_t cross_00_low_f64x2 = zero_f64x2, cross_00_high_f64x2 = zero_f64x2;
     v128_t cross_01_low_f64x2 = zero_f64x2, cross_01_high_f64x2 = zero_f64x2;
     v128_t cross_02_low_f64x2 = zero_f64x2, cross_02_high_f64x2 = zero_f64x2;
@@ -312,7 +338,7 @@ NK_INTERNAL void nk_centroid_and_cross_covariance_and_variance_f32_v128relaxed_(
         cross_22_low_f64x2 = wasm_f64x2_relaxed_madd(a_z_low_f64x2, b_z_low_f64x2, cross_22_low_f64x2),
         cross_22_high_f64x2 = wasm_f64x2_relaxed_madd(a_z_high_f64x2, b_z_high_f64x2, cross_22_high_f64x2);
 
-        // Variance: accumulate ||a||^2.
+        // Norm-squared accumulators for both point sets (used for folded SSD).
         v128_t norm_squared_low_f64x2 = wasm_f64x2_relaxed_madd(a_y_low_f64x2, a_y_low_f64x2,
                                                                 wasm_f64x2_mul(a_x_low_f64x2, a_x_low_f64x2));
         v128_t norm_squared_high_f64x2 = wasm_f64x2_relaxed_madd(a_y_high_f64x2, a_y_high_f64x2,
@@ -321,6 +347,13 @@ NK_INTERNAL void nk_centroid_and_cross_covariance_and_variance_f32_v128relaxed_(
         norm_squared_high_f64x2 = wasm_f64x2_relaxed_madd(a_z_high_f64x2, a_z_high_f64x2, norm_squared_high_f64x2);
         sum_norm_squared_low_f64x2 = wasm_f64x2_add(sum_norm_squared_low_f64x2, norm_squared_low_f64x2);
         sum_norm_squared_high_f64x2 = wasm_f64x2_add(sum_norm_squared_high_f64x2, norm_squared_high_f64x2);
+
+        norm_squared_b_low_f64x2 = wasm_f64x2_relaxed_madd(b_x_low_f64x2, b_x_low_f64x2, norm_squared_b_low_f64x2);
+        norm_squared_b_high_f64x2 = wasm_f64x2_relaxed_madd(b_x_high_f64x2, b_x_high_f64x2, norm_squared_b_high_f64x2);
+        norm_squared_b_low_f64x2 = wasm_f64x2_relaxed_madd(b_y_low_f64x2, b_y_low_f64x2, norm_squared_b_low_f64x2);
+        norm_squared_b_high_f64x2 = wasm_f64x2_relaxed_madd(b_y_high_f64x2, b_y_high_f64x2, norm_squared_b_high_f64x2);
+        norm_squared_b_low_f64x2 = wasm_f64x2_relaxed_madd(b_z_low_f64x2, b_z_low_f64x2, norm_squared_b_low_f64x2);
+        norm_squared_b_high_f64x2 = wasm_f64x2_relaxed_madd(b_z_high_f64x2, b_z_high_f64x2, norm_squared_b_high_f64x2);
     }
 
     nk_f64_t sum_a_x = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_a_x_low_f64x2, sum_a_x_high_f64x2));
@@ -338,8 +371,10 @@ NK_INTERNAL void nk_centroid_and_cross_covariance_and_variance_f32_v128relaxed_(
     nk_f64_t cross_20 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_20_low_f64x2, cross_20_high_f64x2));
     nk_f64_t cross_21 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_21_low_f64x2, cross_21_high_f64x2));
     nk_f64_t cross_22 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_22_low_f64x2, cross_22_high_f64x2));
-    nk_f64_t sum_norm_squared = nk_hsum_f64x2_v128relaxed_(
+    nk_f64_t norm_squared_a_sum = nk_hsum_f64x2_v128relaxed_(
         wasm_f64x2_add(sum_norm_squared_low_f64x2, sum_norm_squared_high_f64x2));
+    nk_f64_t norm_squared_b_sum = nk_hsum_f64x2_v128relaxed_(
+        wasm_f64x2_add(norm_squared_b_low_f64x2, norm_squared_b_high_f64x2));
 
     for (; index < n; ++index) {
         nk_f64_t a_x = a[index * 3 + 0], a_y = a[index * 3 + 1], a_z = a[index * 3 + 2];
@@ -349,7 +384,8 @@ NK_INTERNAL void nk_centroid_and_cross_covariance_and_variance_f32_v128relaxed_(
         cross_00 += a_x * b_x, cross_01 += a_x * b_y, cross_02 += a_x * b_z;
         cross_10 += a_y * b_x, cross_11 += a_y * b_y, cross_12 += a_y * b_z;
         cross_20 += a_z * b_x, cross_21 += a_z * b_y, cross_22 += a_z * b_z;
-        sum_norm_squared += a_x * a_x + a_y * a_y + a_z * a_z;
+        norm_squared_a_sum += a_x * a_x + a_y * a_y + a_z * a_z;
+        norm_squared_b_sum += b_x * b_x + b_y * b_y + b_z * b_z;
     }
 
     nk_f64_t inv_points_count = 1.0 / (nk_f64_t)n;
@@ -359,209 +395,24 @@ NK_INTERNAL void nk_centroid_and_cross_covariance_and_variance_f32_v128relaxed_(
     *centroid_b_z = sum_b_z * inv_points_count;
 
     nk_f64_t n_f64 = (nk_f64_t)n;
-    h[0] = cross_00 - n_f64 * (*centroid_a_x) * (*centroid_b_x),
-    h[1] = cross_01 - n_f64 * (*centroid_a_x) * (*centroid_b_y),
-    h[2] = cross_02 - n_f64 * (*centroid_a_x) * (*centroid_b_z);
-    h[3] = cross_10 - n_f64 * (*centroid_a_y) * (*centroid_b_x),
-    h[4] = cross_11 - n_f64 * (*centroid_a_y) * (*centroid_b_y),
-    h[5] = cross_12 - n_f64 * (*centroid_a_y) * (*centroid_b_z);
-    h[6] = cross_20 - n_f64 * (*centroid_a_z) * (*centroid_b_x),
-    h[7] = cross_21 - n_f64 * (*centroid_a_z) * (*centroid_b_y),
-    h[8] = cross_22 - n_f64 * (*centroid_a_z) * (*centroid_b_z);
-    *variance_a = sum_norm_squared * inv_points_count -
-                  ((*centroid_a_x) * (*centroid_a_x) + (*centroid_a_y) * (*centroid_a_y) +
-                   (*centroid_a_z) * (*centroid_a_z));
-}
-
-NK_INTERNAL nk_f64_t nk_transformed_ssd_f32_v128relaxed_( //
-    nk_f32_t const *a, nk_f32_t const *b, nk_size_t n, nk_f64_t const *r, nk_f64_t scale, nk_f64_t centroid_a_x,
-    nk_f64_t centroid_a_y, nk_f64_t centroid_a_z, nk_f64_t centroid_b_x, nk_f64_t centroid_b_y, nk_f64_t centroid_b_z) {
-    v128_t scaled_rotation_x_x_f64x2 = wasm_f64x2_splat(scale * r[0]);
-    v128_t scaled_rotation_x_y_f64x2 = wasm_f64x2_splat(scale * r[1]);
-    v128_t scaled_rotation_x_z_f64x2 = wasm_f64x2_splat(scale * r[2]);
-    v128_t scaled_rotation_y_x_f64x2 = wasm_f64x2_splat(scale * r[3]);
-    v128_t scaled_rotation_y_y_f64x2 = wasm_f64x2_splat(scale * r[4]);
-    v128_t scaled_rotation_y_z_f64x2 = wasm_f64x2_splat(scale * r[5]);
-    v128_t scaled_rotation_z_x_f64x2 = wasm_f64x2_splat(scale * r[6]);
-    v128_t scaled_rotation_z_y_f64x2 = wasm_f64x2_splat(scale * r[7]);
-    v128_t scaled_rotation_z_z_f64x2 = wasm_f64x2_splat(scale * r[8]);
-    v128_t centroid_a_x_f64x2 = wasm_f64x2_splat(centroid_a_x), centroid_a_y_f64x2 = wasm_f64x2_splat(centroid_a_y);
-    v128_t centroid_a_z_f64x2 = wasm_f64x2_splat(centroid_a_z), centroid_b_x_f64x2 = wasm_f64x2_splat(centroid_b_x);
-    v128_t centroid_b_y_f64x2 = wasm_f64x2_splat(centroid_b_y), centroid_b_z_f64x2 = wasm_f64x2_splat(centroid_b_z);
-    v128_t sum_squared_low_f64x2 = wasm_f64x2_splat(0.0), sum_squared_high_f64x2 = wasm_f64x2_splat(0.0);
-    nk_size_t index = 0;
-
-    for (; index + 4 <= n; index += 4) {
-        v128_t a_x_f32x4, a_y_f32x4, a_z_f32x4, b_x_f32x4, b_y_f32x4, b_z_f32x4;
-        nk_deinterleave_f32x4_v128relaxed_(a + index * 3, &a_x_f32x4, &a_y_f32x4, &a_z_f32x4);
-        nk_deinterleave_f32x4_v128relaxed_(b + index * 3, &b_x_f32x4, &b_y_f32x4, &b_z_f32x4);
-
-        v128_t centered_a_x_low_f64x2 = wasm_f64x2_sub(wasm_f64x2_promote_low_f32x4(a_x_f32x4), centroid_a_x_f64x2);
-        v128_t centered_a_x_high_f64x2 = wasm_f64x2_sub(
-            wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_x_f32x4, a_x_f32x4, 2, 3, 0, 1)), centroid_a_x_f64x2);
-        v128_t centered_a_y_low_f64x2 = wasm_f64x2_sub(wasm_f64x2_promote_low_f32x4(a_y_f32x4), centroid_a_y_f64x2);
-        v128_t centered_a_y_high_f64x2 = wasm_f64x2_sub(
-            wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_y_f32x4, a_y_f32x4, 2, 3, 0, 1)), centroid_a_y_f64x2);
-        v128_t centered_a_z_low_f64x2 = wasm_f64x2_sub(wasm_f64x2_promote_low_f32x4(a_z_f32x4), centroid_a_z_f64x2);
-        v128_t centered_a_z_high_f64x2 = wasm_f64x2_sub(
-            wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_z_f32x4, a_z_f32x4, 2, 3, 0, 1)), centroid_a_z_f64x2);
-        v128_t centered_b_x_low_f64x2 = wasm_f64x2_sub(wasm_f64x2_promote_low_f32x4(b_x_f32x4), centroid_b_x_f64x2);
-        v128_t centered_b_x_high_f64x2 = wasm_f64x2_sub(
-            wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(b_x_f32x4, b_x_f32x4, 2, 3, 0, 1)), centroid_b_x_f64x2);
-        v128_t centered_b_y_low_f64x2 = wasm_f64x2_sub(wasm_f64x2_promote_low_f32x4(b_y_f32x4), centroid_b_y_f64x2);
-        v128_t centered_b_y_high_f64x2 = wasm_f64x2_sub(
-            wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(b_y_f32x4, b_y_f32x4, 2, 3, 0, 1)), centroid_b_y_f64x2);
-        v128_t centered_b_z_low_f64x2 = wasm_f64x2_sub(wasm_f64x2_promote_low_f32x4(b_z_f32x4), centroid_b_z_f64x2);
-        v128_t centered_b_z_high_f64x2 = wasm_f64x2_sub(
-            wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(b_z_f32x4, b_z_f32x4, 2, 3, 0, 1)), centroid_b_z_f64x2);
-
-        v128_t rotated_a_x_low_f64x2 = wasm_f64x2_relaxed_madd(
-            scaled_rotation_x_z_f64x2, centered_a_z_low_f64x2,
-            wasm_f64x2_relaxed_madd(scaled_rotation_x_y_f64x2, centered_a_y_low_f64x2,
-                                    wasm_f64x2_mul(scaled_rotation_x_x_f64x2, centered_a_x_low_f64x2)));
-        v128_t rotated_a_x_high_f64x2 = wasm_f64x2_relaxed_madd(
-            scaled_rotation_x_z_f64x2, centered_a_z_high_f64x2,
-            wasm_f64x2_relaxed_madd(scaled_rotation_x_y_f64x2, centered_a_y_high_f64x2,
-                                    wasm_f64x2_mul(scaled_rotation_x_x_f64x2, centered_a_x_high_f64x2)));
-        v128_t rotated_a_y_low_f64x2 = wasm_f64x2_relaxed_madd(
-            scaled_rotation_y_z_f64x2, centered_a_z_low_f64x2,
-            wasm_f64x2_relaxed_madd(scaled_rotation_y_y_f64x2, centered_a_y_low_f64x2,
-                                    wasm_f64x2_mul(scaled_rotation_y_x_f64x2, centered_a_x_low_f64x2)));
-        v128_t rotated_a_y_high_f64x2 = wasm_f64x2_relaxed_madd(
-            scaled_rotation_y_z_f64x2, centered_a_z_high_f64x2,
-            wasm_f64x2_relaxed_madd(scaled_rotation_y_y_f64x2, centered_a_y_high_f64x2,
-                                    wasm_f64x2_mul(scaled_rotation_y_x_f64x2, centered_a_x_high_f64x2)));
-        v128_t rotated_a_z_low_f64x2 = wasm_f64x2_relaxed_madd(
-            scaled_rotation_z_z_f64x2, centered_a_z_low_f64x2,
-            wasm_f64x2_relaxed_madd(scaled_rotation_z_y_f64x2, centered_a_y_low_f64x2,
-                                    wasm_f64x2_mul(scaled_rotation_z_x_f64x2, centered_a_x_low_f64x2)));
-        v128_t rotated_a_z_high_f64x2 = wasm_f64x2_relaxed_madd(
-            scaled_rotation_z_z_f64x2, centered_a_z_high_f64x2,
-            wasm_f64x2_relaxed_madd(scaled_rotation_z_y_f64x2, centered_a_y_high_f64x2,
-                                    wasm_f64x2_mul(scaled_rotation_z_x_f64x2, centered_a_x_high_f64x2)));
-
-        v128_t delta_x_low_f64x2 = wasm_f64x2_sub(rotated_a_x_low_f64x2, centered_b_x_low_f64x2);
-        v128_t delta_x_high_f64x2 = wasm_f64x2_sub(rotated_a_x_high_f64x2, centered_b_x_high_f64x2);
-        v128_t delta_y_low_f64x2 = wasm_f64x2_sub(rotated_a_y_low_f64x2, centered_b_y_low_f64x2);
-        v128_t delta_y_high_f64x2 = wasm_f64x2_sub(rotated_a_y_high_f64x2, centered_b_y_high_f64x2);
-        v128_t delta_z_low_f64x2 = wasm_f64x2_sub(rotated_a_z_low_f64x2, centered_b_z_low_f64x2);
-        v128_t delta_z_high_f64x2 = wasm_f64x2_sub(rotated_a_z_high_f64x2, centered_b_z_high_f64x2);
-
-        sum_squared_low_f64x2 = wasm_f64x2_relaxed_madd(delta_x_low_f64x2, delta_x_low_f64x2, sum_squared_low_f64x2);
-        sum_squared_high_f64x2 = wasm_f64x2_relaxed_madd(delta_x_high_f64x2, delta_x_high_f64x2,
-                                                         sum_squared_high_f64x2);
-        sum_squared_low_f64x2 = wasm_f64x2_relaxed_madd(delta_y_low_f64x2, delta_y_low_f64x2, sum_squared_low_f64x2);
-        sum_squared_high_f64x2 = wasm_f64x2_relaxed_madd(delta_y_high_f64x2, delta_y_high_f64x2,
-                                                         sum_squared_high_f64x2);
-        sum_squared_low_f64x2 = wasm_f64x2_relaxed_madd(delta_z_low_f64x2, delta_z_low_f64x2, sum_squared_low_f64x2);
-        sum_squared_high_f64x2 = wasm_f64x2_relaxed_madd(delta_z_high_f64x2, delta_z_high_f64x2,
-                                                         sum_squared_high_f64x2);
-    }
-
-    nk_f64_t sum_squared = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_squared_low_f64x2, sum_squared_high_f64x2));
-    for (; index < n; ++index) {
-        nk_f64_t centered_a_x = (nk_f64_t)a[index * 3 + 0] - centroid_a_x,
-                 centered_a_y = (nk_f64_t)a[index * 3 + 1] - centroid_a_y,
-                 centered_a_z = (nk_f64_t)a[index * 3 + 2] - centroid_a_z;
-        nk_f64_t centered_b_x = (nk_f64_t)b[index * 3 + 0] - centroid_b_x,
-                 centered_b_y = (nk_f64_t)b[index * 3 + 1] - centroid_b_y,
-                 centered_b_z = (nk_f64_t)b[index * 3 + 2] - centroid_b_z;
-        nk_f64_t rotated_a_x = scale * (r[0] * centered_a_x + r[1] * centered_a_y + r[2] * centered_a_z),
-                 rotated_a_y = scale * (r[3] * centered_a_x + r[4] * centered_a_y + r[5] * centered_a_z),
-                 rotated_a_z = scale * (r[6] * centered_a_x + r[7] * centered_a_y + r[8] * centered_a_z);
-        nk_f64_t delta_x = rotated_a_x - centered_b_x, delta_y = rotated_a_y - centered_b_y,
-                 delta_z = rotated_a_z - centered_b_z;
-        sum_squared += delta_x * delta_x + delta_y * delta_y + delta_z * delta_z;
-    }
-
-    return sum_squared;
-}
-
-/*  Compute sum of squared distances for f64 after applying rotation (and optional scale). */
-NK_INTERNAL nk_f64_t nk_transformed_ssd_f64_v128relaxed_(nk_f64_t const *a, nk_f64_t const *b, nk_size_t n,
-                                                         nk_f64_t const *r, nk_f64_t scale, nk_f64_t centroid_a_x,
-                                                         nk_f64_t centroid_a_y, nk_f64_t centroid_a_z,
-                                                         nk_f64_t centroid_b_x, nk_f64_t centroid_b_y,
-                                                         nk_f64_t centroid_b_z) {
-    // Broadcast scaled rotation matrix elements
-    v128_t scaled_rotation_x_x_f64x2 = wasm_f64x2_splat(scale * r[0]);
-    v128_t scaled_rotation_x_y_f64x2 = wasm_f64x2_splat(scale * r[1]);
-    v128_t scaled_rotation_x_z_f64x2 = wasm_f64x2_splat(scale * r[2]);
-    v128_t scaled_rotation_y_x_f64x2 = wasm_f64x2_splat(scale * r[3]);
-    v128_t scaled_rotation_y_y_f64x2 = wasm_f64x2_splat(scale * r[4]);
-    v128_t scaled_rotation_y_z_f64x2 = wasm_f64x2_splat(scale * r[5]);
-    v128_t scaled_rotation_z_x_f64x2 = wasm_f64x2_splat(scale * r[6]);
-    v128_t scaled_rotation_z_y_f64x2 = wasm_f64x2_splat(scale * r[7]);
-    v128_t scaled_rotation_z_z_f64x2 = wasm_f64x2_splat(scale * r[8]);
-
-    // Broadcast centroids
-    v128_t centroid_a_x_f64x2 = wasm_f64x2_splat(centroid_a_x);
-    v128_t centroid_a_y_f64x2 = wasm_f64x2_splat(centroid_a_y);
-    v128_t centroid_a_z_f64x2 = wasm_f64x2_splat(centroid_a_z);
-    v128_t centroid_b_x_f64x2 = wasm_f64x2_splat(centroid_b_x);
-    v128_t centroid_b_y_f64x2 = wasm_f64x2_splat(centroid_b_y);
-    v128_t centroid_b_z_f64x2 = wasm_f64x2_splat(centroid_b_z);
-
-    v128_t sum_squared_f64x2 = wasm_f64x2_splat(0), sum_squared_compensation_f64x2 = wasm_f64x2_splat(0);
-    nk_size_t j = 0;
-
-    // Main loop: process 2 points per iteration
-    for (; j + 2 <= n; j += 2) {
-        v128_t a_x_f64x2, a_y_f64x2, a_z_f64x2, b_x_f64x2, b_y_f64x2, b_z_f64x2;
-        nk_deinterleave_f64x2_v128relaxed_(a + j * 3, &a_x_f64x2, &a_y_f64x2, &a_z_f64x2);
-        nk_deinterleave_f64x2_v128relaxed_(b + j * 3, &b_x_f64x2, &b_y_f64x2, &b_z_f64x2);
-
-        v128_t centered_a_x_f64x2 = wasm_f64x2_sub(a_x_f64x2, centroid_a_x_f64x2);
-        v128_t centered_a_y_f64x2 = wasm_f64x2_sub(a_y_f64x2, centroid_a_y_f64x2);
-        v128_t centered_a_z_f64x2 = wasm_f64x2_sub(a_z_f64x2, centroid_a_z_f64x2);
-        v128_t centered_b_x_f64x2 = wasm_f64x2_sub(b_x_f64x2, centroid_b_x_f64x2);
-        v128_t centered_b_y_f64x2 = wasm_f64x2_sub(b_y_f64x2, centroid_b_y_f64x2);
-        v128_t centered_b_z_f64x2 = wasm_f64x2_sub(b_z_f64x2, centroid_b_z_f64x2);
-
-        // Rotate and scale: ra = scale * R * pa
-        v128_t rotated_a_x_f64x2 = wasm_f64x2_relaxed_madd(
-            scaled_rotation_x_z_f64x2, centered_a_z_f64x2,
-            wasm_f64x2_relaxed_madd(scaled_rotation_x_y_f64x2, centered_a_y_f64x2,
-                                    wasm_f64x2_mul(scaled_rotation_x_x_f64x2, centered_a_x_f64x2)));
-        v128_t rotated_a_y_f64x2 = wasm_f64x2_relaxed_madd(
-            scaled_rotation_y_z_f64x2, centered_a_z_f64x2,
-            wasm_f64x2_relaxed_madd(scaled_rotation_y_y_f64x2, centered_a_y_f64x2,
-                                    wasm_f64x2_mul(scaled_rotation_y_x_f64x2, centered_a_x_f64x2)));
-        v128_t rotated_a_z_f64x2 = wasm_f64x2_relaxed_madd(
-            scaled_rotation_z_z_f64x2, centered_a_z_f64x2,
-            wasm_f64x2_relaxed_madd(scaled_rotation_z_y_f64x2, centered_a_y_f64x2,
-                                    wasm_f64x2_mul(scaled_rotation_z_x_f64x2, centered_a_x_f64x2)));
-
-        v128_t delta_x_f64x2 = wasm_f64x2_sub(rotated_a_x_f64x2, centered_b_x_f64x2);
-        v128_t delta_y_f64x2 = wasm_f64x2_sub(rotated_a_y_f64x2, centered_b_y_f64x2);
-        v128_t delta_z_f64x2 = wasm_f64x2_sub(rotated_a_z_f64x2, centered_b_z_f64x2);
-
-        nk_accumulate_square_f64x2_v128relaxed_(&sum_squared_f64x2, &sum_squared_compensation_f64x2, delta_x_f64x2);
-        nk_accumulate_square_f64x2_v128relaxed_(&sum_squared_f64x2, &sum_squared_compensation_f64x2, delta_y_f64x2);
-        nk_accumulate_square_f64x2_v128relaxed_(&sum_squared_f64x2, &sum_squared_compensation_f64x2, delta_z_f64x2);
-    }
-
-    nk_f64_t sum_squared = nk_dot_stable_sum_f64x2_v128relaxed_(sum_squared_f64x2, sum_squared_compensation_f64x2);
-    nk_f64_t sum_squared_compensation = 0.0;
-
-    // Scalar tail
-    for (; j < n; ++j) {
-        nk_f64_t pa_x = a[j * 3 + 0] - centroid_a_x, pa_y = a[j * 3 + 1] - centroid_a_y,
-                 pa_z = a[j * 3 + 2] - centroid_a_z;
-        nk_f64_t pb_x = b[j * 3 + 0] - centroid_b_x, pb_y = b[j * 3 + 1] - centroid_b_y,
-                 pb_z = b[j * 3 + 2] - centroid_b_z;
-
-        nk_f64_t ra_x = scale * (r[0] * pa_x + r[1] * pa_y + r[2] * pa_z),
-                 ra_y = scale * (r[3] * pa_x + r[4] * pa_y + r[5] * pa_z),
-                 ra_z = scale * (r[6] * pa_x + r[7] * pa_y + r[8] * pa_z);
-
-        nk_f64_t delta_x = ra_x - pb_x, delta_y = ra_y - pb_y, delta_z = ra_z - pb_z;
-        nk_accumulate_square_f64_(&sum_squared, &sum_squared_compensation, delta_x);
-        nk_accumulate_square_f64_(&sum_squared, &sum_squared_compensation, delta_y);
-        nk_accumulate_square_f64_(&sum_squared, &sum_squared_compensation, delta_z);
-    }
-
-    return sum_squared + sum_squared_compensation;
+    cross_covariance[0] = cross_00 - n_f64 * (*centroid_a_x) * (*centroid_b_x),
+    cross_covariance[1] = cross_01 - n_f64 * (*centroid_a_x) * (*centroid_b_y),
+    cross_covariance[2] = cross_02 - n_f64 * (*centroid_a_x) * (*centroid_b_z);
+    cross_covariance[3] = cross_10 - n_f64 * (*centroid_a_y) * (*centroid_b_x),
+    cross_covariance[4] = cross_11 - n_f64 * (*centroid_a_y) * (*centroid_b_y),
+    cross_covariance[5] = cross_12 - n_f64 * (*centroid_a_y) * (*centroid_b_z);
+    cross_covariance[6] = cross_20 - n_f64 * (*centroid_a_z) * (*centroid_b_x),
+    cross_covariance[7] = cross_21 - n_f64 * (*centroid_a_z) * (*centroid_b_y),
+    cross_covariance[8] = cross_22 - n_f64 * (*centroid_a_z) * (*centroid_b_z);
+
+    *centered_norm_squared_a = norm_squared_a_sum -
+                               n_f64 * ((*centroid_a_x) * (*centroid_a_x) + (*centroid_a_y) * (*centroid_a_y) +
+                                        (*centroid_a_z) * (*centroid_a_z));
+    *centered_norm_squared_b = norm_squared_b_sum -
+                               n_f64 * ((*centroid_b_x) * (*centroid_b_x) + (*centroid_b_y) * (*centroid_b_y) +
+                                        (*centroid_b_z) * (*centroid_b_z));
+    if (*centered_norm_squared_a < 0.0) *centered_norm_squared_a = 0.0;
+    if (*centered_norm_squared_b < 0.0) *centered_norm_squared_b = 0.0;
 }
 
 NK_PUBLIC void nk_rmsd_f32_v128relaxed(nk_f32_t const *a, nk_f32_t const *b, nk_size_t n, nk_f32_t *a_centroid,
@@ -687,51 +538,79 @@ NK_PUBLIC void nk_rmsd_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b, nk_
 NK_PUBLIC void nk_kabsch_f32_v128relaxed(nk_f32_t const *a, nk_f32_t const *b, nk_size_t n, nk_f32_t *a_centroid,
                                          nk_f32_t *b_centroid, nk_f32_t *rotation, nk_f32_t *scale, nk_f64_t *result) {
     nk_f64_t centroid_a_x, centroid_a_y, centroid_a_z, centroid_b_x, centroid_b_y, centroid_b_z;
-    nk_f64_t h[9];
+    nk_f64_t centered_norm_squared_a, centered_norm_squared_b;
+    nk_f64_t cross_covariance[9];
     nk_centroid_and_cross_covariance_f32_v128relaxed_(a, b, n, &centroid_a_x, &centroid_a_y, &centroid_a_z,
-                                                      &centroid_b_x, &centroid_b_y, &centroid_b_z, h);
+                                                      &centroid_b_x, &centroid_b_y, &centroid_b_z, cross_covariance,
+                                                      &centered_norm_squared_a, &centered_norm_squared_b);
     if (a_centroid)
         a_centroid[0] = (nk_f32_t)centroid_a_x, a_centroid[1] = (nk_f32_t)centroid_a_y,
         a_centroid[2] = (nk_f32_t)centroid_a_z;
     if (b_centroid)
         b_centroid[0] = (nk_f32_t)centroid_b_x, b_centroid[1] = (nk_f32_t)centroid_b_y,
         b_centroid[2] = (nk_f32_t)centroid_b_z;
-    nk_f64_t svd_u[9], svd_s[9], svd_v[9];
-    nk_svd3x3_f64_(h, svd_u, svd_s, svd_v);
-
-    nk_f64_t r[9];
-    r[0] = svd_v[0] * svd_u[0] + svd_v[1] * svd_u[1] + svd_v[2] * svd_u[2];
-    r[1] = svd_v[0] * svd_u[3] + svd_v[1] * svd_u[4] + svd_v[2] * svd_u[5];
-    r[2] = svd_v[0] * svd_u[6] + svd_v[1] * svd_u[7] + svd_v[2] * svd_u[8];
-    r[3] = svd_v[3] * svd_u[0] + svd_v[4] * svd_u[1] + svd_v[5] * svd_u[2];
-    r[4] = svd_v[3] * svd_u[3] + svd_v[4] * svd_u[4] + svd_v[5] * svd_u[5];
-    r[5] = svd_v[3] * svd_u[6] + svd_v[4] * svd_u[7] + svd_v[5] * svd_u[8];
-    r[6] = svd_v[6] * svd_u[0] + svd_v[7] * svd_u[1] + svd_v[8] * svd_u[2];
-    r[7] = svd_v[6] * svd_u[3] + svd_v[7] * svd_u[4] + svd_v[8] * svd_u[5];
-    r[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.
-    if (nk_det3x3_f64_(r) < 0) {
-        svd_v[2] = -svd_v[2], svd_v[5] = -svd_v[5], svd_v[8] = -svd_v[8];
-        r[0] = svd_v[0] * svd_u[0] + svd_v[1] * svd_u[1] + svd_v[2] * svd_u[2];
-        r[1] = svd_v[0] * svd_u[3] + svd_v[1] * svd_u[4] + svd_v[2] * svd_u[5];
-        r[2] = svd_v[0] * svd_u[6] + svd_v[1] * svd_u[7] + svd_v[2] * svd_u[8];
-        r[3] = svd_v[3] * svd_u[0] + svd_v[4] * svd_u[1] + svd_v[5] * svd_u[2];
-        r[4] = svd_v[3] * svd_u[3] + svd_v[4] * svd_u[4] + svd_v[5] * svd_u[5];
-        r[5] = svd_v[3] * svd_u[6] + svd_v[4] * svd_u[7] + svd_v[5] * svd_u[8];
-        r[6] = svd_v[6] * svd_u[0] + svd_v[7] * svd_u[1] + svd_v[8] * svd_u[2];
-        r[7] = svd_v[6] * svd_u[3] + svd_v[7] * svd_u[4] + svd_v[8] * svd_u[5];
-        r[8] = svd_v[6] * svd_u[6] + svd_v[7] * svd_u[7] + svd_v[8] * svd_u[8];
+
+    // Identity-dominant short-circuit: if H ≈ diag(positive entries), R = I and trace(R·H) = trace(H).
+    nk_f64_t covariance_diagonal_norm_squared = cross_covariance[0] * cross_covariance[0] +
+                                                cross_covariance[4] * cross_covariance[4] +
+                                                cross_covariance[8] * cross_covariance[8];
+    nk_f64_t covariance_offdiagonal_norm_squared =
+        cross_covariance[1] * cross_covariance[1] + cross_covariance[2] * cross_covariance[2] +
+        cross_covariance[3] * cross_covariance[3] + cross_covariance[5] * cross_covariance[5] +
+        cross_covariance[6] * cross_covariance[6] + cross_covariance[7] * cross_covariance[7];
+    nk_f64_t optimal_rotation[9];
+    nk_f64_t trace_rotation_covariance;
+    if (covariance_offdiagonal_norm_squared < 1e-20 * covariance_diagonal_norm_squared && cross_covariance[0] > 0.0 &&
+        cross_covariance[4] > 0.0 && cross_covariance[8] > 0.0) {
+        optimal_rotation[0] = 1, optimal_rotation[1] = 0, optimal_rotation[2] = 0, optimal_rotation[3] = 0,
+        optimal_rotation[4] = 1, optimal_rotation[5] = 0, optimal_rotation[6] = 0, optimal_rotation[7] = 0,
+        optimal_rotation[8] = 1;
+        trace_rotation_covariance = cross_covariance[0] + cross_covariance[4] + cross_covariance[8];
+    }
+    else {
+        nk_f64_t svd_left[9], svd_diagonal[9], svd_right[9];
+        nk_svd3x3_f64_(cross_covariance, svd_left, svd_diagonal, svd_right);
+
+        optimal_rotation[0] = svd_right[0] * svd_left[0] + svd_right[1] * svd_left[1] + svd_right[2] * svd_left[2];
+        optimal_rotation[1] = svd_right[0] * svd_left[3] + svd_right[1] * svd_left[4] + svd_right[2] * svd_left[5];
+        optimal_rotation[2] = svd_right[0] * svd_left[6] + svd_right[1] * svd_left[7] + svd_right[2] * svd_left[8];
+        optimal_rotation[3] = svd_right[3] * svd_left[0] + svd_right[4] * svd_left[1] + svd_right[5] * svd_left[2];
+        optimal_rotation[4] = svd_right[3] * svd_left[3] + svd_right[4] * svd_left[4] + svd_right[5] * svd_left[5];
+        optimal_rotation[5] = svd_right[3] * svd_left[6] + svd_right[4] * svd_left[7] + svd_right[5] * svd_left[8];
+        optimal_rotation[6] = svd_right[6] * svd_left[0] + svd_right[7] * svd_left[1] + svd_right[8] * svd_left[2];
+        optimal_rotation[7] = svd_right[6] * svd_left[3] + svd_right[7] * svd_left[4] + svd_right[8] * svd_left[5];
+        optimal_rotation[8] = svd_right[6] * svd_left[6] + svd_right[7] * svd_left[7] + svd_right[8] * svd_left[8];
+
+        // Handle reflection: if det(R) < 0, negate third column of V and recompute R.
+        if (nk_det3x3_f64_(optimal_rotation) < 0) {
+            svd_right[2] = -svd_right[2], svd_right[5] = -svd_right[5], svd_right[8] = -svd_right[8];
+            optimal_rotation[0] = svd_right[0] * svd_left[0] + svd_right[1] * svd_left[1] + svd_right[2] * svd_left[2];
+            optimal_rotation[1] = svd_right[0] * svd_left[3] + svd_right[1] * svd_left[4] + svd_right[2] * svd_left[5];
+            optimal_rotation[2] = svd_right[0] * svd_left[6] + svd_right[1] * svd_left[7] + svd_right[2] * svd_left[8];
+            optimal_rotation[3] = svd_right[3] * svd_left[0] + svd_right[4] * svd_left[1] + svd_right[5] * svd_left[2];
+            optimal_rotation[4] = svd_right[3] * svd_left[3] + svd_right[4] * svd_left[4] + svd_right[5] * svd_left[5];
+            optimal_rotation[5] = svd_right[3] * svd_left[6] + svd_right[4] * svd_left[7] + svd_right[5] * svd_left[8];
+            optimal_rotation[6] = svd_right[6] * svd_left[0] + svd_right[7] * svd_left[1] + svd_right[8] * svd_left[2];
+            optimal_rotation[7] = svd_right[6] * svd_left[3] + svd_right[7] * svd_left[4] + svd_right[8] * svd_left[5];
+            optimal_rotation[8] = svd_right[6] * svd_left[6] + svd_right[7] * svd_left[7] + svd_right[8] * svd_left[8];
+        }
+
+        trace_rotation_covariance =
+            optimal_rotation[0] * cross_covariance[0] + optimal_rotation[1] * cross_covariance[3] +
+            optimal_rotation[2] * cross_covariance[6] + optimal_rotation[3] * cross_covariance[1] +
+            optimal_rotation[4] * cross_covariance[4] + optimal_rotation[5] * cross_covariance[7] +
+            optimal_rotation[6] * cross_covariance[2] + optimal_rotation[7] * cross_covariance[5] +
+            optimal_rotation[8] * cross_covariance[8];
     }
 
     if (rotation)
-        for (int j = 0; j < 9; ++j) rotation[j] = (nk_f32_t)r[j];
+        for (int j = 0; j < 9; ++j) rotation[j] = (nk_f32_t)optimal_rotation[j];
     if (scale) *scale = 1.0f;
 
-    *result = nk_f64_sqrt_v128relaxed(nk_transformed_ssd_f32_v128relaxed_(a, b, n, r, 1.0, centroid_a_x, centroid_a_y,
-                                                                          centroid_a_z, centroid_b_x, centroid_b_y,
-                                                                          centroid_b_z) /
-                                      (nk_f64_t)n);
+    // Folded SSD via trace identity: SSD = ‖a-ā‖² + ‖b-b̄‖² − 2·trace(R · H_centered).
+    nk_f64_t sum_squared = centered_norm_squared_a + centered_norm_squared_b - 2.0 * trace_rotation_covariance;
+    if (sum_squared < 0.0) sum_squared = 0.0;
+    *result = nk_f64_sqrt_v128relaxed(sum_squared / (nk_f64_t)n);
 }
 
 NK_PUBLIC void nk_kabsch_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b, nk_size_t n, nk_f64_t *a_centroid,
@@ -742,9 +621,10 @@ NK_PUBLIC void nk_kabsch_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b, n
     v128_t sum_a_x_f64x2 = zeros_f64x2, sum_a_y_f64x2 = zeros_f64x2, sum_a_z_f64x2 = zeros_f64x2;
     v128_t sum_b_x_f64x2 = zeros_f64x2, sum_b_y_f64x2 = zeros_f64x2, sum_b_z_f64x2 = zeros_f64x2;
 
-    v128_t cov_xx_f64x2 = zeros_f64x2, cov_xy_f64x2 = zeros_f64x2, cov_xz_f64x2 = zeros_f64x2;
-    v128_t cov_yx_f64x2 = zeros_f64x2, cov_yy_f64x2 = zeros_f64x2, cov_yz_f64x2 = zeros_f64x2;
-    v128_t cov_zx_f64x2 = zeros_f64x2, cov_zy_f64x2 = zeros_f64x2, cov_zz_f64x2 = zeros_f64x2;
+    v128_t covariance_xx_f64x2 = zeros_f64x2, covariance_xy_f64x2 = zeros_f64x2, covariance_xz_f64x2 = zeros_f64x2;
+    v128_t covariance_yx_f64x2 = zeros_f64x2, covariance_yy_f64x2 = zeros_f64x2, covariance_yz_f64x2 = zeros_f64x2;
+    v128_t covariance_zx_f64x2 = zeros_f64x2, covariance_zy_f64x2 = zeros_f64x2, covariance_zz_f64x2 = zeros_f64x2;
+    v128_t norm_squared_a_f64x2 = zeros_f64x2, norm_squared_b_f64x2 = zeros_f64x2;
 
     nk_size_t i = 0;
     v128_t a_x_f64x2, a_y_f64x2, a_z_f64x2, b_x_f64x2, b_y_f64x2, b_z_f64x2;
@@ -761,15 +641,21 @@ NK_PUBLIC void nk_kabsch_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b, n
         sum_b_y_f64x2 = wasm_f64x2_add(sum_b_y_f64x2, b_y_f64x2);
         sum_b_z_f64x2 = wasm_f64x2_add(sum_b_z_f64x2, b_z_f64x2);
 
-        cov_xx_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_x_f64x2, cov_xx_f64x2);
-        cov_xy_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_y_f64x2, cov_xy_f64x2);
-        cov_xz_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_z_f64x2, cov_xz_f64x2);
-        cov_yx_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_x_f64x2, cov_yx_f64x2);
-        cov_yy_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_y_f64x2, cov_yy_f64x2);
-        cov_yz_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_z_f64x2, cov_yz_f64x2);
-        cov_zx_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_x_f64x2, cov_zx_f64x2);
-        cov_zy_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_y_f64x2, cov_zy_f64x2);
-        cov_zz_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_z_f64x2, cov_zz_f64x2);
+        covariance_xx_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_x_f64x2, covariance_xx_f64x2);
+        covariance_xy_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_y_f64x2, covariance_xy_f64x2);
+        covariance_xz_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_z_f64x2, covariance_xz_f64x2);
+        covariance_yx_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_x_f64x2, covariance_yx_f64x2);
+        covariance_yy_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_y_f64x2, covariance_yy_f64x2);
+        covariance_yz_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_z_f64x2, covariance_yz_f64x2);
+        covariance_zx_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_x_f64x2, covariance_zx_f64x2);
+        covariance_zy_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_y_f64x2, covariance_zy_f64x2);
+        covariance_zz_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_z_f64x2, covariance_zz_f64x2);
+        norm_squared_a_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, a_x_f64x2, norm_squared_a_f64x2);
+        norm_squared_a_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, a_y_f64x2, norm_squared_a_f64x2);
+        norm_squared_a_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, a_z_f64x2, norm_squared_a_f64x2);
+        norm_squared_b_f64x2 = wasm_f64x2_relaxed_madd(b_x_f64x2, b_x_f64x2, norm_squared_b_f64x2);
+        norm_squared_b_f64x2 = wasm_f64x2_relaxed_madd(b_y_f64x2, b_y_f64x2, norm_squared_b_f64x2);
+        norm_squared_b_f64x2 = wasm_f64x2_relaxed_madd(b_z_f64x2, b_z_f64x2, norm_squared_b_f64x2);
     }
 
     // Reduce vector accumulators.
@@ -780,15 +666,28 @@ NK_PUBLIC void nk_kabsch_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b, n
     nk_f64_t sum_b_y = nk_reduce_stable_f64x2_v128relaxed_(sum_b_y_f64x2), sum_b_y_compensation = 0.0;
     nk_f64_t sum_b_z = nk_reduce_stable_f64x2_v128relaxed_(sum_b_z_f64x2), sum_b_z_compensation = 0.0;
 
-    nk_f64_t covariance_x_x = nk_reduce_stable_f64x2_v128relaxed_(cov_xx_f64x2), covariance_x_x_compensation = 0.0;
-    nk_f64_t covariance_x_y = nk_reduce_stable_f64x2_v128relaxed_(cov_xy_f64x2), covariance_x_y_compensation = 0.0;
-    nk_f64_t covariance_x_z = nk_reduce_stable_f64x2_v128relaxed_(cov_xz_f64x2), covariance_x_z_compensation = 0.0;
-    nk_f64_t covariance_y_x = nk_reduce_stable_f64x2_v128relaxed_(cov_yx_f64x2), covariance_y_x_compensation = 0.0;
-    nk_f64_t covariance_y_y = nk_reduce_stable_f64x2_v128relaxed_(cov_yy_f64x2), covariance_y_y_compensation = 0.0;
-    nk_f64_t covariance_y_z = nk_reduce_stable_f64x2_v128relaxed_(cov_yz_f64x2), covariance_y_z_compensation = 0.0;
-    nk_f64_t covariance_z_x = nk_reduce_stable_f64x2_v128relaxed_(cov_zx_f64x2), covariance_z_x_compensation = 0.0;
-    nk_f64_t covariance_z_y = nk_reduce_stable_f64x2_v128relaxed_(cov_zy_f64x2), covariance_z_y_compensation = 0.0;
-    nk_f64_t covariance_z_z = nk_reduce_stable_f64x2_v128relaxed_(cov_zz_f64x2), covariance_z_z_compensation = 0.0;
+    nk_f64_t covariance_x_x = nk_reduce_stable_f64x2_v128relaxed_(covariance_xx_f64x2),
+             covariance_x_x_compensation = 0.0;
+    nk_f64_t covariance_x_y = nk_reduce_stable_f64x2_v128relaxed_(covariance_xy_f64x2),
+             covariance_x_y_compensation = 0.0;
+    nk_f64_t covariance_x_z = nk_reduce_stable_f64x2_v128relaxed_(covariance_xz_f64x2),
+             covariance_x_z_compensation = 0.0;
+    nk_f64_t covariance_y_x = nk_reduce_stable_f64x2_v128relaxed_(covariance_yx_f64x2),
+             covariance_y_x_compensation = 0.0;
+    nk_f64_t covariance_y_y = nk_reduce_stable_f64x2_v128relaxed_(covariance_yy_f64x2),
+             covariance_y_y_compensation = 0.0;
+    nk_f64_t covariance_y_z = nk_reduce_stable_f64x2_v128relaxed_(covariance_yz_f64x2),
+             covariance_y_z_compensation = 0.0;
+    nk_f64_t covariance_z_x = nk_reduce_stable_f64x2_v128relaxed_(covariance_zx_f64x2),
+             covariance_z_x_compensation = 0.0;
+    nk_f64_t covariance_z_y = nk_reduce_stable_f64x2_v128relaxed_(covariance_zy_f64x2),
+             covariance_z_y_compensation = 0.0;
+    nk_f64_t covariance_z_z = nk_reduce_stable_f64x2_v128relaxed_(covariance_zz_f64x2),
+             covariance_z_z_compensation = 0.0;
+    nk_f64_t norm_squared_a_sum = nk_reduce_stable_f64x2_v128relaxed_(norm_squared_a_f64x2),
+             norm_squared_a_compensation = 0.0;
+    nk_f64_t norm_squared_b_sum = nk_reduce_stable_f64x2_v128relaxed_(norm_squared_b_f64x2),
+             norm_squared_b_compensation = 0.0;
 
     // Scalar tail
     for (; i < n; ++i) {
@@ -809,6 +708,12 @@ NK_PUBLIC void nk_kabsch_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b, n
         nk_accumulate_product_f64_(&covariance_z_x, &covariance_z_x_compensation, az, bx),
             nk_accumulate_product_f64_(&covariance_z_y, &covariance_z_y_compensation, az, by),
             nk_accumulate_product_f64_(&covariance_z_z, &covariance_z_z_compensation, az, bz);
+        nk_accumulate_square_f64_(&norm_squared_a_sum, &norm_squared_a_compensation, ax),
+            nk_accumulate_square_f64_(&norm_squared_a_sum, &norm_squared_a_compensation, ay),
+            nk_accumulate_square_f64_(&norm_squared_a_sum, &norm_squared_a_compensation, az);
+        nk_accumulate_square_f64_(&norm_squared_b_sum, &norm_squared_b_compensation, bx),
+            nk_accumulate_square_f64_(&norm_squared_b_sum, &norm_squared_b_compensation, by),
+            nk_accumulate_square_f64_(&norm_squared_b_sum, &norm_squared_b_compensation, bz);
     }
 
     sum_a_x += sum_a_x_compensation, sum_a_y += sum_a_y_compensation, sum_a_z += sum_a_z_compensation;
@@ -819,6 +724,8 @@ NK_PUBLIC void nk_kabsch_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b, n
         covariance_y_z += covariance_y_z_compensation;
     covariance_z_x += covariance_z_x_compensation, covariance_z_y += covariance_z_y_compensation,
         covariance_z_z += covariance_z_z_compensation;
+    norm_squared_a_sum += norm_squared_a_compensation;
+    norm_squared_b_sum += norm_squared_b_compensation;
 
     // Compute centroids
     nk_f64_t inv_points_count = 1.0 / (nk_f64_t)n;
@@ -829,6 +736,16 @@ NK_PUBLIC void nk_kabsch_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b, n
     if (a_centroid) a_centroid[0] = centroid_a_x, a_centroid[1] = centroid_a_y, a_centroid[2] = centroid_a_z;
     if (b_centroid) b_centroid[0] = centroid_b_x, b_centroid[1] = centroid_b_y, b_centroid[2] = centroid_b_z;
 
+    // Centered norm-squared via parallel-axis identity; clamp at zero for numeric safety.
+    nk_f64_t centered_norm_squared_a = norm_squared_a_sum -
+                                       (nk_f64_t)n * (centroid_a_x * centroid_a_x + centroid_a_y * centroid_a_y +
+                                                      centroid_a_z * centroid_a_z);
+    nk_f64_t centered_norm_squared_b = norm_squared_b_sum -
+                                       (nk_f64_t)n * (centroid_b_x * centroid_b_x + centroid_b_y * centroid_b_y +
+                                                      centroid_b_z * centroid_b_z);
+    if (centered_norm_squared_a < 0.0) centered_norm_squared_a = 0.0;
+    if (centered_norm_squared_b < 0.0) centered_norm_squared_b = 0.0;
+
     // Apply centering correction: H_centered = H - n * centroid_a * centroid_bT
     covariance_x_x -= n * centroid_a_x * centroid_b_x;
     covariance_x_y -= n * centroid_a_x * centroid_b_y;
@@ -843,37 +760,64 @@ NK_PUBLIC void nk_kabsch_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b, n
     // Compute SVD and optimal rotation
     nk_f64_t cross_covariance[9] = {covariance_x_x, covariance_x_y, covariance_x_z, covariance_y_x, covariance_y_y,
                                     covariance_y_z, covariance_z_x, covariance_z_y, covariance_z_z};
-    nk_f64_t svd_u[9], svd_s[9], svd_v[9];
-    nk_svd3x3_f64_(cross_covariance, svd_u, svd_s, svd_v);
-
-    nk_f64_t r[9];
-    nk_rotation_from_svd_f64_v128relaxed_(svd_u, svd_v, r);
 
-    // Handle reflection: if det(R) < 0, negate third column of V and recompute R
-    if (nk_det3x3_f64_(r) < 0) {
-        svd_v[2] = -svd_v[2], svd_v[5] = -svd_v[5], svd_v[8] = -svd_v[8];
-        nk_rotation_from_svd_f64_v128relaxed_(svd_u, svd_v, r);
+    // Identity-dominant short-circuit: if H ≈ diag(positive entries), R = I and trace(R·H) = trace(H).
+    nk_f64_t covariance_diagonal_norm_squared = cross_covariance[0] * cross_covariance[0] +
+                                                cross_covariance[4] * cross_covariance[4] +
+                                                cross_covariance[8] * cross_covariance[8];
+    nk_f64_t covariance_offdiagonal_norm_squared =
+        cross_covariance[1] * cross_covariance[1] + cross_covariance[2] * cross_covariance[2] +
+        cross_covariance[3] * cross_covariance[3] + cross_covariance[5] * cross_covariance[5] +
+        cross_covariance[6] * cross_covariance[6] + cross_covariance[7] * cross_covariance[7];
+    nk_f64_t optimal_rotation[9];
+    nk_f64_t trace_rotation_covariance;
+    if (covariance_offdiagonal_norm_squared < 1e-20 * covariance_diagonal_norm_squared && cross_covariance[0] > 0.0 &&
+        cross_covariance[4] > 0.0 && cross_covariance[8] > 0.0) {
+        optimal_rotation[0] = 1, optimal_rotation[1] = 0, optimal_rotation[2] = 0, optimal_rotation[3] = 0,
+        optimal_rotation[4] = 1, optimal_rotation[5] = 0, optimal_rotation[6] = 0, optimal_rotation[7] = 0,
+        optimal_rotation[8] = 1;
+        trace_rotation_covariance = cross_covariance[0] + cross_covariance[4] + cross_covariance[8];
+    }
+    else {
+        nk_f64_t svd_left[9], svd_diagonal[9], svd_right[9];
+        nk_svd3x3_f64_(cross_covariance, svd_left, svd_diagonal, svd_right);
+        nk_rotation_from_svd_f64_serial_(svd_left, svd_right, optimal_rotation);
+
+        // Handle reflection: if det(R) < 0, negate third column of V and recompute R
+        if (nk_det3x3_f64_(optimal_rotation) < 0) {
+            svd_right[2] = -svd_right[2], svd_right[5] = -svd_right[5], svd_right[8] = -svd_right[8];
+            nk_rotation_from_svd_f64_serial_(svd_left, svd_right, optimal_rotation);
+        }
+
+        trace_rotation_covariance =
+            optimal_rotation[0] * cross_covariance[0] + optimal_rotation[1] * cross_covariance[3] +
+            optimal_rotation[2] * cross_covariance[6] + optimal_rotation[3] * cross_covariance[1] +
+            optimal_rotation[4] * cross_covariance[4] + optimal_rotation[5] * cross_covariance[7] +
+            optimal_rotation[6] * cross_covariance[2] + optimal_rotation[7] * cross_covariance[5] +
+            optimal_rotation[8] * cross_covariance[8];
     }
 
     // Output rotation matrix and scale=1.0
     if (rotation)
-        for (int j = 0; j < 9; ++j) rotation[j] = r[j];
+        for (int j = 0; j < 9; ++j) rotation[j] = optimal_rotation[j];
 
     if (scale) *scale = 1.0;
 
-    // Compute RMSD after optimal rotation
-    nk_f64_t sum_squared = nk_transformed_ssd_f64_v128relaxed_(a, b, n, r, 1.0, centroid_a_x, centroid_a_y,
-                                                               centroid_a_z, centroid_b_x, centroid_b_y, centroid_b_z);
+    // Folded SSD via trace identity: SSD = ‖a-ā‖² + ‖b-b̄‖² − 2·trace(R · H_centered).
+    nk_f64_t sum_squared = centered_norm_squared_a + centered_norm_squared_b - 2.0 * trace_rotation_covariance;
+    if (sum_squared < 0.0) sum_squared = 0.0;
     *result = nk_f64_sqrt_v128relaxed(sum_squared * inv_points_count);
 }
 
 NK_PUBLIC void nk_umeyama_f32_v128relaxed(nk_f32_t const *a, nk_f32_t const *b, nk_size_t n, nk_f32_t *a_centroid,
                                           nk_f32_t *b_centroid, nk_f32_t *rotation, nk_f32_t *scale, nk_f64_t *result) {
-    nk_f64_t centroid_a_x, centroid_a_y, centroid_a_z, centroid_b_x, centroid_b_y, centroid_b_z, variance_a;
-    nk_f64_t h[9];
+    nk_f64_t centroid_a_x, centroid_a_y, centroid_a_z, centroid_b_x, centroid_b_y, centroid_b_z;
+    nk_f64_t centered_norm_squared_a, centered_norm_squared_b;
+    nk_f64_t cross_covariance[9];
     nk_centroid_and_cross_covariance_and_variance_f32_v128relaxed_( //
         a, b, n, &centroid_a_x, &centroid_a_y, &centroid_a_z,       //
-        &centroid_b_x, &centroid_b_y, &centroid_b_z, h, &variance_a);
+        &centroid_b_x, &centroid_b_y, &centroid_b_z, cross_covariance, &centered_norm_squared_a,
+        &centered_norm_squared_b);
     if (a_centroid)
         a_centroid[0] = (nk_f32_t)centroid_a_x, a_centroid[1] = (nk_f32_t)centroid_a_y,
         a_centroid[2] = (nk_f32_t)centroid_a_z;
@@ -881,44 +825,73 @@ NK_PUBLIC void nk_umeyama_f32_v128relaxed(nk_f32_t const *a, nk_f32_t const *b,
         b_centroid[0] = (nk_f32_t)centroid_b_x, b_centroid[1] = (nk_f32_t)centroid_b_y,
         b_centroid[2] = (nk_f32_t)centroid_b_z;
 
-    nk_f64_t svd_u[9], svd_s[9], svd_v[9];
-    nk_svd3x3_f64_(h, svd_u, svd_s, svd_v);
-
-    nk_f64_t r[9];
-    r[0] = svd_v[0] * svd_u[0] + svd_v[1] * svd_u[1] + svd_v[2] * svd_u[2];
-    r[1] = svd_v[0] * svd_u[3] + svd_v[1] * svd_u[4] + svd_v[2] * svd_u[5];
-    r[2] = svd_v[0] * svd_u[6] + svd_v[1] * svd_u[7] + svd_v[2] * svd_u[8];
-    r[3] = svd_v[3] * svd_u[0] + svd_v[4] * svd_u[1] + svd_v[5] * svd_u[2];
-    r[4] = svd_v[3] * svd_u[3] + svd_v[4] * svd_u[4] + svd_v[5] * svd_u[5];
-    r[5] = svd_v[3] * svd_u[6] + svd_v[4] * svd_u[7] + svd_v[5] * svd_u[8];
-    r[6] = svd_v[6] * svd_u[0] + svd_v[7] * svd_u[1] + svd_v[8] * svd_u[2];
-    r[7] = svd_v[6] * svd_u[3] + svd_v[7] * svd_u[4] + svd_v[8] * svd_u[5];
-    r[8] = svd_v[6] * svd_u[6] + svd_v[7] * svd_u[7] + svd_v[8] * svd_u[8];
-
-    nk_f64_t det = nk_det3x3_f64_(r);
-    if (det < 0) {
-        svd_v[2] = -svd_v[2], svd_v[5] = -svd_v[5], svd_v[8] = -svd_v[8];
-        r[0] = svd_v[0] * svd_u[0] + svd_v[1] * svd_u[1] + svd_v[2] * svd_u[2];
-        r[1] = svd_v[0] * svd_u[3] + svd_v[1] * svd_u[4] + svd_v[2] * svd_u[5];
-        r[2] = svd_v[0] * svd_u[6] + svd_v[1] * svd_u[7] + svd_v[2] * svd_u[8];
-        r[3] = svd_v[3] * svd_u[0] + svd_v[4] * svd_u[1] + svd_v[5] * svd_u[2];
-        r[4] = svd_v[3] * svd_u[3] + svd_v[4] * svd_u[4] + svd_v[5] * svd_u[5];
-        r[5] = svd_v[3] * svd_u[6] + svd_v[4] * svd_u[7] + svd_v[5] * svd_u[8];
-        r[6] = svd_v[6] * svd_u[0] + svd_v[7] * svd_u[1] + svd_v[8] * svd_u[2];
-        r[7] = svd_v[6] * svd_u[3] + svd_v[7] * svd_u[4] + svd_v[8] * svd_u[5];
-        r[8] = svd_v[6] * svd_u[6] + svd_v[7] * svd_u[7] + svd_v[8] * svd_u[8];
+    // Identity-dominant short-circuit: if H ≈ diag(positive entries), R = I and trace(R·H) = trace(H).
+    nk_f64_t covariance_diagonal_norm_squared = cross_covariance[0] * cross_covariance[0] +
+                                                cross_covariance[4] * cross_covariance[4] +
+                                                cross_covariance[8] * cross_covariance[8];
+    nk_f64_t covariance_offdiagonal_norm_squared =
+        cross_covariance[1] * cross_covariance[1] + cross_covariance[2] * cross_covariance[2] +
+        cross_covariance[3] * cross_covariance[3] + cross_covariance[5] * cross_covariance[5] +
+        cross_covariance[6] * cross_covariance[6] + cross_covariance[7] * cross_covariance[7];
+    nk_f64_t optimal_rotation[9];
+    nk_f64_t trace_rotation_covariance;
+    nk_f64_t computed_scale;
+    if (covariance_offdiagonal_norm_squared < 1e-20 * covariance_diagonal_norm_squared && cross_covariance[0] > 0.0 &&
+        cross_covariance[4] > 0.0 && cross_covariance[8] > 0.0) {
+        optimal_rotation[0] = 1, optimal_rotation[1] = 0, optimal_rotation[2] = 0, optimal_rotation[3] = 0,
+        optimal_rotation[4] = 1, optimal_rotation[5] = 0, optimal_rotation[6] = 0, optimal_rotation[7] = 0,
+        optimal_rotation[8] = 1;
+        trace_rotation_covariance = cross_covariance[0] + cross_covariance[4] + cross_covariance[8];
+        computed_scale = centered_norm_squared_a > 0.0 ? trace_rotation_covariance / centered_norm_squared_a : 0.0;
+    }
+    else {
+        nk_f64_t svd_left[9], svd_diagonal[9], svd_right[9];
+        nk_svd3x3_f64_(cross_covariance, svd_left, svd_diagonal, svd_right);
+
+        optimal_rotation[0] = svd_right[0] * svd_left[0] + svd_right[1] * svd_left[1] + svd_right[2] * svd_left[2];
+        optimal_rotation[1] = svd_right[0] * svd_left[3] + svd_right[1] * svd_left[4] + svd_right[2] * svd_left[5];
+        optimal_rotation[2] = svd_right[0] * svd_left[6] + svd_right[1] * svd_left[7] + svd_right[2] * svd_left[8];
+        optimal_rotation[3] = svd_right[3] * svd_left[0] + svd_right[4] * svd_left[1] + svd_right[5] * svd_left[2];
+        optimal_rotation[4] = svd_right[3] * svd_left[3] + svd_right[4] * svd_left[4] + svd_right[5] * svd_left[5];
+        optimal_rotation[5] = svd_right[3] * svd_left[6] + svd_right[4] * svd_left[7] + svd_right[5] * svd_left[8];
+        optimal_rotation[6] = svd_right[6] * svd_left[0] + svd_right[7] * svd_left[1] + svd_right[8] * svd_left[2];
+        optimal_rotation[7] = svd_right[6] * svd_left[3] + svd_right[7] * svd_left[4] + svd_right[8] * svd_left[5];
+        optimal_rotation[8] = svd_right[6] * svd_left[6] + svd_right[7] * svd_left[7] + svd_right[8] * svd_left[8];
+
+        nk_f64_t det = nk_det3x3_f64_(optimal_rotation);
+        if (det < 0) {
+            svd_right[2] = -svd_right[2], svd_right[5] = -svd_right[5], svd_right[8] = -svd_right[8];
+            optimal_rotation[0] = svd_right[0] * svd_left[0] + svd_right[1] * svd_left[1] + svd_right[2] * svd_left[2];
+            optimal_rotation[1] = svd_right[0] * svd_left[3] + svd_right[1] * svd_left[4] + svd_right[2] * svd_left[5];
+            optimal_rotation[2] = svd_right[0] * svd_left[6] + svd_right[1] * svd_left[7] + svd_right[2] * svd_left[8];
+            optimal_rotation[3] = svd_right[3] * svd_left[0] + svd_right[4] * svd_left[1] + svd_right[5] * svd_left[2];
+            optimal_rotation[4] = svd_right[3] * svd_left[3] + svd_right[4] * svd_left[4] + svd_right[5] * svd_left[5];
+            optimal_rotation[5] = svd_right[3] * svd_left[6] + svd_right[4] * svd_left[7] + svd_right[5] * svd_left[8];
+            optimal_rotation[6] = svd_right[6] * svd_left[0] + svd_right[7] * svd_left[1] + svd_right[8] * svd_left[2];
+            optimal_rotation[7] = svd_right[6] * svd_left[3] + svd_right[7] * svd_left[4] + svd_right[8] * svd_left[5];
+            optimal_rotation[8] = svd_right[6] * svd_left[6] + svd_right[7] * svd_left[7] + svd_right[8] * svd_left[8];
+        }
+
+        nk_f64_t trace_signed_singular_values = svd_diagonal[0] + svd_diagonal[4] +
+                                                (det < 0 ? -svd_diagonal[8] : svd_diagonal[8]);
+        computed_scale = centered_norm_squared_a > 0.0 ? trace_signed_singular_values / centered_norm_squared_a : 0.0;
+
+        trace_rotation_covariance =
+            optimal_rotation[0] * cross_covariance[0] + optimal_rotation[1] * cross_covariance[3] +
+            optimal_rotation[2] * cross_covariance[6] + optimal_rotation[3] * cross_covariance[1] +
+            optimal_rotation[4] * cross_covariance[4] + optimal_rotation[5] * cross_covariance[7] +
+            optimal_rotation[6] * cross_covariance[2] + optimal_rotation[7] * cross_covariance[5] +
+            optimal_rotation[8] * cross_covariance[8];
     }
-
-    nk_f64_t trace_signed_singular_values = svd_s[0] + svd_s[4] + (det < 0 ? -svd_s[8] : svd_s[8]);
-    nk_f64_t computed_scale = trace_signed_singular_values / ((nk_f64_t)n * variance_a);
     if (rotation)
-        for (int j = 0; j < 9; ++j) rotation[j] = (nk_f32_t)r[j];
+        for (int j = 0; j < 9; ++j) rotation[j] = (nk_f32_t)optimal_rotation[j];
     if (scale) *scale = (nk_f32_t)computed_scale;
 
-    *result = nk_f64_sqrt_v128relaxed(nk_transformed_ssd_f32_v128relaxed_(a, b, n, r, computed_scale, centroid_a_x,
-                                                                          centroid_a_y, centroid_a_z, centroid_b_x,
-                                                                          centroid_b_y, centroid_b_z) /
-                                      (nk_f64_t)n);
+    // Folded SSD with scale: c²·‖a-ā‖² + ‖b-b̄‖² − 2c·trace(R · H_centered).
+    nk_f64_t sum_squared = computed_scale * computed_scale * centered_norm_squared_a + centered_norm_squared_b -
+                           2.0 * computed_scale * trace_rotation_covariance;
+    if (sum_squared < 0.0) sum_squared = 0.0;
+    *result = nk_f64_sqrt_v128relaxed(sum_squared / (nk_f64_t)n);
 }
 
 NK_PUBLIC void nk_umeyama_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b, nk_size_t n, nk_f64_t *a_centroid,
@@ -929,10 +902,10 @@ NK_PUBLIC void nk_umeyama_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b,
     v128_t sum_a_x_f64x2 = zeros_f64x2, sum_a_y_f64x2 = zeros_f64x2, sum_a_z_f64x2 = zeros_f64x2;
     v128_t sum_b_x_f64x2 = zeros_f64x2, sum_b_y_f64x2 = zeros_f64x2, sum_b_z_f64x2 = zeros_f64x2;
 
-    v128_t cov_xx_f64x2 = zeros_f64x2, cov_xy_f64x2 = zeros_f64x2, cov_xz_f64x2 = zeros_f64x2;
-    v128_t cov_yx_f64x2 = zeros_f64x2, cov_yy_f64x2 = zeros_f64x2, cov_yz_f64x2 = zeros_f64x2;
-    v128_t cov_zx_f64x2 = zeros_f64x2, cov_zy_f64x2 = zeros_f64x2, cov_zz_f64x2 = zeros_f64x2;
-    v128_t variance_a_f64x2 = zeros_f64x2;
+    v128_t covariance_xx_f64x2 = zeros_f64x2, covariance_xy_f64x2 = zeros_f64x2, covariance_xz_f64x2 = zeros_f64x2;
+    v128_t covariance_yx_f64x2 = zeros_f64x2, covariance_yy_f64x2 = zeros_f64x2, covariance_yz_f64x2 = zeros_f64x2;
+    v128_t covariance_zx_f64x2 = zeros_f64x2, covariance_zy_f64x2 = zeros_f64x2, covariance_zz_f64x2 = zeros_f64x2;
+    v128_t norm_squared_a_f64x2 = zeros_f64x2, norm_squared_b_f64x2 = zeros_f64x2;
 
     nk_size_t i = 0;
     v128_t a_x_f64x2, a_y_f64x2, a_z_f64x2, b_x_f64x2, b_y_f64x2, b_z_f64x2;
@@ -949,19 +922,22 @@ NK_PUBLIC void nk_umeyama_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b,
         sum_b_y_f64x2 = wasm_f64x2_add(sum_b_y_f64x2, b_y_f64x2);
         sum_b_z_f64x2 = wasm_f64x2_add(sum_b_z_f64x2, b_z_f64x2);
 
-        cov_xx_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_x_f64x2, cov_xx_f64x2);
-        cov_xy_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_y_f64x2, cov_xy_f64x2);
-        cov_xz_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_z_f64x2, cov_xz_f64x2);
-        cov_yx_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_x_f64x2, cov_yx_f64x2);
-        cov_yy_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_y_f64x2, cov_yy_f64x2);
-        cov_yz_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_z_f64x2, cov_yz_f64x2);
-        cov_zx_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_x_f64x2, cov_zx_f64x2);
-        cov_zy_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_y_f64x2, cov_zy_f64x2);
-        cov_zz_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_z_f64x2, cov_zz_f64x2);
-
-        variance_a_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, a_x_f64x2, variance_a_f64x2);
-        variance_a_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, a_y_f64x2, variance_a_f64x2);
-        variance_a_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, a_z_f64x2, variance_a_f64x2);
+        covariance_xx_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_x_f64x2, covariance_xx_f64x2);
+        covariance_xy_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_y_f64x2, covariance_xy_f64x2);
+        covariance_xz_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_z_f64x2, covariance_xz_f64x2);
+        covariance_yx_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_x_f64x2, covariance_yx_f64x2);
+        covariance_yy_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_y_f64x2, covariance_yy_f64x2);
+        covariance_yz_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_z_f64x2, covariance_yz_f64x2);
+        covariance_zx_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_x_f64x2, covariance_zx_f64x2);
+        covariance_zy_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_y_f64x2, covariance_zy_f64x2);
+        covariance_zz_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_z_f64x2, covariance_zz_f64x2);
+
+        norm_squared_a_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, a_x_f64x2, norm_squared_a_f64x2);
+        norm_squared_a_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, a_y_f64x2, norm_squared_a_f64x2);
+        norm_squared_a_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, a_z_f64x2, norm_squared_a_f64x2);
+        norm_squared_b_f64x2 = wasm_f64x2_relaxed_madd(b_x_f64x2, b_x_f64x2, norm_squared_b_f64x2);
+        norm_squared_b_f64x2 = wasm_f64x2_relaxed_madd(b_y_f64x2, b_y_f64x2, norm_squared_b_f64x2);
+        norm_squared_b_f64x2 = wasm_f64x2_relaxed_madd(b_z_f64x2, b_z_f64x2, norm_squared_b_f64x2);
     }
 
     // Reduce vector accumulators.
@@ -971,16 +947,28 @@ NK_PUBLIC void nk_umeyama_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b,
     nk_f64_t sum_b_x = nk_reduce_stable_f64x2_v128relaxed_(sum_b_x_f64x2), sum_b_x_compensation = 0.0;
     nk_f64_t sum_b_y = nk_reduce_stable_f64x2_v128relaxed_(sum_b_y_f64x2), sum_b_y_compensation = 0.0;
     nk_f64_t sum_b_z = nk_reduce_stable_f64x2_v128relaxed_(sum_b_z_f64x2), sum_b_z_compensation = 0.0;
-    nk_f64_t covariance_x_x = nk_reduce_stable_f64x2_v128relaxed_(cov_xx_f64x2), covariance_x_x_compensation = 0.0;
-    nk_f64_t covariance_x_y = nk_reduce_stable_f64x2_v128relaxed_(cov_xy_f64x2), covariance_x_y_compensation = 0.0;
-    nk_f64_t covariance_x_z = nk_reduce_stable_f64x2_v128relaxed_(cov_xz_f64x2), covariance_x_z_compensation = 0.0;
-    nk_f64_t covariance_y_x = nk_reduce_stable_f64x2_v128relaxed_(cov_yx_f64x2), covariance_y_x_compensation = 0.0;
-    nk_f64_t covariance_y_y = nk_reduce_stable_f64x2_v128relaxed_(cov_yy_f64x2), covariance_y_y_compensation = 0.0;
-    nk_f64_t covariance_y_z = nk_reduce_stable_f64x2_v128relaxed_(cov_yz_f64x2), covariance_y_z_compensation = 0.0;
-    nk_f64_t covariance_z_x = nk_reduce_stable_f64x2_v128relaxed_(cov_zx_f64x2), covariance_z_x_compensation = 0.0;
-    nk_f64_t covariance_z_y = nk_reduce_stable_f64x2_v128relaxed_(cov_zy_f64x2), covariance_z_y_compensation = 0.0;
-    nk_f64_t covariance_z_z = nk_reduce_stable_f64x2_v128relaxed_(cov_zz_f64x2), covariance_z_z_compensation = 0.0;
-    nk_f64_t sum_sq_a = nk_reduce_stable_f64x2_v128relaxed_(variance_a_f64x2), sum_sq_a_compensation = 0.0;
+    nk_f64_t covariance_x_x = nk_reduce_stable_f64x2_v128relaxed_(covariance_xx_f64x2),
+             covariance_x_x_compensation = 0.0;
+    nk_f64_t covariance_x_y = nk_reduce_stable_f64x2_v128relaxed_(covariance_xy_f64x2),
+             covariance_x_y_compensation = 0.0;
+    nk_f64_t covariance_x_z = nk_reduce_stable_f64x2_v128relaxed_(covariance_xz_f64x2),
+             covariance_x_z_compensation = 0.0;
+    nk_f64_t covariance_y_x = nk_reduce_stable_f64x2_v128relaxed_(covariance_yx_f64x2),
+             covariance_y_x_compensation = 0.0;
+    nk_f64_t covariance_y_y = nk_reduce_stable_f64x2_v128relaxed_(covariance_yy_f64x2),
+             covariance_y_y_compensation = 0.0;
+    nk_f64_t covariance_y_z = nk_reduce_stable_f64x2_v128relaxed_(covariance_yz_f64x2),
+             covariance_y_z_compensation = 0.0;
+    nk_f64_t covariance_z_x = nk_reduce_stable_f64x2_v128relaxed_(covariance_zx_f64x2),
+             covariance_z_x_compensation = 0.0;
+    nk_f64_t covariance_z_y = nk_reduce_stable_f64x2_v128relaxed_(covariance_zy_f64x2),
+             covariance_z_y_compensation = 0.0;
+    nk_f64_t covariance_z_z = nk_reduce_stable_f64x2_v128relaxed_(covariance_zz_f64x2),
+             covariance_z_z_compensation = 0.0;
+    nk_f64_t norm_squared_a_sum = nk_reduce_stable_f64x2_v128relaxed_(norm_squared_a_f64x2),
+             norm_squared_a_compensation = 0.0;
+    nk_f64_t norm_squared_b_sum = nk_reduce_stable_f64x2_v128relaxed_(norm_squared_b_f64x2),
+             norm_squared_b_compensation = 0.0;
 
     // Scalar tail
     for (; i < n; ++i) {
@@ -1001,9 +989,12 @@ NK_PUBLIC void nk_umeyama_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b,
         nk_accumulate_product_f64_(&covariance_z_x, &covariance_z_x_compensation, az, bx),
             nk_accumulate_product_f64_(&covariance_z_y, &covariance_z_y_compensation, az, by),
             nk_accumulate_product_f64_(&covariance_z_z, &covariance_z_z_compensation, az, bz);
-        nk_accumulate_square_f64_(&sum_sq_a, &sum_sq_a_compensation, ax),
-            nk_accumulate_square_f64_(&sum_sq_a, &sum_sq_a_compensation, ay),
-            nk_accumulate_square_f64_(&sum_sq_a, &sum_sq_a_compensation, az);
+        nk_accumulate_square_f64_(&norm_squared_a_sum, &norm_squared_a_compensation, ax),
+            nk_accumulate_square_f64_(&norm_squared_a_sum, &norm_squared_a_compensation, ay),
+            nk_accumulate_square_f64_(&norm_squared_a_sum, &norm_squared_a_compensation, az);
+        nk_accumulate_square_f64_(&norm_squared_b_sum, &norm_squared_b_compensation, bx),
+            nk_accumulate_square_f64_(&norm_squared_b_sum, &norm_squared_b_compensation, by),
+            nk_accumulate_square_f64_(&norm_squared_b_sum, &norm_squared_b_compensation, bz);
     }
 
     sum_a_x += sum_a_x_compensation, sum_a_y += sum_a_y_compensation, sum_a_z += sum_a_z_compensation;
@@ -1014,7 +1005,8 @@ NK_PUBLIC void nk_umeyama_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b,
         covariance_y_z += covariance_y_z_compensation;
     covariance_z_x += covariance_z_x_compensation, covariance_z_y += covariance_z_y_compensation,
         covariance_z_z += covariance_z_z_compensation;
-    sum_sq_a += sum_sq_a_compensation;
+    norm_squared_a_sum += norm_squared_a_compensation;
+    norm_squared_b_sum += norm_squared_b_compensation;
 
     // Compute centroids
     nk_f64_t inv_points_count = 1.0 / (nk_f64_t)n;
@@ -1025,9 +1017,15 @@ NK_PUBLIC void nk_umeyama_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b,
     if (a_centroid) a_centroid[0] = centroid_a_x, a_centroid[1] = centroid_a_y, a_centroid[2] = centroid_a_z;
     if (b_centroid) b_centroid[0] = centroid_b_x, b_centroid[1] = centroid_b_y, b_centroid[2] = centroid_b_z;
 
-    // Compute variance of A (centered)
-    nk_f64_t centroid_sq = centroid_a_x * centroid_a_x + centroid_a_y * centroid_a_y + centroid_a_z * centroid_a_z;
-    nk_f64_t var_a = sum_sq_a * inv_points_count - centroid_sq;
+    // Centered norm-squared via parallel-axis identity; clamp at zero for numeric safety.
+    nk_f64_t centered_norm_squared_a = norm_squared_a_sum -
+                                       (nk_f64_t)n * (centroid_a_x * centroid_a_x + centroid_a_y * centroid_a_y +
+                                                      centroid_a_z * centroid_a_z);
+    nk_f64_t centered_norm_squared_b = norm_squared_b_sum -
+                                       (nk_f64_t)n * (centroid_b_x * centroid_b_x + centroid_b_y * centroid_b_y +
+                                                      centroid_b_z * centroid_b_z);
+    if (centered_norm_squared_a < 0.0) centered_norm_squared_a = 0.0;
+    if (centered_norm_squared_b < 0.0) centered_norm_squared_b = 0.0;
 
     // Apply centering correction: H_centered = H - n * centroid_a * centroid_bT
     covariance_x_x -= n * centroid_a_x * centroid_b_x;
@@ -1043,29 +1041,57 @@ NK_PUBLIC void nk_umeyama_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b,
     // Compute SVD
     nk_f64_t cross_covariance[9] = {covariance_x_x, covariance_x_y, covariance_x_z, covariance_y_x, covariance_y_y,
                                     covariance_y_z, covariance_z_x, covariance_z_y, covariance_z_z};
-    nk_f64_t svd_u[9], svd_s[9], svd_v[9];
-    nk_svd3x3_f64_(cross_covariance, svd_u, svd_s, svd_v);
-
-    nk_f64_t r[9];
-    nk_rotation_from_svd_f64_v128relaxed_(svd_u, svd_v, r);
 
-    // Handle reflection and compute scale
-    nk_f64_t det = nk_det3x3_f64_(r);
-    nk_f64_t trace_d_s = svd_s[0] + svd_s[4] + (det < 0 ? -svd_s[8] : svd_s[8]);
-    nk_f64_t computed_scale = trace_d_s / (n * var_a);
-
-    if (det < 0) {
-        svd_v[2] = -svd_v[2], svd_v[5] = -svd_v[5], svd_v[8] = -svd_v[8];
-        nk_rotation_from_svd_f64_v128relaxed_(svd_u, svd_v, r);
+    // Identity-dominant short-circuit: if H ≈ diag(positive entries), R = I and trace(R·H) = trace(H).
+    nk_f64_t covariance_diagonal_norm_squared = cross_covariance[0] * cross_covariance[0] +
+                                                cross_covariance[4] * cross_covariance[4] +
+                                                cross_covariance[8] * cross_covariance[8];
+    nk_f64_t covariance_offdiagonal_norm_squared =
+        cross_covariance[1] * cross_covariance[1] + cross_covariance[2] * cross_covariance[2] +
+        cross_covariance[3] * cross_covariance[3] + cross_covariance[5] * cross_covariance[5] +
+        cross_covariance[6] * cross_covariance[6] + cross_covariance[7] * cross_covariance[7];
+    nk_f64_t optimal_rotation[9];
+    nk_f64_t trace_rotation_covariance;
+    nk_f64_t computed_scale;
+    if (covariance_offdiagonal_norm_squared < 1e-20 * covariance_diagonal_norm_squared && cross_covariance[0] > 0.0 &&
+        cross_covariance[4] > 0.0 && cross_covariance[8] > 0.0) {
+        optimal_rotation[0] = 1, optimal_rotation[1] = 0, optimal_rotation[2] = 0, optimal_rotation[3] = 0,
+        optimal_rotation[4] = 1, optimal_rotation[5] = 0, optimal_rotation[6] = 0, optimal_rotation[7] = 0,
+        optimal_rotation[8] = 1;
+        trace_rotation_covariance = cross_covariance[0] + cross_covariance[4] + cross_covariance[8];
+        computed_scale = centered_norm_squared_a > 0.0 ? trace_rotation_covariance / centered_norm_squared_a : 0.0;
+    }
+    else {
+        nk_f64_t svd_left[9], svd_diagonal[9], svd_right[9];
+        nk_svd3x3_f64_(cross_covariance, svd_left, svd_diagonal, svd_right);
+        nk_rotation_from_svd_f64_serial_(svd_left, svd_right, optimal_rotation);
+
+        // Handle reflection and compute scale
+        nk_f64_t det = nk_det3x3_f64_(optimal_rotation);
+        nk_f64_t trace_d_s = svd_diagonal[0] + svd_diagonal[4] + (det < 0 ? -svd_diagonal[8] : svd_diagonal[8]);
+        computed_scale = centered_norm_squared_a > 0.0 ? trace_d_s / centered_norm_squared_a : 0.0;
+
+        if (det < 0) {
+            svd_right[2] = -svd_right[2], svd_right[5] = -svd_right[5], svd_right[8] = -svd_right[8];
+            nk_rotation_from_svd_f64_serial_(svd_left, svd_right, optimal_rotation);
+        }
+
+        trace_rotation_covariance =
+            optimal_rotation[0] * cross_covariance[0] + optimal_rotation[1] * cross_covariance[3] +
+            optimal_rotation[2] * cross_covariance[6] + optimal_rotation[3] * cross_covariance[1] +
+            optimal_rotation[4] * cross_covariance[4] + optimal_rotation[5] * cross_covariance[7] +
+            optimal_rotation[6] * cross_covariance[2] + optimal_rotation[7] * cross_covariance[5] +
+            optimal_rotation[8] * cross_covariance[8];
     }
 
     if (rotation)
-        for (int j = 0; j < 9; ++j) rotation[j] = r[j];
+        for (int j = 0; j < 9; ++j) rotation[j] = optimal_rotation[j];
     if (scale) *scale = computed_scale;
 
-    // Compute RMSD after transformation
-    nk_f64_t sum_squared = nk_transformed_ssd_f64_v128relaxed_(a, b, n, r, computed_scale, centroid_a_x, centroid_a_y,
-                                                               centroid_a_z, centroid_b_x, centroid_b_y, centroid_b_z);
+    // Folded SSD with scale: c²·‖a-ā‖² + ‖b-b̄‖² − 2c·trace(R · H_centered).
+    nk_f64_t sum_squared = computed_scale * computed_scale * centered_norm_squared_a + centered_norm_squared_b -
+                           2.0 * computed_scale * trace_rotation_covariance;
+    if (sum_squared < 0.0) sum_squared = 0.0;
     *result = nk_f64_sqrt_v128relaxed(sum_squared * inv_points_count);
 }