diff options
| author | Mike Turvey <mturvey6@gmail.com> | 2017-05-21 20:32:43 -0700 |
|---|---|---|
| committer | Mike Turvey <mturvey6@gmail.com> | 2017-05-21 20:32:43 -0700 |
| commit | fd7d4f53145fb691a8a1d636af3b38abc36e6516 (patch) | |
| tree | 79adbae4010147ebccdbcd8a4d1c5e16f7b0d33d /src | |
| parent | ce0aa4741f273fbdd936d6d5a8badd61ae112c60 (diff) | |
| download | libsurvive-fd7d4f53145fb691a8a1d636af3b38abc36e6516.tar.gz libsurvive-fd7d4f53145fb691a8a1d636af3b38abc36e6516.tar.bz2 | |
Fix the orientation from ToriPoser
Also updated the visualization in calibrate.c to show
position & orientation as calculated by each lighthouse
at the same time
Diffstat (limited to 'src')
| -rw-r--r-- | src/poser_turveytori.c | 12 |
1 files changed, 11 insertions, 1 deletions
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 4837f94..b4f685c 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1342,7 +1342,8 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos); FLT newOrientation[4]; - quatrotateabout(newOrientation, rotQuat, so->ctx->bsd[lh].Pose.Rot ); + //quatrotateabout(newOrientation, rotQuat, so->ctx->bsd[lh].Pose.Rot); + quatrotateabout(newOrientation, so->ctx->bsd[lh].Pose.Rot, rotQuat); wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0]; wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1]; @@ -1357,6 +1358,15 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob so->OutPose.Rot[2] = newOrientation[2]; so->OutPose.Rot[3] = newOrientation[3]; + so->FromLHPose[lh].Pos[0] = so->OutPose.Pos[0]; + so->FromLHPose[lh].Pos[1] = so->OutPose.Pos[1]; + so->FromLHPose[lh].Pos[2] = so->OutPose.Pos[2]; + + so->FromLHPose[lh].Rot[0] = so->OutPose.Rot[0]; + so->FromLHPose[lh].Rot[1] = so->OutPose.Rot[1]; + so->FromLHPose[lh].Rot[2] = so->OutPose.Rot[2]; + so->FromLHPose[lh].Rot[3] = so->OutPose.Rot[3]; + printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); if (logFile) |