From fd7d4f53145fb691a8a1d636af3b38abc36e6516 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Sun, 21 May 2017 20:32:43 -0700 Subject: 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 --- src/poser_turveytori.c | 12 +++++++++++- 1 file changed, 11 insertions(+), 1 deletion(-) (limited to 'src') 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) -- cgit v1.2.3