1 #include "common/math/kasa.h"
2
3 #include <stdint.h>
4 #include <sys/types.h>
5
6 #include "common/math/mat.h"
7
kasaReset(struct KasaFit * kasa)8 void kasaReset(struct KasaFit *kasa) {
9 kasa->acc_x = kasa->acc_y = kasa->acc_z = kasa->acc_w = 0.0f;
10 kasa->acc_xx = kasa->acc_xy = kasa->acc_xz = kasa->acc_xw = 0.0f;
11 kasa->acc_yy = kasa->acc_yz = kasa->acc_yw = 0.0f;
12 kasa->acc_zz = kasa->acc_zw = 0.0f;
13 kasa->nsamples = 0;
14 }
15
kasaInit(struct KasaFit * kasa)16 void kasaInit(struct KasaFit *kasa) { kasaReset(kasa); }
17
kasaAccumulate(struct KasaFit * kasa,float x,float y,float z)18 void kasaAccumulate(struct KasaFit *kasa, float x, float y, float z) {
19 float w = x * x + y * y + z * z;
20
21 kasa->acc_x += x;
22 kasa->acc_y += y;
23 kasa->acc_z += z;
24 kasa->acc_w += w;
25
26 kasa->acc_xx += x * x;
27 kasa->acc_xy += x * y;
28 kasa->acc_xz += x * z;
29 kasa->acc_xw += x * w;
30
31 kasa->acc_yy += y * y;
32 kasa->acc_yz += y * z;
33 kasa->acc_yw += y * w;
34
35 kasa->acc_zz += z * z;
36 kasa->acc_zw += z * w;
37
38 kasa->nsamples += 1;
39 }
40
kasaNormalize(struct KasaFit * kasa)41 bool kasaNormalize(struct KasaFit *kasa) {
42 if (kasa->nsamples == 0) {
43 return false;
44 }
45
46 float inv = 1.0f / kasa->nsamples;
47
48 kasa->acc_x *= inv;
49 kasa->acc_y *= inv;
50 kasa->acc_z *= inv;
51 kasa->acc_w *= inv;
52
53 kasa->acc_xx *= inv;
54 kasa->acc_xy *= inv;
55 kasa->acc_xz *= inv;
56 kasa->acc_xw *= inv;
57
58 kasa->acc_yy *= inv;
59 kasa->acc_yz *= inv;
60 kasa->acc_yw *= inv;
61
62 kasa->acc_zz *= inv;
63 kasa->acc_zw *= inv;
64
65 return true;
66 }
67
kasaFit(struct KasaFit * kasa,struct Vec3 * bias,float * radius,float max_fit,float min_fit)68 int kasaFit(struct KasaFit *kasa, struct Vec3 *bias, float *radius,
69 float max_fit, float min_fit) {
70 // A * out = b
71 // (4 x 4) (4 x 1) (4 x 1)
72 struct Mat44 A;
73 A.elem[0][0] = kasa->acc_xx;
74 A.elem[0][1] = kasa->acc_xy;
75 A.elem[0][2] = kasa->acc_xz;
76 A.elem[0][3] = kasa->acc_x;
77 A.elem[1][0] = kasa->acc_xy;
78 A.elem[1][1] = kasa->acc_yy;
79 A.elem[1][2] = kasa->acc_yz;
80 A.elem[1][3] = kasa->acc_y;
81 A.elem[2][0] = kasa->acc_xz;
82 A.elem[2][1] = kasa->acc_yz;
83 A.elem[2][2] = kasa->acc_zz;
84 A.elem[2][3] = kasa->acc_z;
85 A.elem[3][0] = kasa->acc_x;
86 A.elem[3][1] = kasa->acc_y;
87 A.elem[3][2] = kasa->acc_z;
88 A.elem[3][3] = 1.0f;
89
90 struct Vec4 b;
91 initVec4(&b, -kasa->acc_xw, -kasa->acc_yw, -kasa->acc_zw, -kasa->acc_w);
92
93 struct Size4 pivot;
94 mat44DecomposeLup(&A, &pivot);
95
96 struct Vec4 out;
97 mat44Solve(&A, &out, &b, &pivot);
98
99 // sphere: (x - xc)^2 + (y - yc)^2 + (z - zc)^2 = r^2
100 //
101 // xc = -out[0] / 2, yc = -out[1] / 2, zc = -out[2] / 2
102 // r = sqrt(xc^2 + yc^2 + zc^2 - out[3])
103
104 struct Vec3 v;
105 initVec3(&v, out.x, out.y, out.z);
106 vec3ScalarMul(&v, -0.5f);
107
108 float r_square = vec3Dot(&v, &v) - out.w;
109 float r = (r_square > 0) ? sqrtf(r_square) : 0;
110
111 initVec3(bias, v.x, v.y, v.z);
112 *radius = r;
113
114 int success = 0;
115 if (r > min_fit && r < max_fit) {
116 success = 1;
117 }
118
119 return success;
120 }
121