Start work on determining rotation using quaternions only

Rotation was previously approximated using axis/angle
This change starts down the path of using quaternions exclusively.
This change appears to give at least as good as answers as the axis/angle
model in basic cases (also only tested with 1 lighthouse), but it is
currently much slower and runs in unpredictable time.

4 files changed, 218 insertions, 36 deletions

diff --git a/calibrate.c b/calibrate.c
index 8fb0986..cfd3e17 100644
--- a/calibrate.c
+++ b/calibrate.c
@@ -207,9 +207,9 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution)
 
 		CNFGTackSegment(
 			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
-			yResolution-(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+			(short)yResolution-(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
 			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
-			yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+			(short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
 	}
 
 	// line for the (0,-y) to (0,+y))
@@ -233,11 +233,36 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution)
 
 		CNFGTackSegment(
 			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
-			yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+			(short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
 			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
-			yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+			(short)yResolution -(short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
 	}
 
+	// Small line to indicate (0,+y) 
+	{
+		FLT tmp1[3];
+		FLT tmp1out[3];
+		FLT tmp2[3];
+		FLT tmp2out[3];
+
+		tmp1[1] = minRectSize + ((pose.Pos[2] * 40.0)*1.3);
+		tmp1[2] = 0;
+		tmp1[0] = ((pose.Pos[2] * 40.0)) * 0.3;
+
+		quatrotatevector(tmp1out, pose.Rot, tmp1);
+
+		tmp2[1] = minRectSize + ((pose.Pos[2] * 40.0)*0.7);
+		tmp2[2] = 0;
+		tmp2[0] = -(((pose.Pos[2] * 40.0)) * 0.3);
+
+		quatrotatevector(tmp2out, pose.Rot, tmp2);
+
+		CNFGTackSegment(
+			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
+			(short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
+			(short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+	}
 	// Small line to indicate (+x,0)
 	{
 		FLT tmp1[3];
@@ -245,13 +270,38 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution)
 		FLT tmp2[3];
 		FLT tmp2out[3];
 
-		tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0));
+		tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0)*1.3);
 		tmp1[2] = 0;
 		tmp1[1] = tmp1[0] * 0.3;
 
 		quatrotatevector(tmp1out, pose.Rot, tmp1);
 
-		tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0));
+		tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)*.7);
+		tmp2[2] = 0;
+		tmp2[1] = -(tmp1[0] * 0.3);
+
+		quatrotatevector(tmp2out, pose.Rot, tmp2);
+
+		CNFGTackSegment(
+			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
+			(short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
+			(short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+	}
+	// Small line to indicate (+x,0)
+	{
+		FLT tmp1[3];
+		FLT tmp1out[3];
+		FLT tmp2[3];
+		FLT tmp2out[3];
+
+		tmp1[0] = minRectSize + ((pose.Pos[2] * 40.0)*.7);
+		tmp1[2] = 0;
+		tmp1[1] = tmp1[0] * 0.3;
+
+		quatrotatevector(tmp1out, pose.Rot, tmp1);
+
+		tmp2[0] = minRectSize + ((pose.Pos[2] * 40.0)*1.3);
 		tmp2[2] = 0;
 		tmp2[1] = -(tmp1[0] * 0.3);
 
@@ -259,9 +309,9 @@ void DisplayPose(SurvivePose pose, size_t xResolution, size_t yResolution)
 
 		CNFGTackSegment(
 			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp1out[0]),
-			yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
+			(short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp1out[1]),
 			(short)(windowCenterX + (pose.Pos[0] * sizeScale) + tmp2out[0]),
-			yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
+			(short)yResolution - (short)(windowCenterY + (pose.Pos[1] * sizeScale) + tmp2out[1]));
 	}
 
 
diff --git a/redist/linmath.c b/redist/linmath.c
index 5d51708..1d70ee1 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -143,6 +143,42 @@ void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest)
 }
 
 
+void axisanglefromquat(FLT *angle, FLT *axis, FLT *q)
+{
+	// this way might be fine, too.
+	//FLT dist = FLT_SQRT((q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3]));
+	//
+	//*angle = 2 * FLT_ATAN2(dist, q[0]);
+
+	//axis[0] = q[1] / dist;
+	//axis[1] = q[2] / dist;
+	//axis[2] = q[3] / dist;
+	
+
+	// Good mathematical foundation for this algorithm found here:
+	// http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToAngle/index.htm
+
+	FLT tmp[4] = { q[0], q[1], q[2], q[3] };
+
+	quatnormalize(tmp, q);
+
+	if (FLT_FABS(q[0] - 1) < FLT_EPSILON)
+	{
+		// we have a degenerate case where we're rotating approx. 0 degrees
+		*angle = 0;
+		axis[0] = 1;
+		axis[1] = 0;
+		axis[2] = 0;
+		return;
+	}
+
+	axis[0] = tmp[1] / sqrt(1 - (tmp[0] * tmp[0]));
+	axis[1] = tmp[2] / sqrt(1 - (tmp[0] * tmp[0]));
+	axis[2] = tmp[3] / sqrt(1 - (tmp[0] * tmp[0]));
+
+	*angle = 2 * FLT_ACOS(tmp[0]);
+}
+
 /////////////////////////////////////QUATERNIONS//////////////////////////////////////////
 //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman)
 //Under the mit/X11 license.
diff --git a/redist/linmath.h b/redist/linmath.h
index 8d6cf05..1876b34 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -72,6 +72,8 @@ FLT anglebetween3d( FLT * a, FLT * b );
 
 void rotatearoundaxis(FLT *outvec3, FLT *invec3, FLT *axis, FLT angle);
 void angleaxisfrom2vect(FLT *angle, FLT *axis, FLT *src, FLT *dest);
+void axisanglefromquat(FLT *angle, FLT *axis, FLT *quat);
+
 
 //Quaternion things...
 
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index ae4592d..fed5762 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -87,7 +87,8 @@ typedef struct
 	int lastAxis[NUM_LIGHTHOUSES];
 
 	Point lastLhPos[NUM_LIGHTHOUSES];
-	FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4];
+//	FLT lastLhRotAxisAngle[NUM_LIGHTHOUSES][4];
+	FLT lastLhRotQuat[NUM_LIGHTHOUSES][4];
 } ToriData;
 
 
@@ -774,6 +775,21 @@ FLT RotationEstimateFitnessAxisAngleOriginal(Point lhPoint, FLT *quaternion, Tra
 // just an x or y axis to make our estimate better.  TODO: bring that data to this fn.
 FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObject *obj)
 {
+
+// TODO: ************************************************************************************************** THIS LIES!!!! NEED TO DO THIS IN QUATERNIONS!!!!!!!!!!!!!!!!!
+	{
+		FLT axisAngle[4];
+
+		axisanglefromquat(&(axisAngle[3]), axisAngle, quaternion);
+
+		FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj);
+
+		int a = throwaway;
+		return throwaway;
+	}
+
+
+
 	FLT fitness = 0;
 	for (size_t i = 0; i < obj->numSensors; i++)
 	{
@@ -1015,23 +1031,61 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini
 	}
 	if (ttDebug) printf(" Ri=%d ", i);
 }
-static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint)
+//static void WhereIsTheTrackedObjectQuaternion(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]);
+//	quatrotatevector(objPoint, rotation, objPoint);
+//	if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
+//}
+static void WhereIsTheTrackedObjectQuaternion(FLT *posOut, FLT *rotation, Point lhPoint)
 {
-	FLT reverseRotation[4] = {rotation[0], rotation[1], rotation[2], -rotation[3]};
-	FLT objPoint[3] = {lhPoint.x, lhPoint.y, lhPoint.z};
-	
+	posOut[0] = -lhPoint.x;
+	posOut[1] = -lhPoint.y;
+	posOut[2] = -lhPoint.z;
+
+	FLT inverseRotation[4];
+
+	quatgetreciprocal(inverseRotation, rotation);
+
+	FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z };
+
 	//rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]);
-	quatrotatevector(objPoint, rotation, objPoint);
-	if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
+	quatrotatevector(posOut, inverseRotation, posOut);
+//	if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
 }
 
+//static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint)
+//{
+//	posOut[0] = -lhPoint.x;
+//	posOut[1] = -lhPoint.y;
+//	posOut[2] = -lhPoint.z;
+//
+//	rotatearoundaxis(posOut, posOut, rotation, rotation[3]);
+//
+//	if (ttDebug) printf("{% 04.4f, % 04.4f, % 04.4f}  ", posOut[0], posOut[1], posOut[2]);
+//}
 
 
 static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj)
 {
 	int i = 0;
+
 	FLT lastMatchFitness = RotationEstimateFitnessQuaternion(lhPoint, initialEstimate, obj);
 
+	//{
+	//	FLT axisAngle[4];
+
+	//	axisanglefromquat(&(axisAngle[3]), axisAngle, initialEstimate);
+
+	//	FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj);
+
+	//	int a = throwaway;
+	//}
+
+
 	quatcopy(rotOut, initialEstimate);
 
 	// The values below are somewhat magic, and definitely tunable
@@ -1108,8 +1162,8 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in
 			//printf("+  %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]);
 //#endif
 			g *= 1.02;
-			if (ttDebug) printf("+");
-			WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint);
+			 printf("+");
+			//WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint);
 		}
 		else
 		{
@@ -1117,12 +1171,12 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in
 			//printf("-         , %f\n", point4[3]);
 //#endif
 			g *= 0.7;
-			if (ttDebug) printf("-");
+			printf("-");
 		}
 
 
 	}
-	if (ttDebug) printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
+	printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
 }
 
 
@@ -1133,7 +1187,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
 	// This should have the lighthouse directly facing the tracked object.
 	Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z };
 	FLT theta = atan2(-lh.x, -lh.y);
-	FLT zAxis[4] = { 0, 0, 1 , theta-LINMATHPI/2};
+	FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 };
 	FLT quat1[4];
 	quatfromaxisangle(quat1, zAxis, theta);
 
@@ -1152,6 +1206,32 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
 
 }
 
+void SolveForRotationQuat(FLT rotOut[4], TrackedObject *obj, Point lh)
+{
+
+	// Step 1, create initial quaternion for guess.  
+	// This should have the lighthouse directly facing the tracked object.
+	Point trackedObjRelativeToLh = { .x = -lh.x,.y = -lh.y,.z = -lh.z };
+	FLT theta = atan2(-lh.x, -lh.y);
+	FLT zAxis[4] = { 0, 0, 1 , theta - LINMATHPI / 2 };
+	FLT quat1[4];
+	quatfromaxisangle(quat1, zAxis, theta);
+
+	//quatfrom2vectors(0,0)
+	// not correcting for phi, but that's less important.
+
+
+	// Step 2, optimize the axis/ angle to match the data.
+	//RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj);
+
+
+	//// Step 2, optimize the quaternion to match the data.
+	RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj);
+
+	//WhereIsTheTrackedObjectQuaternion(rotOut, lh);
+
+}
+
 
 static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput,const int lh,const int setLhCalibration)
 {
@@ -1286,17 +1366,25 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	//printf("Distance is %f,   Fitness is %f\n", distance, fitGd);
 
 	FLT rot[4]; // this is axis/ angle rotation, not a quaternion!
+	FLT rotQuat[4]; // this is a quaternion!
 
 	// if we've already guessed at the rotation of the lighthouse,
 	// then let's use that as a starting guess, because it's probably
 	// going to make convergence happen much faster.
-	if (toriData->lastLhRotAxisAngle[lh][0] != 0)
-	{
-		rot[0] = toriData->lastLhRotAxisAngle[lh][0];
-		rot[1] = toriData->lastLhRotAxisAngle[lh][1];
-		rot[2] = toriData->lastLhRotAxisAngle[lh][2];
-		rot[3] = toriData->lastLhRotAxisAngle[lh][3];
-	}
+	//if (toriData->lastLhRotAxisAngle[lh][0] != 0)
+	//{
+	//	rot[0] = toriData->lastLhRotAxisAngle[lh][0];
+	//	rot[1] = toriData->lastLhRotAxisAngle[lh][1];
+	//	rot[2] = toriData->lastLhRotAxisAngle[lh][2];
+	//	rot[3] = toriData->lastLhRotAxisAngle[lh][3];
+	//}
+	//if (toriData->lastLhRotQuat[lh][0] != 0)
+	//{
+	//	rotQuat[0] = toriData->lastLhRotQuat[lh][0];
+	//	rotQuat[1] = toriData->lastLhRotQuat[lh][1];
+	//	rotQuat[2] = toriData->lastLhRotQuat[lh][2];
+	//	rotQuat[3] = toriData->lastLhRotQuat[lh][3];
+	//}
 
 	// Given the relative position of the lighthouse
 	// to the tracked object, in the tracked object's coordinate
@@ -1304,22 +1392,28 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	// tracked object's coordinate system.
 	// TODO: I believe this could be radically improved
 	// using an SVD.  
+	SolveForRotationQuat(rotQuat, obj, refinedEstimateGd);
 	SolveForRotation(rot, obj, refinedEstimateGd);
 	FLT objPos[3];
+	FLT objPos2[3];
 
-	{
-		toriData->lastLhRotAxisAngle[lh][0] = rot[0];
-		toriData->lastLhRotAxisAngle[lh][1] = rot[1];
-		toriData->lastLhRotAxisAngle[lh][2] = rot[2];
-		toriData->lastLhRotAxisAngle[lh][3] = rot[3];
-	}
+	//{
+	//	toriData->lastLhRotQuat[lh][0] = rotQuat[0];
+	//	toriData->lastLhRotQuat[lh][1] = rotQuat[1];
+	//	toriData->lastLhRotQuat[lh][2] = rotQuat[2];
+	//	toriData->lastLhRotQuat[lh][3] = rotQuat[3];
+	//}
+
+	WhereIsTheTrackedObjectAxisAngle(objPos2, rot, refinedEstimateGd);
+	WhereIsTheTrackedObjectQuaternion(objPos, rotQuat, refinedEstimateGd);
 
-	WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd);
 
+	FLT rotQuat2[4];
+	FLT rot2[4];
 
-	FLT rotQuat[4];
+	quatfromaxisangle(rotQuat2, rot, rot[3]);
+	axisanglefromquat(&(rot2[3]), rot2, rotQuat);
 
-	quatfromaxisangle(rotQuat, rot, rot[3]);
 
 	//{
 		//FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z};