numkong 7.0.0
This diff represents the content of publicly available package versions that have been released to one of the supported registries. The information contained in this diff is provided for informational purposes only and reflects changes between package versions as they appear in their respective public registries.
- package/LICENSE +201 -0
- package/README.md +495 -0
- package/binding.gyp +540 -0
- package/c/dispatch.h +512 -0
- package/c/dispatch_bf16.c +389 -0
- package/c/dispatch_bf16c.c +52 -0
- package/c/dispatch_e2m3.c +263 -0
- package/c/dispatch_e3m2.c +243 -0
- package/c/dispatch_e4m3.c +276 -0
- package/c/dispatch_e5m2.c +272 -0
- package/c/dispatch_f16.c +376 -0
- package/c/dispatch_f16c.c +58 -0
- package/c/dispatch_f32.c +378 -0
- package/c/dispatch_f32c.c +99 -0
- package/c/dispatch_f64.c +296 -0
- package/c/dispatch_f64c.c +98 -0
- package/c/dispatch_i16.c +96 -0
- package/c/dispatch_i32.c +89 -0
- package/c/dispatch_i4.c +150 -0
- package/c/dispatch_i64.c +86 -0
- package/c/dispatch_i8.c +289 -0
- package/c/dispatch_other.c +330 -0
- package/c/dispatch_u1.c +148 -0
- package/c/dispatch_u16.c +124 -0
- package/c/dispatch_u32.c +118 -0
- package/c/dispatch_u4.c +150 -0
- package/c/dispatch_u64.c +102 -0
- package/c/dispatch_u8.c +303 -0
- package/c/numkong.c +950 -0
- package/include/README.md +573 -0
- package/include/module.modulemap +129 -0
- package/include/numkong/attention/sapphireamx.h +1361 -0
- package/include/numkong/attention/sme.h +2066 -0
- package/include/numkong/attention.h +49 -0
- package/include/numkong/capabilities.h +748 -0
- package/include/numkong/cast/README.md +262 -0
- package/include/numkong/cast/haswell.h +975 -0
- package/include/numkong/cast/icelake.h +470 -0
- package/include/numkong/cast/neon.h +1192 -0
- package/include/numkong/cast/rvv.h +1021 -0
- package/include/numkong/cast/sapphire.h +262 -0
- package/include/numkong/cast/serial.h +2262 -0
- package/include/numkong/cast/skylake.h +856 -0
- package/include/numkong/cast/v128relaxed.h +180 -0
- package/include/numkong/cast.h +230 -0
- package/include/numkong/curved/README.md +223 -0
- package/include/numkong/curved/genoa.h +182 -0
- package/include/numkong/curved/haswell.h +276 -0
- package/include/numkong/curved/neon.h +205 -0
- package/include/numkong/curved/neonbfdot.h +212 -0
- package/include/numkong/curved/neonhalf.h +212 -0
- package/include/numkong/curved/rvv.h +305 -0
- package/include/numkong/curved/serial.h +207 -0
- package/include/numkong/curved/skylake.h +457 -0
- package/include/numkong/curved/smef64.h +506 -0
- package/include/numkong/curved.h +517 -0
- package/include/numkong/curved.hpp +144 -0
- package/include/numkong/dot/README.md +425 -0
- package/include/numkong/dot/alder.h +563 -0
- package/include/numkong/dot/genoa.h +315 -0
- package/include/numkong/dot/haswell.h +1688 -0
- package/include/numkong/dot/icelake.h +883 -0
- package/include/numkong/dot/neon.h +818 -0
- package/include/numkong/dot/neonbfdot.h +244 -0
- package/include/numkong/dot/neonfhm.h +360 -0
- package/include/numkong/dot/neonhalf.h +198 -0
- package/include/numkong/dot/neonsdot.h +508 -0
- package/include/numkong/dot/rvv.h +714 -0
- package/include/numkong/dot/rvvbb.h +72 -0
- package/include/numkong/dot/rvvbf16.h +123 -0
- package/include/numkong/dot/rvvhalf.h +129 -0
- package/include/numkong/dot/sapphire.h +141 -0
- package/include/numkong/dot/serial.h +838 -0
- package/include/numkong/dot/sierra.h +405 -0
- package/include/numkong/dot/skylake.h +1084 -0
- package/include/numkong/dot/sve.h +379 -0
- package/include/numkong/dot/svebfdot.h +74 -0
- package/include/numkong/dot/svehalf.h +123 -0
- package/include/numkong/dot/v128relaxed.h +1258 -0
- package/include/numkong/dot.h +1070 -0
- package/include/numkong/dot.hpp +94 -0
- package/include/numkong/dots/README.md +496 -0
- package/include/numkong/dots/alder.h +114 -0
- package/include/numkong/dots/genoa.h +94 -0
- package/include/numkong/dots/haswell.h +295 -0
- package/include/numkong/dots/icelake.h +171 -0
- package/include/numkong/dots/neon.h +120 -0
- package/include/numkong/dots/neonbfdot.h +58 -0
- package/include/numkong/dots/neonfhm.h +94 -0
- package/include/numkong/dots/neonhalf.h +57 -0
- package/include/numkong/dots/neonsdot.h +108 -0
- package/include/numkong/dots/rvv.h +2486 -0
- package/include/numkong/dots/sapphireamx.h +3973 -0
- package/include/numkong/dots/serial.h +2844 -0
- package/include/numkong/dots/sierra.h +97 -0
- package/include/numkong/dots/skylake.h +196 -0
- package/include/numkong/dots/sme.h +5372 -0
- package/include/numkong/dots/smebi32.h +461 -0
- package/include/numkong/dots/smef64.h +1318 -0
- package/include/numkong/dots/smehalf.h +47 -0
- package/include/numkong/dots/v128relaxed.h +294 -0
- package/include/numkong/dots.h +2804 -0
- package/include/numkong/dots.hpp +639 -0
- package/include/numkong/each/README.md +469 -0
- package/include/numkong/each/haswell.h +1658 -0
- package/include/numkong/each/icelake.h +272 -0
- package/include/numkong/each/neon.h +1104 -0
- package/include/numkong/each/neonbfdot.h +212 -0
- package/include/numkong/each/neonhalf.h +410 -0
- package/include/numkong/each/rvv.h +1121 -0
- package/include/numkong/each/sapphire.h +477 -0
- package/include/numkong/each/serial.h +260 -0
- package/include/numkong/each/skylake.h +1562 -0
- package/include/numkong/each.h +2146 -0
- package/include/numkong/each.hpp +434 -0
- package/include/numkong/geospatial/README.md +147 -0
- package/include/numkong/geospatial/haswell.h +593 -0
- package/include/numkong/geospatial/neon.h +571 -0
- package/include/numkong/geospatial/rvv.h +701 -0
- package/include/numkong/geospatial/serial.h +309 -0
- package/include/numkong/geospatial/skylake.h +577 -0
- package/include/numkong/geospatial/v128relaxed.h +613 -0
- package/include/numkong/geospatial.h +453 -0
- package/include/numkong/geospatial.hpp +235 -0
- package/include/numkong/matrix.hpp +336 -0
- package/include/numkong/maxsim/README.md +187 -0
- package/include/numkong/maxsim/alder.h +511 -0
- package/include/numkong/maxsim/genoa.h +115 -0
- package/include/numkong/maxsim/haswell.h +553 -0
- package/include/numkong/maxsim/icelake.h +480 -0
- package/include/numkong/maxsim/neonsdot.h +394 -0
- package/include/numkong/maxsim/sapphireamx.h +877 -0
- package/include/numkong/maxsim/serial.h +490 -0
- package/include/numkong/maxsim/sme.h +929 -0
- package/include/numkong/maxsim/v128relaxed.h +280 -0
- package/include/numkong/maxsim.h +571 -0
- package/include/numkong/maxsim.hpp +133 -0
- package/include/numkong/mesh/README.md +227 -0
- package/include/numkong/mesh/haswell.h +2235 -0
- package/include/numkong/mesh/neon.h +1329 -0
- package/include/numkong/mesh/neonbfdot.h +842 -0
- package/include/numkong/mesh/neonhalf.h +616 -0
- package/include/numkong/mesh/rvv.h +916 -0
- package/include/numkong/mesh/serial.h +742 -0
- package/include/numkong/mesh/skylake.h +1135 -0
- package/include/numkong/mesh/v128relaxed.h +1052 -0
- package/include/numkong/mesh.h +652 -0
- package/include/numkong/mesh.hpp +762 -0
- package/include/numkong/numkong.h +78 -0
- package/include/numkong/numkong.hpp +57 -0
- package/include/numkong/probability/README.md +173 -0
- package/include/numkong/probability/haswell.h +267 -0
- package/include/numkong/probability/neon.h +225 -0
- package/include/numkong/probability/rvv.h +409 -0
- package/include/numkong/probability/serial.h +169 -0
- package/include/numkong/probability/skylake.h +324 -0
- package/include/numkong/probability.h +383 -0
- package/include/numkong/probability.hpp +120 -0
- package/include/numkong/random.h +50 -0
- package/include/numkong/random.hpp +285 -0
- package/include/numkong/reduce/README.md +547 -0
- package/include/numkong/reduce/alder.h +632 -0
- package/include/numkong/reduce/genoa.h +201 -0
- package/include/numkong/reduce/haswell.h +3783 -0
- package/include/numkong/reduce/icelake.h +549 -0
- package/include/numkong/reduce/neon.h +3841 -0
- package/include/numkong/reduce/neonbfdot.h +353 -0
- package/include/numkong/reduce/neonfhm.h +665 -0
- package/include/numkong/reduce/neonhalf.h +157 -0
- package/include/numkong/reduce/neonsdot.h +357 -0
- package/include/numkong/reduce/rvv.h +3407 -0
- package/include/numkong/reduce/serial.h +757 -0
- package/include/numkong/reduce/sierra.h +338 -0
- package/include/numkong/reduce/skylake.h +3792 -0
- package/include/numkong/reduce/v128relaxed.h +2302 -0
- package/include/numkong/reduce.h +1597 -0
- package/include/numkong/reduce.hpp +633 -0
- package/include/numkong/scalar/README.md +89 -0
- package/include/numkong/scalar/haswell.h +113 -0
- package/include/numkong/scalar/neon.h +122 -0
- package/include/numkong/scalar/neonhalf.h +70 -0
- package/include/numkong/scalar/rvv.h +211 -0
- package/include/numkong/scalar/sapphire.h +63 -0
- package/include/numkong/scalar/serial.h +332 -0
- package/include/numkong/scalar/v128relaxed.h +56 -0
- package/include/numkong/scalar.h +683 -0
- package/include/numkong/set/README.md +179 -0
- package/include/numkong/set/haswell.h +334 -0
- package/include/numkong/set/icelake.h +485 -0
- package/include/numkong/set/neon.h +364 -0
- package/include/numkong/set/rvv.h +226 -0
- package/include/numkong/set/rvvbb.h +117 -0
- package/include/numkong/set/serial.h +174 -0
- package/include/numkong/set/sve.h +185 -0
- package/include/numkong/set/v128relaxed.h +240 -0
- package/include/numkong/set.h +457 -0
- package/include/numkong/set.hpp +114 -0
- package/include/numkong/sets/README.md +149 -0
- package/include/numkong/sets/haswell.h +63 -0
- package/include/numkong/sets/icelake.h +66 -0
- package/include/numkong/sets/neon.h +61 -0
- package/include/numkong/sets/serial.h +43 -0
- package/include/numkong/sets/smebi32.h +1099 -0
- package/include/numkong/sets/v128relaxed.h +58 -0
- package/include/numkong/sets.h +339 -0
- package/include/numkong/sparse/README.md +156 -0
- package/include/numkong/sparse/icelake.h +463 -0
- package/include/numkong/sparse/neon.h +288 -0
- package/include/numkong/sparse/serial.h +117 -0
- package/include/numkong/sparse/sve2.h +507 -0
- package/include/numkong/sparse/turin.h +322 -0
- package/include/numkong/sparse.h +363 -0
- package/include/numkong/sparse.hpp +113 -0
- package/include/numkong/spatial/README.md +435 -0
- package/include/numkong/spatial/alder.h +607 -0
- package/include/numkong/spatial/genoa.h +290 -0
- package/include/numkong/spatial/haswell.h +960 -0
- package/include/numkong/spatial/icelake.h +586 -0
- package/include/numkong/spatial/neon.h +773 -0
- package/include/numkong/spatial/neonbfdot.h +165 -0
- package/include/numkong/spatial/neonhalf.h +118 -0
- package/include/numkong/spatial/neonsdot.h +261 -0
- package/include/numkong/spatial/rvv.h +984 -0
- package/include/numkong/spatial/rvvbf16.h +123 -0
- package/include/numkong/spatial/rvvhalf.h +117 -0
- package/include/numkong/spatial/sapphire.h +343 -0
- package/include/numkong/spatial/serial.h +346 -0
- package/include/numkong/spatial/sierra.h +323 -0
- package/include/numkong/spatial/skylake.h +606 -0
- package/include/numkong/spatial/sve.h +224 -0
- package/include/numkong/spatial/svebfdot.h +122 -0
- package/include/numkong/spatial/svehalf.h +109 -0
- package/include/numkong/spatial/v128relaxed.h +717 -0
- package/include/numkong/spatial.h +1425 -0
- package/include/numkong/spatial.hpp +183 -0
- package/include/numkong/spatials/README.md +580 -0
- package/include/numkong/spatials/alder.h +94 -0
- package/include/numkong/spatials/genoa.h +94 -0
- package/include/numkong/spatials/haswell.h +219 -0
- package/include/numkong/spatials/icelake.h +113 -0
- package/include/numkong/spatials/neon.h +109 -0
- package/include/numkong/spatials/neonbfdot.h +60 -0
- package/include/numkong/spatials/neonfhm.h +92 -0
- package/include/numkong/spatials/neonhalf.h +58 -0
- package/include/numkong/spatials/neonsdot.h +109 -0
- package/include/numkong/spatials/rvv.h +1960 -0
- package/include/numkong/spatials/sapphireamx.h +1149 -0
- package/include/numkong/spatials/serial.h +226 -0
- package/include/numkong/spatials/sierra.h +96 -0
- package/include/numkong/spatials/skylake.h +184 -0
- package/include/numkong/spatials/sme.h +1901 -0
- package/include/numkong/spatials/smef64.h +465 -0
- package/include/numkong/spatials/v128relaxed.h +240 -0
- package/include/numkong/spatials.h +3021 -0
- package/include/numkong/spatials.hpp +508 -0
- package/include/numkong/tensor.hpp +1592 -0
- package/include/numkong/trigonometry/README.md +184 -0
- package/include/numkong/trigonometry/haswell.h +652 -0
- package/include/numkong/trigonometry/neon.h +639 -0
- package/include/numkong/trigonometry/rvv.h +699 -0
- package/include/numkong/trigonometry/serial.h +703 -0
- package/include/numkong/trigonometry/skylake.h +721 -0
- package/include/numkong/trigonometry/v128relaxed.h +666 -0
- package/include/numkong/trigonometry.h +467 -0
- package/include/numkong/trigonometry.hpp +166 -0
- package/include/numkong/types.h +1384 -0
- package/include/numkong/types.hpp +5603 -0
- package/include/numkong/vector.hpp +698 -0
- package/javascript/README.md +246 -0
- package/javascript/dist/cjs/numkong-wasm.d.ts +166 -0
- package/javascript/dist/cjs/numkong-wasm.js +617 -0
- package/javascript/dist/cjs/numkong.d.ts +343 -0
- package/javascript/dist/cjs/numkong.js +523 -0
- package/javascript/dist/cjs/package.json +3 -0
- package/javascript/dist/cjs/types.d.ts +284 -0
- package/javascript/dist/cjs/types.js +653 -0
- package/javascript/dist/esm/numkong-wasm.d.ts +166 -0
- package/javascript/dist/esm/numkong-wasm.js +595 -0
- package/javascript/dist/esm/numkong.d.ts +343 -0
- package/javascript/dist/esm/numkong.js +452 -0
- package/javascript/dist/esm/package.json +3 -0
- package/javascript/dist/esm/types.d.ts +284 -0
- package/javascript/dist/esm/types.js +630 -0
- package/javascript/dist-package-cjs.json +3 -0
- package/javascript/dist-package-esm.json +3 -0
- package/javascript/node-gyp-build.d.ts +1 -0
- package/javascript/numkong-wasm.ts +756 -0
- package/javascript/numkong.c +689 -0
- package/javascript/numkong.ts +575 -0
- package/javascript/tsconfig-base.json +39 -0
- package/javascript/tsconfig-cjs.json +8 -0
- package/javascript/tsconfig-esm.json +8 -0
- package/javascript/types.ts +674 -0
- package/package.json +87 -0
|
@@ -0,0 +1,1052 @@
|
|
|
1
|
+
/**
|
|
2
|
+
* @brief SIMD-accelerated Point Cloud Alignment for WASM Relaxed SIMD.
|
|
3
|
+
* @file include/numkong/mesh/v128relaxed.h
|
|
4
|
+
* @author Ash Vardanian
|
|
5
|
+
* @date March 10, 2026
|
|
6
|
+
*
|
|
7
|
+
* @sa include/numkong/mesh.h
|
|
8
|
+
*
|
|
9
|
+
* @section wasm_mesh_instructions Key WASM Relaxed SIMD Mesh Instructions
|
|
10
|
+
*
|
|
11
|
+
* Point cloud operations use these WASM Relaxed SIMD instructions:
|
|
12
|
+
*
|
|
13
|
+
* Intrinsic Operation
|
|
14
|
+
* wasm_f32x4_relaxed_madd Fused multiply-add (4-way f32)
|
|
15
|
+
* wasm_f64x2_relaxed_madd Fused multiply-add (2-way f64)
|
|
16
|
+
* wasm_f32x4_mul Multiply (4-way f32)
|
|
17
|
+
* wasm_f64x2_mul Multiply (2-way f64)
|
|
18
|
+
* wasm_f32x4_add/sub Add/subtract (4-way f32)
|
|
19
|
+
* wasm_f64x2_add/sub Add/subtract (2-way f64)
|
|
20
|
+
* wasm_f32x4_splat Broadcast scalar to all lanes
|
|
21
|
+
* wasm_f64x2_splat Broadcast scalar to all lanes
|
|
22
|
+
* wasm_i32x4_shuffle Cross-vector lane permutation (f32)
|
|
23
|
+
* wasm_i64x2_shuffle Cross-vector lane permutation (f64)
|
|
24
|
+
*
|
|
25
|
+
* WASM lacks hardware stride-3 deinterleaving (no LD3 equivalent), so XYZ
|
|
26
|
+
* deinterleaving is done via shuffle chains. No dual-accumulator unrolling is
|
|
27
|
+
* used since WASM engines already handle instruction scheduling.
|
|
28
|
+
*/
|
|
29
|
+
#ifndef NK_MESH_V128RELAXED_H
|
|
30
|
+
#define NK_MESH_V128RELAXED_H
|
|
31
|
+
|
|
32
|
+
#if NK_TARGET_V128RELAXED
|
|
33
|
+
|
|
34
|
+
#include "numkong/types.h"
|
|
35
|
+
#include "numkong/mesh/serial.h"
|
|
36
|
+
#include "numkong/dot/v128relaxed.h"
|
|
37
|
+
#include "numkong/reduce/v128relaxed.h"
|
|
38
|
+
#include "numkong/scalar/v128relaxed.h"
|
|
39
|
+
|
|
40
|
+
#if defined(__cplusplus)
|
|
41
|
+
extern "C" {
|
|
42
|
+
#endif
|
|
43
|
+
|
|
44
|
+
#if defined(__clang__)
|
|
45
|
+
#pragma clang attribute push(__attribute__((target("relaxed-simd"))), apply_to = function)
|
|
46
|
+
#endif
|
|
47
|
+
|
|
48
|
+
/* Deinterleave 12 contiguous f32 values (4 XYZ triplets) into separate x, y, z vectors.
|
|
49
|
+
*
|
|
50
|
+
* Input: [x0,y0,z0, x1,y1,z1, x2,y2,z2, x3,y3,z3]
|
|
51
|
+
* Output: x[4], y[4], z[4] vectors
|
|
52
|
+
*/
|
|
53
|
+
NK_INTERNAL void nk_deinterleave_f32x4_v128relaxed_(nk_f32_t const *ptr, v128_t *xs_f32x4, v128_t *ys_f32x4,
|
|
54
|
+
v128_t *zs_f32x4) {
|
|
55
|
+
v128_t v0_f32x4 = wasm_v128_load(ptr); // x0 y0 z0 x1
|
|
56
|
+
v128_t v1_f32x4 = wasm_v128_load(ptr + 4); // y1 z1 x2 y2
|
|
57
|
+
v128_t v2_f32x4 = wasm_v128_load(ptr + 8); // z2 x3 y3 z3
|
|
58
|
+
// x0 x1 x2 x3
|
|
59
|
+
v128_t tmp01 = wasm_i32x4_shuffle(v0_f32x4, v1_f32x4, 0, 3, 6, 0); // x0 x1 x2 _
|
|
60
|
+
*xs_f32x4 = wasm_i32x4_shuffle(tmp01, v2_f32x4, 0, 1, 2, 5); // x0 x1 x2 x3
|
|
61
|
+
// y0 y1 y2 y3
|
|
62
|
+
v128_t tmp23 = wasm_i32x4_shuffle(v0_f32x4, v1_f32x4, 1, 4, 7, 0); // y0 y1 y2 _
|
|
63
|
+
*ys_f32x4 = wasm_i32x4_shuffle(tmp23, v2_f32x4, 0, 1, 2, 6); // y0 y1 y2 y3
|
|
64
|
+
// z0 z1 z2 z3
|
|
65
|
+
v128_t tmp45 = wasm_i32x4_shuffle(v0_f32x4, v1_f32x4, 2, 5, 0, 0); // z0 z1 _ _
|
|
66
|
+
*zs_f32x4 = wasm_i32x4_shuffle(tmp45, v2_f32x4, 0, 1, 4, 7); // z0 z1 z2 z3
|
|
67
|
+
}
|
|
68
|
+
|
|
69
|
+
/* Deinterleave 6 contiguous f64 values (2 XYZ triplets) into separate x, y, z vectors.
|
|
70
|
+
*
|
|
71
|
+
* Input: [x0,y0,z0, x1,y1,z1]
|
|
72
|
+
* Output: x[2], y[2], z[2] vectors
|
|
73
|
+
*/
|
|
74
|
+
NK_INTERNAL void nk_deinterleave_f64x2_v128relaxed_(nk_f64_t const *ptr, v128_t *xs_f64x2, v128_t *ys_f64x2,
|
|
75
|
+
v128_t *zs_f64x2) {
|
|
76
|
+
v128_t v0_f64x2 = wasm_v128_load(ptr); // x0 y0
|
|
77
|
+
v128_t v1_f64x2 = wasm_v128_load(ptr + 2); // z0 x1
|
|
78
|
+
v128_t v2_f64x2 = wasm_v128_load(ptr + 4); // y1 z1
|
|
79
|
+
*xs_f64x2 = wasm_i64x2_shuffle(v0_f64x2, v1_f64x2, 0, 3); // x0 x1
|
|
80
|
+
*ys_f64x2 = wasm_i64x2_shuffle(v0_f64x2, v2_f64x2, 1, 2); // y0 y1
|
|
81
|
+
*zs_f64x2 = wasm_i64x2_shuffle(v1_f64x2, v2_f64x2, 0, 3); // z0 z1
|
|
82
|
+
}
|
|
83
|
+
|
|
84
|
+
/* Horizontal sum of all 4 f32 lanes. */
|
|
85
|
+
NK_INTERNAL nk_f32_t nk_hsum_f32x4_v128relaxed_(v128_t v) {
|
|
86
|
+
return wasm_f32x4_extract_lane(v, 0) + wasm_f32x4_extract_lane(v, 1) + wasm_f32x4_extract_lane(v, 2) +
|
|
87
|
+
wasm_f32x4_extract_lane(v, 3);
|
|
88
|
+
}
|
|
89
|
+
|
|
90
|
+
/* Horizontal sum of both f64 lanes. */
|
|
91
|
+
NK_INTERNAL nk_f64_t nk_hsum_f64x2_v128relaxed_(v128_t v) {
|
|
92
|
+
return wasm_f64x2_extract_lane(v, 0) + wasm_f64x2_extract_lane(v, 1);
|
|
93
|
+
}
|
|
94
|
+
|
|
95
|
+
NK_INTERNAL nk_f64_t nk_reduce_stable_f64x2_v128relaxed_(v128_t values_f64x2) {
|
|
96
|
+
nk_b128_vec_t values;
|
|
97
|
+
values.v128 = values_f64x2;
|
|
98
|
+
nk_f64_t sum = 0.0, compensation = 0.0;
|
|
99
|
+
nk_accumulate_sum_f64_(&sum, &compensation, values.f64s[0]);
|
|
100
|
+
nk_accumulate_sum_f64_(&sum, &compensation, values.f64s[1]);
|
|
101
|
+
return sum + compensation;
|
|
102
|
+
}
|
|
103
|
+
|
|
104
|
+
NK_INTERNAL void nk_rotation_from_svd_f64_v128relaxed_(nk_f64_t const *svd_u, nk_f64_t const *svd_v,
|
|
105
|
+
nk_f64_t *rotation) {
|
|
106
|
+
nk_rotation_from_svd_f64_serial_(svd_u, svd_v, rotation);
|
|
107
|
+
}
|
|
108
|
+
|
|
109
|
+
NK_INTERNAL void nk_accumulate_square_f64x2_v128relaxed_(v128_t *sum_f64x2, v128_t *compensation_f64x2,
|
|
110
|
+
v128_t values_f64x2) {
|
|
111
|
+
v128_t product_f64x2 = wasm_f64x2_mul(values_f64x2, values_f64x2);
|
|
112
|
+
v128_t product_error_f64x2 = wasm_f64x2_sub(
|
|
113
|
+
wasm_f64x2_relaxed_madd(values_f64x2, values_f64x2, wasm_f64x2_splat(0.0)), product_f64x2);
|
|
114
|
+
v128_t tentative_sum_f64x2 = wasm_f64x2_add(*sum_f64x2, product_f64x2);
|
|
115
|
+
v128_t virtual_addend_f64x2 = wasm_f64x2_sub(tentative_sum_f64x2, *sum_f64x2);
|
|
116
|
+
v128_t sum_error_f64x2 = wasm_f64x2_add(
|
|
117
|
+
wasm_f64x2_sub(*sum_f64x2, wasm_f64x2_sub(tentative_sum_f64x2, virtual_addend_f64x2)),
|
|
118
|
+
wasm_f64x2_sub(product_f64x2, virtual_addend_f64x2));
|
|
119
|
+
*sum_f64x2 = tentative_sum_f64x2;
|
|
120
|
+
*compensation_f64x2 = wasm_f64x2_add(*compensation_f64x2, wasm_f64x2_add(sum_error_f64x2, product_error_f64x2));
|
|
121
|
+
}
|
|
122
|
+
|
|
123
|
+
NK_INTERNAL void nk_bicentroid_f32_v128relaxed_( //
|
|
124
|
+
nk_f32_t const *a, nk_f32_t const *b, nk_size_t n, //
|
|
125
|
+
nk_f64_t *ca_x, nk_f64_t *ca_y, nk_f64_t *ca_z, //
|
|
126
|
+
nk_f64_t *cb_x, nk_f64_t *cb_y, nk_f64_t *cb_z) { //
|
|
127
|
+
v128_t zero_f64x2 = wasm_f64x2_splat(0.0);
|
|
128
|
+
v128_t sum_a_x_lower_f64x2 = zero_f64x2, sum_a_x_upper_f64x2 = zero_f64x2;
|
|
129
|
+
v128_t sum_a_y_lower_f64x2 = zero_f64x2, sum_a_y_upper_f64x2 = zero_f64x2;
|
|
130
|
+
v128_t sum_a_z_lower_f64x2 = zero_f64x2, sum_a_z_upper_f64x2 = zero_f64x2;
|
|
131
|
+
v128_t sum_b_x_lower_f64x2 = zero_f64x2, sum_b_x_upper_f64x2 = zero_f64x2;
|
|
132
|
+
v128_t sum_b_y_lower_f64x2 = zero_f64x2, sum_b_y_upper_f64x2 = zero_f64x2;
|
|
133
|
+
v128_t sum_b_z_lower_f64x2 = zero_f64x2, sum_b_z_upper_f64x2 = zero_f64x2;
|
|
134
|
+
nk_size_t index = 0;
|
|
135
|
+
|
|
136
|
+
for (; index + 4 <= n; index += 4) {
|
|
137
|
+
v128_t a_x_f32x4, a_y_f32x4, a_z_f32x4, b_x_f32x4, b_y_f32x4, b_z_f32x4;
|
|
138
|
+
nk_deinterleave_f32x4_v128relaxed_(a + index * 3, &a_x_f32x4, &a_y_f32x4, &a_z_f32x4);
|
|
139
|
+
nk_deinterleave_f32x4_v128relaxed_(b + index * 3, &b_x_f32x4, &b_y_f32x4, &b_z_f32x4);
|
|
140
|
+
|
|
141
|
+
v128_t a_x_lower_f64x2 = wasm_f64x2_promote_low_f32x4(a_x_f32x4);
|
|
142
|
+
v128_t a_x_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_x_f32x4, a_x_f32x4, 2, 3, 0, 1));
|
|
143
|
+
v128_t a_y_lower_f64x2 = wasm_f64x2_promote_low_f32x4(a_y_f32x4);
|
|
144
|
+
v128_t a_y_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_y_f32x4, a_y_f32x4, 2, 3, 0, 1));
|
|
145
|
+
v128_t a_z_lower_f64x2 = wasm_f64x2_promote_low_f32x4(a_z_f32x4);
|
|
146
|
+
v128_t a_z_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_z_f32x4, a_z_f32x4, 2, 3, 0, 1));
|
|
147
|
+
v128_t b_x_lower_f64x2 = wasm_f64x2_promote_low_f32x4(b_x_f32x4);
|
|
148
|
+
v128_t b_x_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(b_x_f32x4, b_x_f32x4, 2, 3, 0, 1));
|
|
149
|
+
v128_t b_y_lower_f64x2 = wasm_f64x2_promote_low_f32x4(b_y_f32x4);
|
|
150
|
+
v128_t b_y_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(b_y_f32x4, b_y_f32x4, 2, 3, 0, 1));
|
|
151
|
+
v128_t b_z_lower_f64x2 = wasm_f64x2_promote_low_f32x4(b_z_f32x4);
|
|
152
|
+
v128_t b_z_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(b_z_f32x4, b_z_f32x4, 2, 3, 0, 1));
|
|
153
|
+
|
|
154
|
+
sum_a_x_lower_f64x2 = wasm_f64x2_add(sum_a_x_lower_f64x2, a_x_lower_f64x2),
|
|
155
|
+
sum_a_x_upper_f64x2 = wasm_f64x2_add(sum_a_x_upper_f64x2, a_x_upper_f64x2);
|
|
156
|
+
sum_a_y_lower_f64x2 = wasm_f64x2_add(sum_a_y_lower_f64x2, a_y_lower_f64x2),
|
|
157
|
+
sum_a_y_upper_f64x2 = wasm_f64x2_add(sum_a_y_upper_f64x2, a_y_upper_f64x2);
|
|
158
|
+
sum_a_z_lower_f64x2 = wasm_f64x2_add(sum_a_z_lower_f64x2, a_z_lower_f64x2),
|
|
159
|
+
sum_a_z_upper_f64x2 = wasm_f64x2_add(sum_a_z_upper_f64x2, a_z_upper_f64x2);
|
|
160
|
+
sum_b_x_lower_f64x2 = wasm_f64x2_add(sum_b_x_lower_f64x2, b_x_lower_f64x2),
|
|
161
|
+
sum_b_x_upper_f64x2 = wasm_f64x2_add(sum_b_x_upper_f64x2, b_x_upper_f64x2);
|
|
162
|
+
sum_b_y_lower_f64x2 = wasm_f64x2_add(sum_b_y_lower_f64x2, b_y_lower_f64x2),
|
|
163
|
+
sum_b_y_upper_f64x2 = wasm_f64x2_add(sum_b_y_upper_f64x2, b_y_upper_f64x2);
|
|
164
|
+
sum_b_z_lower_f64x2 = wasm_f64x2_add(sum_b_z_lower_f64x2, b_z_lower_f64x2),
|
|
165
|
+
sum_b_z_upper_f64x2 = wasm_f64x2_add(sum_b_z_upper_f64x2, b_z_upper_f64x2);
|
|
166
|
+
}
|
|
167
|
+
|
|
168
|
+
nk_f64_t sum_a_x = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_a_x_lower_f64x2, sum_a_x_upper_f64x2));
|
|
169
|
+
nk_f64_t sum_a_y = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_a_y_lower_f64x2, sum_a_y_upper_f64x2));
|
|
170
|
+
nk_f64_t sum_a_z = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_a_z_lower_f64x2, sum_a_z_upper_f64x2));
|
|
171
|
+
nk_f64_t sum_b_x = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_b_x_lower_f64x2, sum_b_x_upper_f64x2));
|
|
172
|
+
nk_f64_t sum_b_y = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_b_y_lower_f64x2, sum_b_y_upper_f64x2));
|
|
173
|
+
nk_f64_t sum_b_z = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_b_z_lower_f64x2, sum_b_z_upper_f64x2));
|
|
174
|
+
|
|
175
|
+
for (; index < n; ++index) {
|
|
176
|
+
sum_a_x += a[index * 3 + 0], sum_a_y += a[index * 3 + 1], sum_a_z += a[index * 3 + 2];
|
|
177
|
+
sum_b_x += b[index * 3 + 0], sum_b_y += b[index * 3 + 1], sum_b_z += b[index * 3 + 2];
|
|
178
|
+
}
|
|
179
|
+
|
|
180
|
+
nk_f64_t inv_n = 1.0 / (nk_f64_t)n;
|
|
181
|
+
*ca_x = sum_a_x * inv_n, *ca_y = sum_a_y * inv_n, *ca_z = sum_a_z * inv_n;
|
|
182
|
+
*cb_x = sum_b_x * inv_n, *cb_y = sum_b_y * inv_n, *cb_z = sum_b_z * inv_n;
|
|
183
|
+
}
|
|
184
|
+
|
|
185
|
+
NK_INTERNAL void nk_centroid_and_cross_covariance_f32_v128relaxed_( //
|
|
186
|
+
nk_f32_t const *a, nk_f32_t const *b, nk_size_t n, //
|
|
187
|
+
nk_f64_t *ca_x, nk_f64_t *ca_y, nk_f64_t *ca_z, //
|
|
188
|
+
nk_f64_t *cb_x, nk_f64_t *cb_y, nk_f64_t *cb_z, //
|
|
189
|
+
nk_f64_t h[9]) {
|
|
190
|
+
v128_t zero_f64x2 = wasm_f64x2_splat(0.0);
|
|
191
|
+
v128_t sum_a_x_lower_f64x2 = zero_f64x2, sum_a_x_upper_f64x2 = zero_f64x2;
|
|
192
|
+
v128_t sum_a_y_lower_f64x2 = zero_f64x2, sum_a_y_upper_f64x2 = zero_f64x2;
|
|
193
|
+
v128_t sum_a_z_lower_f64x2 = zero_f64x2, sum_a_z_upper_f64x2 = zero_f64x2;
|
|
194
|
+
v128_t sum_b_x_lower_f64x2 = zero_f64x2, sum_b_x_upper_f64x2 = zero_f64x2;
|
|
195
|
+
v128_t sum_b_y_lower_f64x2 = zero_f64x2, sum_b_y_upper_f64x2 = zero_f64x2;
|
|
196
|
+
v128_t sum_b_z_lower_f64x2 = zero_f64x2, sum_b_z_upper_f64x2 = zero_f64x2;
|
|
197
|
+
v128_t cross_00_lower_f64x2 = zero_f64x2, cross_00_upper_f64x2 = zero_f64x2;
|
|
198
|
+
v128_t cross_01_lower_f64x2 = zero_f64x2, cross_01_upper_f64x2 = zero_f64x2;
|
|
199
|
+
v128_t cross_02_lower_f64x2 = zero_f64x2, cross_02_upper_f64x2 = zero_f64x2;
|
|
200
|
+
v128_t cross_10_lower_f64x2 = zero_f64x2, cross_10_upper_f64x2 = zero_f64x2;
|
|
201
|
+
v128_t cross_11_lower_f64x2 = zero_f64x2, cross_11_upper_f64x2 = zero_f64x2;
|
|
202
|
+
v128_t cross_12_lower_f64x2 = zero_f64x2, cross_12_upper_f64x2 = zero_f64x2;
|
|
203
|
+
v128_t cross_20_lower_f64x2 = zero_f64x2, cross_20_upper_f64x2 = zero_f64x2;
|
|
204
|
+
v128_t cross_21_lower_f64x2 = zero_f64x2, cross_21_upper_f64x2 = zero_f64x2;
|
|
205
|
+
v128_t cross_22_lower_f64x2 = zero_f64x2, cross_22_upper_f64x2 = zero_f64x2;
|
|
206
|
+
nk_size_t index = 0;
|
|
207
|
+
|
|
208
|
+
for (; index + 4 <= n; index += 4) {
|
|
209
|
+
v128_t a_x_f32x4, a_y_f32x4, a_z_f32x4, b_x_f32x4, b_y_f32x4, b_z_f32x4;
|
|
210
|
+
nk_deinterleave_f32x4_v128relaxed_(a + index * 3, &a_x_f32x4, &a_y_f32x4, &a_z_f32x4);
|
|
211
|
+
nk_deinterleave_f32x4_v128relaxed_(b + index * 3, &b_x_f32x4, &b_y_f32x4, &b_z_f32x4);
|
|
212
|
+
|
|
213
|
+
v128_t a_x_lower_f64x2 = wasm_f64x2_promote_low_f32x4(a_x_f32x4);
|
|
214
|
+
v128_t a_x_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_x_f32x4, a_x_f32x4, 2, 3, 0, 1));
|
|
215
|
+
v128_t a_y_lower_f64x2 = wasm_f64x2_promote_low_f32x4(a_y_f32x4);
|
|
216
|
+
v128_t a_y_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_y_f32x4, a_y_f32x4, 2, 3, 0, 1));
|
|
217
|
+
v128_t a_z_lower_f64x2 = wasm_f64x2_promote_low_f32x4(a_z_f32x4);
|
|
218
|
+
v128_t a_z_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_z_f32x4, a_z_f32x4, 2, 3, 0, 1));
|
|
219
|
+
v128_t b_x_lower_f64x2 = wasm_f64x2_promote_low_f32x4(b_x_f32x4);
|
|
220
|
+
v128_t b_x_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(b_x_f32x4, b_x_f32x4, 2, 3, 0, 1));
|
|
221
|
+
v128_t b_y_lower_f64x2 = wasm_f64x2_promote_low_f32x4(b_y_f32x4);
|
|
222
|
+
v128_t b_y_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(b_y_f32x4, b_y_f32x4, 2, 3, 0, 1));
|
|
223
|
+
v128_t b_z_lower_f64x2 = wasm_f64x2_promote_low_f32x4(b_z_f32x4);
|
|
224
|
+
v128_t b_z_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(b_z_f32x4, b_z_f32x4, 2, 3, 0, 1));
|
|
225
|
+
|
|
226
|
+
sum_a_x_lower_f64x2 = wasm_f64x2_add(sum_a_x_lower_f64x2, a_x_lower_f64x2),
|
|
227
|
+
sum_a_x_upper_f64x2 = wasm_f64x2_add(sum_a_x_upper_f64x2, a_x_upper_f64x2);
|
|
228
|
+
sum_a_y_lower_f64x2 = wasm_f64x2_add(sum_a_y_lower_f64x2, a_y_lower_f64x2),
|
|
229
|
+
sum_a_y_upper_f64x2 = wasm_f64x2_add(sum_a_y_upper_f64x2, a_y_upper_f64x2);
|
|
230
|
+
sum_a_z_lower_f64x2 = wasm_f64x2_add(sum_a_z_lower_f64x2, a_z_lower_f64x2),
|
|
231
|
+
sum_a_z_upper_f64x2 = wasm_f64x2_add(sum_a_z_upper_f64x2, a_z_upper_f64x2);
|
|
232
|
+
sum_b_x_lower_f64x2 = wasm_f64x2_add(sum_b_x_lower_f64x2, b_x_lower_f64x2),
|
|
233
|
+
sum_b_x_upper_f64x2 = wasm_f64x2_add(sum_b_x_upper_f64x2, b_x_upper_f64x2);
|
|
234
|
+
sum_b_y_lower_f64x2 = wasm_f64x2_add(sum_b_y_lower_f64x2, b_y_lower_f64x2),
|
|
235
|
+
sum_b_y_upper_f64x2 = wasm_f64x2_add(sum_b_y_upper_f64x2, b_y_upper_f64x2);
|
|
236
|
+
sum_b_z_lower_f64x2 = wasm_f64x2_add(sum_b_z_lower_f64x2, b_z_lower_f64x2),
|
|
237
|
+
sum_b_z_upper_f64x2 = wasm_f64x2_add(sum_b_z_upper_f64x2, b_z_upper_f64x2);
|
|
238
|
+
|
|
239
|
+
cross_00_lower_f64x2 = wasm_f64x2_relaxed_madd(a_x_lower_f64x2, b_x_lower_f64x2, cross_00_lower_f64x2),
|
|
240
|
+
cross_00_upper_f64x2 = wasm_f64x2_relaxed_madd(a_x_upper_f64x2, b_x_upper_f64x2, cross_00_upper_f64x2);
|
|
241
|
+
cross_01_lower_f64x2 = wasm_f64x2_relaxed_madd(a_x_lower_f64x2, b_y_lower_f64x2, cross_01_lower_f64x2),
|
|
242
|
+
cross_01_upper_f64x2 = wasm_f64x2_relaxed_madd(a_x_upper_f64x2, b_y_upper_f64x2, cross_01_upper_f64x2);
|
|
243
|
+
cross_02_lower_f64x2 = wasm_f64x2_relaxed_madd(a_x_lower_f64x2, b_z_lower_f64x2, cross_02_lower_f64x2),
|
|
244
|
+
cross_02_upper_f64x2 = wasm_f64x2_relaxed_madd(a_x_upper_f64x2, b_z_upper_f64x2, cross_02_upper_f64x2);
|
|
245
|
+
cross_10_lower_f64x2 = wasm_f64x2_relaxed_madd(a_y_lower_f64x2, b_x_lower_f64x2, cross_10_lower_f64x2),
|
|
246
|
+
cross_10_upper_f64x2 = wasm_f64x2_relaxed_madd(a_y_upper_f64x2, b_x_upper_f64x2, cross_10_upper_f64x2);
|
|
247
|
+
cross_11_lower_f64x2 = wasm_f64x2_relaxed_madd(a_y_lower_f64x2, b_y_lower_f64x2, cross_11_lower_f64x2),
|
|
248
|
+
cross_11_upper_f64x2 = wasm_f64x2_relaxed_madd(a_y_upper_f64x2, b_y_upper_f64x2, cross_11_upper_f64x2);
|
|
249
|
+
cross_12_lower_f64x2 = wasm_f64x2_relaxed_madd(a_y_lower_f64x2, b_z_lower_f64x2, cross_12_lower_f64x2),
|
|
250
|
+
cross_12_upper_f64x2 = wasm_f64x2_relaxed_madd(a_y_upper_f64x2, b_z_upper_f64x2, cross_12_upper_f64x2);
|
|
251
|
+
cross_20_lower_f64x2 = wasm_f64x2_relaxed_madd(a_z_lower_f64x2, b_x_lower_f64x2, cross_20_lower_f64x2),
|
|
252
|
+
cross_20_upper_f64x2 = wasm_f64x2_relaxed_madd(a_z_upper_f64x2, b_x_upper_f64x2, cross_20_upper_f64x2);
|
|
253
|
+
cross_21_lower_f64x2 = wasm_f64x2_relaxed_madd(a_z_lower_f64x2, b_y_lower_f64x2, cross_21_lower_f64x2),
|
|
254
|
+
cross_21_upper_f64x2 = wasm_f64x2_relaxed_madd(a_z_upper_f64x2, b_y_upper_f64x2, cross_21_upper_f64x2);
|
|
255
|
+
cross_22_lower_f64x2 = wasm_f64x2_relaxed_madd(a_z_lower_f64x2, b_z_lower_f64x2, cross_22_lower_f64x2),
|
|
256
|
+
cross_22_upper_f64x2 = wasm_f64x2_relaxed_madd(a_z_upper_f64x2, b_z_upper_f64x2, cross_22_upper_f64x2);
|
|
257
|
+
}
|
|
258
|
+
|
|
259
|
+
nk_f64_t sum_a_x = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_a_x_lower_f64x2, sum_a_x_upper_f64x2));
|
|
260
|
+
nk_f64_t sum_a_y = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_a_y_lower_f64x2, sum_a_y_upper_f64x2));
|
|
261
|
+
nk_f64_t sum_a_z = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_a_z_lower_f64x2, sum_a_z_upper_f64x2));
|
|
262
|
+
nk_f64_t sum_b_x = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_b_x_lower_f64x2, sum_b_x_upper_f64x2));
|
|
263
|
+
nk_f64_t sum_b_y = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_b_y_lower_f64x2, sum_b_y_upper_f64x2));
|
|
264
|
+
nk_f64_t sum_b_z = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_b_z_lower_f64x2, sum_b_z_upper_f64x2));
|
|
265
|
+
nk_f64_t cross_00 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_00_lower_f64x2, cross_00_upper_f64x2));
|
|
266
|
+
nk_f64_t cross_01 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_01_lower_f64x2, cross_01_upper_f64x2));
|
|
267
|
+
nk_f64_t cross_02 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_02_lower_f64x2, cross_02_upper_f64x2));
|
|
268
|
+
nk_f64_t cross_10 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_10_lower_f64x2, cross_10_upper_f64x2));
|
|
269
|
+
nk_f64_t cross_11 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_11_lower_f64x2, cross_11_upper_f64x2));
|
|
270
|
+
nk_f64_t cross_12 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_12_lower_f64x2, cross_12_upper_f64x2));
|
|
271
|
+
nk_f64_t cross_20 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_20_lower_f64x2, cross_20_upper_f64x2));
|
|
272
|
+
nk_f64_t cross_21 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_21_lower_f64x2, cross_21_upper_f64x2));
|
|
273
|
+
nk_f64_t cross_22 = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(cross_22_lower_f64x2, cross_22_upper_f64x2));
|
|
274
|
+
|
|
275
|
+
for (; index < n; ++index) {
|
|
276
|
+
nk_f64_t a_x = a[index * 3 + 0], a_y = a[index * 3 + 1], a_z = a[index * 3 + 2];
|
|
277
|
+
nk_f64_t b_x = b[index * 3 + 0], b_y = b[index * 3 + 1], b_z = b[index * 3 + 2];
|
|
278
|
+
sum_a_x += a_x, sum_a_y += a_y, sum_a_z += a_z;
|
|
279
|
+
sum_b_x += b_x, sum_b_y += b_y, sum_b_z += b_z;
|
|
280
|
+
cross_00 += a_x * b_x, cross_01 += a_x * b_y, cross_02 += a_x * b_z;
|
|
281
|
+
cross_10 += a_y * b_x, cross_11 += a_y * b_y, cross_12 += a_y * b_z;
|
|
282
|
+
cross_20 += a_z * b_x, cross_21 += a_z * b_y, cross_22 += a_z * b_z;
|
|
283
|
+
}
|
|
284
|
+
|
|
285
|
+
nk_f64_t inv_n = 1.0 / (nk_f64_t)n;
|
|
286
|
+
*ca_x = sum_a_x * inv_n, *ca_y = sum_a_y * inv_n, *ca_z = sum_a_z * inv_n;
|
|
287
|
+
*cb_x = sum_b_x * inv_n, *cb_y = sum_b_y * inv_n, *cb_z = sum_b_z * inv_n;
|
|
288
|
+
|
|
289
|
+
nk_f64_t n_f64 = (nk_f64_t)n;
|
|
290
|
+
h[0] = cross_00 - n_f64 * (*ca_x) * (*cb_x), h[1] = cross_01 - n_f64 * (*ca_x) * (*cb_y),
|
|
291
|
+
h[2] = cross_02 - n_f64 * (*ca_x) * (*cb_z);
|
|
292
|
+
h[3] = cross_10 - n_f64 * (*ca_y) * (*cb_x), h[4] = cross_11 - n_f64 * (*ca_y) * (*cb_y),
|
|
293
|
+
h[5] = cross_12 - n_f64 * (*ca_y) * (*cb_z);
|
|
294
|
+
h[6] = cross_20 - n_f64 * (*ca_z) * (*cb_x), h[7] = cross_21 - n_f64 * (*ca_z) * (*cb_y),
|
|
295
|
+
h[8] = cross_22 - n_f64 * (*ca_z) * (*cb_z);
|
|
296
|
+
}
|
|
297
|
+
|
|
298
|
+
NK_INTERNAL void nk_centroid_and_cross_covariance_and_variance_f32_v128relaxed_( //
|
|
299
|
+
nk_f32_t const *a, nk_f32_t const *b, nk_size_t n, //
|
|
300
|
+
nk_f64_t *ca_x, nk_f64_t *ca_y, nk_f64_t *ca_z, //
|
|
301
|
+
nk_f64_t *cb_x, nk_f64_t *cb_y, nk_f64_t *cb_z, //
|
|
302
|
+
nk_f64_t h[9], nk_f64_t *variance_a) {
|
|
303
|
+
v128_t zero_f64x2 = wasm_f64x2_splat(0.0);
|
|
304
|
+
v128_t sum_norm_squared_lower_f64x2 = zero_f64x2, sum_norm_squared_upper_f64x2 = zero_f64x2;
|
|
305
|
+
nk_centroid_and_cross_covariance_f32_v128relaxed_(a, b, n, ca_x, ca_y, ca_z, cb_x, cb_y, cb_z, h);
|
|
306
|
+
|
|
307
|
+
nk_size_t index = 0;
|
|
308
|
+
for (; index + 4 <= n; index += 4) {
|
|
309
|
+
v128_t a_x_f32x4, a_y_f32x4, a_z_f32x4;
|
|
310
|
+
nk_deinterleave_f32x4_v128relaxed_(a + index * 3, &a_x_f32x4, &a_y_f32x4, &a_z_f32x4);
|
|
311
|
+
|
|
312
|
+
v128_t a_x_lower_f64x2 = wasm_f64x2_promote_low_f32x4(a_x_f32x4);
|
|
313
|
+
v128_t a_x_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_x_f32x4, a_x_f32x4, 2, 3, 0, 1));
|
|
314
|
+
v128_t a_y_lower_f64x2 = wasm_f64x2_promote_low_f32x4(a_y_f32x4);
|
|
315
|
+
v128_t a_y_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_y_f32x4, a_y_f32x4, 2, 3, 0, 1));
|
|
316
|
+
v128_t a_z_lower_f64x2 = wasm_f64x2_promote_low_f32x4(a_z_f32x4);
|
|
317
|
+
v128_t a_z_upper_f64x2 = wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_z_f32x4, a_z_f32x4, 2, 3, 0, 1));
|
|
318
|
+
|
|
319
|
+
v128_t norm_squared_lower_f64x2 = wasm_f64x2_relaxed_madd(a_y_lower_f64x2, a_y_lower_f64x2,
|
|
320
|
+
wasm_f64x2_mul(a_x_lower_f64x2, a_x_lower_f64x2));
|
|
321
|
+
v128_t norm_squared_upper_f64x2 = wasm_f64x2_relaxed_madd(a_y_upper_f64x2, a_y_upper_f64x2,
|
|
322
|
+
wasm_f64x2_mul(a_x_upper_f64x2, a_x_upper_f64x2));
|
|
323
|
+
norm_squared_lower_f64x2 = wasm_f64x2_relaxed_madd(a_z_lower_f64x2, a_z_lower_f64x2, norm_squared_lower_f64x2);
|
|
324
|
+
norm_squared_upper_f64x2 = wasm_f64x2_relaxed_madd(a_z_upper_f64x2, a_z_upper_f64x2, norm_squared_upper_f64x2);
|
|
325
|
+
sum_norm_squared_lower_f64x2 = wasm_f64x2_add(sum_norm_squared_lower_f64x2, norm_squared_lower_f64x2),
|
|
326
|
+
sum_norm_squared_upper_f64x2 = wasm_f64x2_add(sum_norm_squared_upper_f64x2, norm_squared_upper_f64x2);
|
|
327
|
+
}
|
|
328
|
+
|
|
329
|
+
nk_f64_t sum_norm_squared = nk_hsum_f64x2_v128relaxed_(
|
|
330
|
+
wasm_f64x2_add(sum_norm_squared_lower_f64x2, sum_norm_squared_upper_f64x2));
|
|
331
|
+
for (; index < n; ++index) {
|
|
332
|
+
nk_f64_t a_x = a[index * 3 + 0], a_y = a[index * 3 + 1], a_z = a[index * 3 + 2];
|
|
333
|
+
sum_norm_squared += a_x * a_x + a_y * a_y + a_z * a_z;
|
|
334
|
+
}
|
|
335
|
+
|
|
336
|
+
nk_f64_t inv_n = 1.0 / (nk_f64_t)n;
|
|
337
|
+
*variance_a = sum_norm_squared * inv_n - ((*ca_x) * (*ca_x) + (*ca_y) * (*ca_y) + (*ca_z) * (*ca_z));
|
|
338
|
+
}
|
|
339
|
+
|
|
340
|
+
NK_INTERNAL nk_f64_t nk_transformed_ssd_f32_v128relaxed_( //
|
|
341
|
+
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,
|
|
342
|
+
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) {
|
|
343
|
+
v128_t scaled_rotation_x_x_f64x2 = wasm_f64x2_splat(scale * r[0]);
|
|
344
|
+
v128_t scaled_rotation_x_y_f64x2 = wasm_f64x2_splat(scale * r[1]);
|
|
345
|
+
v128_t scaled_rotation_x_z_f64x2 = wasm_f64x2_splat(scale * r[2]);
|
|
346
|
+
v128_t scaled_rotation_y_x_f64x2 = wasm_f64x2_splat(scale * r[3]);
|
|
347
|
+
v128_t scaled_rotation_y_y_f64x2 = wasm_f64x2_splat(scale * r[4]);
|
|
348
|
+
v128_t scaled_rotation_y_z_f64x2 = wasm_f64x2_splat(scale * r[5]);
|
|
349
|
+
v128_t scaled_rotation_z_x_f64x2 = wasm_f64x2_splat(scale * r[6]);
|
|
350
|
+
v128_t scaled_rotation_z_y_f64x2 = wasm_f64x2_splat(scale * r[7]);
|
|
351
|
+
v128_t scaled_rotation_z_z_f64x2 = wasm_f64x2_splat(scale * r[8]);
|
|
352
|
+
v128_t centroid_a_x_f64x2 = wasm_f64x2_splat(centroid_a_x), centroid_a_y_f64x2 = wasm_f64x2_splat(centroid_a_y);
|
|
353
|
+
v128_t centroid_a_z_f64x2 = wasm_f64x2_splat(centroid_a_z), centroid_b_x_f64x2 = wasm_f64x2_splat(centroid_b_x);
|
|
354
|
+
v128_t centroid_b_y_f64x2 = wasm_f64x2_splat(centroid_b_y), centroid_b_z_f64x2 = wasm_f64x2_splat(centroid_b_z);
|
|
355
|
+
v128_t sum_squared_lower_f64x2 = wasm_f64x2_splat(0.0), sum_squared_upper_f64x2 = wasm_f64x2_splat(0.0);
|
|
356
|
+
nk_size_t index = 0;
|
|
357
|
+
|
|
358
|
+
for (; index + 4 <= n; index += 4) {
|
|
359
|
+
v128_t a_x_f32x4, a_y_f32x4, a_z_f32x4, b_x_f32x4, b_y_f32x4, b_z_f32x4;
|
|
360
|
+
nk_deinterleave_f32x4_v128relaxed_(a + index * 3, &a_x_f32x4, &a_y_f32x4, &a_z_f32x4);
|
|
361
|
+
nk_deinterleave_f32x4_v128relaxed_(b + index * 3, &b_x_f32x4, &b_y_f32x4, &b_z_f32x4);
|
|
362
|
+
|
|
363
|
+
v128_t centered_a_x_lower_f64x2 = wasm_f64x2_sub(wasm_f64x2_promote_low_f32x4(a_x_f32x4), centroid_a_x_f64x2);
|
|
364
|
+
v128_t centered_a_x_upper_f64x2 = wasm_f64x2_sub(
|
|
365
|
+
wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_x_f32x4, a_x_f32x4, 2, 3, 0, 1)), centroid_a_x_f64x2);
|
|
366
|
+
v128_t centered_a_y_lower_f64x2 = wasm_f64x2_sub(wasm_f64x2_promote_low_f32x4(a_y_f32x4), centroid_a_y_f64x2);
|
|
367
|
+
v128_t centered_a_y_upper_f64x2 = wasm_f64x2_sub(
|
|
368
|
+
wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_y_f32x4, a_y_f32x4, 2, 3, 0, 1)), centroid_a_y_f64x2);
|
|
369
|
+
v128_t centered_a_z_lower_f64x2 = wasm_f64x2_sub(wasm_f64x2_promote_low_f32x4(a_z_f32x4), centroid_a_z_f64x2);
|
|
370
|
+
v128_t centered_a_z_upper_f64x2 = wasm_f64x2_sub(
|
|
371
|
+
wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(a_z_f32x4, a_z_f32x4, 2, 3, 0, 1)), centroid_a_z_f64x2);
|
|
372
|
+
v128_t centered_b_x_lower_f64x2 = wasm_f64x2_sub(wasm_f64x2_promote_low_f32x4(b_x_f32x4), centroid_b_x_f64x2);
|
|
373
|
+
v128_t centered_b_x_upper_f64x2 = wasm_f64x2_sub(
|
|
374
|
+
wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(b_x_f32x4, b_x_f32x4, 2, 3, 0, 1)), centroid_b_x_f64x2);
|
|
375
|
+
v128_t centered_b_y_lower_f64x2 = wasm_f64x2_sub(wasm_f64x2_promote_low_f32x4(b_y_f32x4), centroid_b_y_f64x2);
|
|
376
|
+
v128_t centered_b_y_upper_f64x2 = wasm_f64x2_sub(
|
|
377
|
+
wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(b_y_f32x4, b_y_f32x4, 2, 3, 0, 1)), centroid_b_y_f64x2);
|
|
378
|
+
v128_t centered_b_z_lower_f64x2 = wasm_f64x2_sub(wasm_f64x2_promote_low_f32x4(b_z_f32x4), centroid_b_z_f64x2);
|
|
379
|
+
v128_t centered_b_z_upper_f64x2 = wasm_f64x2_sub(
|
|
380
|
+
wasm_f64x2_promote_low_f32x4(wasm_i32x4_shuffle(b_z_f32x4, b_z_f32x4, 2, 3, 0, 1)), centroid_b_z_f64x2);
|
|
381
|
+
|
|
382
|
+
v128_t rotated_a_x_lower_f64x2 = wasm_f64x2_relaxed_madd(
|
|
383
|
+
scaled_rotation_x_z_f64x2, centered_a_z_lower_f64x2,
|
|
384
|
+
wasm_f64x2_relaxed_madd(scaled_rotation_x_y_f64x2, centered_a_y_lower_f64x2,
|
|
385
|
+
wasm_f64x2_mul(scaled_rotation_x_x_f64x2, centered_a_x_lower_f64x2)));
|
|
386
|
+
v128_t rotated_a_x_upper_f64x2 = wasm_f64x2_relaxed_madd(
|
|
387
|
+
scaled_rotation_x_z_f64x2, centered_a_z_upper_f64x2,
|
|
388
|
+
wasm_f64x2_relaxed_madd(scaled_rotation_x_y_f64x2, centered_a_y_upper_f64x2,
|
|
389
|
+
wasm_f64x2_mul(scaled_rotation_x_x_f64x2, centered_a_x_upper_f64x2)));
|
|
390
|
+
v128_t rotated_a_y_lower_f64x2 = wasm_f64x2_relaxed_madd(
|
|
391
|
+
scaled_rotation_y_z_f64x2, centered_a_z_lower_f64x2,
|
|
392
|
+
wasm_f64x2_relaxed_madd(scaled_rotation_y_y_f64x2, centered_a_y_lower_f64x2,
|
|
393
|
+
wasm_f64x2_mul(scaled_rotation_y_x_f64x2, centered_a_x_lower_f64x2)));
|
|
394
|
+
v128_t rotated_a_y_upper_f64x2 = wasm_f64x2_relaxed_madd(
|
|
395
|
+
scaled_rotation_y_z_f64x2, centered_a_z_upper_f64x2,
|
|
396
|
+
wasm_f64x2_relaxed_madd(scaled_rotation_y_y_f64x2, centered_a_y_upper_f64x2,
|
|
397
|
+
wasm_f64x2_mul(scaled_rotation_y_x_f64x2, centered_a_x_upper_f64x2)));
|
|
398
|
+
v128_t rotated_a_z_lower_f64x2 = wasm_f64x2_relaxed_madd(
|
|
399
|
+
scaled_rotation_z_z_f64x2, centered_a_z_lower_f64x2,
|
|
400
|
+
wasm_f64x2_relaxed_madd(scaled_rotation_z_y_f64x2, centered_a_y_lower_f64x2,
|
|
401
|
+
wasm_f64x2_mul(scaled_rotation_z_x_f64x2, centered_a_x_lower_f64x2)));
|
|
402
|
+
v128_t rotated_a_z_upper_f64x2 = wasm_f64x2_relaxed_madd(
|
|
403
|
+
scaled_rotation_z_z_f64x2, centered_a_z_upper_f64x2,
|
|
404
|
+
wasm_f64x2_relaxed_madd(scaled_rotation_z_y_f64x2, centered_a_y_upper_f64x2,
|
|
405
|
+
wasm_f64x2_mul(scaled_rotation_z_x_f64x2, centered_a_x_upper_f64x2)));
|
|
406
|
+
|
|
407
|
+
v128_t delta_x_lower_f64x2 = wasm_f64x2_sub(rotated_a_x_lower_f64x2, centered_b_x_lower_f64x2);
|
|
408
|
+
v128_t delta_x_upper_f64x2 = wasm_f64x2_sub(rotated_a_x_upper_f64x2, centered_b_x_upper_f64x2);
|
|
409
|
+
v128_t delta_y_lower_f64x2 = wasm_f64x2_sub(rotated_a_y_lower_f64x2, centered_b_y_lower_f64x2);
|
|
410
|
+
v128_t delta_y_upper_f64x2 = wasm_f64x2_sub(rotated_a_y_upper_f64x2, centered_b_y_upper_f64x2);
|
|
411
|
+
v128_t delta_z_lower_f64x2 = wasm_f64x2_sub(rotated_a_z_lower_f64x2, centered_b_z_lower_f64x2);
|
|
412
|
+
v128_t delta_z_upper_f64x2 = wasm_f64x2_sub(rotated_a_z_upper_f64x2, centered_b_z_upper_f64x2);
|
|
413
|
+
|
|
414
|
+
sum_squared_lower_f64x2 = wasm_f64x2_relaxed_madd(delta_x_lower_f64x2, delta_x_lower_f64x2,
|
|
415
|
+
sum_squared_lower_f64x2);
|
|
416
|
+
sum_squared_upper_f64x2 = wasm_f64x2_relaxed_madd(delta_x_upper_f64x2, delta_x_upper_f64x2,
|
|
417
|
+
sum_squared_upper_f64x2);
|
|
418
|
+
sum_squared_lower_f64x2 = wasm_f64x2_relaxed_madd(delta_y_lower_f64x2, delta_y_lower_f64x2,
|
|
419
|
+
sum_squared_lower_f64x2);
|
|
420
|
+
sum_squared_upper_f64x2 = wasm_f64x2_relaxed_madd(delta_y_upper_f64x2, delta_y_upper_f64x2,
|
|
421
|
+
sum_squared_upper_f64x2);
|
|
422
|
+
sum_squared_lower_f64x2 = wasm_f64x2_relaxed_madd(delta_z_lower_f64x2, delta_z_lower_f64x2,
|
|
423
|
+
sum_squared_lower_f64x2);
|
|
424
|
+
sum_squared_upper_f64x2 = wasm_f64x2_relaxed_madd(delta_z_upper_f64x2, delta_z_upper_f64x2,
|
|
425
|
+
sum_squared_upper_f64x2);
|
|
426
|
+
}
|
|
427
|
+
|
|
428
|
+
nk_f64_t sum_squared = nk_hsum_f64x2_v128relaxed_(wasm_f64x2_add(sum_squared_lower_f64x2, sum_squared_upper_f64x2));
|
|
429
|
+
for (; index < n; ++index) {
|
|
430
|
+
nk_f64_t centered_a_x = (nk_f64_t)a[index * 3 + 0] - centroid_a_x;
|
|
431
|
+
nk_f64_t centered_a_y = (nk_f64_t)a[index * 3 + 1] - centroid_a_y;
|
|
432
|
+
nk_f64_t centered_a_z = (nk_f64_t)a[index * 3 + 2] - centroid_a_z;
|
|
433
|
+
nk_f64_t centered_b_x = (nk_f64_t)b[index * 3 + 0] - centroid_b_x;
|
|
434
|
+
nk_f64_t centered_b_y = (nk_f64_t)b[index * 3 + 1] - centroid_b_y;
|
|
435
|
+
nk_f64_t centered_b_z = (nk_f64_t)b[index * 3 + 2] - centroid_b_z;
|
|
436
|
+
nk_f64_t rotated_a_x = scale * (r[0] * centered_a_x + r[1] * centered_a_y + r[2] * centered_a_z);
|
|
437
|
+
nk_f64_t rotated_a_y = scale * (r[3] * centered_a_x + r[4] * centered_a_y + r[5] * centered_a_z);
|
|
438
|
+
nk_f64_t rotated_a_z = scale * (r[6] * centered_a_x + r[7] * centered_a_y + r[8] * centered_a_z);
|
|
439
|
+
nk_f64_t delta_x = rotated_a_x - centered_b_x, delta_y = rotated_a_y - centered_b_y,
|
|
440
|
+
delta_z = rotated_a_z - centered_b_z;
|
|
441
|
+
sum_squared += delta_x * delta_x + delta_y * delta_y + delta_z * delta_z;
|
|
442
|
+
}
|
|
443
|
+
|
|
444
|
+
return sum_squared;
|
|
445
|
+
}
|
|
446
|
+
|
|
447
|
+
/* Compute sum of squared distances for f64 after applying rotation (and optional scale). */
|
|
448
|
+
NK_INTERNAL nk_f64_t nk_transformed_ssd_f64_v128relaxed_(nk_f64_t const *a, nk_f64_t const *b, nk_size_t n,
|
|
449
|
+
nk_f64_t const *r, nk_f64_t scale, nk_f64_t centroid_a_x,
|
|
450
|
+
nk_f64_t centroid_a_y, nk_f64_t centroid_a_z,
|
|
451
|
+
nk_f64_t centroid_b_x, nk_f64_t centroid_b_y,
|
|
452
|
+
nk_f64_t centroid_b_z) {
|
|
453
|
+
// Broadcast scaled rotation matrix elements
|
|
454
|
+
v128_t scaled_rotation_x_x_f64x2 = wasm_f64x2_splat(scale * r[0]);
|
|
455
|
+
v128_t scaled_rotation_x_y_f64x2 = wasm_f64x2_splat(scale * r[1]);
|
|
456
|
+
v128_t scaled_rotation_x_z_f64x2 = wasm_f64x2_splat(scale * r[2]);
|
|
457
|
+
v128_t scaled_rotation_y_x_f64x2 = wasm_f64x2_splat(scale * r[3]);
|
|
458
|
+
v128_t scaled_rotation_y_y_f64x2 = wasm_f64x2_splat(scale * r[4]);
|
|
459
|
+
v128_t scaled_rotation_y_z_f64x2 = wasm_f64x2_splat(scale * r[5]);
|
|
460
|
+
v128_t scaled_rotation_z_x_f64x2 = wasm_f64x2_splat(scale * r[6]);
|
|
461
|
+
v128_t scaled_rotation_z_y_f64x2 = wasm_f64x2_splat(scale * r[7]);
|
|
462
|
+
v128_t scaled_rotation_z_z_f64x2 = wasm_f64x2_splat(scale * r[8]);
|
|
463
|
+
|
|
464
|
+
// Broadcast centroids
|
|
465
|
+
v128_t centroid_a_x_f64x2 = wasm_f64x2_splat(centroid_a_x);
|
|
466
|
+
v128_t centroid_a_y_f64x2 = wasm_f64x2_splat(centroid_a_y);
|
|
467
|
+
v128_t centroid_a_z_f64x2 = wasm_f64x2_splat(centroid_a_z);
|
|
468
|
+
v128_t centroid_b_x_f64x2 = wasm_f64x2_splat(centroid_b_x);
|
|
469
|
+
v128_t centroid_b_y_f64x2 = wasm_f64x2_splat(centroid_b_y);
|
|
470
|
+
v128_t centroid_b_z_f64x2 = wasm_f64x2_splat(centroid_b_z);
|
|
471
|
+
|
|
472
|
+
v128_t sum_squared_f64x2 = wasm_f64x2_splat(0), sum_squared_compensation_f64x2 = wasm_f64x2_splat(0);
|
|
473
|
+
nk_size_t j = 0;
|
|
474
|
+
|
|
475
|
+
// Main loop: process 2 points per iteration
|
|
476
|
+
for (; j + 2 <= n; j += 2) {
|
|
477
|
+
v128_t a_x, a_y, a_z, b_x, b_y, b_z;
|
|
478
|
+
nk_deinterleave_f64x2_v128relaxed_(a + j * 3, &a_x, &a_y, &a_z);
|
|
479
|
+
nk_deinterleave_f64x2_v128relaxed_(b + j * 3, &b_x, &b_y, &b_z);
|
|
480
|
+
|
|
481
|
+
v128_t pa_x = wasm_f64x2_sub(a_x, centroid_a_x_f64x2);
|
|
482
|
+
v128_t pa_y = wasm_f64x2_sub(a_y, centroid_a_y_f64x2);
|
|
483
|
+
v128_t pa_z = wasm_f64x2_sub(a_z, centroid_a_z_f64x2);
|
|
484
|
+
v128_t pb_x = wasm_f64x2_sub(b_x, centroid_b_x_f64x2);
|
|
485
|
+
v128_t pb_y = wasm_f64x2_sub(b_y, centroid_b_y_f64x2);
|
|
486
|
+
v128_t pb_z = wasm_f64x2_sub(b_z, centroid_b_z_f64x2);
|
|
487
|
+
|
|
488
|
+
// Rotate and scale: ra = scale * R * pa
|
|
489
|
+
v128_t ra_x = wasm_f64x2_relaxed_madd(
|
|
490
|
+
scaled_rotation_x_z_f64x2, pa_z,
|
|
491
|
+
wasm_f64x2_relaxed_madd(scaled_rotation_x_y_f64x2, pa_y, wasm_f64x2_mul(scaled_rotation_x_x_f64x2, pa_x)));
|
|
492
|
+
v128_t ra_y = wasm_f64x2_relaxed_madd(
|
|
493
|
+
scaled_rotation_y_z_f64x2, pa_z,
|
|
494
|
+
wasm_f64x2_relaxed_madd(scaled_rotation_y_y_f64x2, pa_y, wasm_f64x2_mul(scaled_rotation_y_x_f64x2, pa_x)));
|
|
495
|
+
v128_t ra_z = wasm_f64x2_relaxed_madd(
|
|
496
|
+
scaled_rotation_z_z_f64x2, pa_z,
|
|
497
|
+
wasm_f64x2_relaxed_madd(scaled_rotation_z_y_f64x2, pa_y, wasm_f64x2_mul(scaled_rotation_z_x_f64x2, pa_x)));
|
|
498
|
+
|
|
499
|
+
v128_t delta_x = wasm_f64x2_sub(ra_x, pb_x);
|
|
500
|
+
v128_t delta_y = wasm_f64x2_sub(ra_y, pb_y);
|
|
501
|
+
v128_t delta_z = wasm_f64x2_sub(ra_z, pb_z);
|
|
502
|
+
|
|
503
|
+
nk_accumulate_square_f64x2_v128relaxed_(&sum_squared_f64x2, &sum_squared_compensation_f64x2, delta_x);
|
|
504
|
+
nk_accumulate_square_f64x2_v128relaxed_(&sum_squared_f64x2, &sum_squared_compensation_f64x2, delta_y);
|
|
505
|
+
nk_accumulate_square_f64x2_v128relaxed_(&sum_squared_f64x2, &sum_squared_compensation_f64x2, delta_z);
|
|
506
|
+
}
|
|
507
|
+
|
|
508
|
+
nk_f64_t sum_squared = nk_dot_stable_sum_f64x2_v128relaxed_(sum_squared_f64x2, sum_squared_compensation_f64x2);
|
|
509
|
+
nk_f64_t sum_squared_compensation = 0.0;
|
|
510
|
+
|
|
511
|
+
// Scalar tail
|
|
512
|
+
for (; j < n; ++j) {
|
|
513
|
+
nk_f64_t pa_x = a[j * 3 + 0] - centroid_a_x;
|
|
514
|
+
nk_f64_t pa_y = a[j * 3 + 1] - centroid_a_y;
|
|
515
|
+
nk_f64_t pa_z = a[j * 3 + 2] - centroid_a_z;
|
|
516
|
+
nk_f64_t pb_x = b[j * 3 + 0] - centroid_b_x;
|
|
517
|
+
nk_f64_t pb_y = b[j * 3 + 1] - centroid_b_y;
|
|
518
|
+
nk_f64_t pb_z = b[j * 3 + 2] - centroid_b_z;
|
|
519
|
+
|
|
520
|
+
nk_f64_t ra_x = scale * (r[0] * pa_x + r[1] * pa_y + r[2] * pa_z);
|
|
521
|
+
nk_f64_t ra_y = scale * (r[3] * pa_x + r[4] * pa_y + r[5] * pa_z);
|
|
522
|
+
nk_f64_t ra_z = scale * (r[6] * pa_x + r[7] * pa_y + r[8] * pa_z);
|
|
523
|
+
|
|
524
|
+
nk_f64_t delta_x = ra_x - pb_x;
|
|
525
|
+
nk_f64_t delta_y = ra_y - pb_y;
|
|
526
|
+
nk_f64_t delta_z = ra_z - pb_z;
|
|
527
|
+
nk_accumulate_square_f64_(&sum_squared, &sum_squared_compensation, delta_x);
|
|
528
|
+
nk_accumulate_square_f64_(&sum_squared, &sum_squared_compensation, delta_y);
|
|
529
|
+
nk_accumulate_square_f64_(&sum_squared, &sum_squared_compensation, delta_z);
|
|
530
|
+
}
|
|
531
|
+
|
|
532
|
+
return sum_squared + sum_squared_compensation;
|
|
533
|
+
}
|
|
534
|
+
|
|
535
|
+
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,
|
|
536
|
+
nk_f32_t *b_centroid, nk_f32_t *rotation, nk_f32_t *scale, nk_f64_t *result) {
|
|
537
|
+
// RMSD uses identity rotation and scale=1.0.
|
|
538
|
+
if (rotation) {
|
|
539
|
+
rotation[0] = 1, rotation[1] = 0, rotation[2] = 0;
|
|
540
|
+
rotation[3] = 0, rotation[4] = 1, rotation[5] = 0;
|
|
541
|
+
rotation[6] = 0, rotation[7] = 0, rotation[8] = 1;
|
|
542
|
+
}
|
|
543
|
+
if (scale) *scale = 1.0f;
|
|
544
|
+
nk_f64_t centroid_a_x, centroid_a_y, centroid_a_z, centroid_b_x, centroid_b_y, centroid_b_z;
|
|
545
|
+
nk_f64_t identity[9] = {1, 0, 0, 0, 1, 0, 0, 0, 1};
|
|
546
|
+
nk_bicentroid_f32_v128relaxed_(a, b, n, ¢roid_a_x, ¢roid_a_y, ¢roid_a_z, ¢roid_b_x, ¢roid_b_y,
|
|
547
|
+
¢roid_b_z);
|
|
548
|
+
if (a_centroid)
|
|
549
|
+
a_centroid[0] = (nk_f32_t)centroid_a_x, a_centroid[1] = (nk_f32_t)centroid_a_y,
|
|
550
|
+
a_centroid[2] = (nk_f32_t)centroid_a_z;
|
|
551
|
+
if (b_centroid)
|
|
552
|
+
b_centroid[0] = (nk_f32_t)centroid_b_x, b_centroid[1] = (nk_f32_t)centroid_b_y,
|
|
553
|
+
b_centroid[2] = (nk_f32_t)centroid_b_z;
|
|
554
|
+
*result = nk_f64_sqrt_v128relaxed(nk_transformed_ssd_f32_v128relaxed_(a, b, n, identity, 1.0, centroid_a_x,
|
|
555
|
+
centroid_a_y, centroid_a_z, centroid_b_x,
|
|
556
|
+
centroid_b_y, centroid_b_z) /
|
|
557
|
+
(nk_f64_t)n);
|
|
558
|
+
}
|
|
559
|
+
|
|
560
|
+
NK_PUBLIC void nk_rmsd_f64_v128relaxed(nk_f64_t const *a, nk_f64_t const *b, nk_size_t n, nk_f64_t *a_centroid,
|
|
561
|
+
nk_f64_t *b_centroid, nk_f64_t *rotation, nk_f64_t *scale, nk_f64_t *result) {
|
|
562
|
+
// RMSD uses identity rotation and scale=1.0
|
|
563
|
+
if (rotation) {
|
|
564
|
+
rotation[0] = 1, rotation[1] = 0, rotation[2] = 0;
|
|
565
|
+
rotation[3] = 0, rotation[4] = 1, rotation[5] = 0;
|
|
566
|
+
rotation[6] = 0, rotation[7] = 0, rotation[8] = 1;
|
|
567
|
+
}
|
|
568
|
+
if (scale) *scale = 1.0;
|
|
569
|
+
|
|
570
|
+
v128_t const zeros_f64x2 = wasm_f64x2_splat(0);
|
|
571
|
+
|
|
572
|
+
// Accumulators for centroids and squared differences
|
|
573
|
+
v128_t sum_a_x_f64x2 = zeros_f64x2, sum_a_y_f64x2 = zeros_f64x2, sum_a_z_f64x2 = zeros_f64x2;
|
|
574
|
+
v128_t sum_b_x_f64x2 = zeros_f64x2, sum_b_y_f64x2 = zeros_f64x2, sum_b_z_f64x2 = zeros_f64x2;
|
|
575
|
+
v128_t sum_squared_x_f64x2 = zeros_f64x2, sum_squared_y_f64x2 = zeros_f64x2, sum_squared_z_f64x2 = zeros_f64x2;
|
|
576
|
+
|
|
577
|
+
v128_t a_x_f64x2, a_y_f64x2, a_z_f64x2, b_x_f64x2, b_y_f64x2, b_z_f64x2;
|
|
578
|
+
nk_size_t i = 0;
|
|
579
|
+
|
|
580
|
+
// Main loop processing 2 points at a time
|
|
581
|
+
for (; i + 2 <= n; i += 2) {
|
|
582
|
+
nk_deinterleave_f64x2_v128relaxed_(a + i * 3, &a_x_f64x2, &a_y_f64x2, &a_z_f64x2);
|
|
583
|
+
nk_deinterleave_f64x2_v128relaxed_(b + i * 3, &b_x_f64x2, &b_y_f64x2, &b_z_f64x2);
|
|
584
|
+
|
|
585
|
+
sum_a_x_f64x2 = wasm_f64x2_add(sum_a_x_f64x2, a_x_f64x2);
|
|
586
|
+
sum_a_y_f64x2 = wasm_f64x2_add(sum_a_y_f64x2, a_y_f64x2);
|
|
587
|
+
sum_a_z_f64x2 = wasm_f64x2_add(sum_a_z_f64x2, a_z_f64x2);
|
|
588
|
+
sum_b_x_f64x2 = wasm_f64x2_add(sum_b_x_f64x2, b_x_f64x2);
|
|
589
|
+
sum_b_y_f64x2 = wasm_f64x2_add(sum_b_y_f64x2, b_y_f64x2);
|
|
590
|
+
sum_b_z_f64x2 = wasm_f64x2_add(sum_b_z_f64x2, b_z_f64x2);
|
|
591
|
+
|
|
592
|
+
v128_t delta_x_f64x2 = wasm_f64x2_sub(a_x_f64x2, b_x_f64x2);
|
|
593
|
+
v128_t delta_y_f64x2 = wasm_f64x2_sub(a_y_f64x2, b_y_f64x2);
|
|
594
|
+
v128_t delta_z_f64x2 = wasm_f64x2_sub(a_z_f64x2, b_z_f64x2);
|
|
595
|
+
|
|
596
|
+
sum_squared_x_f64x2 = wasm_f64x2_relaxed_madd(delta_x_f64x2, delta_x_f64x2, sum_squared_x_f64x2);
|
|
597
|
+
sum_squared_y_f64x2 = wasm_f64x2_relaxed_madd(delta_y_f64x2, delta_y_f64x2, sum_squared_y_f64x2);
|
|
598
|
+
sum_squared_z_f64x2 = wasm_f64x2_relaxed_madd(delta_z_f64x2, delta_z_f64x2, sum_squared_z_f64x2);
|
|
599
|
+
}
|
|
600
|
+
|
|
601
|
+
// Reduce vectors to scalars.
|
|
602
|
+
nk_f64_t total_ax = nk_reduce_stable_f64x2_v128relaxed_(sum_a_x_f64x2), total_ax_compensation = 0.0;
|
|
603
|
+
nk_f64_t total_ay = nk_reduce_stable_f64x2_v128relaxed_(sum_a_y_f64x2), total_ay_compensation = 0.0;
|
|
604
|
+
nk_f64_t total_az = nk_reduce_stable_f64x2_v128relaxed_(sum_a_z_f64x2), total_az_compensation = 0.0;
|
|
605
|
+
nk_f64_t total_bx = nk_reduce_stable_f64x2_v128relaxed_(sum_b_x_f64x2), total_bx_compensation = 0.0;
|
|
606
|
+
nk_f64_t total_by = nk_reduce_stable_f64x2_v128relaxed_(sum_b_y_f64x2), total_by_compensation = 0.0;
|
|
607
|
+
nk_f64_t total_bz = nk_reduce_stable_f64x2_v128relaxed_(sum_b_z_f64x2), total_bz_compensation = 0.0;
|
|
608
|
+
nk_f64_t total_squared_x = nk_reduce_stable_f64x2_v128relaxed_(sum_squared_x_f64x2),
|
|
609
|
+
total_squared_x_compensation = 0.0;
|
|
610
|
+
nk_f64_t total_squared_y = nk_reduce_stable_f64x2_v128relaxed_(sum_squared_y_f64x2),
|
|
611
|
+
total_squared_y_compensation = 0.0;
|
|
612
|
+
nk_f64_t total_squared_z = nk_reduce_stable_f64x2_v128relaxed_(sum_squared_z_f64x2),
|
|
613
|
+
total_squared_z_compensation = 0.0;
|
|
614
|
+
|
|
615
|
+
// Scalar tail
|
|
616
|
+
for (; i < n; ++i) {
|
|
617
|
+
nk_f64_t ax = a[i * 3 + 0], ay = a[i * 3 + 1], az = a[i * 3 + 2];
|
|
618
|
+
nk_f64_t bx = b[i * 3 + 0], by = b[i * 3 + 1], bz = b[i * 3 + 2];
|
|
619
|
+
nk_accumulate_sum_f64_(&total_ax, &total_ax_compensation, ax);
|
|
620
|
+
nk_accumulate_sum_f64_(&total_ay, &total_ay_compensation, ay);
|
|
621
|
+
nk_accumulate_sum_f64_(&total_az, &total_az_compensation, az);
|
|
622
|
+
nk_accumulate_sum_f64_(&total_bx, &total_bx_compensation, bx);
|
|
623
|
+
nk_accumulate_sum_f64_(&total_by, &total_by_compensation, by);
|
|
624
|
+
nk_accumulate_sum_f64_(&total_bz, &total_bz_compensation, bz);
|
|
625
|
+
nk_f64_t delta_x = ax - bx, delta_y = ay - by, delta_z = az - bz;
|
|
626
|
+
nk_accumulate_square_f64_(&total_squared_x, &total_squared_x_compensation, delta_x);
|
|
627
|
+
nk_accumulate_square_f64_(&total_squared_y, &total_squared_y_compensation, delta_y);
|
|
628
|
+
nk_accumulate_square_f64_(&total_squared_z, &total_squared_z_compensation, delta_z);
|
|
629
|
+
}
|
|
630
|
+
|
|
631
|
+
total_ax += total_ax_compensation, total_ay += total_ay_compensation, total_az += total_az_compensation;
|
|
632
|
+
total_bx += total_bx_compensation, total_by += total_by_compensation, total_bz += total_bz_compensation;
|
|
633
|
+
total_squared_x += total_squared_x_compensation, total_squared_y += total_squared_y_compensation,
|
|
634
|
+
total_squared_z += total_squared_z_compensation;
|
|
635
|
+
|
|
636
|
+
// Compute centroids
|
|
637
|
+
nk_f64_t inv_n = 1.0 / (nk_f64_t)n;
|
|
638
|
+
nk_f64_t centroid_a_x = total_ax * inv_n, centroid_a_y = total_ay * inv_n, centroid_a_z = total_az * inv_n;
|
|
639
|
+
nk_f64_t centroid_b_x = total_bx * inv_n, centroid_b_y = total_by * inv_n, centroid_b_z = total_bz * inv_n;
|
|
640
|
+
if (a_centroid) a_centroid[0] = centroid_a_x, a_centroid[1] = centroid_a_y, a_centroid[2] = centroid_a_z;
|
|
641
|
+
if (b_centroid) b_centroid[0] = centroid_b_x, b_centroid[1] = centroid_b_y, b_centroid[2] = centroid_b_z;
|
|
642
|
+
|
|
643
|
+
// Compute RMSD
|
|
644
|
+
nk_f64_t mean_diff_x = centroid_a_x - centroid_b_x;
|
|
645
|
+
nk_f64_t mean_diff_y = centroid_a_y - centroid_b_y;
|
|
646
|
+
nk_f64_t mean_diff_z = centroid_a_z - centroid_b_z;
|
|
647
|
+
nk_f64_t sum_squared = total_squared_x + total_squared_y + total_squared_z;
|
|
648
|
+
nk_f64_t mean_diff_sq = mean_diff_x * mean_diff_x + mean_diff_y * mean_diff_y + mean_diff_z * mean_diff_z;
|
|
649
|
+
|
|
650
|
+
*result = nk_f64_sqrt_v128relaxed(sum_squared * inv_n - mean_diff_sq);
|
|
651
|
+
}
|
|
652
|
+
|
|
653
|
+
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,
|
|
654
|
+
nk_f32_t *b_centroid, nk_f32_t *rotation, nk_f32_t *scale, nk_f64_t *result) {
|
|
655
|
+
nk_f64_t centroid_a_x, centroid_a_y, centroid_a_z, centroid_b_x, centroid_b_y, centroid_b_z;
|
|
656
|
+
nk_f64_t h[9];
|
|
657
|
+
nk_centroid_and_cross_covariance_f32_v128relaxed_(a, b, n, ¢roid_a_x, ¢roid_a_y, ¢roid_a_z,
|
|
658
|
+
¢roid_b_x, ¢roid_b_y, ¢roid_b_z, h);
|
|
659
|
+
if (a_centroid)
|
|
660
|
+
a_centroid[0] = (nk_f32_t)centroid_a_x, a_centroid[1] = (nk_f32_t)centroid_a_y,
|
|
661
|
+
a_centroid[2] = (nk_f32_t)centroid_a_z;
|
|
662
|
+
if (b_centroid)
|
|
663
|
+
b_centroid[0] = (nk_f32_t)centroid_b_x, b_centroid[1] = (nk_f32_t)centroid_b_y,
|
|
664
|
+
b_centroid[2] = (nk_f32_t)centroid_b_z;
|
|
665
|
+
nk_f64_t svd_u[9], svd_s[9], svd_v[9];
|
|
666
|
+
nk_svd3x3_f64_(h, svd_u, svd_s, svd_v);
|
|
667
|
+
|
|
668
|
+
nk_f64_t r[9];
|
|
669
|
+
r[0] = svd_v[0] * svd_u[0] + svd_v[1] * svd_u[1] + svd_v[2] * svd_u[2];
|
|
670
|
+
r[1] = svd_v[0] * svd_u[3] + svd_v[1] * svd_u[4] + svd_v[2] * svd_u[5];
|
|
671
|
+
r[2] = svd_v[0] * svd_u[6] + svd_v[1] * svd_u[7] + svd_v[2] * svd_u[8];
|
|
672
|
+
r[3] = svd_v[3] * svd_u[0] + svd_v[4] * svd_u[1] + svd_v[5] * svd_u[2];
|
|
673
|
+
r[4] = svd_v[3] * svd_u[3] + svd_v[4] * svd_u[4] + svd_v[5] * svd_u[5];
|
|
674
|
+
r[5] = svd_v[3] * svd_u[6] + svd_v[4] * svd_u[7] + svd_v[5] * svd_u[8];
|
|
675
|
+
r[6] = svd_v[6] * svd_u[0] + svd_v[7] * svd_u[1] + svd_v[8] * svd_u[2];
|
|
676
|
+
r[7] = svd_v[6] * svd_u[3] + svd_v[7] * svd_u[4] + svd_v[8] * svd_u[5];
|
|
677
|
+
r[8] = svd_v[6] * svd_u[6] + svd_v[7] * svd_u[7] + svd_v[8] * svd_u[8];
|
|
678
|
+
|
|
679
|
+
// Handle reflection: if det(R) < 0, negate third column of V and recompute R.
|
|
680
|
+
if (nk_det3x3_f64_(r) < 0) {
|
|
681
|
+
svd_v[2] = -svd_v[2];
|
|
682
|
+
svd_v[5] = -svd_v[5];
|
|
683
|
+
svd_v[8] = -svd_v[8];
|
|
684
|
+
r[0] = svd_v[0] * svd_u[0] + svd_v[1] * svd_u[1] + svd_v[2] * svd_u[2];
|
|
685
|
+
r[1] = svd_v[0] * svd_u[3] + svd_v[1] * svd_u[4] + svd_v[2] * svd_u[5];
|
|
686
|
+
r[2] = svd_v[0] * svd_u[6] + svd_v[1] * svd_u[7] + svd_v[2] * svd_u[8];
|
|
687
|
+
r[3] = svd_v[3] * svd_u[0] + svd_v[4] * svd_u[1] + svd_v[5] * svd_u[2];
|
|
688
|
+
r[4] = svd_v[3] * svd_u[3] + svd_v[4] * svd_u[4] + svd_v[5] * svd_u[5];
|
|
689
|
+
r[5] = svd_v[3] * svd_u[6] + svd_v[4] * svd_u[7] + svd_v[5] * svd_u[8];
|
|
690
|
+
r[6] = svd_v[6] * svd_u[0] + svd_v[7] * svd_u[1] + svd_v[8] * svd_u[2];
|
|
691
|
+
r[7] = svd_v[6] * svd_u[3] + svd_v[7] * svd_u[4] + svd_v[8] * svd_u[5];
|
|
692
|
+
r[8] = svd_v[6] * svd_u[6] + svd_v[7] * svd_u[7] + svd_v[8] * svd_u[8];
|
|
693
|
+
}
|
|
694
|
+
|
|
695
|
+
if (rotation) {
|
|
696
|
+
for (int j = 0; j < 9; ++j) rotation[j] = (nk_f32_t)r[j];
|
|
697
|
+
}
|
|
698
|
+
if (scale) *scale = 1.0f;
|
|
699
|
+
|
|
700
|
+
*result = nk_f64_sqrt_v128relaxed(nk_transformed_ssd_f32_v128relaxed_(a, b, n, r, 1.0, centroid_a_x, centroid_a_y,
|
|
701
|
+
centroid_a_z, centroid_b_x, centroid_b_y,
|
|
702
|
+
centroid_b_z) /
|
|
703
|
+
(nk_f64_t)n);
|
|
704
|
+
}
|
|
705
|
+
|
|
706
|
+
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,
|
|
707
|
+
nk_f64_t *b_centroid, nk_f64_t *rotation, nk_f64_t *scale, nk_f64_t *result) {
|
|
708
|
+
v128_t const zeros_f64x2 = wasm_f64x2_splat(0);
|
|
709
|
+
|
|
710
|
+
// Single set of accumulators (1x unrolling)
|
|
711
|
+
v128_t sum_a_x_f64x2 = zeros_f64x2, sum_a_y_f64x2 = zeros_f64x2, sum_a_z_f64x2 = zeros_f64x2;
|
|
712
|
+
v128_t sum_b_x_f64x2 = zeros_f64x2, sum_b_y_f64x2 = zeros_f64x2, sum_b_z_f64x2 = zeros_f64x2;
|
|
713
|
+
|
|
714
|
+
v128_t cov_xx_f64x2 = zeros_f64x2, cov_xy_f64x2 = zeros_f64x2, cov_xz_f64x2 = zeros_f64x2;
|
|
715
|
+
v128_t cov_yx_f64x2 = zeros_f64x2, cov_yy_f64x2 = zeros_f64x2, cov_yz_f64x2 = zeros_f64x2;
|
|
716
|
+
v128_t cov_zx_f64x2 = zeros_f64x2, cov_zy_f64x2 = zeros_f64x2, cov_zz_f64x2 = zeros_f64x2;
|
|
717
|
+
|
|
718
|
+
nk_size_t i = 0;
|
|
719
|
+
v128_t a_x_f64x2, a_y_f64x2, a_z_f64x2, b_x_f64x2, b_y_f64x2, b_z_f64x2;
|
|
720
|
+
|
|
721
|
+
// Main loop: 2 points per iteration
|
|
722
|
+
for (; i + 2 <= n; i += 2) {
|
|
723
|
+
nk_deinterleave_f64x2_v128relaxed_(a + i * 3, &a_x_f64x2, &a_y_f64x2, &a_z_f64x2);
|
|
724
|
+
nk_deinterleave_f64x2_v128relaxed_(b + i * 3, &b_x_f64x2, &b_y_f64x2, &b_z_f64x2);
|
|
725
|
+
|
|
726
|
+
sum_a_x_f64x2 = wasm_f64x2_add(sum_a_x_f64x2, a_x_f64x2);
|
|
727
|
+
sum_a_y_f64x2 = wasm_f64x2_add(sum_a_y_f64x2, a_y_f64x2);
|
|
728
|
+
sum_a_z_f64x2 = wasm_f64x2_add(sum_a_z_f64x2, a_z_f64x2);
|
|
729
|
+
sum_b_x_f64x2 = wasm_f64x2_add(sum_b_x_f64x2, b_x_f64x2);
|
|
730
|
+
sum_b_y_f64x2 = wasm_f64x2_add(sum_b_y_f64x2, b_y_f64x2);
|
|
731
|
+
sum_b_z_f64x2 = wasm_f64x2_add(sum_b_z_f64x2, b_z_f64x2);
|
|
732
|
+
|
|
733
|
+
cov_xx_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_x_f64x2, cov_xx_f64x2);
|
|
734
|
+
cov_xy_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_y_f64x2, cov_xy_f64x2);
|
|
735
|
+
cov_xz_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_z_f64x2, cov_xz_f64x2);
|
|
736
|
+
cov_yx_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_x_f64x2, cov_yx_f64x2);
|
|
737
|
+
cov_yy_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_y_f64x2, cov_yy_f64x2);
|
|
738
|
+
cov_yz_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_z_f64x2, cov_yz_f64x2);
|
|
739
|
+
cov_zx_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_x_f64x2, cov_zx_f64x2);
|
|
740
|
+
cov_zy_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_y_f64x2, cov_zy_f64x2);
|
|
741
|
+
cov_zz_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_z_f64x2, cov_zz_f64x2);
|
|
742
|
+
}
|
|
743
|
+
|
|
744
|
+
// Reduce vector accumulators.
|
|
745
|
+
nk_f64_t sum_a_x = nk_reduce_stable_f64x2_v128relaxed_(sum_a_x_f64x2), sum_a_x_compensation = 0.0;
|
|
746
|
+
nk_f64_t sum_a_y = nk_reduce_stable_f64x2_v128relaxed_(sum_a_y_f64x2), sum_a_y_compensation = 0.0;
|
|
747
|
+
nk_f64_t sum_a_z = nk_reduce_stable_f64x2_v128relaxed_(sum_a_z_f64x2), sum_a_z_compensation = 0.0;
|
|
748
|
+
nk_f64_t sum_b_x = nk_reduce_stable_f64x2_v128relaxed_(sum_b_x_f64x2), sum_b_x_compensation = 0.0;
|
|
749
|
+
nk_f64_t sum_b_y = nk_reduce_stable_f64x2_v128relaxed_(sum_b_y_f64x2), sum_b_y_compensation = 0.0;
|
|
750
|
+
nk_f64_t sum_b_z = nk_reduce_stable_f64x2_v128relaxed_(sum_b_z_f64x2), sum_b_z_compensation = 0.0;
|
|
751
|
+
|
|
752
|
+
nk_f64_t covariance_x_x = nk_reduce_stable_f64x2_v128relaxed_(cov_xx_f64x2), covariance_x_x_compensation = 0.0;
|
|
753
|
+
nk_f64_t covariance_x_y = nk_reduce_stable_f64x2_v128relaxed_(cov_xy_f64x2), covariance_x_y_compensation = 0.0;
|
|
754
|
+
nk_f64_t covariance_x_z = nk_reduce_stable_f64x2_v128relaxed_(cov_xz_f64x2), covariance_x_z_compensation = 0.0;
|
|
755
|
+
nk_f64_t covariance_y_x = nk_reduce_stable_f64x2_v128relaxed_(cov_yx_f64x2), covariance_y_x_compensation = 0.0;
|
|
756
|
+
nk_f64_t covariance_y_y = nk_reduce_stable_f64x2_v128relaxed_(cov_yy_f64x2), covariance_y_y_compensation = 0.0;
|
|
757
|
+
nk_f64_t covariance_y_z = nk_reduce_stable_f64x2_v128relaxed_(cov_yz_f64x2), covariance_y_z_compensation = 0.0;
|
|
758
|
+
nk_f64_t covariance_z_x = nk_reduce_stable_f64x2_v128relaxed_(cov_zx_f64x2), covariance_z_x_compensation = 0.0;
|
|
759
|
+
nk_f64_t covariance_z_y = nk_reduce_stable_f64x2_v128relaxed_(cov_zy_f64x2), covariance_z_y_compensation = 0.0;
|
|
760
|
+
nk_f64_t covariance_z_z = nk_reduce_stable_f64x2_v128relaxed_(cov_zz_f64x2), covariance_z_z_compensation = 0.0;
|
|
761
|
+
|
|
762
|
+
// Scalar tail
|
|
763
|
+
for (; i < n; ++i) {
|
|
764
|
+
nk_f64_t ax = a[i * 3 + 0], ay = a[i * 3 + 1], az = a[i * 3 + 2];
|
|
765
|
+
nk_f64_t bx = b[i * 3 + 0], by = b[i * 3 + 1], bz = b[i * 3 + 2];
|
|
766
|
+
nk_accumulate_sum_f64_(&sum_a_x, &sum_a_x_compensation, ax),
|
|
767
|
+
nk_accumulate_sum_f64_(&sum_a_y, &sum_a_y_compensation, ay),
|
|
768
|
+
nk_accumulate_sum_f64_(&sum_a_z, &sum_a_z_compensation, az);
|
|
769
|
+
nk_accumulate_sum_f64_(&sum_b_x, &sum_b_x_compensation, bx),
|
|
770
|
+
nk_accumulate_sum_f64_(&sum_b_y, &sum_b_y_compensation, by),
|
|
771
|
+
nk_accumulate_sum_f64_(&sum_b_z, &sum_b_z_compensation, bz);
|
|
772
|
+
nk_accumulate_product_f64_(&covariance_x_x, &covariance_x_x_compensation, ax, bx),
|
|
773
|
+
nk_accumulate_product_f64_(&covariance_x_y, &covariance_x_y_compensation, ax, by),
|
|
774
|
+
nk_accumulate_product_f64_(&covariance_x_z, &covariance_x_z_compensation, ax, bz);
|
|
775
|
+
nk_accumulate_product_f64_(&covariance_y_x, &covariance_y_x_compensation, ay, bx),
|
|
776
|
+
nk_accumulate_product_f64_(&covariance_y_y, &covariance_y_y_compensation, ay, by),
|
|
777
|
+
nk_accumulate_product_f64_(&covariance_y_z, &covariance_y_z_compensation, ay, bz);
|
|
778
|
+
nk_accumulate_product_f64_(&covariance_z_x, &covariance_z_x_compensation, az, bx),
|
|
779
|
+
nk_accumulate_product_f64_(&covariance_z_y, &covariance_z_y_compensation, az, by),
|
|
780
|
+
nk_accumulate_product_f64_(&covariance_z_z, &covariance_z_z_compensation, az, bz);
|
|
781
|
+
}
|
|
782
|
+
|
|
783
|
+
sum_a_x += sum_a_x_compensation, sum_a_y += sum_a_y_compensation, sum_a_z += sum_a_z_compensation;
|
|
784
|
+
sum_b_x += sum_b_x_compensation, sum_b_y += sum_b_y_compensation, sum_b_z += sum_b_z_compensation;
|
|
785
|
+
covariance_x_x += covariance_x_x_compensation, covariance_x_y += covariance_x_y_compensation,
|
|
786
|
+
covariance_x_z += covariance_x_z_compensation;
|
|
787
|
+
covariance_y_x += covariance_y_x_compensation, covariance_y_y += covariance_y_y_compensation,
|
|
788
|
+
covariance_y_z += covariance_y_z_compensation;
|
|
789
|
+
covariance_z_x += covariance_z_x_compensation, covariance_z_y += covariance_z_y_compensation,
|
|
790
|
+
covariance_z_z += covariance_z_z_compensation;
|
|
791
|
+
|
|
792
|
+
// Compute centroids
|
|
793
|
+
nk_f64_t inv_n = 1.0 / (nk_f64_t)n;
|
|
794
|
+
nk_f64_t centroid_a_x = sum_a_x * inv_n, centroid_a_y = sum_a_y * inv_n, centroid_a_z = sum_a_z * inv_n;
|
|
795
|
+
nk_f64_t centroid_b_x = sum_b_x * inv_n, centroid_b_y = sum_b_y * inv_n, centroid_b_z = sum_b_z * inv_n;
|
|
796
|
+
if (a_centroid) a_centroid[0] = centroid_a_x, a_centroid[1] = centroid_a_y, a_centroid[2] = centroid_a_z;
|
|
797
|
+
if (b_centroid) b_centroid[0] = centroid_b_x, b_centroid[1] = centroid_b_y, b_centroid[2] = centroid_b_z;
|
|
798
|
+
|
|
799
|
+
// Apply centering correction: H_centered = H - n * centroid_a * centroid_bT
|
|
800
|
+
covariance_x_x -= n * centroid_a_x * centroid_b_x;
|
|
801
|
+
covariance_x_y -= n * centroid_a_x * centroid_b_y;
|
|
802
|
+
covariance_x_z -= n * centroid_a_x * centroid_b_z;
|
|
803
|
+
covariance_y_x -= n * centroid_a_y * centroid_b_x;
|
|
804
|
+
covariance_y_y -= n * centroid_a_y * centroid_b_y;
|
|
805
|
+
covariance_y_z -= n * centroid_a_y * centroid_b_z;
|
|
806
|
+
covariance_z_x -= n * centroid_a_z * centroid_b_x;
|
|
807
|
+
covariance_z_y -= n * centroid_a_z * centroid_b_y;
|
|
808
|
+
covariance_z_z -= n * centroid_a_z * centroid_b_z;
|
|
809
|
+
|
|
810
|
+
// Compute SVD and optimal rotation
|
|
811
|
+
nk_f64_t cross_covariance[9] = {covariance_x_x, covariance_x_y, covariance_x_z, covariance_y_x, covariance_y_y,
|
|
812
|
+
covariance_y_z, covariance_z_x, covariance_z_y, covariance_z_z};
|
|
813
|
+
nk_f64_t svd_u[9], svd_s[9], svd_v[9];
|
|
814
|
+
nk_svd3x3_f64_(cross_covariance, svd_u, svd_s, svd_v);
|
|
815
|
+
|
|
816
|
+
nk_f64_t r[9];
|
|
817
|
+
nk_rotation_from_svd_f64_v128relaxed_(svd_u, svd_v, r);
|
|
818
|
+
|
|
819
|
+
// Handle reflection: if det(R) < 0, negate third column of V and recompute R
|
|
820
|
+
if (nk_det3x3_f64_(r) < 0) {
|
|
821
|
+
svd_v[2] = -svd_v[2];
|
|
822
|
+
svd_v[5] = -svd_v[5];
|
|
823
|
+
svd_v[8] = -svd_v[8];
|
|
824
|
+
nk_rotation_from_svd_f64_v128relaxed_(svd_u, svd_v, r);
|
|
825
|
+
}
|
|
826
|
+
|
|
827
|
+
// Output rotation matrix and scale=1.0
|
|
828
|
+
if (rotation)
|
|
829
|
+
for (int j = 0; j < 9; ++j) rotation[j] = r[j];
|
|
830
|
+
|
|
831
|
+
if (scale) *scale = 1.0;
|
|
832
|
+
|
|
833
|
+
// Compute RMSD after optimal rotation
|
|
834
|
+
nk_f64_t sum_squared = nk_transformed_ssd_f64_v128relaxed_(a, b, n, r, 1.0, centroid_a_x, centroid_a_y,
|
|
835
|
+
centroid_a_z, centroid_b_x, centroid_b_y, centroid_b_z);
|
|
836
|
+
*result = nk_f64_sqrt_v128relaxed(sum_squared * inv_n);
|
|
837
|
+
}
|
|
838
|
+
|
|
839
|
+
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,
|
|
840
|
+
nk_f32_t *b_centroid, nk_f32_t *rotation, nk_f32_t *scale, nk_f64_t *result) {
|
|
841
|
+
nk_f64_t centroid_a_x, centroid_a_y, centroid_a_z, centroid_b_x, centroid_b_y, centroid_b_z, variance_a;
|
|
842
|
+
nk_f64_t h[9];
|
|
843
|
+
nk_centroid_and_cross_covariance_and_variance_f32_v128relaxed_( //
|
|
844
|
+
a, b, n, ¢roid_a_x, ¢roid_a_y, ¢roid_a_z, //
|
|
845
|
+
¢roid_b_x, ¢roid_b_y, ¢roid_b_z, h, &variance_a);
|
|
846
|
+
if (a_centroid)
|
|
847
|
+
a_centroid[0] = (nk_f32_t)centroid_a_x, a_centroid[1] = (nk_f32_t)centroid_a_y,
|
|
848
|
+
a_centroid[2] = (nk_f32_t)centroid_a_z;
|
|
849
|
+
if (b_centroid)
|
|
850
|
+
b_centroid[0] = (nk_f32_t)centroid_b_x, b_centroid[1] = (nk_f32_t)centroid_b_y,
|
|
851
|
+
b_centroid[2] = (nk_f32_t)centroid_b_z;
|
|
852
|
+
|
|
853
|
+
nk_f64_t svd_u[9], svd_s[9], svd_v[9];
|
|
854
|
+
nk_svd3x3_f64_(h, svd_u, svd_s, svd_v);
|
|
855
|
+
|
|
856
|
+
nk_f64_t r[9];
|
|
857
|
+
r[0] = svd_v[0] * svd_u[0] + svd_v[1] * svd_u[1] + svd_v[2] * svd_u[2];
|
|
858
|
+
r[1] = svd_v[0] * svd_u[3] + svd_v[1] * svd_u[4] + svd_v[2] * svd_u[5];
|
|
859
|
+
r[2] = svd_v[0] * svd_u[6] + svd_v[1] * svd_u[7] + svd_v[2] * svd_u[8];
|
|
860
|
+
r[3] = svd_v[3] * svd_u[0] + svd_v[4] * svd_u[1] + svd_v[5] * svd_u[2];
|
|
861
|
+
r[4] = svd_v[3] * svd_u[3] + svd_v[4] * svd_u[4] + svd_v[5] * svd_u[5];
|
|
862
|
+
r[5] = svd_v[3] * svd_u[6] + svd_v[4] * svd_u[7] + svd_v[5] * svd_u[8];
|
|
863
|
+
r[6] = svd_v[6] * svd_u[0] + svd_v[7] * svd_u[1] + svd_v[8] * svd_u[2];
|
|
864
|
+
r[7] = svd_v[6] * svd_u[3] + svd_v[7] * svd_u[4] + svd_v[8] * svd_u[5];
|
|
865
|
+
r[8] = svd_v[6] * svd_u[6] + svd_v[7] * svd_u[7] + svd_v[8] * svd_u[8];
|
|
866
|
+
|
|
867
|
+
nk_f64_t det = nk_det3x3_f64_(r);
|
|
868
|
+
if (det < 0) {
|
|
869
|
+
svd_v[2] = -svd_v[2];
|
|
870
|
+
svd_v[5] = -svd_v[5];
|
|
871
|
+
svd_v[8] = -svd_v[8];
|
|
872
|
+
r[0] = svd_v[0] * svd_u[0] + svd_v[1] * svd_u[1] + svd_v[2] * svd_u[2];
|
|
873
|
+
r[1] = svd_v[0] * svd_u[3] + svd_v[1] * svd_u[4] + svd_v[2] * svd_u[5];
|
|
874
|
+
r[2] = svd_v[0] * svd_u[6] + svd_v[1] * svd_u[7] + svd_v[2] * svd_u[8];
|
|
875
|
+
r[3] = svd_v[3] * svd_u[0] + svd_v[4] * svd_u[1] + svd_v[5] * svd_u[2];
|
|
876
|
+
r[4] = svd_v[3] * svd_u[3] + svd_v[4] * svd_u[4] + svd_v[5] * svd_u[5];
|
|
877
|
+
r[5] = svd_v[3] * svd_u[6] + svd_v[4] * svd_u[7] + svd_v[5] * svd_u[8];
|
|
878
|
+
r[6] = svd_v[6] * svd_u[0] + svd_v[7] * svd_u[1] + svd_v[8] * svd_u[2];
|
|
879
|
+
r[7] = svd_v[6] * svd_u[3] + svd_v[7] * svd_u[4] + svd_v[8] * svd_u[5];
|
|
880
|
+
r[8] = svd_v[6] * svd_u[6] + svd_v[7] * svd_u[7] + svd_v[8] * svd_u[8];
|
|
881
|
+
}
|
|
882
|
+
|
|
883
|
+
nk_f64_t trace_signed_singular_values = svd_s[0] + svd_s[4] + (det < 0 ? -svd_s[8] : svd_s[8]);
|
|
884
|
+
nk_f64_t computed_scale = trace_signed_singular_values / ((nk_f64_t)n * variance_a);
|
|
885
|
+
if (rotation)
|
|
886
|
+
for (int j = 0; j < 9; ++j) rotation[j] = (nk_f32_t)r[j];
|
|
887
|
+
if (scale) *scale = (nk_f32_t)computed_scale;
|
|
888
|
+
|
|
889
|
+
*result = nk_f64_sqrt_v128relaxed(nk_transformed_ssd_f32_v128relaxed_(a, b, n, r, computed_scale, centroid_a_x,
|
|
890
|
+
centroid_a_y, centroid_a_z, centroid_b_x,
|
|
891
|
+
centroid_b_y, centroid_b_z) /
|
|
892
|
+
(nk_f64_t)n);
|
|
893
|
+
}
|
|
894
|
+
|
|
895
|
+
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,
|
|
896
|
+
nk_f64_t *b_centroid, nk_f64_t *rotation, nk_f64_t *scale, nk_f64_t *result) {
|
|
897
|
+
v128_t const zeros_f64x2 = wasm_f64x2_splat(0);
|
|
898
|
+
|
|
899
|
+
// Single set of accumulators (1x unrolling)
|
|
900
|
+
v128_t sum_a_x_f64x2 = zeros_f64x2, sum_a_y_f64x2 = zeros_f64x2, sum_a_z_f64x2 = zeros_f64x2;
|
|
901
|
+
v128_t sum_b_x_f64x2 = zeros_f64x2, sum_b_y_f64x2 = zeros_f64x2, sum_b_z_f64x2 = zeros_f64x2;
|
|
902
|
+
|
|
903
|
+
v128_t cov_xx_f64x2 = zeros_f64x2, cov_xy_f64x2 = zeros_f64x2, cov_xz_f64x2 = zeros_f64x2;
|
|
904
|
+
v128_t cov_yx_f64x2 = zeros_f64x2, cov_yy_f64x2 = zeros_f64x2, cov_yz_f64x2 = zeros_f64x2;
|
|
905
|
+
v128_t cov_zx_f64x2 = zeros_f64x2, cov_zy_f64x2 = zeros_f64x2, cov_zz_f64x2 = zeros_f64x2;
|
|
906
|
+
v128_t variance_a_f64x2 = zeros_f64x2;
|
|
907
|
+
|
|
908
|
+
nk_size_t i = 0;
|
|
909
|
+
v128_t a_x_f64x2, a_y_f64x2, a_z_f64x2, b_x_f64x2, b_y_f64x2, b_z_f64x2;
|
|
910
|
+
|
|
911
|
+
// Main loop: 2 points per iteration
|
|
912
|
+
for (; i + 2 <= n; i += 2) {
|
|
913
|
+
nk_deinterleave_f64x2_v128relaxed_(a + i * 3, &a_x_f64x2, &a_y_f64x2, &a_z_f64x2);
|
|
914
|
+
nk_deinterleave_f64x2_v128relaxed_(b + i * 3, &b_x_f64x2, &b_y_f64x2, &b_z_f64x2);
|
|
915
|
+
|
|
916
|
+
sum_a_x_f64x2 = wasm_f64x2_add(sum_a_x_f64x2, a_x_f64x2);
|
|
917
|
+
sum_a_y_f64x2 = wasm_f64x2_add(sum_a_y_f64x2, a_y_f64x2);
|
|
918
|
+
sum_a_z_f64x2 = wasm_f64x2_add(sum_a_z_f64x2, a_z_f64x2);
|
|
919
|
+
sum_b_x_f64x2 = wasm_f64x2_add(sum_b_x_f64x2, b_x_f64x2);
|
|
920
|
+
sum_b_y_f64x2 = wasm_f64x2_add(sum_b_y_f64x2, b_y_f64x2);
|
|
921
|
+
sum_b_z_f64x2 = wasm_f64x2_add(sum_b_z_f64x2, b_z_f64x2);
|
|
922
|
+
|
|
923
|
+
cov_xx_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_x_f64x2, cov_xx_f64x2);
|
|
924
|
+
cov_xy_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_y_f64x2, cov_xy_f64x2);
|
|
925
|
+
cov_xz_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, b_z_f64x2, cov_xz_f64x2);
|
|
926
|
+
cov_yx_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_x_f64x2, cov_yx_f64x2);
|
|
927
|
+
cov_yy_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_y_f64x2, cov_yy_f64x2);
|
|
928
|
+
cov_yz_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, b_z_f64x2, cov_yz_f64x2);
|
|
929
|
+
cov_zx_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_x_f64x2, cov_zx_f64x2);
|
|
930
|
+
cov_zy_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_y_f64x2, cov_zy_f64x2);
|
|
931
|
+
cov_zz_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, b_z_f64x2, cov_zz_f64x2);
|
|
932
|
+
|
|
933
|
+
variance_a_f64x2 = wasm_f64x2_relaxed_madd(a_x_f64x2, a_x_f64x2, variance_a_f64x2);
|
|
934
|
+
variance_a_f64x2 = wasm_f64x2_relaxed_madd(a_y_f64x2, a_y_f64x2, variance_a_f64x2);
|
|
935
|
+
variance_a_f64x2 = wasm_f64x2_relaxed_madd(a_z_f64x2, a_z_f64x2, variance_a_f64x2);
|
|
936
|
+
}
|
|
937
|
+
|
|
938
|
+
// Reduce vector accumulators.
|
|
939
|
+
nk_f64_t sum_a_x = nk_reduce_stable_f64x2_v128relaxed_(sum_a_x_f64x2), sum_a_x_compensation = 0.0;
|
|
940
|
+
nk_f64_t sum_a_y = nk_reduce_stable_f64x2_v128relaxed_(sum_a_y_f64x2), sum_a_y_compensation = 0.0;
|
|
941
|
+
nk_f64_t sum_a_z = nk_reduce_stable_f64x2_v128relaxed_(sum_a_z_f64x2), sum_a_z_compensation = 0.0;
|
|
942
|
+
nk_f64_t sum_b_x = nk_reduce_stable_f64x2_v128relaxed_(sum_b_x_f64x2), sum_b_x_compensation = 0.0;
|
|
943
|
+
nk_f64_t sum_b_y = nk_reduce_stable_f64x2_v128relaxed_(sum_b_y_f64x2), sum_b_y_compensation = 0.0;
|
|
944
|
+
nk_f64_t sum_b_z = nk_reduce_stable_f64x2_v128relaxed_(sum_b_z_f64x2), sum_b_z_compensation = 0.0;
|
|
945
|
+
nk_f64_t covariance_x_x = nk_reduce_stable_f64x2_v128relaxed_(cov_xx_f64x2), covariance_x_x_compensation = 0.0;
|
|
946
|
+
nk_f64_t covariance_x_y = nk_reduce_stable_f64x2_v128relaxed_(cov_xy_f64x2), covariance_x_y_compensation = 0.0;
|
|
947
|
+
nk_f64_t covariance_x_z = nk_reduce_stable_f64x2_v128relaxed_(cov_xz_f64x2), covariance_x_z_compensation = 0.0;
|
|
948
|
+
nk_f64_t covariance_y_x = nk_reduce_stable_f64x2_v128relaxed_(cov_yx_f64x2), covariance_y_x_compensation = 0.0;
|
|
949
|
+
nk_f64_t covariance_y_y = nk_reduce_stable_f64x2_v128relaxed_(cov_yy_f64x2), covariance_y_y_compensation = 0.0;
|
|
950
|
+
nk_f64_t covariance_y_z = nk_reduce_stable_f64x2_v128relaxed_(cov_yz_f64x2), covariance_y_z_compensation = 0.0;
|
|
951
|
+
nk_f64_t covariance_z_x = nk_reduce_stable_f64x2_v128relaxed_(cov_zx_f64x2), covariance_z_x_compensation = 0.0;
|
|
952
|
+
nk_f64_t covariance_z_y = nk_reduce_stable_f64x2_v128relaxed_(cov_zy_f64x2), covariance_z_y_compensation = 0.0;
|
|
953
|
+
nk_f64_t covariance_z_z = nk_reduce_stable_f64x2_v128relaxed_(cov_zz_f64x2), covariance_z_z_compensation = 0.0;
|
|
954
|
+
nk_f64_t sum_sq_a = nk_reduce_stable_f64x2_v128relaxed_(variance_a_f64x2), sum_sq_a_compensation = 0.0;
|
|
955
|
+
|
|
956
|
+
// Scalar tail
|
|
957
|
+
for (; i < n; ++i) {
|
|
958
|
+
nk_f64_t ax = a[i * 3 + 0], ay = a[i * 3 + 1], az = a[i * 3 + 2];
|
|
959
|
+
nk_f64_t bx = b[i * 3 + 0], by = b[i * 3 + 1], bz = b[i * 3 + 2];
|
|
960
|
+
nk_accumulate_sum_f64_(&sum_a_x, &sum_a_x_compensation, ax),
|
|
961
|
+
nk_accumulate_sum_f64_(&sum_a_y, &sum_a_y_compensation, ay),
|
|
962
|
+
nk_accumulate_sum_f64_(&sum_a_z, &sum_a_z_compensation, az);
|
|
963
|
+
nk_accumulate_sum_f64_(&sum_b_x, &sum_b_x_compensation, bx),
|
|
964
|
+
nk_accumulate_sum_f64_(&sum_b_y, &sum_b_y_compensation, by),
|
|
965
|
+
nk_accumulate_sum_f64_(&sum_b_z, &sum_b_z_compensation, bz);
|
|
966
|
+
nk_accumulate_product_f64_(&covariance_x_x, &covariance_x_x_compensation, ax, bx),
|
|
967
|
+
nk_accumulate_product_f64_(&covariance_x_y, &covariance_x_y_compensation, ax, by),
|
|
968
|
+
nk_accumulate_product_f64_(&covariance_x_z, &covariance_x_z_compensation, ax, bz);
|
|
969
|
+
nk_accumulate_product_f64_(&covariance_y_x, &covariance_y_x_compensation, ay, bx),
|
|
970
|
+
nk_accumulate_product_f64_(&covariance_y_y, &covariance_y_y_compensation, ay, by),
|
|
971
|
+
nk_accumulate_product_f64_(&covariance_y_z, &covariance_y_z_compensation, ay, bz);
|
|
972
|
+
nk_accumulate_product_f64_(&covariance_z_x, &covariance_z_x_compensation, az, bx),
|
|
973
|
+
nk_accumulate_product_f64_(&covariance_z_y, &covariance_z_y_compensation, az, by),
|
|
974
|
+
nk_accumulate_product_f64_(&covariance_z_z, &covariance_z_z_compensation, az, bz);
|
|
975
|
+
nk_accumulate_square_f64_(&sum_sq_a, &sum_sq_a_compensation, ax),
|
|
976
|
+
nk_accumulate_square_f64_(&sum_sq_a, &sum_sq_a_compensation, ay),
|
|
977
|
+
nk_accumulate_square_f64_(&sum_sq_a, &sum_sq_a_compensation, az);
|
|
978
|
+
}
|
|
979
|
+
|
|
980
|
+
sum_a_x += sum_a_x_compensation, sum_a_y += sum_a_y_compensation, sum_a_z += sum_a_z_compensation;
|
|
981
|
+
sum_b_x += sum_b_x_compensation, sum_b_y += sum_b_y_compensation, sum_b_z += sum_b_z_compensation;
|
|
982
|
+
covariance_x_x += covariance_x_x_compensation, covariance_x_y += covariance_x_y_compensation,
|
|
983
|
+
covariance_x_z += covariance_x_z_compensation;
|
|
984
|
+
covariance_y_x += covariance_y_x_compensation, covariance_y_y += covariance_y_y_compensation,
|
|
985
|
+
covariance_y_z += covariance_y_z_compensation;
|
|
986
|
+
covariance_z_x += covariance_z_x_compensation, covariance_z_y += covariance_z_y_compensation,
|
|
987
|
+
covariance_z_z += covariance_z_z_compensation;
|
|
988
|
+
sum_sq_a += sum_sq_a_compensation;
|
|
989
|
+
|
|
990
|
+
// Compute centroids
|
|
991
|
+
nk_f64_t inv_n = 1.0 / (nk_f64_t)n;
|
|
992
|
+
nk_f64_t centroid_a_x = sum_a_x * inv_n, centroid_a_y = sum_a_y * inv_n, centroid_a_z = sum_a_z * inv_n;
|
|
993
|
+
nk_f64_t centroid_b_x = sum_b_x * inv_n, centroid_b_y = sum_b_y * inv_n, centroid_b_z = sum_b_z * inv_n;
|
|
994
|
+
if (a_centroid) a_centroid[0] = centroid_a_x, a_centroid[1] = centroid_a_y, a_centroid[2] = centroid_a_z;
|
|
995
|
+
if (b_centroid) b_centroid[0] = centroid_b_x, b_centroid[1] = centroid_b_y, b_centroid[2] = centroid_b_z;
|
|
996
|
+
|
|
997
|
+
// Compute variance of A (centered)
|
|
998
|
+
nk_f64_t centroid_sq = centroid_a_x * centroid_a_x + centroid_a_y * centroid_a_y + centroid_a_z * centroid_a_z;
|
|
999
|
+
nk_f64_t var_a = sum_sq_a * inv_n - centroid_sq;
|
|
1000
|
+
|
|
1001
|
+
// Apply centering correction: H_centered = H - n * centroid_a * centroid_bT
|
|
1002
|
+
covariance_x_x -= n * centroid_a_x * centroid_b_x;
|
|
1003
|
+
covariance_x_y -= n * centroid_a_x * centroid_b_y;
|
|
1004
|
+
covariance_x_z -= n * centroid_a_x * centroid_b_z;
|
|
1005
|
+
covariance_y_x -= n * centroid_a_y * centroid_b_x;
|
|
1006
|
+
covariance_y_y -= n * centroid_a_y * centroid_b_y;
|
|
1007
|
+
covariance_y_z -= n * centroid_a_y * centroid_b_z;
|
|
1008
|
+
covariance_z_x -= n * centroid_a_z * centroid_b_x;
|
|
1009
|
+
covariance_z_y -= n * centroid_a_z * centroid_b_y;
|
|
1010
|
+
covariance_z_z -= n * centroid_a_z * centroid_b_z;
|
|
1011
|
+
|
|
1012
|
+
// Compute SVD
|
|
1013
|
+
nk_f64_t cross_covariance[9] = {covariance_x_x, covariance_x_y, covariance_x_z, covariance_y_x, covariance_y_y,
|
|
1014
|
+
covariance_y_z, covariance_z_x, covariance_z_y, covariance_z_z};
|
|
1015
|
+
nk_f64_t svd_u[9], svd_s[9], svd_v[9];
|
|
1016
|
+
nk_svd3x3_f64_(cross_covariance, svd_u, svd_s, svd_v);
|
|
1017
|
+
|
|
1018
|
+
nk_f64_t r[9];
|
|
1019
|
+
nk_rotation_from_svd_f64_v128relaxed_(svd_u, svd_v, r);
|
|
1020
|
+
|
|
1021
|
+
// Handle reflection and compute scale
|
|
1022
|
+
nk_f64_t det = nk_det3x3_f64_(r);
|
|
1023
|
+
nk_f64_t trace_d_s = svd_s[0] + svd_s[4] + (det < 0 ? -svd_s[8] : svd_s[8]);
|
|
1024
|
+
nk_f64_t computed_scale = trace_d_s / (n * var_a);
|
|
1025
|
+
|
|
1026
|
+
if (det < 0) {
|
|
1027
|
+
svd_v[2] = -svd_v[2];
|
|
1028
|
+
svd_v[5] = -svd_v[5];
|
|
1029
|
+
svd_v[8] = -svd_v[8];
|
|
1030
|
+
nk_rotation_from_svd_f64_v128relaxed_(svd_u, svd_v, r);
|
|
1031
|
+
}
|
|
1032
|
+
|
|
1033
|
+
if (rotation)
|
|
1034
|
+
for (int j = 0; j < 9; ++j) rotation[j] = r[j];
|
|
1035
|
+
if (scale) *scale = computed_scale;
|
|
1036
|
+
|
|
1037
|
+
// Compute RMSD after transformation
|
|
1038
|
+
nk_f64_t sum_squared = nk_transformed_ssd_f64_v128relaxed_(a, b, n, r, computed_scale, centroid_a_x, centroid_a_y,
|
|
1039
|
+
centroid_a_z, centroid_b_x, centroid_b_y, centroid_b_z);
|
|
1040
|
+
*result = nk_f64_sqrt_v128relaxed(sum_squared * inv_n);
|
|
1041
|
+
}
|
|
1042
|
+
|
|
1043
|
+
#if defined(__clang__)
|
|
1044
|
+
#pragma clang attribute pop
|
|
1045
|
+
#endif
|
|
1046
|
+
|
|
1047
|
+
#if defined(__cplusplus)
|
|
1048
|
+
} // extern "C"
|
|
1049
|
+
#endif
|
|
1050
|
+
|
|
1051
|
+
#endif // NK_TARGET_V128RELAXED
|
|
1052
|
+
#endif // NK_MESH_V128RELAXED_H
|