Sane World View

The z=0 plane is now initially aligned to the
device's z=0 plane during calibration.
(works really well for the tracker, which is
 oriented with Z in the proper direction when
 sitting on a horizontal surface)

1 files changed, 18 insertions, 12 deletions

diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 8b131b5..d3dd2ff 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -895,9 +895,9 @@ void getNormalizedAndScaledRotationGradient(FLT *vectorToScale, FLT desiredMagni
 
 static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point lhPoint)
 {
-	posOut[0] = lhPoint.x;
-	posOut[1] = lhPoint.y;
-	posOut[2] = lhPoint.z;
+	posOut[0] = -lhPoint.x;
+	posOut[1] = -lhPoint.y;
+	posOut[2] = -lhPoint.z;
 	
 	rotatearoundaxis(posOut, posOut, rotation, rotation[3]);
 
@@ -1273,8 +1273,12 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 		quatrotatevector(tmpPos, rotQuat, tmpPos);
 	//}
 
+	//static int foo = 0;
+
+	//if (0 == foo)
 	if (setLhCalibration)
-	{
+	{ 
+		//foo = 1;
 		if (so->ctx->bsd[lh].PositionSet)
 		{
 			printf("Warning: resetting base station calibration data");
@@ -1297,20 +1301,22 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 
 	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];
+	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];
 
-	printf(" <% 04.4f, % 04.4f, % 04.4f >  ", wcPos[0], wcPos[1], wcPos[2]);
+	so->OutPose.Pos[0] = wcPos[0];
+	so->OutPose.Pos[1] = wcPos[1];
+	so->OutPose.Pos[2] = wcPos[2];
 
-	//posOut = 
+	printf(" <% 04.4f, % 04.4f, % 04.4f >  ", wcPos[0], wcPos[1], wcPos[2]);
 
 	if (logFile)
 	{
 		updateHeader(logFile);
 		fclose(logFile);
 	}
-	//fgetc(stdin);
+
 	return refinedEstimateGd;
 }
 
@@ -1526,8 +1532,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					FLT norm[3] = { so->sensor_normals[i * 3 + 0] , so->sensor_normals[i * 3 + 1] , so->sensor_normals[i * 3 + 2] };
 					FLT point[3] = { so->sensor_locations[i * 3 + 0] , so->sensor_locations[i * 3 + 1] , so->sensor_locations[i * 3 + 2] };
 
-					quatrotatevector(norm, downQuat, norm);
-					quatrotatevector(point, downQuat, point);
+					//quatrotatevector(norm, downQuat, norm);
+					//quatrotatevector(point, downQuat, point);
 
 					to->sensor[sensorCount].normal.x = norm[0];
 					to->sensor[sensorCount].normal.y = norm[1];