diff options
Diffstat (limited to 'src/poser_turveytori.c')
-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) |