aboutsummaryrefslogtreecommitdiff
path: root/tools
diff options
context:
space:
mode:
Diffstat (limited to 'tools')
-rw-r--r--tools/lightlengthparams/Makefile15
-rw-r--r--tools/lightlengthparams/lightlengthparams.c112
-rw-r--r--tools/viz/survive_viewer.js19
3 files changed, 138 insertions, 8 deletions
diff --git a/tools/lightlengthparams/Makefile b/tools/lightlengthparams/Makefile
new file mode 100644
index 0000000..b743f4f
--- /dev/null
+++ b/tools/lightlengthparams/Makefile
@@ -0,0 +1,15 @@
+all : lightlengthparams
+
+SRT:=../..
+
+LIBSURVIVE:=$(SRT)/lib/libsurvive.so
+
+CFLAGS:=-I$(SRT)/redist -I$(SRT)/include -O0 -g -DFLT=double -DUSE_DOUBLE
+LDFLAGS:=-lm -lpthread -llapacke -lcblas
+
+lightlengthparams : lightlengthparams.c $(LIBSURVIVE)
+ gcc $(CFLAGS) -o $@ $^ $(LDFLAGS)
+
+clean :
+ rm -rf lightlengthparams
+
diff --git a/tools/lightlengthparams/lightlengthparams.c b/tools/lightlengthparams/lightlengthparams.c
new file mode 100644
index 0000000..3b28b3f
--- /dev/null
+++ b/tools/lightlengthparams/lightlengthparams.c
@@ -0,0 +1,112 @@
+#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;
+}
diff --git a/tools/viz/survive_viewer.js b/tools/viz/survive_viewer.js
index 70aecfa..67e65f0 100644
--- a/tools/viz/survive_viewer.js
+++ b/tools/viz/survive_viewer.js
@@ -164,16 +164,20 @@ function create_tracked_object(info) {
group.sensors = [];
if (info.config && info.config.lighthouse_config) {
for (var idx in info.config.lighthouse_config.modelPoints) {
- var p = info.config.lighthouse_config.modelPoints[idx];
- var color = 0xFFFFFF; // / info.points.length * idx;
- if (idx === 10)
+ var p = info.config.lighthouse_config.modelPoints[idx];
+ var pn = info.config.lighthouse_config.modelNormals[idx];
+ var color = idx / info.config.lighthouse_config.modelPoints * 0xFFFFFF;
+ if (idx === 0)
color = 0x00ff00;
- if (idx === 12)
- color = 0x0000ff;
var sensorMaterial = new THREE.MeshBasicMaterial({color : color});
var newSensor = new THREE.Mesh(sensorGeometry, sensorMaterial);
newSensor.position.set(p[0], p[1], p[2]);
+ var normalGeom = new THREE.Geometry();
+ normalGeom.vertices.push(newSensor.position,
+ new THREE.Vector3(p[0] + pn[0] * .02, p[1] + pn[1] * .02, p[2] + pn[2] * .02));
+ var normal= new THREE.Line(normalGeom, new THREE.LineBasicMaterial({color : idx == 4 ? 0xFF0000 : 0x00FF00}));
+ group.add(normal);
group.sensors[idx] = sensorMaterial;
group.add(newSensor);
}
@@ -236,7 +240,6 @@ var survive_log_handlers = {
objs[obj.tracker].position.set(obj.position[0], obj.position[1], obj.position[2]);
objs[obj.tracker].quaternion.set(obj.quat[1], obj.quat[2], obj.quat[3], obj.quat[0]);
objs[obj.tracker].verticesNeedUpdate = true;
- timecode[obj.tracker] = obj.timecode;
if (trails) {
@@ -296,7 +299,7 @@ var survive_log_handlers = {
downAxes[obj.tracker] = new THREE.Geometry();
downAxes[obj.tracker].vertices.push(new THREE.Vector3(0, 0, 0), new THREE.Vector3(0, 0, 0));
- var line = new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff}));
+ var line = new THREE.Line(downAxes[obj.tracker], new THREE.LineBasicMaterial({color : 0xffffff}));
scene.add(line);
}
@@ -333,7 +336,7 @@ $(function() {
var url = new URL(window.location.href);
var remote = url.searchParams.get("remote");
- if (remote.length) {
+ if (remote && remote.length) {
survive_ws = new WebSocket("ws://" + remote + "/ws");
} else if (window.location.protocol === "file:") {
survive_ws = new WebSocket("ws://localhost:8080/ws");