1 #include "common/math/kasa.h" 2 3 #include <stdint.h> 4 #include <sys/types.h> 5 6 #include "common/math/mat.h" 7 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 16 void kasaInit(struct KasaFit *kasa) { kasaReset(kasa); } 17 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 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 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
