Adding comments

1 files changed, 59 insertions, 16 deletions

diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 45a1122..e10a323 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1253,21 +1253,28 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 		p1.z = toriData->lastLhPos[lh].z;
 	}
 
+	// refinedEstimateGd is the estimate for the location of the lighthouse in the tracked 
+	// object's local coordinate system.
 	Point refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p1, pna, pnaCount, logFile);
 
 	FLT pf1[3] = { refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z };
 
+	// here we're checking the direction of the found point against the average direction of the 
+	// normal direction of the sensors that saw the light pulse.
+	// This is because there are two possible points of convergence for the tori.  One is the correct
+	// location of the lighthouse.  The other is in almost exactly the opposite direction.
+	// The easiest way to determine that we've converged correctly is to see if the sensors' normal
+	// are pointing in the direction of the point we've converged on.
+	// if we have converged on the wrong point, we can try to converge one more time, using a starting estimate of 
+	// the point we converged on rotated to be directly opposite of its current position.  Such a point
+	// is guaranteed, in practice, to converge on the other location.
+	// Note: in practice, we pretty much always converge on the correct point in the first place, 
+	// but this check just makes extra sure.
 	FLT a1 = anglebetween3d(pf1, avgNormF);
-
 	if (a1 > M_PI / 2)
 	{
 		Point p2 = { .x = -refinedEstimateGd.x,.y = -refinedEstimateGd.y,.z = -refinedEstimateGd.z };
 		refinedEstimateGd = RefineEstimateUsingModifiedGradientDescent1(p2, pna, pnaCount, logFile);
-
-		//FLT pf2[3] = { refinedEstimageGd2.x, refinedEstimageGd2.y, refinedEstimageGd2.z };
-
-		//FLT a2 = anglebetween3d(pf2, avgNormF);
-
 	}
 
 	FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0);
@@ -1278,6 +1285,9 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 
 	FLT rot[4]; // this is axis/ angle rotation, not 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];
@@ -1286,7 +1296,12 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 		rot[3] = toriData->lastLhRotAxisAngle[lh][3];
 	}
 
-
+	// Given the relative position of the lighthouse
+	// to the tracked object, in the tracked object's coordinate
+	// system, find the rotation of the lighthouse, again in the
+	// tracked object's coordinate system.
+	// TODO: I believe this could be radically improved
+	// using an SVD.  
 	SolveForRotation(rot, obj, refinedEstimateGd);
 	FLT objPos[3];
 
@@ -1305,9 +1320,9 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	quatfromaxisangle(rotQuat, rot, rot[3]);
 
 	//{
-		FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z};
+		//FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z};
 
-		quatrotatevector(tmpPos, rotQuat, tmpPos);
+		//quatrotatevector(tmpPos, rotQuat, tmpPos);
 	//}
 
 	//static int foo = 0;
@@ -1334,17 +1349,31 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 		so->ctx->bsd[lh].PositionSet = 1;
 	}
 
+
 	FLT wcPos[3]; // position in wold coordinates
 
+	// Take the position of the tracked object from the lighthouse's
+	// frame of reference, and then rotate it in the reverse
+	// direction of the orientation of the lighthouse
+	// in the world reference freame.
+	// Then, change the offset based on the position of the lighthouse
+	// in the world reference frame.
+	// The result is the position of the tracked object
+	// in the world reference frame.
 	quatrotatevector(wcPos, so->ctx->bsd[lh].Pose.Rot, objPos);
+	wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0];
+	wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1];
+	wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2];
 
 	FLT newOrientation[4];
 	//quatrotateabout(newOrientation, rotQuat, so->ctx->bsd[lh].Pose.Rot);
 	quatrotateabout(newOrientation, so->ctx->bsd[lh].Pose.Rot, rotQuat);
 
-	wcPos[0] += so->ctx->bsd[lh].Pose.Pos[0];
-	wcPos[1] += so->ctx->bsd[lh].Pose.Pos[1];
-	wcPos[2] += so->ctx->bsd[lh].Pose.Pos[2];
+	//FLT invRot[4];
+	//quatgetreciprocal(invRot, rotQuat);
+	//quatrotateabout(newOrientation, invRot, so->ctx->bsd[lh].Pose.Rot);
+	////quatrotateabout(newOrientation, so->ctx->bsd[lh].Pose.Rot, invRot);
+
 
 	so->OutPose.Pos[0] = wcPos[0];
 	so->OutPose.Pos[1] = wcPos[1];
@@ -1619,8 +1648,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					//quatrotatevector(norm, downQuat, norm);
 					//quatrotatevector(point, downQuat, point);
 
-					rotatearoundaxis(norm, norm, axis, angle);
-					rotatearoundaxis(point, point, axis, angle);
+					//rotatearoundaxis(norm, norm, axis, angle);
+					//rotatearoundaxis(point, point, axis, angle);
 
 
 					to->sensor[sensorCount].normal.x = norm[0];
@@ -1655,8 +1684,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					//quatrotatevector(norm, downQuat, norm);
 					//quatrotatevector(point, downQuat, point);
 
-					rotatearoundaxis(norm, norm, axis, angle);
-					rotatearoundaxis(point, point, axis, angle);
+					//rotatearoundaxis(norm, norm, axis, angle);
+					//rotatearoundaxis(point, point, axis, angle);
 
 
 					to->sensor[sensorCount].normal.x = norm[0];
@@ -1678,6 +1707,20 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 			SolveForLighthouse(pos, quat, to, so, 0, 1, 1);
 		}
 
+
+		// This code block rotates the lighthouse fixes to accound for any time the tracked object
+		// is oriented other than +z = up
+		// This REALLY DOESN'T WORK!!!
+		//{
+		//	for (int lh = 0; lh < 2; lh++)
+		//	{
+		//		quatrotatevector(&(so->ctx->bsd[lh].Pose.Pos[0]), downQuat, &(so->ctx->bsd[lh].Pose.Pos[0]));
+		//		//quatrotateabout(&(so->ctx->bsd[lh].Pose.Rot[0]), &(so->ctx->bsd[lh].Pose.Rot[0]), downQuat);
+		//		quatrotateabout(&(so->ctx->bsd[lh].Pose.Rot[0]), downQuat, &(so->ctx->bsd[lh].Pose.Rot[0]));
+		//	}
+		//}
+
+
 		free(to);
 		//printf( "Full scene data.\n" );
 		break;