diff --git a/src/xrt/auxiliary/CMakeLists.txt b/src/xrt/auxiliary/CMakeLists.txt index 81d8d8722..f955103c8 100644 --- a/src/xrt/auxiliary/CMakeLists.txt +++ b/src/xrt/auxiliary/CMakeLists.txt @@ -5,6 +5,8 @@ set(MATH_SOURCE_FILES math/m_api.h math/m_base.cpp math/m_eigen_interop.hpp + math/m_filter_fifo.c + math/m_filter_fifo.h math/m_hash.cpp math/m_optics.c math/m_quatexpmap.cpp diff --git a/src/xrt/auxiliary/math/m_filter_fifo.c b/src/xrt/auxiliary/math/m_filter_fifo.c new file mode 100644 index 000000000..a23bd8b40 --- /dev/null +++ b/src/xrt/auxiliary/math/m_filter_fifo.c @@ -0,0 +1,161 @@ +// Copyright 2020, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief A fifo that also allows you to dynamically filter. + * @author Jakob Bornecrantz + * @ingroup aux_math + */ + +#include "util/u_misc.h" +#include "math/m_filter_fifo.h" + +#include + + +struct m_ff_vec3_f32 +{ + size_t num; + size_t latest; + struct xrt_vec3 *samples; + uint64_t *timestamps_ns; +}; + + +/* + * + * Internal functions. + * + */ + +static void +ff_init(struct m_ff_vec3_f32 *ff, size_t num) +{ + ff->samples = U_TYPED_ARRAY_CALLOC(struct xrt_vec3, num); + ff->timestamps_ns = U_TYPED_ARRAY_CALLOC(uint64_t, num); + ff->num = num; + ff->latest = 0; +} + +static void +ff_destroy(struct m_ff_vec3_f32 *ff) +{ + if (ff->samples != NULL) { + free(ff->samples); + ff->samples = NULL; + } + + if (ff->timestamps_ns != NULL) { + free(ff->timestamps_ns); + ff->timestamps_ns = NULL; + } + + ff->num = 0; + ff->latest = 0; +} + + +/* + * + * 'Exported' functions. + * + */ + +void +m_ff_vec3_f32_alloc(struct m_ff_vec3_f32 **ff_out, size_t num) +{ + struct m_ff_vec3_f32 *ff = U_TYPED_CALLOC(struct m_ff_vec3_f32); + ff_init(ff, num); + *ff_out = ff; +} + +void +m_ff_vec3_f32_free(struct m_ff_vec3_f32 **ff_ptr) +{ + struct m_ff_vec3_f32 *ff = *ff_ptr; + if (ff == NULL) { + return; + } + + ff_destroy(ff); + free(ff); + *ff_ptr = NULL; +} + +void +m_ff_vec3_f32_push(struct m_ff_vec3_f32 *ff, + const struct xrt_vec3 *sample, + uint64_t timestamp_ns) +{ + assert(ff->timestamps_ns[ff->latest] <= timestamp_ns); + + // We write samples backwards in the queue. + size_t i = ff->latest == 0 ? ff->num - 1 : --ff->latest; + ff->latest = i; + + ff->samples[i] = *sample; + ff->timestamps_ns[i] = timestamp_ns; +} + +void +m_ff_vec3_f32_get(struct m_ff_vec3_f32 *ff, + size_t num, + struct xrt_vec3 *out_sample, + uint64_t *out_timestamp_ns) +{ + size_t pos = (ff->latest + num) % ff->num; + *out_sample = ff->samples[pos]; + *out_timestamp_ns = ff->timestamps_ns[pos]; +} + +size_t +m_ff_vec3_f32_filter(struct m_ff_vec3_f32 *ff, + uint64_t start_ns, + uint64_t stop_ns, + struct xrt_vec3 *out_average) +{ + size_t num_sampled = 0; + size_t count = 0; + // Use double precision internally. + double x = 0, y = 0, z = 0; + + // Error, skip averaging. + if (start_ns > stop_ns) { + count = ff->num; + } + + while (count < ff->num) { + size_t pos = (ff->latest + count) % ff->num; + + // We have not yet reached where to start. + if (ff->timestamps_ns[pos] > stop_ns) { + count++; + continue; + } + + // If the sample is before the start we have reach the end. + if (ff->timestamps_ns[pos] < start_ns) { + count++; + break; + } + + x += ff->samples[pos].x; + y += ff->samples[pos].y; + z += ff->samples[pos].z; + num_sampled++; + count++; + } + + // Avoid division by zero. + if (num_sampled > 0) { + x /= num_sampled; + y /= num_sampled; + z /= num_sampled; + } + + out_average->x = (float)x; + out_average->y = (float)y; + out_average->z = (float)z; + + return num_sampled; +} diff --git a/src/xrt/auxiliary/math/m_filter_fifo.h b/src/xrt/auxiliary/math/m_filter_fifo.h new file mode 100644 index 000000000..58485f8dc --- /dev/null +++ b/src/xrt/auxiliary/math/m_filter_fifo.h @@ -0,0 +1,117 @@ +// Copyright 2020, Collabora, Ltd. +// SPDX-License-Identifier: BSL-1.0 +/*! + * @file + * @brief A fifo that also allows you to dynamically filter. + * @author Jakob Bornecrantz + * @ingroup aux_math + */ + +#pragma once + +#include "xrt/xrt_defines.h" + +#ifdef __cplusplus +extern "C" { +#endif + + +struct m_ff_vec3_f32; + +/*! + * Allocates a filter fifo tracking @p num samples and fills it with @p num + * samples at timepoint zero. + */ +void +m_ff_vec3_f32_alloc(struct m_ff_vec3_f32 **ff_out, size_t num); + +/*! + * Frees the given filter fifo and all it's samples. + */ +void +m_ff_vec3_f32_free(struct m_ff_vec3_f32 **ff_ptr); + +/*! + * Pushes a sample at the given timepoint, pushing samples out of order yields + * unspecified behaviour, so samples must be pushed in time order. + */ +void +m_ff_vec3_f32_push(struct m_ff_vec3_f32 *ff, + const struct xrt_vec3 *sample, + uint64_t timestamp_ns); + +/*! + * Return the sample at the index, zero means the last sample push, one second + * last and so on. + */ +void +m_ff_vec3_f32_get(struct m_ff_vec3_f32 *ff, + size_t num, + struct xrt_vec3 *out_sample, + uint64_t *out_timestamp_ns); + +/*! + * Averages all samples in the fifo between the two timepoints, returns number + * of samples sampled, if no samples was found between the timpoints returns 0 + * and sets @p out_average to all zeros. + * + * @param ff Filter fifo to search in. + * @param start_ns Timepoint furthest in the past, to start searching for + * samples. + * @param stop_ns Timepoint closest in the past, or now, to stop searching + * for samples. + * @param out_average Average of all samples in the given timeframe. + */ +size_t +m_ff_vec3_f32_filter(struct m_ff_vec3_f32 *ff, + uint64_t start_ns, + uint64_t stop_ns, + struct xrt_vec3 *out_average); + + +#ifdef __cplusplus +} + +/*! + * Helper class to wrap a C filter fifo. + */ +class FilterFifo3F +{ +private: + struct m_ff_vec3_f32 *ff; + + +public: + FilterFifo3F() = delete; + + FilterFifo3F(size_t size) + { + m_ff_vec3_f32_alloc(&ff, size); + } + + ~FilterFifo3F() + { + m_ff_vec3_f32_free(&ff); + } + + inline void + push(const xrt_vec3 &sample, uint64_t timestamp_ns) + { + m_ff_vec3_f32_push(ff, &sample, timestamp_ns); + } + + inline void + get(size_t num, xrt_vec3 *out_sample, uint64_t *out_timestamp_ns) + { + m_ff_vec3_f32_get(ff, num, out_sample, out_timestamp_ns); + } + + inline size_t + filter(uint64_t start_ns, + uint64_t stop_ns, + struct xrt_vec3 *out_average) + { + return m_ff_vec3_f32_filter(ff, start_ns, stop_ns, out_average); + } +}; +#endif diff --git a/src/xrt/auxiliary/meson.build b/src/xrt/auxiliary/meson.build index 86b8c59ab..ccbcdde5a 100644 --- a/src/xrt/auxiliary/meson.build +++ b/src/xrt/auxiliary/meson.build @@ -88,6 +88,8 @@ lib_aux_math = static_library( 'math/m_api.h', 'math/m_base.cpp', 'math/m_eigen_interop.hpp', + 'math/m_filter_fifo.c', + 'math/m_filter_fifo.h', 'math/m_hash.cpp', 'math/m_optics.c', 'math/m_quatexpmap.cpp',