1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
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;
}
|
