From 029b2909b37cc58ba35e0f46be1802866ee59730 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Thu, 15 Mar 2018 21:17:42 -0600 Subject: Forgot files --- src/persistent_scene.c | 36 ++++++++++++++++++++++++++++++++++++ src/persistent_scene.h | 32 ++++++++++++++++++++++++++++++++ 2 files changed, 68 insertions(+) create mode 100644 src/persistent_scene.c create mode 100644 src/persistent_scene.h (limited to 'src') 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 +#include +#include + +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 + +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