diff options
| author | mwturvey <michael.w.turvey@intel.com> | 2017-03-31 15:36:36 -0700 |
|---|---|---|
| committer | mwturvey <michael.w.turvey@intel.com> | 2017-03-31 15:36:36 -0700 |
| commit | 66eb9b0508c6a5b1af5d1bc8fb5cac74573f8b20 (patch) | |
| tree | 7b4d1f5a7f4d1db825f78cc859e3b8c3c71bed4b | |
| parent | eb5fae1c2aa6007ec84fdf0c5405308c7ea274ca (diff) | |
| download | libsurvive-66eb9b0508c6a5b1af5d1bc8fb5cac74573f8b20.tar.gz libsurvive-66eb9b0508c6a5b1af5d1bc8fb5cac74573f8b20.tar.bz2 | |
Added rotation to object pose
Haven't tested it yet, low confidence it's right,
but the code is on the right track.
| -rw-r--r-- | src/poser_turveytori.c | 8 |
1 files changed, 8 insertions, 0 deletions
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) |