Getting very close

2 files changed, 19 insertions, 1 deletions

diff --git a/redist/linmath.c b/redist/linmath.c
index 0e06156..69a70f6 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -85,6 +85,9 @@ FLT anglebetween3d( FLT * a, FLT * b )
 // algorithm found here: http://inside.mines.edu/fs_home/gmurray/ArbitraryAxisRotation/
 void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle)
 {
+	// TODO: this really should be external.
+	normalize3d(axis, axis);
+
 	FLT s = FLT_SIN(angle);
 	FLT c = FLT_COS(angle);
 
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);
+
 }