math: Add filter fifo helper

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

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 <jakob@collabora.com>
+ * @ingroup aux_math
+ */
+
+#include "util/u_misc.h"
+#include "math/m_filter_fifo.h"
+
+#include <assert.h>
+
+
+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;
+}

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 <jakob@collabora.com>
+ * @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

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',