Merge pull request #113 from cnlohr/epnp

Epnp

1 files changed, 36 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;
+}