From a9739c252fab9b32684148625b08b2cfb89a3751 Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Wed, 20 Dec 2017 18:19:03 -0700
Subject: Save Calibration Output To Config

---
 src/poser_turveytori.c | 4 ++++
 1 file changed, 4 insertions(+)

(limited to 'src/poser_turveytori.c')

diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 80e8d89..d0b9ab8 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1732,6 +1732,10 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 		//	}
 		//}
 
+		config_set_lighthouse(ctx->lh_config, ctx->bsd[0], 0);
+		config_set_lighthouse(ctx->lh_config, ctx->bsd[1], 1);
+
+		config_save(ctx, "config.json");
 
 		free(to);
 		//printf( "Full scene data.\n" );
-- 
cgit v1.3.1


From 283a395d8d63e4532173f9497f2ea8cd154a4758 Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Fri, 22 Dec 2017 23:06:26 -0700
Subject: Configuration Changes

Added some config options.  Changed a few defaults.  Better output from PoserTurveyTori.
Hopefully, this will make it easier for someone new to get up and running more easily.
---
 src/poser_turveytori.c | 45 ++++++++++++++++++++++++++++++++++-----------
 src/survive.c          |  6 +++++-
 src/survive_cal.c      | 10 +++++++---
 src/survive_config.c   |  2 +-
 4 files changed, 47 insertions(+), 16 deletions(-)

(limited to 'src/poser_turveytori.c')

diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index d0b9ab8..ae4592d 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -15,6 +15,8 @@
 #endif
 
 
+static int ttDebug = 0;
+
 #define PointToFlts(x) ((FLT*)(x))
 
 typedef struct
@@ -412,7 +414,7 @@ FLT getPointFitness(Point pointIn, PointsAndAngle *pna, size_t pnaCount, int deu
 		}
 
 		fitnesses[i] = FLT_FABS(fitness);
-		if (deubgPrint)
+		if (0)
 		{
 			printf("  [%d, %d](%f)\n", pna[i].ai, pna[i].bi, fitness);
 		}
@@ -585,7 +587,7 @@ static Point RefineEstimateUsingModifiedGradientDescent1(Point initialEstimate,
 			break;
 		}
 	}
-	printf(" i=%3d ", i);
+	if (ttDebug) printf(" i=%3d ", i);
 
 	return lastPoint;
 }
@@ -910,7 +912,7 @@ static void WhereIsTheTrackedObjectAxisAngle(FLT *posOut, FLT *rotation, Point l
 	
 	rotatearoundaxis(posOut, posOut, rotation, rotation[3]);
 
-	printf("{% 04.4f, % 04.4f, % 04.4f}  ", posOut[0], posOut[1], posOut[2]);
+	if (ttDebug) printf("{% 04.4f, % 04.4f, % 04.4f}  ", posOut[0], posOut[1], posOut[2]);
 }
 
 static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *initialEstimate, TrackedObject *obj)
@@ -1011,7 +1013,7 @@ static void RefineRotationEstimateAxisAngle(FLT *rotOut, Point lhPoint, FLT *ini
 			break;
 		}
 	}
-	printf(" Ri=%d ", i);
+	if (ttDebug) printf(" Ri=%d ", i);
 }
 static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint)
 {
@@ -1020,7 +1022,7 @@ static void WhereIsTheTrackedObjectQuaternion(FLT *rotation, Point lhPoint)
 	
 	//rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]);
 	quatrotatevector(objPoint, rotation, objPoint);
-	printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
+	if (ttDebug) printf("(%f, %f, %f)\n", objPoint[0], objPoint[1], objPoint[2]);
 }
 
 
@@ -1106,7 +1108,7 @@ 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;
-			printf("+");
+			if (ttDebug) printf("+");
 			WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint);
 		}
 		else
@@ -1115,12 +1117,12 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in
 			//printf("-         , %f\n", point4[3]);
 //#endif
 			g *= 0.7;
-			printf("-");
+			if (ttDebug) printf("-");
 		}
 
 
 	}
-	printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
+	if (ttDebug) printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
 }
 
 
@@ -1280,7 +1282,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	FLT fitGd = getPointFitness(refinedEstimateGd, pna, pnaCount, 0);
 
 	FLT distance = FLT_SQRT(SQUARED(refinedEstimateGd.x) + SQUARED(refinedEstimateGd.y) + SQUARED(refinedEstimateGd.z));
-	printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance);
+	if (ttDebug) printf(" la(% 04.4f) SnsrCnt(%2d) LhPos:(% 04.4f, % 04.4f, % 04.4f) Dist: % 08.8f ", largestAngle, (int)obj->numSensors, refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z, distance);
 	//printf("Distance is %f,   Fitness is %f\n", distance, fitGd);
 
 	FLT rot[4]; // this is axis/ angle rotation, not a quaternion!
@@ -1405,7 +1407,16 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	so->FromLHPose[lh].Rot[2] = so->OutPose.Rot[2];
 	so->FromLHPose[lh].Rot[3] = so->OutPose.Rot[3];
 
-	printf(" <% 04.4f, % 04.4f, % 04.4f >  ", wcPos[0], wcPos[1], wcPos[2]);
+	if (ttDebug) printf(" <% 04.4f, % 04.4f, % 04.4f >  ", wcPos[0], wcPos[1], wcPos[2]);
+
+	posOut[0] = wcPos[0];
+	posOut[1] = wcPos[1];
+	posOut[2] = wcPos[2];
+
+	quatOut[0] = so->OutPose.Rot[0];
+	quatOut[1] = so->OutPose.Rot[1];
+	quatOut[2] = so->OutPose.Rot[2];
+	quatOut[3] = so->OutPose.Rot[3];
 
 	if (logFile)
 	{
@@ -1418,6 +1429,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	toriData->lastLhPos[lh].y = refinedEstimateGd.y;
 	toriData->lastLhPos[lh].z = refinedEstimateGd.z;
 
+
 	return refinedEstimateGd;
 }
 
@@ -1528,7 +1540,10 @@ static void QuickPose(SurviveObject *so, int lh)
 
 
 			SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
-			printf("!\n");
+
+			printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+
+			if (ttDebug) printf("!\n");
 		}
 
 		
@@ -1547,6 +1562,14 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 	SurviveContext * ctx = so->ctx;
 	ToriData * td = so->PoserData;
 
+	static int firstRun = 1;
+
+	if (firstRun)
+	{
+		ttDebug = config_read_uint32(ctx->global_config_values, "TurveyToriDebug", 0);
+
+		firstRun = 0;
+	}
 
 	if (!td)
 	{
diff --git a/src/survive.c b/src/survive.c
index 057462f..97c839b 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -96,7 +96,8 @@ SurviveContext * survive_init( int headless )
 	}
 
 	i = 0;
-	const char * PreferredPoser = config_read_str( ctx->global_config_values, "DefaultPoser", "PoserDummy" );
+	//const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserDummy");
+	const char * PreferredPoser = config_read_str(ctx->global_config_values, "DefaultPoser", "PoserTurveyTori");
 	PoserCB PreferredPoserCB = 0;
 	const char * FirstPoser = 0;
 	printf( "Available posers:\n" );
@@ -119,6 +120,9 @@ SurviveContext * survive_init( int headless )
 		ctx->objs[i]->PoserFn = PreferredPoserCB;
 	}
 
+	// saving the config extra to make sure that the user has a config file they can change.
+	config_save(ctx, "config.json");
+
 	return ctx;
 }
 
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 0dbadd3..87d8c0b 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -124,8 +124,11 @@ void survive_cal_install( struct SurviveContext * ctx )
 
 	cd->numPoseObjects = 0;
 
-	const char * RequiredTrackersForCal = config_read_str( ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1" );
-	const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 0 );
+	// setting the required trackers for calibration to be permissive to make it easier for a newbie to start-- 
+	// basically, libsurvive will detect whatever they have plugged in and start using that.  
+//	const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", "HMD,WM0,WM1");	
+	const char * RequiredTrackersForCal = config_read_str(ctx->global_config_values, "RequiredTrackersForCal", ""); 
+	const uint32_t AllowAllTrackersForCal = config_read_uint32( ctx->global_config_values, "AllowAllTrackersForCal", 1 );
 	size_t requiredTrackersFound = 0;
 
 	for (int j=0; j < ctx->objs_ct; j++)
@@ -169,7 +172,8 @@ void survive_cal_install( struct SurviveContext * ctx )
 	}
 
 	const char * DriverName;
-	const char * PreferredPoser = config_read_str( ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow" );
+//	const char * PreferredPoser = config_read_str(ctx->global_config_values, "ConfigPoser", "PoserCharlesSlow");
+	const char * PreferredPoser = config_read_str(ctx->global_config_values, "ConfigPoser", "PoserTurveyTori");
 	PoserCB PreferredPoserCB = 0;
 	const char * FirstPoser = 0;
 	printf( "Available posers:\n" );
diff --git a/src/survive_config.c b/src/survive_config.c
index a24364f..0961651 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -103,7 +103,7 @@ void config_read_lighthouse(config_group* lh_config, BaseStationData* bsd, uint8
 		}
 	}
 
-	assert(found); // throw an assertion if we didn't find it...  Is this good?  not necessarily?
+//	assert(found); // throw an assertion if we didn't find it...  Is this good?  not necessarily?
 	if (!found)
 	{
 		return;
-- 
cgit v1.3.1


From ec564d70daa8c1a66018f9606b02b873ae792c84 Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Thu, 28 Dec 2017 08:03:37 -0700
Subject: 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.
---
 calibrate.c            |  66 +++++++++++++++++++---
 redist/linmath.c       |  36 ++++++++++++
 redist/linmath.h       |   2 +
 src/poser_turveytori.c | 150 ++++++++++++++++++++++++++++++++++++++++---------
 4 files changed, 218 insertions(+), 36 deletions(-)

(limited to 'src/poser_turveytori.c')

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};
-- 
cgit v1.3.1


From 6dea9ec51d21c9f58a6c82837b6dea0e67c20207 Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Thu, 28 Dec 2017 08:25:02 -0700
Subject: MUCH better tracking

Works with 2 lighthouses.
Tracking from both lighthouses agree *much* better than before
Inverting the tracker no longer screws up tracking
Still much work to do to remove all axis angle and
speed up/ make predictable the algorithm to estimate the rotation of the
LH relative to the tracked object.
---
 src/poser_turveytori.c | 13 ++++++++-----
 1 file changed, 8 insertions(+), 5 deletions(-)

(limited to 'src/poser_turveytori.c')

diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index fed5762..bd165ec 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1161,8 +1161,8 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in
 //#ifdef TORI_DEBUG
 			//printf("+  %8.8f, (%8.8f, %8.8f, %8.8f) %f\n", newMatchFitness, point4[0], point4[1], point4[2], point4[3]);
 //#endif
-			g *= 1.02;
-			 printf("+");
+			g *= 1.04;
+			 //printf("+");
 			//WhereIsTheTrackedObjectQuaternion(rotOut, lhPoint);
 		}
 		else
@@ -1171,7 +1171,8 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in
 			//printf("-         , %f\n", point4[3]);
 //#endif
 			g *= 0.7;
-			printf("-");
+			//printf("-");
+			//printf("%3f", lastMatchFitness);
 		}
 
 
@@ -1404,8 +1405,6 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	//	toriData->lastLhRotQuat[lh][3] = rotQuat[3];
 	//}
 
-	WhereIsTheTrackedObjectAxisAngle(objPos2, rot, refinedEstimateGd);
-	WhereIsTheTrackedObjectQuaternion(objPos, rotQuat, refinedEstimateGd);
 
 
 	FLT rotQuat2[4];
@@ -1415,6 +1414,10 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	axisanglefromquat(&(rot2[3]), rot2, rotQuat);
 
 
+//	WhereIsTheTrackedObjectAxisAngle(objPos, rot, refinedEstimateGd); // this is the original axis angle one
+	WhereIsTheTrackedObjectAxisAngle(objPos, rot2, refinedEstimateGd); // this one is axis angle, but using data derived by quaternions.
+	//	WhereIsTheTrackedObjectQuaternion(objPos, rotQuat, refinedEstimateGd);  <--------------This is hte one we need to use, might need to be fixed.
+
 	//{
 		//FLT tmpPos[3] = {refinedEstimateGd.x, refinedEstimateGd.y, refinedEstimateGd.z};
 
-- 
cgit v1.3.1


From 8eda10ed94b66a8bb388cc5710dc6b45b1901993 Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Thu, 28 Dec 2017 08:56:43 -0700
Subject: Use accelerometer to determine "up"

The accelerometer will be used to determine "up" instead of
blindly using +z during calibration
---
 src/poser_turveytori.c | 12 ++++++------
 1 file changed, 6 insertions(+), 6 deletions(-)

(limited to 'src/poser_turveytori.c')

diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index bd165ec..ac979c7 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1777,8 +1777,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);
 
 					//rotatearoundaxis(norm, norm, axis, angle);
 					//rotatearoundaxis(point, point, axis, angle);
@@ -1813,8 +1813,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);
 
 					//rotatearoundaxis(norm, norm, axis, angle);
 					//rotatearoundaxis(point, point, axis, angle);
@@ -1840,9 +1840,9 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 		}
 
 
-		// This code block rotates the lighthouse fixes to accound for any time the tracked object
+		// This code block rotates the lighthouse fixes to account for any time the tracked object
 		// is oriented other than +z = up
-		// This REALLY DOESN'T WORK!!!
+		 //This REALLY DOESN'T WORK!!!
 		//{
 		//	for (int lh = 0; lh < 2; lh++)
 		//	{
-- 
cgit v1.3.1


From 735a8bd11070b0c563e891ff8b70ce297a52a367 Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Fri, 5 Jan 2018 03:54:29 -0700
Subject: Add standard output mechanism for posers

Added a raw pose output/ callback that the posers can
call when they have calculated a pose.
---
 include/libsurvive/survive.h       |  5 ++++-
 include/libsurvive/survive_types.h |  3 ++-
 src/poser_turveytori.c             |  8 ++++++--
 src/survive.c                      |  9 ++++++++-
 src/survive_data.c                 |  4 ++--
 src/survive_process.c              | 39 +++++++++++++++++++++++++++++---------
 6 files changed, 52 insertions(+), 16 deletions(-)

(limited to 'src/poser_turveytori.c')

diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 0cfab1f..4821e63 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -129,6 +129,7 @@ struct SurviveContext
 	imu_process_func imuproc;
 	angle_process_func angleproc;
 	button_process_func buttonproc;
+	raw_pose_func rawposeproc;
 
 	struct config_group* global_config_values;
 	struct config_group* lh_config; //lighthouse configs
@@ -163,7 +164,8 @@ void survive_install_error_fn( SurviveContext * ctx,  text_feedback_func fbp );
 void survive_install_light_fn( SurviveContext * ctx,  light_process_func fbp );
 void survive_install_imu_fn( SurviveContext * ctx,  imu_process_func fbp );
 void survive_install_angle_fn( SurviveContext * ctx,  angle_process_func fbp );
-void survive_install_button_fn( SurviveContext * ctx, button_process_func fbp );
+void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp);
+void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp);
 
 void survive_close( SurviveContext * ctx );
 int survive_poll( SurviveContext * ctx );
@@ -190,6 +192,7 @@ void survive_default_light_process( SurviveObject * so, int sensor_id, int acode
 void survive_default_imu_process( SurviveObject * so, int mode, FLT * accelgyro, uint32_t timecode, int id );
 void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh );
 void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
+void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion);
 
 
 ////////////////////// Survive Drivers ////////////////////////////
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index 9a6e148..5384345 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -42,7 +42,8 @@ typedef void (*text_feedback_func)( SurviveContext * ctx, const char * fault );
 typedef void (*light_process_func)( SurviveObject * so, int sensor_id, int acode, int timeinsweep, uint32_t timecode, uint32_t length, uint32_t lighthouse);
 typedef void (*imu_process_func)( SurviveObject * so, int mask, FLT * accelgyro, uint32_t timecode, int id );
 typedef void (*angle_process_func)( SurviveObject * so, int sensor_id, int acode, uint32_t timecode, FLT length, FLT angle, uint32_t lh);
-typedef void (*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
+typedef void(*button_process_func)(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val);
+typedef void(*raw_pose_func)(SurviveObject * so, uint8_t lighthouse, FLT *position, FLT *quaternion);
 
 typedef int(*haptic_func)(SurviveObject * so, uint8_t reserved, uint16_t pulseHigh , uint16_t pulseLow, uint16_t repeatCount);
 
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index ac979c7..28e533e 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1177,7 +1177,7 @@ static void RefineRotationEstimateQuaternion(FLT *rotOut, Point lhPoint, FLT *in
 
 
 	}
-	printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
+	if (ttDebug) printf("Ri=%3d  Fitness=%3f ", i, lastMatchFitness);
 }
 
 
@@ -1638,7 +1638,11 @@ static void QuickPose(SurviveObject *so, int lh)
 
 			SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
 
-			printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+			//printf("P&O: [% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+			if (so->ctx->rawposeproc)
+			{
+				so->ctx->rawposeproc(so, lh, pos, quat);
+			}
 
 			if (ttDebug) printf("!\n");
 		}
diff --git a/src/survive.c b/src/survive.c
index 75c07bf..1c406e8 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -184,6 +184,7 @@ SurviveContext * survive_init( int headless )
 	// start the thread to process button data
 	ctx->buttonservicethread = OGCreateThread(button_servicer, ctx);
 	survive_install_button_fn(ctx, NULL);
+	survive_install_raw_pose_fn(ctx, NULL);
 
 	return ctx;
 }
@@ -235,9 +236,15 @@ void survive_install_button_fn(SurviveContext * ctx, button_process_func fbp)
 		ctx->buttonproc = fbp;
 	else
 		ctx->buttonproc = survive_default_button_process;
-
 }
 
+void survive_install_raw_pose_fn(SurviveContext * ctx, raw_pose_func fbp)
+{
+	if (fbp)
+		ctx->rawposeproc = fbp;
+	else
+		ctx->rawposeproc = survive_default_raw_pose_process;
+}
 int survive_add_object( SurviveContext * ctx, SurviveObject * obj )
 {
 	int oldct = ctx->objs_ct;
diff --git a/src/survive_data.c b/src/survive_data.c
index 1b80269..0427659 100644
--- a/src/survive_data.c
+++ b/src/survive_data.c
@@ -419,8 +419,8 @@ void handle_lightcap2_sweep(SurviveObject * so, LightcapElement * le )
 	}
 
 	if (lcd->per_sweep.activeLighthouse < 0) {
-		fprintf(stderr, "WARNING: No active lighthouse!\n");
-		fprintf(stderr, "            %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]);
+		//fprintf(stderr, "WARNING: No active lighthouse!\n");
+		//fprintf(stderr, "            %2d %8d %d %d\n", le->sensor_id, le->length,lcd->per_sweep.lh_acode[0],lcd->per_sweep.lh_acode[1]);
 		return;
 	}
 
diff --git a/src/survive_process.c b/src/survive_process.c
index 4b86144..42d2897 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -66,20 +66,34 @@ void survive_default_angle_process( SurviveObject * so, int sensor_id, int acode
 void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8_t buttonId, uint8_t axis1Id, uint16_t axis1Val, uint8_t axis2Id, uint16_t axis2Val)
 {
 	// do nothing.
-	//printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n",
-	//	eventType,
-	//	buttonId,
-	//	axis1Id,
-	//	axis1Val,
-	//	axis2Id,
-	//	axis2Val);
+	printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n",
+		eventType,
+		buttonId,
+		axis1Id,
+		axis1Val,
+		axis2Id,
+		axis2Val);
 	if (buttonId == 24 && eventType == 1) // trigger engage
 	{
-		for (int j = 0; j < 40; j++)
+		for (int j = 0; j < 6; j++)
 		{
-			for (int i = 0; i < 0x1; i++)
+			for (int i = 0; i < 0x5; i++)
 			{
 				survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+				//survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
+				OGUSleep(1000);
+			}
+			OGUSleep(20000);
+		}
+	}
+	if (buttonId == 2 && eventType == 1) // trigger engage
+	{
+		for (int j = 0; j < 6; j++)
+		{
+			for (int i = 0; i < 0x1; i++)
+			{
+				survive_haptic(so, 0, 0xf401, 0x05a2, 0xf100);
+				//survive_haptic(so, 0, 0xf401, 0xb5a2, 0x0100);
 				OGUSleep(5000);
 			}
 			OGUSleep(20000);
@@ -87,6 +101,13 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8
 	}
 }
 
+void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat)
+{
+	// print the pose;
+	printf("Pose: [%2.2x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+
+}
+
 void survive_default_imu_process( SurviveObject * so, int mask, FLT * accelgyromag, uint32_t timecode, int id )
 {
 	if( so->PoserFn )
-- 
cgit v1.3.1


From ed85661121c3ed8769a69d059fd3162aec33c6fe Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Fri, 5 Jan 2018 04:09:54 -0700
Subject: Update test.c to do calibration

---
 src/poser_turveytori.c | 12 ++++++++++++
 src/survive_process.c  |  2 +-
 test.c                 |  2 ++
 3 files changed, 15 insertions(+), 1 deletion(-)

(limited to 'src/poser_turveytori.c')

diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 28e533e..ad30f6c 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1856,6 +1856,18 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 		//	}
 		//}
 
+		for (int i=0; i < ctx->activeLighthouses; i++)
+		{ 
+			printf("Lighthouse Pose: [%1.1x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n",
+				i,
+				ctx->bsd[i].Pose.Pos[0],
+				ctx->bsd[i].Pose.Pos[1],
+				ctx->bsd[i].Pose.Pos[2],
+				ctx->bsd[i].Pose.Rot[0],
+				ctx->bsd[i].Pose.Rot[1],
+				ctx->bsd[i].Pose.Rot[2],
+				ctx->bsd[i].Pose.Rot[3]);
+		}
 		config_set_lighthouse(ctx->lh_config, ctx->bsd[0], 0);
 		config_set_lighthouse(ctx->lh_config, ctx->bsd[1], 1);
 
diff --git a/src/survive_process.c b/src/survive_process.c
index 42d2897..1df24e9 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -104,7 +104,7 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8
 void survive_default_raw_pose_process(SurviveObject * so, uint8_t lighthouse, FLT *pos, FLT *quat)
 {
 	// print the pose;
-	printf("Pose: [%2.2x][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
+	printf("Pose: [%1.1x][%s][% 08.8f,% 08.8f,% 08.8f] [% 08.8f,% 08.8f,% 08.8f,% 08.8f]\n", lighthouse, so->codename, pos[0], pos[1], pos[2], quat[0], quat[1], quat[2], quat[3]);
 
 }
 
diff --git a/test.c b/test.c
index 4909d50..c9f03d7 100644
--- a/test.c
+++ b/test.c
@@ -85,6 +85,8 @@ int main()
 	dump_iface( survive_get_so_by_name( ctx, "TR0" ), "TR0" );
 	dump_iface( survive_get_so_by_name( ctx, "WW0" ), "WW0" );
 
+	survive_cal_install(ctx);
+
 	while(survive_poll(ctx) == 0)
 	{
 		double Now = OGGetAbsoluteTime();
-- 
cgit v1.3.1


From 91fc90bf9171fe588fd11492bdae7419d5ccd921 Mon Sep 17 00:00:00 2001
From: Christoph Haag <haagch@frickel.club>
Date: Mon, 15 Jan 2018 17:10:15 +0100
Subject: fix segfault in config_set_lighthouse()

also include the survive_config.h header to avoid various
implicit function declaration warnings on gcc
---
 src/poser_turveytori.c | 5 +++--
 1 file changed, 3 insertions(+), 2 deletions(-)

(limited to 'src/poser_turveytori.c')

diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index ad30f6c..59ff25e 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1,4 +1,5 @@
 #include <survive.h>
+#include <src/survive_config.h>
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -1868,8 +1869,8 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 				ctx->bsd[i].Pose.Rot[2],
 				ctx->bsd[i].Pose.Rot[3]);
 		}
-		config_set_lighthouse(ctx->lh_config, ctx->bsd[0], 0);
-		config_set_lighthouse(ctx->lh_config, ctx->bsd[1], 1);
+		config_set_lighthouse(ctx->lh_config, &ctx->bsd[0], 0);
+		config_set_lighthouse(ctx->lh_config, &ctx->bsd[1], 1);
 
 		config_save(ctx, "config.json");
 
-- 
cgit v1.3.1


From 7817da63526f35d10d6d4f6b3d9c02280719e023 Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Mon, 15 Jan 2018 21:50:10 -0700
Subject: Fix compiler warnings

---
 include/libsurvive/survive.h |  5 ++---
 redist/json_helpers.c        |  2 +-
 src/poser_octavioradii.c     |  2 +-
 src/poser_turveytori.c       |  5 ++---
 src/survive.c                | 10 ++++++----
 src/survive_config.c         |  2 +-
 6 files changed, 13 insertions(+), 13 deletions(-)

(limited to 'src/poser_turveytori.c')

diff --git a/include/libsurvive/survive.h b/include/libsurvive/survive.h
index 4821e63..3d0d472 100644
--- a/include/libsurvive/survive.h
+++ b/include/libsurvive/survive.h
@@ -4,7 +4,6 @@
 #include <stdint.h>
 #include "survive_types.h"
 #include "poser.h"
-#include "os_generic.h"
 
 #ifdef __cplusplus
 extern "C" {
@@ -117,7 +116,7 @@ typedef struct
 {
 	uint8_t nextReadIndex; //init to 0
 	uint8_t nextWriteIndex; // init to 0
-	og_sema_t buttonservicesem;
+	void* buttonservicesem;
 	ButtonQueueEntry entry[BUTTON_QUEUE_MAX_LEN];
 } ButtonQueue;
 
@@ -150,7 +149,7 @@ struct SurviveContext
 
 	uint8_t isClosing; // flag to indicate if threads should terminate themselves
 
-	og_thread_t buttonservicethread;
+	void* buttonservicethread;
 	ButtonQueue buttonQueue;
 
 };
diff --git a/redist/json_helpers.c b/redist/json_helpers.c
index af7ddda..8da841b 100644
--- a/redist/json_helpers.c
+++ b/redist/json_helpers.c
@@ -121,7 +121,7 @@ char* load_file_to_mem(const char* path) {
 	fseek( f, 0, SEEK_SET );
 	char * JSON_STRING = malloc( len + 1);
 	memset(JSON_STRING,0,len+1); 
-	int i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value.
+	size_t i = fread( JSON_STRING, len, 1, f ); i = i; //Ignore return value.
 	fclose( f );
 	return JSON_STRING;
 }
diff --git a/src/poser_octavioradii.c b/src/poser_octavioradii.c
index be1e9f7..5805e1a 100644
--- a/src/poser_octavioradii.c
+++ b/src/poser_octavioradii.c
@@ -591,7 +591,7 @@ int PoserOctavioRadii( SurviveObject * so, PoserData * pd )
 				}
 				else
 				{
-					dd->hitCount[i][l->lh][axis] *= 0.5;
+					dd->hitCount[i][l->lh][axis] = (int)((double)dd->hitCount[i][l->lh][axis] * 0.5);
 				}
 			}
 			//if (0 == l->lh)
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 59ff25e..de1516a 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1,5 +1,5 @@
 #include <survive.h>
-#include <src/survive_config.h>
+#include "survive_config.h"
 #include <stdio.h>
 #include <stdlib.h>
 #include <string.h>
@@ -785,7 +785,6 @@ FLT RotationEstimateFitnessQuaternion(Point lhPoint, FLT *quaternion, TrackedObj
 
 		FLT throwaway = RotationEstimateFitnessAxisAngle(lhPoint, axisAngle, obj);
 
-		int a = throwaway;
 		return throwaway;
 	}
 
@@ -1397,7 +1396,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	SolveForRotationQuat(rotQuat, obj, refinedEstimateGd);
 	SolveForRotation(rot, obj, refinedEstimateGd);
 	FLT objPos[3];
-	FLT objPos2[3];
+	//FLT objPos2[3];
 
 	//{
 	//	toriData->lastLhRotQuat[lh][0] = rotQuat[0];
diff --git a/src/survive.c b/src/survive.c
index a5ca68f..0386275 100755
--- a/src/survive.c
+++ b/src/survive.c
@@ -8,6 +8,7 @@
 #include <string.h>
 
 #include "survive_config.h"
+#include "os_generic.h"
 
 #ifdef __APPLE__
 #define z_const const
@@ -40,7 +41,7 @@ static void survivenote( struct SurviveContext * ctx, const char * fault )
 	fprintf( stderr, "Info: %s\n", fault );
 }
 
-static void button_servicer(void * context)
+static void *button_servicer(void * context)
 {
 	SurviveContext *ctx = (SurviveContext*)context;
 
@@ -51,7 +52,7 @@ static void button_servicer(void * context)
 		if (ctx->isClosing)
 		{
 			// we're shutting down.  Close.
-			return;
+			return NULL;
 		}
 
 		ButtonQueueEntry *entry = &(ctx->buttonQueue.entry[ctx->buttonQueue.nextReadIndex]);
@@ -61,7 +62,7 @@ static void button_servicer(void * context)
 			// the buttonQueue
 			// if it does happen, it will kill all future button input
 			printf("ERROR: Unpopulated ButtonQueueEntry! NextReadIndex=%d\n", ctx->buttonQueue.nextReadIndex);
-			return; 
+			return NULL;
 		}
 
 		//printf("ButtonEntry: eventType:%x, buttonId:%d, axis1:%d, axis1Val:%8.8x, axis2:%d, axis2Val:%8.8x\n",
@@ -90,6 +91,7 @@ static void button_servicer(void * context)
 			ctx->buttonQueue.nextReadIndex = 0;
 		}
 	};
+	return NULL;
 }
 
 SurviveContext * survive_init( int headless )
@@ -182,7 +184,7 @@ SurviveContext * survive_init( int headless )
 	ctx->buttonQueue.buttonservicesem = OGCreateSema();	
 
 	// start the thread to process button data
-	ctx->buttonservicethread = OGCreateThread(&button_servicer, ctx);
+	ctx->buttonservicethread = OGCreateThread(button_servicer, ctx);
 	survive_install_button_fn(ctx, NULL);
 	survive_install_raw_pose_fn(ctx, NULL);
 
diff --git a/src/survive_config.c b/src/survive_config.c
index 0961651..46c6658 100644
--- a/src/survive_config.c
+++ b/src/survive_config.c
@@ -187,7 +187,7 @@ uint16_t config_read_float_array(config_group *cg, const char *tag, FLT* values,
 	config_entry *cv = find_config_entry(cg, tag);
 
 	if (cv != NULL) {
-		for (int i=0; i < CFG_MIN(count, cv->elements); i++)
+		for (unsigned int i=0; i < CFG_MIN(count, cv->elements); i++)
 		{
 			values[i] = ((double*)cv->data)[i];
 		}
-- 
cgit v1.3.1


From b2eb7569a8963917116c4520e15b17f0578a2509 Mon Sep 17 00:00:00 2001
From: Mike Turvey <mturvey6@gmail.com>
Date: Mon, 15 Jan 2018 22:40:14 -0700
Subject: Fix a few warnings

---
 redist/CNFGWinDriver.c | 5 ++---
 redist/json_helpers.c  | 2 +-
 src/ootx_decoder.c     | 2 +-
 src/poser_turveytori.c | 5 +++--
 4 files changed, 7 insertions(+), 7 deletions(-)

(limited to 'src/poser_turveytori.c')

diff --git a/redist/CNFGWinDriver.c b/redist/CNFGWinDriver.c
index b1c1eb0..9d1be6a 100644
--- a/redist/CNFGWinDriver.c
+++ b/redist/CNFGWinDriver.c
@@ -54,8 +54,8 @@ void CNFGGetDimensions( short * x, short * y )
 		lasty = buffery;
 		InternalHandleResize();
 	}
-	*x = bufferx;
-	*y = buffery;
+	*x = (short)bufferx;
+	*y = (short)buffery;
 }
 
 
@@ -212,7 +212,6 @@ void CNFGSetup( const char * name_of_window, int width, int height )
 
 void CNFGHandleInput()
 {
-	int ldown = 0;
 
 	MSG msg;
 	while( PeekMessage( &msg, lsHWND, 0, 0xFFFF, 1 ) )
diff --git a/redist/json_helpers.c b/redist/json_helpers.c
index 8da841b..109708a 100644
--- a/redist/json_helpers.c
+++ b/redist/json_helpers.c
@@ -193,7 +193,7 @@ void json_load_file(const char* path) {
 		} else if (value_t->type == JSMN_OBJECT) {
 			printf("Begin Object\n");
 			if (json_begin_object != NULL) json_begin_object(tag);
-			children = value_t->size +1; //+1 to account for this loop where we are not yed parsing children
+			children = (int16_t)(value_t->size +1); //+1 to account for this loop where we are not yed parsing children
 //			i += decode_jsmn_object(JSON_STRING, tokens+i+2,value_t->size);
 		}
 		else {
diff --git a/src/ootx_decoder.c b/src/ootx_decoder.c
index f7a7938..7bf7d7e 100644
--- a/src/ootx_decoder.c
+++ b/src/ootx_decoder.c
@@ -44,7 +44,7 @@ void ootx_free_decoder_context(ootx_decoder_context *ctx) {
 }
 
 uint8_t ootx_decode_bit(uint32_t length) {
-	uint8_t t = (length - 2750) / 500; //why 2750?
+	uint8_t t = (uint8_t)((length - 2750) / 500); //why 2750?
 //	return ((t & 0x02)>0)?0xFF:0x00; //easier if we need to bitshift right
 	return ((t & 0x02)>>1);
 }
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index de1516a..94d572e 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1050,7 +1050,7 @@ static void WhereIsTheTrackedObjectQuaternion(FLT *posOut, FLT *rotation, Point
 
 	quatgetreciprocal(inverseRotation, rotation);
 
-	FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z };
+	//FLT objPoint[3] = { lhPoint.x, lhPoint.y, lhPoint.z };
 
 	//rotatearoundaxis(objPoint, objPoint, reverseRotation, reverseRotation[3]);
 	quatrotatevector(posOut, inverseRotation, posOut);
@@ -1186,7 +1186,7 @@ void SolveForRotation(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 };
+	//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];
@@ -1200,6 +1200,7 @@ void SolveForRotation(FLT rotOut[4], TrackedObject *obj, Point lh)
 	RefineRotationEstimateAxisAngle(rotOut, lh, zAxis, obj);
 
 
+	// TODO:  Need to use the quaternion version here!!!
 	//// Step 2, optimize the quaternion to match the data.
 	//RefineRotationEstimateQuaternion(rotOut, lh, quat1, obj);
 
-- 
cgit v1.3.1