diff options
author | Justin Berger <j.david.berger@gmail.com> | 2018-03-11 15:31:28 -0600 |
---|---|---|
committer | Justin Berger <j.david.berger@gmail.com> | 2018-03-12 09:52:50 -0600 |
commit | 95eab1b8938e0033a0aa0d10d6ec53d7d2782907 (patch) | |
tree | e92f9abf0287fea8c91abadf8a095318507a8885 /src/poser_turveytori.c | |
parent | 7fadeeff29074520224be385ecd6c50271a07220 (diff) | |
download | libsurvive-95eab1b8938e0033a0aa0d10d6ec53d7d2782907.tar.gz libsurvive-95eab1b8938e0033a0aa0d10d6ec53d7d2782907.tar.bz2 |
Added callback for lighthouse found; example in test.c
Diffstat (limited to 'src/poser_turveytori.c')
-rw-r--r-- | src/poser_turveytori.c | 20 |
1 files changed, 10 insertions, 10 deletions
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index dae51e6..ddc4ad2 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1436,17 +1436,17 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob printf("Warning: resetting base station calibration data"); } + SurvivePose lighthousePose; FLT invRot[4]; - quatgetreciprocal(invRot, rotQuat); - - so->ctx->bsd[lh].Pose.Pos[0] = refinedEstimateGd.x; - so->ctx->bsd[lh].Pose.Pos[1] = refinedEstimateGd.y; - so->ctx->bsd[lh].Pose.Pos[2] = refinedEstimateGd.z; - so->ctx->bsd[lh].Pose.Rot[0] = invRot[0]; - so->ctx->bsd[lh].Pose.Rot[1] = invRot[1]; - so->ctx->bsd[lh].Pose.Rot[2] = invRot[2]; - so->ctx->bsd[lh].Pose.Rot[3] = invRot[3]; - so->ctx->bsd[lh].PositionSet = 1; + quatgetreciprocal(invRot, lighthousePose.Rot); + + lighthousePose.Pos[0] = refinedEstimateGd.x; + lighthousePose.Pos[1] = refinedEstimateGd.y; + lighthousePose.Pos[2] = refinedEstimateGd.z; + + if (so->ctx->lighthouseposeproc) { + so->ctx->lighthouseposeproc(so->ctx, lh, &lighthousePose); + } } |