mmo-ref/og-libsurvive
1
0
Fork 0
mirror of https://github.com/collabora/libsurvive.git synced 2026-08-04 00:51:21 -04:00
Code Issues Projects Releases Packages Wiki Activity Actions

Mapped external to world space

Browse source
This commit is contained in:
Justin Berger 2022-02-01 11:35:42 -07:00
parent e770ffad3b
commit cc63de0843
10 changed files with 131 additions and 34 deletions
Download patch file Download diff file Expand all files Collapse all files

4
include/libsurvive/survive.h
View file

@ -350,6 +350,7 @@ enum SurviveCalFlag {
SVCal_All = SVCal_Gib | SVCal_Curve | SVCal_Tilt | SVCal_Phase
};
struct SurviveContext_private;
struct SurviveContext {
int lh_version_configed;
int lh_version_forced;
@ -400,7 +401,7 @@ struct SurviveContext {
struct config_group *temporary_config_values; // Set per-session, from command-line. Not saved but override global_config_values
// Additional details that we don't want / need to expose to every single include
void *private_members;
struct SurviveContext_private *private_members;
bool request_floor_set;
FLT floor_offset;
@ -647,6 +648,7 @@ SURVIVE_EXPORT void survive_default_new_object_process(SurviveObject *so);
SURVIVE_EXPORT double survive_run_time(const SurviveContext *ctx);
SURVIVE_EXPORT double survive_run_time_since_epoch(const SurviveContext *ctx);
SURVIVE_EXPORT const SurvivePose* survive_external_to_world(const SurviveContext *ctx);
SURVIVE_EXPORT size_t survive_input_event_count(const SurviveContext *ctx);
////////////////////// Survive Drivers ////////////////////////////

8
redist/linmath.h
View file

@ -265,12 +265,12 @@ LINMATH_EXPORT void PoseToMatrix(FLT *mat44, const LinmathPose *pose_in);
// which transforms from B to A.
//
// This assumes that the space A and B share an origin.
LINMATH_EXPORT void KabschCentered(LinmathQuat B2Atx, const FLT *ptsA, const FLT *ptsB, int num_pts);
LINMATH_EXPORT void KabschCenteredScaled(LinmathQuat B2Atx, FLT *scale, const FLT *ptsA, const FLT *ptsB, int num_pts);
LINMATH_EXPORT void KabschCentered(LinmathQuat A2Btx, const FLT *ptsA, const FLT *ptsB, int num_pts);
LINMATH_EXPORT void KabschCenteredScaled(LinmathQuat A2Btx, FLT *scale, const FLT *ptsA, const FLT *ptsB, int num_pts);
// Same as above except it solves for the center for you
LINMATH_EXPORT void Kabsch(LinmathPose *B2Atx, const FLT *ptsA, const FLT *ptsB, int num_pts);
LINMATH_EXPORT void KabschScaled(LinmathPose *B2Atx, FLT *scale, const FLT *ptsA, const FLT *ptsB, int num_pts);
LINMATH_EXPORT void Kabsch(LinmathPose *A2Btx, const FLT *ptsA, const FLT *ptsB, int num_pts);
LINMATH_EXPORT void KabschScaled(LinmathPose *A2Btx, FLT *scale, const FLT *ptsA, const FLT *ptsB, int num_pts);
// Matrix Stuff
typedef struct {

16
redist/lintest.c
View file

@ -120,29 +120,29 @@ void testApplyPoseToPoint() {
void testKabsch() {
FLT pts[] = {0, 0, 0, 100, 100, 100, 10, 0, 10, 50, 50, 0, 0, 0, 1000, -100, 0, 100};
LinmathPose tx = {.Pos = { 0 }, .Rot = {4, 3, 2, 1}};
LinmathPose pts2txPts = {.Pos = { 0 }, .Rot = {4, 3, 2, 1}};
LinmathPose tx2 = {.Pos = {1, 2, 3}, .Rot = {1, 2, 3, 4}};
LinmathPose pts2tx2Pts = {.Pos = {1, 2, 3}, .Rot = {1, 2, 3, 4}};
quatnormalize(tx.Rot, tx.Rot);
quatnormalize(tx2.Rot, tx2.Rot);
quatnormalize(pts2txPts.Rot, pts2txPts.Rot);
quatnormalize(pts2tx2Pts.Rot, pts2tx2Pts.Rot);
const int N = sizeof(pts) / sizeof(FLT) / 3;
FLT *txPts = alloca(N * 3 * sizeof(FLT));
FLT *txPts2 = alloca(N * 3 * sizeof(FLT));
for (int i = 0; i < N; i++) {
ApplyPoseToPoint(txPts + i * 3, &tx, pts + i * 3);
ApplyPoseToPoint(txPts2 + i * 3, &tx2, pts + i * 3);
ApplyPoseToPoint(txPts + i * 3, &pts2txPts, pts + i * 3);
ApplyPoseToPoint(txPts2 + i * 3, &pts2tx2Pts, pts + i * 3);
}
LinmathQuat should_be_tx = { 0 };
KabschCentered(should_be_tx, pts, txPts, N);
ASSERT_FLTA_EQUALS(should_be_tx, tx.Rot, 4);
ASSERT_FLTA_EQUALS(should_be_tx, pts2txPts.Rot, 4);
LinmathPose should_be_tx2 = { 0 };
Kabsch(&should_be_tx2, pts, txPts2, N);
ASSERT_FLTA_EQUALS(should_be_tx2.Pos, tx2.Pos, 7);
ASSERT_FLTA_EQUALS(should_be_tx2.Pos, pts2tx2Pts.Pos, 7);
}
static void testKabsch2() {

5
src/driver_playback.c
View file

@ -244,8 +244,9 @@ static int parse_and_run_imu(const char *line, SurvivePlaybackData *driver, bool
static int parse_and_run_lhpose(const char *line, struct SurvivePlaybackData *driver) {
SurvivePose pose;
int lh = -1;
int rr = sscanf(line, "%d LH_POSE " SurvivePose_sformat "\n", &lh, &pose.Pos[0], &pose.Pos[1], &pose.Pos[2],
&pose.Rot[0], &pose.Rot[1], &pose.Rot[2], &pose.Rot[3]);
uint32_t basestationId = 0;
int rr = sscanf(line, "%d LH_POSE " SurvivePose_sformat "%u\n", &lh, &pose.Pos[0], &pose.Pos[1], &pose.Pos[2],
&pose.Rot[0], &pose.Rot[1], &pose.Rot[2], &pose.Rot[3], &basestationId);
SurviveContext *ctx = driver->ctx;
if (driver->outputCalculatedPose) {

13
src/survive.c
View file

@ -28,6 +28,8 @@
#define z_const const
#endif
#include "survive_private.h"
#define DEFAULT_CONFIG_PATH "config.json"
STATIC_CONFIG_ITEM(SURVIVE_VERBOSE, "v", 'i', "Verbosity level", 0)
STATIC_CONFIG_ITEM(BLACKLIST_DEVS, "blacklist-devs", 's', "List any devs (or substrings of devs) to blacklist.", "")
@ -254,16 +256,6 @@ SURVIVE_EXPORT int8_t survive_get_bsd_idx(SurviveContext *ctx, survive_channel c
return -1;
}
struct SurviveContext_private {
og_sema_t poll_sema;
survive_run_time_fn runTimeFn;
void *runTimeFnUser;
double lastRunTime;
double callbackStatsTimeBetween;
double lastCallbackStats;
};
void survive_get_ctx_lock(SurviveContext *ctx) {
struct SurviveContext_private *pctx = ctx->private_members;
// SV_VERBOSE(100, "Trying to get lock on %lx", pthread_self());
@ -331,6 +323,7 @@ SurviveContext *survive_init_internal(int argc, char *const *argv, void *userDat
ctx->poll_min_time_ms = 10;
struct SurviveContext_private *pctx = ctx->private_members = SV_CALLOC(sizeof(struct SurviveContext_private));
pctx->external2world.Rot[0] = 1;
pctx->poll_sema = OGCreateSema();

5
src/survive_api.c
View file

@ -448,8 +448,9 @@ FLT survive_simple_object_get_latest_pose(const SurviveSimpleObject *sao, Surviv
timecode = SurviveSensorActivations_runtime(&sao->data.so->activations, sao->data.so->OutPose_timecode) * 1e-6;
break;
case SurviveSimpleObject_EXTERNAL:
if (pose)
*pose = sao->data.seo.pose;
if (pose) {
ApplyPoseToPose(pose, survive_external_to_world(sao->actx->ctx), &sao->data.seo.pose);
}
break;
default: {

19
src/survive_private.h Normal file
View file

@ -0,0 +1,19 @@
#pragma once
struct SurviveExternalPose {
char name[32];
SurvivePose pose;
};
struct SurviveContext_private {
og_sema_t poll_sema;
survive_run_time_fn runTimeFn;
void *runTimeFnUser;
double lastRunTime;
double callbackStatsTimeBetween;
double lastCallbackStats;
struct SurviveExternalPose ExternalPoses[16];
SurvivePose external2world;
};

79
src/survive_process.c
View file

@ -13,6 +13,8 @@
#include "survive_kalman_tracker.h"
#include "survive_str.h"
#include "survive_private.h"
void survive_default_button_process(SurviveObject *so, enum SurviveInputEvent eventType, enum SurviveButton buttonId,
const enum SurviveAxis *axisIds, const SurviveAxisVal_t *axisValues) {
}
@ -75,6 +77,76 @@ static inline bool check_str(const char* blacklist, const char* name) {
}
return false;
}
static inline void calculate_external2world(SurviveContext *ctx) {
SurvivePose externalLH[NUM_GEN2_LIGHTHOUSES] = { 0 };
for(int i = 0;i < SURVIVE_ARRAY_SIZE(ctx->private_members->ExternalPoses) && ctx->private_members->ExternalPoses->name[0] != 0;i++) {
for (int j = 0; j < ctx->activeLighthouses; j++) {
if(!ctx->bsd[j].PositionSet) continue;
char buf[32] = {0};
snprintf(buf, 32, "LHB-%08X", ctx->bsd[j].BaseStationID);
if (strcmp(buf, ctx->private_members->ExternalPoses[i].name) == 0) {
externalLH[j] = ctx->private_members->ExternalPoses[i].pose;
}
snprintf(buf, 32, "previous_LH%d", ctx->bsd[j].mode);
if (strcmp(buf, ctx->private_members->ExternalPoses[i].name) == 0) {
externalLH[j] = ctx->private_members->ExternalPoses[i].pose;
}
}
}
LinmathVec3d pts[] = {
{1, 0, 0},
{0, 1, 0},
{0, 0, 1},
};
int num_pairs = 0;
CN_CREATE_STACK_MAT(ptsExtLH, ctx->activeLighthouses * SURVIVE_ARRAY_SIZE(pts), 3);
CN_CREATE_STACK_MAT(ptsWorldLH, ctx->activeLighthouses * SURVIVE_ARRAY_SIZE(pts), 3);
for (int j = 0; j < ctx->activeLighthouses; j++) {
if (quatiszero(externalLH[j].Rot)) continue;
for(int h = 0;h < SURVIVE_ARRAY_SIZE(pts);h++) {
ApplyPoseToPoint(ptsExtLH.data + (num_pairs) * 3, &externalLH[j], pts[h]);
ApplyPoseToPoint(ptsWorldLH.data + (num_pairs) * 3, survive_get_lighthouse_true_position(ctx, j), pts[h]);
(ptsWorldLH.data + (num_pairs) * 3)[2] -= ctx->floor_offset;
num_pairs++;
}
}
if(num_pairs == 0) {
ctx->private_members->external2world = (SurvivePose) { .Rot = { 1 }};
} else {
Kabsch(&ctx->private_members->external2world, ptsExtLH.data, ptsWorldLH.data, num_pairs);
survive_recording_write_to_output(ctx->recptr, "EXTERNAL_TO_WORLD " SurvivePose_format "\n", SURVIVE_POSE_EXPAND(ctx->private_members->external2world));
}
}
const SurvivePose* survive_external_to_world(const SurviveContext *ctx) {
return &ctx->private_members->external2world;
}
static inline void insert_external_pose(SurviveContext *ctx, const char *name, const SurvivePose *pose) {
bool isLH = strncmp(name, "LHB-", 4) == 0 ||
strncmp(name, "previous_LH", strlen("previous_LH")) == 0;
for(int i = 0;i < SURVIVE_ARRAY_SIZE(ctx->private_members->ExternalPoses);i++) {
if(ctx->private_members->ExternalPoses[i].name[0] == 0) {
strncpy(ctx->private_members->ExternalPoses[i].name, name, 16);
}
if(strncmp(name, ctx->private_members->ExternalPoses[i].name, 16) == 0) {
ctx->private_members->ExternalPoses[i].pose = *pose;
break;
}
}
if(isLH) {
calculate_external2world(ctx);
}
}
void survive_default_external_pose_process(SurviveContext *ctx, const char *name, const SurvivePose *pose) {
if (strncmp(name, "LHB", 3) == 0) {
bool useExternal = survive_configb(ctx, USE_EXTERNAL_LH_TAG, SC_GET, 0);
@ -107,6 +179,8 @@ void survive_default_external_pose_process(SurviveContext *ctx, const char *name
return;
}
insert_external_pose(ctx, name, pose);
survive_recording_external_pose_process(ctx, name, pose);
}
@ -131,7 +205,7 @@ void survive_default_raw_lighthouse_pose_process(SurviveContext *ctx, uint8_t li
if (ctx->bsd[lighthouse].PositionSet) {
FLT d = dist3d(lighthouse_pose->Pos, ctx->bsd[lighthouse].Pose.Pos);
if (d < ctx->settings.lh_max_update || d > ctx->settings.lh_max_nudge_distance) {
ctx->bsd[lighthouse].Pose = *lighthouse_pose;
ctx->bsd[lighthouse].true_pos = ctx->bsd[lighthouse].Pose = *lighthouse_pose;
} else {
ctx->bsd[lighthouse].old_pos = ctx->bsd[lighthouse].Pose;
ctx->bsd[lighthouse].true_pos = *lighthouse_pose;
@ -144,7 +218,7 @@ void survive_default_raw_lighthouse_pose_process(SurviveContext *ctx, uint8_t li
ctx->bsd[lighthouse].mode, SURVIVE_POSE_EXPAND(*lighthouse_pose));
}
} else {
ctx->bsd[lighthouse].Pose = *lighthouse_pose;
ctx->bsd[lighthouse].true_pos = ctx->bsd[lighthouse].Pose = *lighthouse_pose;
ctx->bsd[lighthouse].PositionSet = 1;
}
survive_kalman_lighthouse_update_position(ctx->bsd[lighthouse].tracker, lighthouse_pose);
@ -172,6 +246,7 @@ void survive_default_raw_lighthouse_pose_process(SurviveContext *ctx, uint8_t li
SurvivePose external_pose = ctx->bsd[lighthouse].Pose;
external_pose.Pos[2] -= ctx->floor_offset;
calculate_external2world(ctx);
SURVIVE_INVOKE_HOOK(lighthouse_pose, ctx, lighthouse, &external_pose);
}

4
src/survive_recording.c
View file

@ -157,9 +157,9 @@ void survive_recording_lighthouse_process(SurviveContext *ctx, uint8_t lighthous
int8_t mode = ctx->bsd[lighthouse].mode;
survive_recording_write_to_output(
recordingData,
"%d LH_POSE " FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF "\r\n", mode,
"%d LH_POSE " FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF FLT_PRINTF " %u\r\n", mode,
lh_pose->Pos[0], lh_pose->Pos[1], lh_pose->Pos[2], lh_pose->Rot[0], lh_pose->Rot[1], lh_pose->Rot[2],
lh_pose->Rot[3]);
lh_pose->Rot[3], ctx->bsd[lighthouse].BaseStationID);
}
void survive_recording_velocity_process(SurviveObject *so, uint8_t lighthouse, const SurviveVelocity *pose) {
SurviveRecordingData *recordingData = so->ctx->recptr;

12
tools/viz/survive_viewer.js
View file

@ -570,8 +570,9 @@ function update_trails() {
$(function() { $("#trails").change(update_trails); });
function update_external() {
var val = this.checked ? 1.5 : 0;
external_group.position.set(0, val, 0);
var val = $("#external_displace")[0].checked ? 1.5 : 0;
external_group.position.set(external2world[0], external2world[1] + val, external2world[2]);
external_group.quaternion.set(external2world[4], external2world[5], external2world[6], external2world[3]);
}
$(function() { $("#external_displace").change(update_external); });
@ -954,8 +955,13 @@ function add_poly(v) {
scene.add(line);
}
}
var external2world = [0, 0, 0, 0, 0, 0, 1];
var survive_log_handlers = {
"EXTERNAL_TO_WORLD": function(v) {
external2world = v.slice(2).map(parseFloat);
update_external();
},
"LH_POSE" : function(v) {
var obj = {
lighthouse : parseInt(v[1]),
@ -1118,7 +1124,7 @@ var survive_log_handlers = {
function add_survive_log_handler(name, entry) { survive_log_handlers[name] = entry; }
function process_survive_handlers(msg) {
var s = msg.split(' ').filter(function(x) { return x; });
var s = msg.replace('\t', ' ').split(' ').filter(function(x) { return x; });
var handled = false;
if (survive_log_handlers[s[2]]) {
survive_log_handlers[s[2]](s);