From 66eb9b0508c6a5b1af5d1bc8fb5cac74573f8b20 Mon Sep 17 00:00:00 2001 From: mwturvey Date: Fri, 31 Mar 2017 15:36:36 -0700 Subject: Added rotation to object pose Haven't tested it yet, low confidence it's right, but the code is on the right track. --- src/poser_turveytori.c | 8 ++++++++ 1 file changed, 8 insertions(+) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 6997aa7..c251040 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1301,6 +1301,9 @@ 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 ); + wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0]; wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1]; wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2]; @@ -1309,6 +1312,11 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob so->OutPose.Pos[1] = wcPos[1]; so->OutPose.Pos[2] = wcPos[2]; + so->OutPose.Rot[0] = newOrientation[0]; + so->OutPose.Rot[1] = newOrientation[1]; + so->OutPose.Rot[2] = newOrientation[2]; + so->OutPose.Rot[3] = newOrientation[3]; + printf(" <% 04.4f, % 04.4f, % 04.4f > ", wcPos[0], wcPos[1], wcPos[2]); if (logFile) -- cgit v1.2.3