mirror of
https://github.com/collabora/libsurvive.git
synced 2026-08-04 00:51:21 -04:00
112 lines
3.3 KiB
C
112 lines
3.3 KiB
C
#include <assert.h>
|
|
#include <libsurvive/survive.h>
|
|
#include <math.h>
|
|
#include <stdio.h>
|
|
#include <string.h>
|
|
|
|
uint32_t current_timecode;
|
|
|
|
void light_process(SurviveObject *so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length,
|
|
uint32_t lighthouse) {
|
|
current_timecode = timecode;
|
|
survive_default_light_process(so, sensor_id, acode, timeinsweep, timecode, length, lighthouse);
|
|
}
|
|
|
|
void raw_pose_process(SurviveObject *so, uint8_t _lh, SurvivePose *pose2w) {
|
|
survive_default_raw_pose_process(so, _lh, pose2w);
|
|
|
|
/*printf("Pose: ");
|
|
for(int i = 0;i < 7;i++) {
|
|
printf("%f, ", pose2w->Pos[i]);
|
|
};
|
|
printf("\n");
|
|
*/
|
|
for (int lh = 0; lh < 2; lh++) {
|
|
SurvivePose pose2lh = {};
|
|
SurvivePose w2lh = {};
|
|
InvertPose(&w2lh, &so->ctx->bsd[lh].Pose);
|
|
ApplyPoseToPose(&pose2lh, &w2lh, pose2w);
|
|
|
|
SurviveSensorActivations *scene = &so->activations;
|
|
for (size_t sensor_idx = 0; sensor_idx < so->sensor_ct; sensor_idx++) {
|
|
if (SurviveSensorActivations_isPairValid(scene, SurviveSensorActivations_default_tolerance / 2,
|
|
current_timecode, sensor_idx, lh)) {
|
|
uint32_t *lengths = scene->lengths[sensor_idx][lh];
|
|
|
|
const FLT *sensor_location = so->sensor_locations + 3 * sensor_idx;
|
|
const FLT *sensor_normals = so->sensor_normals + 3 * sensor_idx;
|
|
|
|
LinmathPoint3d sensor = {}, sensorN = {};
|
|
|
|
scale3d(sensorN, sensor_normals, .01);
|
|
add3d(sensorN, sensor_location, sensor_normals);
|
|
|
|
/*
|
|
printf("\n");
|
|
printf("%f, %f, %f\n", sensor_location[0], sensor_location[1], sensor_location[2]);
|
|
printf("%f, %f, %f\n", sensorN[0], sensorN[1], sensorN[2]);
|
|
|
|
FLT m[4][4];
|
|
PoseToMatrix((FLT*)m, &pose2lh);
|
|
|
|
for(int i = 0;i < 7;i++) {
|
|
printf("%f, ", pose2lh.Pos[i]);
|
|
};
|
|
printf("\n");
|
|
for(int i = 0;i < 4;i++) {
|
|
for(int j = 0;j < 4;j++) {
|
|
printf("%f, ", m[i][j]);
|
|
}
|
|
printf("\n");
|
|
}
|
|
printf("\n");
|
|
*/
|
|
ApplyPoseToPoint(sensor, &pose2lh, sensor_location);
|
|
ApplyPoseToPoint(sensorN, &pose2lh, sensorN);
|
|
|
|
// printf("%f, %f, %f\n", sensor[0], sensor[1], sensor[2]);
|
|
// printf("%f, %f, %f\n", sensorN[0], sensorN[1], sensorN[2]);
|
|
FLT dist = magnitude3d(sensor);
|
|
LinmathVec3d zout = {0, 0, -dist};
|
|
LinmathQuat r;
|
|
quatfrom2vectors(r, sensor, zout);
|
|
quatrotatevector(sensorN, r, sensorN);
|
|
sub3d(sensorN, sensorN, zout);
|
|
|
|
FLT dot = dot3d(sensor, sensorN);
|
|
if (dist > 20 || dist < 0.25)
|
|
continue;
|
|
|
|
FLT angs[2] = {};
|
|
for (int axis = 0; axis < 2; axis++) {
|
|
angs[axis] = cos(atan2(fabs(sensorN[axis ? 0 : 1]), sensorN[2]));
|
|
}
|
|
FLT area = angs[0] * angs[1];
|
|
|
|
printf("%u\t%u\t%lu\t%f\t%f\t%f\t%f\t%u\t%u\t%f\t%f\t%f\t%f\t%f\t%f\t%f\n", current_timecode, lh,
|
|
sensor_idx, dist, angs[0], angs[1], area, lengths[0], lengths[1], dot, sensor[0], sensor[1],
|
|
sensor[2], sensorN[0], sensorN[1], sensorN[2]);
|
|
|
|
// assert(angs[0] >= 0);
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
int main(int argc, char **argv) {
|
|
SurviveContext *ctx = survive_init(argc, argv);
|
|
if (ctx == 0) // implies -help or similiar
|
|
return 0;
|
|
|
|
printf("timecode\tlh\tsensor_idx\tdistance\tnorm_x\tnorm_y\tarea\tlength_x\tlength_y\tdot\tx\ty\tz\tnx\tny\tnz\n");
|
|
|
|
survive_startup(ctx);
|
|
|
|
survive_install_raw_pose_fn(ctx, raw_pose_process);
|
|
survive_install_light_fn(ctx, light_process);
|
|
while (survive_poll(ctx) == 0) {
|
|
}
|
|
|
|
survive_close(ctx);
|
|
return 0;
|
|
}
|