From 029b2909b37cc58ba35e0f46be1802866ee59730 Mon Sep 17 00:00:00 2001
From: Justin Berger <j.david.berger@gmail.com>
Date: Thu, 15 Mar 2018 21:17:42 -0600
Subject: Forgot files

---
 Makefile               |  3 +--
 src/persistent_scene.c | 36 ++++++++++++++++++++++++++++++++++++
 src/persistent_scene.h | 32 ++++++++++++++++++++++++++++++++
 3 files changed, 69 insertions(+), 2 deletions(-)
 create mode 100644 src/persistent_scene.c
 create mode 100644 src/persistent_scene.h

diff --git a/Makefile b/Makefile
index d0071a1..99d29a8 100644
--- a/Makefile
+++ b/Makefile
@@ -30,13 +30,12 @@ GRAPHICS_LOFI:=redist/CNFGFunctions.o redist/CNFGXDriver.o
 
 endif
 
-
 POSERS:=src/poser_dummy.o src/poser_daveortho.o src/poser_charlesslow.o src/poser_octavioradii.o src/poser_turveytori.o src/poser_epnp.o
 REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.o redist/minimal_opencv.o
 ifeq ($(UNAME), Darwin)
 REDISTS:=$(REDISTS) redist/hid-osx.c
 endif
-LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/poser_imu.o src/survive_imu.o src/epnp/epnp.c src/persistent_scene.o
+LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o src/epnp/epnp.c src/persistent_scene.o 
 
 
 #If you want to use HIDAPI on Linux.
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);
-- 
cgit v1.2.3