1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
|
/* Copyright 2020 The ChromiumOS Authors
* Use of this source code is governed by a BSD-style license that can be
* found in the LICENSE file.
*/
#include "common.h"
#include "kasa.h"
#include "mat44.h"
#include <string.h>
void kasa_reset(struct kasa_fit *kasa)
{
memset(kasa, 0, sizeof(struct kasa_fit));
}
void kasa_accumulate(struct kasa_fit *kasa, fp_t x, fp_t y, fp_t z)
{
fp_t w = fp_sq(x) + fp_sq(y) + fp_sq(z);
kasa->acc_x += x;
kasa->acc_y += y;
kasa->acc_z += z;
kasa->acc_w += w;
kasa->acc_xx += fp_sq(x);
kasa->acc_xy += fp_mul(x, y);
kasa->acc_xz += fp_mul(x, z);
kasa->acc_xw += fp_mul(x, w);
kasa->acc_yy += fp_sq(y);
kasa->acc_yz += fp_mul(y, z);
kasa->acc_yw += fp_mul(y, w);
kasa->acc_zz += fp_sq(z);
kasa->acc_zw += fp_mul(z, w);
kasa->nsamples += 1;
}
void kasa_compute(struct kasa_fit *kasa, fpv3_t bias, fp_t *radius)
{
/* A * out = b
* (4 x 4) (4 x 1) (4 x 1)
*/
mat44_fp_t A;
fpv4_t b, out;
sizev4_t pivot;
A[0][0] = kasa->nsamples;
A[0][1] = A[1][0] = kasa->acc_x;
A[0][2] = A[2][0] = kasa->acc_y;
A[0][3] = A[3][0] = kasa->acc_z;
A[1][1] = kasa->acc_xx;
A[1][2] = A[2][1] = kasa->acc_xy;
A[1][3] = A[3][1] = kasa->acc_xz;
A[2][2] = kasa->acc_yy;
A[2][3] = A[3][2] = kasa->acc_yz;
A[3][3] = kasa->acc_zz;
b[0] = -kasa->acc_w;
b[1] = -kasa->acc_xw;
b[2] = -kasa->acc_yw;
b[3] = -kasa->acc_zw;
mat44_fp_decompose_lup(A, pivot);
mat44_fp_solve(A, out, b, pivot);
bias[0] = fp_mul(out[1], FLOAT_TO_FP(-0.5f));
bias[1] = fp_mul(out[2], FLOAT_TO_FP(-0.5f));
bias[2] = fp_mul(out[3], FLOAT_TO_FP(-0.5f));
*radius = fpv3_dot(bias, bias) - out[0];
*radius = (*radius > 0) ? fp_sqrtf(*radius) : FLOAT_TO_FP(0.0f);
}
|
