Forgot files

2 files changed, 68 insertions, 0 deletions

diff --git a/src/persistent_scene.c b/src/persistent_scene.c
new file mode 100644
index 0000000..9bbbf41
--- /dev/null
+++ b/src/persistent_scene.c
@@ -0,0 +1,36 @@
+#include "persistent_scene.h"
+#include "linmath.h"
+#include <stdlib.h>
+#include <string.h>
+#include <survive.h>
+
+bool PersistentScene_isStillValid(const PersistentScene *self, uint32_t timecode_now, uint32_t idx, int lh) {
+	const uint32_t *data_timecode = self->timecode[idx][lh];
+	return !(timecode_now - data_timecode[0] > self->tolerance || timecode_now - data_timecode[1] > self->tolerance);
+}
+
+void PersistentScene_add(PersistentScene *self, SurviveObject *so, PoserDataLight *lightData) {
+	int axis = (lightData->acode & 1);
+	uint32_t *data_timecode = &self->timecode[lightData->sensor_id][lightData->lh][axis];
+	FLT *angle = &self->angles[lightData->sensor_id][lightData->lh][axis];
+
+	*angle = lightData->angle;
+	*data_timecode = lightData->timecode;
+}
+
+void PersistentScene_ForEachCorrespondence(PersistentScene *self, PersistentScene_ForEachCorrespondence_fn fn,
+										   SurviveObject *so, uint32_t timecode_now, void *user) {
+	for (int lh = 0; lh < NUM_LIGHTHOUSES; lh++) {
+		for (size_t i = 0; i < so->nr_locations; i++) {
+			if (PersistentScene_isStillValid(self, timecode_now, i, lh)) {
+				double *pts = self->angles[i][lh];
+				fn(so, lh, i, pts, user);
+			}
+		}
+	}
+}
+
+void PersistentScene_ctor(PersistentScene *self) {
+	memset(self, 0, sizeof(PersistentScene));
+	self->tolerance = 1500000;
+}
diff --git a/src/persistent_scene.h b/src/persistent_scene.h
new file mode 100644
index 0000000..07d9056
--- /dev/null
+++ b/src/persistent_scene.h
@@ -0,0 +1,32 @@
+#pragma once
+#include "persistent_scene.h"
+#include "stdbool.h"
+
+#ifndef USE_DOUBLE
+#define FLT double
+#define USE_DOUBLE
+#endif
+
+#include "linmath.h"
+#include <survive.h>
+
+typedef struct {
+	uint32_t tolerance;
+
+	// If "lengths[...]" < 0, means not a valid piece of sweep information.
+	FLT angles[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2]; // 2 Axes  (Angles in LH space)
+	uint32_t timecode[SENSORS_PER_OBJECT][NUM_LIGHTHOUSES][2];
+
+	PoserDataIMU lastimu;
+
+} PersistentScene;
+
+typedef void (*PersistentScene_ForEachCorrespondence_fn)(SurviveObject *so, int lh, int sensor_idx, FLT *angles,
+														 void *);
+void PersistentScene_ForEachCorrespondence(PersistentScene *self, PersistentScene_ForEachCorrespondence_fn fn,
+										   SurviveObject *so, uint32_t timecode_now, void *user);
+
+void PersistentScene_add(PersistentScene *self, SurviveObject *so, PoserDataLight *lightData);
+
+bool PersistentScene_isStillValid(const PersistentScene *self, uint32_t timecode_now, uint32_t idx, int lh);
+void PersistentScene_ctor(PersistentScene *self);