The open source OpenXR runtime
math: Add filter fifo helper
+282
+2
src/xrt/auxiliary/CMakeLists.txt
+2
src/xrt/auxiliary/CMakeLists.txt
+161
src/xrt/auxiliary/math/m_filter_fifo.c
+161
src/xrt/auxiliary/math/m_filter_fifo.c
···
1
+
// Copyright 2020, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief A fifo that also allows you to dynamically filter.
6
+
* @author Jakob Bornecrantz <jakob@collabora.com>
7
+
* @ingroup aux_math
8
+
*/
9
+
10
+
#include "util/u_misc.h"
11
+
#include "math/m_filter_fifo.h"
12
+
13
+
#include <assert.h>
14
+
15
+
16
+
struct m_ff_vec3_f32
17
+
{
18
+
size_t num;
19
+
size_t latest;
20
+
struct xrt_vec3 *samples;
21
+
uint64_t *timestamps_ns;
22
+
};
23
+
24
+
25
+
/*
26
+
*
27
+
* Internal functions.
28
+
*
29
+
*/
30
+
31
+
static void
32
+
ff_init(struct m_ff_vec3_f32 *ff, size_t num)
33
+
{
34
+
ff->samples = U_TYPED_ARRAY_CALLOC(struct xrt_vec3, num);
35
+
ff->timestamps_ns = U_TYPED_ARRAY_CALLOC(uint64_t, num);
36
+
ff->num = num;
37
+
ff->latest = 0;
38
+
}
39
+
40
+
static void
41
+
ff_destroy(struct m_ff_vec3_f32 *ff)
42
+
{
43
+
if (ff->samples != NULL) {
44
+
free(ff->samples);
45
+
ff->samples = NULL;
46
+
}
47
+
48
+
if (ff->timestamps_ns != NULL) {
49
+
free(ff->timestamps_ns);
50
+
ff->timestamps_ns = NULL;
51
+
}
52
+
53
+
ff->num = 0;
54
+
ff->latest = 0;
55
+
}
56
+
57
+
58
+
/*
59
+
*
60
+
* 'Exported' functions.
61
+
*
62
+
*/
63
+
64
+
void
65
+
m_ff_vec3_f32_alloc(struct m_ff_vec3_f32 **ff_out, size_t num)
66
+
{
67
+
struct m_ff_vec3_f32 *ff = U_TYPED_CALLOC(struct m_ff_vec3_f32);
68
+
ff_init(ff, num);
69
+
*ff_out = ff;
70
+
}
71
+
72
+
void
73
+
m_ff_vec3_f32_free(struct m_ff_vec3_f32 **ff_ptr)
74
+
{
75
+
struct m_ff_vec3_f32 *ff = *ff_ptr;
76
+
if (ff == NULL) {
77
+
return;
78
+
}
79
+
80
+
ff_destroy(ff);
81
+
free(ff);
82
+
*ff_ptr = NULL;
83
+
}
84
+
85
+
void
86
+
m_ff_vec3_f32_push(struct m_ff_vec3_f32 *ff,
87
+
const struct xrt_vec3 *sample,
88
+
uint64_t timestamp_ns)
89
+
{
90
+
assert(ff->timestamps_ns[ff->latest] <= timestamp_ns);
91
+
92
+
// We write samples backwards in the queue.
93
+
size_t i = ff->latest == 0 ? ff->num - 1 : --ff->latest;
94
+
ff->latest = i;
95
+
96
+
ff->samples[i] = *sample;
97
+
ff->timestamps_ns[i] = timestamp_ns;
98
+
}
99
+
100
+
void
101
+
m_ff_vec3_f32_get(struct m_ff_vec3_f32 *ff,
102
+
size_t num,
103
+
struct xrt_vec3 *out_sample,
104
+
uint64_t *out_timestamp_ns)
105
+
{
106
+
size_t pos = (ff->latest + num) % ff->num;
107
+
*out_sample = ff->samples[pos];
108
+
*out_timestamp_ns = ff->timestamps_ns[pos];
109
+
}
110
+
111
+
size_t
112
+
m_ff_vec3_f32_filter(struct m_ff_vec3_f32 *ff,
113
+
uint64_t start_ns,
114
+
uint64_t stop_ns,
115
+
struct xrt_vec3 *out_average)
116
+
{
117
+
size_t num_sampled = 0;
118
+
size_t count = 0;
119
+
// Use double precision internally.
120
+
double x = 0, y = 0, z = 0;
121
+
122
+
// Error, skip averaging.
123
+
if (start_ns > stop_ns) {
124
+
count = ff->num;
125
+
}
126
+
127
+
while (count < ff->num) {
128
+
size_t pos = (ff->latest + count) % ff->num;
129
+
130
+
// We have not yet reached where to start.
131
+
if (ff->timestamps_ns[pos] > stop_ns) {
132
+
count++;
133
+
continue;
134
+
}
135
+
136
+
// If the sample is before the start we have reach the end.
137
+
if (ff->timestamps_ns[pos] < start_ns) {
138
+
count++;
139
+
break;
140
+
}
141
+
142
+
x += ff->samples[pos].x;
143
+
y += ff->samples[pos].y;
144
+
z += ff->samples[pos].z;
145
+
num_sampled++;
146
+
count++;
147
+
}
148
+
149
+
// Avoid division by zero.
150
+
if (num_sampled > 0) {
151
+
x /= num_sampled;
152
+
y /= num_sampled;
153
+
z /= num_sampled;
154
+
}
155
+
156
+
out_average->x = (float)x;
157
+
out_average->y = (float)y;
158
+
out_average->z = (float)z;
159
+
160
+
return num_sampled;
161
+
}
+117
src/xrt/auxiliary/math/m_filter_fifo.h
+117
src/xrt/auxiliary/math/m_filter_fifo.h
···
1
+
// Copyright 2020, Collabora, Ltd.
2
+
// SPDX-License-Identifier: BSL-1.0
3
+
/*!
4
+
* @file
5
+
* @brief A fifo that also allows you to dynamically filter.
6
+
* @author Jakob Bornecrantz <jakob@collabora.com>
7
+
* @ingroup aux_math
8
+
*/
9
+
10
+
#pragma once
11
+
12
+
#include "xrt/xrt_defines.h"
13
+
14
+
#ifdef __cplusplus
15
+
extern "C" {
16
+
#endif
17
+
18
+
19
+
struct m_ff_vec3_f32;
20
+
21
+
/*!
22
+
* Allocates a filter fifo tracking @p num samples and fills it with @p num
23
+
* samples at timepoint zero.
24
+
*/
25
+
void
26
+
m_ff_vec3_f32_alloc(struct m_ff_vec3_f32 **ff_out, size_t num);
27
+
28
+
/*!
29
+
* Frees the given filter fifo and all it's samples.
30
+
*/
31
+
void
32
+
m_ff_vec3_f32_free(struct m_ff_vec3_f32 **ff_ptr);
33
+
34
+
/*!
35
+
* Pushes a sample at the given timepoint, pushing samples out of order yields
36
+
* unspecified behaviour, so samples must be pushed in time order.
37
+
*/
38
+
void
39
+
m_ff_vec3_f32_push(struct m_ff_vec3_f32 *ff,
40
+
const struct xrt_vec3 *sample,
41
+
uint64_t timestamp_ns);
42
+
43
+
/*!
44
+
* Return the sample at the index, zero means the last sample push, one second
45
+
* last and so on.
46
+
*/
47
+
void
48
+
m_ff_vec3_f32_get(struct m_ff_vec3_f32 *ff,
49
+
size_t num,
50
+
struct xrt_vec3 *out_sample,
51
+
uint64_t *out_timestamp_ns);
52
+
53
+
/*!
54
+
* Averages all samples in the fifo between the two timepoints, returns number
55
+
* of samples sampled, if no samples was found between the timpoints returns 0
56
+
* and sets @p out_average to all zeros.
57
+
*
58
+
* @param ff Filter fifo to search in.
59
+
* @param start_ns Timepoint furthest in the past, to start searching for
60
+
* samples.
61
+
* @param stop_ns Timepoint closest in the past, or now, to stop searching
62
+
* for samples.
63
+
* @param out_average Average of all samples in the given timeframe.
64
+
*/
65
+
size_t
66
+
m_ff_vec3_f32_filter(struct m_ff_vec3_f32 *ff,
67
+
uint64_t start_ns,
68
+
uint64_t stop_ns,
69
+
struct xrt_vec3 *out_average);
70
+
71
+
72
+
#ifdef __cplusplus
73
+
}
74
+
75
+
/*!
76
+
* Helper class to wrap a C filter fifo.
77
+
*/
78
+
class FilterFifo3F
79
+
{
80
+
private:
81
+
struct m_ff_vec3_f32 *ff;
82
+
83
+
84
+
public:
85
+
FilterFifo3F() = delete;
86
+
87
+
FilterFifo3F(size_t size)
88
+
{
89
+
m_ff_vec3_f32_alloc(&ff, size);
90
+
}
91
+
92
+
~FilterFifo3F()
93
+
{
94
+
m_ff_vec3_f32_free(&ff);
95
+
}
96
+
97
+
inline void
98
+
push(const xrt_vec3 &sample, uint64_t timestamp_ns)
99
+
{
100
+
m_ff_vec3_f32_push(ff, &sample, timestamp_ns);
101
+
}
102
+
103
+
inline void
104
+
get(size_t num, xrt_vec3 *out_sample, uint64_t *out_timestamp_ns)
105
+
{
106
+
m_ff_vec3_f32_get(ff, num, out_sample, out_timestamp_ns);
107
+
}
108
+
109
+
inline size_t
110
+
filter(uint64_t start_ns,
111
+
uint64_t stop_ns,
112
+
struct xrt_vec3 *out_average)
113
+
{
114
+
return m_ff_vec3_f32_filter(ff, start_ns, stop_ns, out_average);
115
+
}
116
+
};
117
+
#endif