aboutsummaryrefslogtreecommitdiff
path: root/src
diff options
context:
space:
mode:
Diffstat (limited to 'src')
-rw-r--r--src/persistent_scene.c36
-rw-r--r--src/persistent_scene.h32
-rw-r--r--src/poser_epnp.c32
-rw-r--r--src/survive_process.c37
-rw-r--r--src/survive_sensor_activations.c19
5 files changed, 52 insertions, 104 deletions
diff --git a/src/persistent_scene.c b/src/persistent_scene.c
deleted file mode 100644
index cfc389f..0000000
--- a/src/persistent_scene.c
+++ /dev/null
@@ -1,36 +0,0 @@
-#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->sensor_ct; 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
deleted file mode 100644
index 07d9056..0000000
--- a/src/persistent_scene.h
+++ /dev/null
@@ -1,32 +0,0 @@
-#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);
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index e416456..40a2dd0 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -1,4 +1,3 @@
-#include "persistent_scene.h"
#ifndef USE_DOUBLE
#define FLT double
@@ -91,17 +90,18 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
return 0;
}
-struct add_correspondence_for_lh {
- epnp *pnp;
- int lh;
-};
-
-void add_correspondence_for_lh(SurviveObject *so, int lh, int sensor_idx, FLT *angles, void *_user) {
- struct add_correspondence_for_lh *user = (struct add_correspondence_for_lh *)_user;
- if (user->lh == lh)
- epnp_add_correspondence(user->pnp, so->sensor_locations[sensor_idx * 3 + 0],
- so->sensor_locations[sensor_idx * 3 + 1], so->sensor_locations[sensor_idx * 3 + 2],
- tan(angles[0]), tan(angles[1]));
+static void add_correspondences(SurviveObject *so, epnp *pnp, SurviveSensorActivations *scene,
+ const PoserDataLight *lightData) {
+ int lh = lightData->lh;
+ for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) {
+ if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance, lightData->timecode,
+ sensor_idx, lh)) {
+ double *angles = scene->angles[sensor_idx][lh];
+ epnp_add_correspondence(pnp, so->sensor_locations[sensor_idx * 3 + 0],
+ so->sensor_locations[sensor_idx * 3 + 1], so->sensor_locations[sensor_idx * 3 + 2],
+ tan(angles[0]), tan(angles[1]));
+ }
+ }
}
int PoserEPNP(SurviveObject *so, PoserData *pd) {
@@ -112,19 +112,15 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
return 0;
}
case POSERDATA_LIGHT: {
- static PersistentScene _scene = {.tolerance = 1500000};
- PersistentScene *scene = &_scene;
+ SurviveSensorActivations *scene = &so->activations;
PoserDataLight *lightData = (PoserDataLight *)pd;
- PersistentScene_add(scene, so, lightData);
-
int lh = lightData->lh;
if (so->ctx->bsd[lh].PositionSet) {
epnp pnp = {.fu = 1, .fv = 1};
epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct);
- struct add_correspondence_for_lh user = {.lh = lh, .pnp = &pnp};
- PersistentScene_ForEachCorrespondence(scene, add_correspondence_for_lh, so, lightData->timecode, &user);
+ add_correspondences(so, &pnp, scene, lightData);
if (pnp.number_of_correspondences > 4) {
diff --git a/src/survive_process.c b/src/survive_process.c
index fe6dc45..c9225e4 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -45,25 +45,26 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh)
{
SurviveContext * ctx = so->ctx;
- if( ctx->calptr )
- {
- survive_cal_angle( so, sensor_id, acode, timecode, length, angle, lh );
- }
- if( so->PoserFn )
- {
- PoserDataLight l = {
- .hdr =
- {
- .pt = POSERDATA_LIGHT,
- },
- .sensor_id = sensor_id,
- .acode = acode,
- .timecode = timecode,
- .length = length,
- .angle = angle,
- .lh = lh,
- };
+ PoserDataLight l = {
+ .hdr =
+ {
+ .pt = POSERDATA_LIGHT,
+ },
+ .sensor_id = sensor_id,
+ .acode = acode,
+ .timecode = timecode,
+ .length = length,
+ .angle = angle,
+ .lh = lh,
+ };
+
+ SurviveSensorActivations_add(&so->activations, &l);
+
+ if (ctx->calptr) {
+ survive_cal_angle(so, sensor_id, acode, timecode, length, angle, lh);
+ }
+ if (so->PoserFn) {
so->PoserFn( so, (PoserData *)&l );
}
}
diff --git a/src/survive_sensor_activations.c b/src/survive_sensor_activations.c
new file mode 100644
index 0000000..6daded4
--- /dev/null
+++ b/src/survive_sensor_activations.c
@@ -0,0 +1,19 @@
+#include <survive.h>
+
+bool SurviveSensorActivations_isPairValid(const SurviveSensorActivations *self, uint32_t tolerance,
+ uint32_t timecode_now, uint32_t idx, int lh) {
+ const uint32_t *data_timecode = self->timecode[idx][lh];
+ return !(timecode_now - data_timecode[0] > tolerance || timecode_now - data_timecode[1] > tolerance);
+}
+
+void SurviveSensorActivations_add(SurviveSensorActivations *self, struct 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;
+}
+
+// Roughly 31ms at a 48mhz clock rate
+uint32_t SurviveSensorActivations_default_tolerance = 1500000; \ No newline at end of file