diff options
author | CNLohr <charles@cnlohr.com> | 2018-03-12 12:29:14 -0400 |
---|---|---|
committer | GitHub <noreply@github.com> | 2018-03-12 12:29:14 -0400 |
commit | e456a3ec1b0e1f04d55cdad7d95898f20bd26c89 (patch) | |
tree | c651c71625a8dbecc0b8293a1701a648eb836984 /src/poser_turveytori.c | |
parent | 7fadeeff29074520224be385ecd6c50271a07220 (diff) | |
parent | ee0fd0be34cd6715b704be534b7e97087905571f (diff) | |
download | libsurvive-e456a3ec1b0e1f04d55cdad7d95898f20bd26c89.tar.gz libsurvive-e456a3ec1b0e1f04d55cdad7d95898f20bd26c89.tar.bz2 |
Merge pull request #112 from cnlohr/poser_callbacks
Poser callbacks
Diffstat (limited to 'src/poser_turveytori.c')
-rw-r--r-- | src/poser_turveytori.c | 44 |
1 files changed, 16 insertions, 28 deletions
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index dae51e6..db46b09 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -1234,9 +1234,8 @@ void SolveForRotationQuat(FLT rotOut[4], TrackedObject *obj, Point lh) } - -static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput,const int lh,const int setLhCalibration) -{ +static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, PoserData *pd, + char doLogOutput, const int lh, const int setLhCalibration) { ToriData *toriData = so->PoserData; //printf("Solving for Lighthouse\n"); @@ -1436,17 +1435,15 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob printf("Warning: resetting base station calibration data"); } + SurvivePose lighthousePose; FLT invRot[4]; - quatgetreciprocal(invRot, rotQuat); - - so->ctx->bsd[lh].Pose.Pos[0] = refinedEstimateGd.x; - so->ctx->bsd[lh].Pose.Pos[1] = refinedEstimateGd.y; - so->ctx->bsd[lh].Pose.Pos[2] = refinedEstimateGd.z; - so->ctx->bsd[lh].Pose.Rot[0] = invRot[0]; - so->ctx->bsd[lh].Pose.Rot[1] = invRot[1]; - so->ctx->bsd[lh].Pose.Rot[2] = invRot[2]; - so->ctx->bsd[lh].Pose.Rot[3] = invRot[3]; - so->ctx->bsd[lh].PositionSet = 1; + quatgetreciprocal(invRot, lighthousePose.Rot); + + lighthousePose.Pos[0] = refinedEstimateGd.x; + lighthousePose.Pos[1] = refinedEstimateGd.y; + lighthousePose.Pos[2] = refinedEstimateGd.z; + + PoserData_lighthouse_pose_func(pd, so, lh, &lighthousePose); } @@ -1531,16 +1528,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob return refinedEstimateGd; } - - - - - - - -static void QuickPose(SurviveObject *so, int lh) -{ - +static void QuickPose(SurviveObject *so, PoserData *pd, int lh) { ToriData * td = so->PoserData; @@ -1634,7 +1622,7 @@ static void QuickPose(SurviveObject *so, int lh) // SolveForLighthouse(pos, quat, to, so, 0, lh, 0); //} - SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, 0, lh, 0); + SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, lh, 0); //printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]); if (so->ctx->rawposeproc) @@ -1717,7 +1705,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's just do this occasionally for now... if (counter % 4 == 0) - QuickPose(so, 0); + QuickPose(so, poserData, 0); } if (1 == l->lh && axis) // only once per full cycle... { @@ -1727,7 +1715,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) // let's just do this occasionally for now... if (counter % 4 == 0) - QuickPose(so, 1); + QuickPose(so, poserData, 1); } // axis changed, time to increment the circular buffer index. td->angleIndex[l->lh][axis]++; @@ -1802,7 +1790,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT pos[3], quat[4]; - SolveForLighthouse(pos, quat, to, so, 0, 0, 1); + SolveForLighthouse(pos, quat, to, so, poserData, 0, 0, 1); } { int sensorCount = 0; @@ -1838,7 +1826,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData ) FLT pos[3], quat[4]; - SolveForLighthouse(pos, quat, to, so, 0, 1, 1); + SolveForLighthouse(pos, quat, to, so, poserData, 0, 1, 1); } |