From 046188c2168a20f94db8f68e15c9813fe3087515 Mon Sep 17 00:00:00 2001 From: Mike Turvey Date: Thu, 1 Jun 2017 22:19:43 -0700 Subject: Forgot to normalize the quaternion --- src/poser_turveytori.c | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 3f0a0d5..ff7f47d 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1151,7 +1151,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) } -static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput, int lh, int setLhCalibration) +static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput,const int lh,const int setLhCalibration) { ToriData *toriData = so->PoserData; @@ -1385,6 +1385,8 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob //quatrotateabout(newOrientation, invRot, invPoseRot); // turns correctly, rotations aligned <-- This seems to be the best. //quatrotateabout(newOrientation, invPoseRot, invRot); // turns correctly, rotations aligned, (x & y flipped?) + quatnormalize(newOrientation, newOrientation); + so->OutPose.Pos[0] = wcPos[0]; so->OutPose.Pos[1] = wcPos[1]; so->OutPose.Pos[2] = wcPos[2]; @@ -1641,7 +1643,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT angle; FLT axis[3]; - angleaxisfrom2vect(&angle, &axis, td->down, negZ); + angleaxisfrom2vect(&angle, axis, td->down, negZ); //angleaxisfrom2vect(&angle, &axis, negZ, td->down); { -- cgit v1.2.3