diff options
| -rw-r--r-- | Makefile | 3 | ||||
| -rw-r--r-- | src/persistent_scene.c | 36 | ||||
| -rw-r--r-- | src/persistent_scene.h | 32 |
3 files changed, 69 insertions, 2 deletions
@@ -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); |