Cleanup

1 files changed, 21 insertions, 37 deletions

diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index d3dd2ff..6997aa7 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1367,12 +1367,12 @@ static void QuickPose(SurviveObject *so, int lh)
 	{
 		int sensorCount = 0;
 
-		// TODO: remove, for debug purposes only!
-		FLT downQuat[4];
-		FLT negZ[3] = { 0,0,-1 };
-		quatfrom2vectors(downQuat, negZ, td->down);
+		//// TODO: remove, for debug purposes only!
+		//FLT downQuat[4];
+		//FLT negZ[3] = { 0,0,-1 };
+		////quatfrom2vectors(downQuat, negZ, td->down);
 		//quatfrom2vectors(downQuat, td->down, negZ);
-		// end TODO
+		//// end TODO
 
 
 		for (int i = 0; i < so->nr_locations; i++)
@@ -1397,11 +1397,6 @@ static void QuickPose(SurviveObject *so, int lh)
 				to->sensor[sensorCount].theta = td->oldAngles[i][0][lh][angleIndex0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
 				to->sensor[sensorCount].phi = td->oldAngles[i][1][lh][angleIndex1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical)
 
-				//printf("%2d: %8.8f, %8.8f   \n", 
-				//	i,
-				//	to->sensor[sensorCount].theta,
-				//	to->sensor[sensorCount].phi
-				//	);
 
 				sensorCount++;
 			}
@@ -1470,33 +1465,28 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 		//printf( "LIG:%s %d @ %f rad, %f s (AC %d) (TC %d)\n", so->codename, l->sensor_id, l->angle, l->length, l->acode, l->timecode );
 		if ((td->lastAxis[l->lh] != (l->acode & 0x1)) )
 		{
-			int foo = l->acode & 0x1;
-			//printf("%d", foo);
 
 
-			//if (axis)
+			if (0 == l->lh && axis) // only once per full cycle...
 			{
-				if (0 == l->lh && axis) // only once per full cycle...
-				{
-					static unsigned int counter = 1;
+				static unsigned int counter = 1;
 
-					counter++;
+				counter++;
 
-					// let's just do this occasionally for now...
-					if (counter % 2 == 0)
-						QuickPose(so, 0);
-				}
-				// axis changed, time to increment the circular buffer index.
-				td->angleIndex[l->lh][axis]++;
-				td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN;
-
-				// and clear out the data.
-				for (int i=0; i < SENSORS_PER_OBJECT; i++)
-				{
-					td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0;
-				}
+				// let's just do this occasionally for now...
+				if (counter % 2 == 0)
+					QuickPose(so, 0);
+			}
+			// axis changed, time to increment the circular buffer index.
+			td->angleIndex[l->lh][axis]++;
+			td->angleIndex[l->lh][axis] = td->angleIndex[l->lh][axis] % OLD_ANGLES_BUFF_LEN;
 
+			// and clear out the data.
+			for (int i=0; i < SENSORS_PER_OBJECT; i++)
+			{
+				td->oldAngles[i][axis][l->lh][td->angleIndex[l->lh][axis]] = 0;
 			}
+
 			td->lastAxis[l->lh] = axis;
 		}
 
@@ -1517,7 +1507,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 
 		// let's get the quaternion that represents this rotation.
 		FLT downQuat[4];
-		FLT negZ[3] = { 0,0,-1 };
+		FLT negZ[3] = { 0,0,1 };
 		//quatfrom2vectors(downQuat, negZ, td->down);
 		quatfrom2vectors(downQuat, td->down, negZ);
 
@@ -1544,12 +1534,6 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 					to->sensor[sensorCount].theta = fs->angles[i][0][0] + LINMATHPI / 2; // lighthouse 0, angle 0 (horizontal)
 					to->sensor[sensorCount].phi = fs->angles[i][0][1] + LINMATHPI / 2; // lighthouse 0, angle 1 (vertical)
 
-					//printf("%2d: %8.8f, %8.8f   \n", 
-					//	i,
-					//	to->sensor[sensorCount].theta,
-					//	to->sensor[sensorCount].phi
-					//	);
-
 					sensorCount++;
 				}
 			}