From e5c15af3af93356bb056624726e0b6068354690f Mon Sep 17 00:00:00 2001 From: mwturvey Date: Mon, 27 Mar 2017 14:20:29 -0700 Subject: Getting very close --- src/poser_turveytori.c | 17 ++++++++++++++++- 1 file changed, 16 insertions(+), 1 deletion(-) (limited to 'src') diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c index 824fabb..d737723 100644 --- a/src/poser_turveytori.c +++ b/src/poser_turveytori.c @@ -779,8 +779,10 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim lastMatchFitness = newMatchFitness; quatcopy(rotOut, point4); //#ifdef TORI_DEBUG - printf("+ %8.8f, %f\n", newMatchFitness, point4[3]); + printf("+ %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]); //#endif + g *= 1.03; + } else { @@ -796,6 +798,17 @@ static void RefineRotationEstimate(FLT *rotOut, Point lhPoint, FLT *initialEstim printf("\nRi=%d\n", i); } +static void WhereIsTheTrackedObject(FLT *rotation, Point lhPoint) +{ + FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]}; + FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z}; + + rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]); + + printf("The tracked object is at location (%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]); +} + + void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) { @@ -812,6 +825,8 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh) // Step 2, optimize the quaternion to match the data. RefineRotationEstimate(rotOut, lh, zAxis, obj); + WhereIsTheTrackedObject(rotOut, lh); + } -- cgit v1.2.3