Redirected posers to used passed in callbacks

6 files changed, 41 insertions, 26 deletions

diff --git a/Makefile b/Makefile
index 332f59d..b9cfbdc 100644
--- a/Makefile
+++ b/Makefile
@@ -37,7 +37,7 @@ REDISTS:=redist/json_helpers.o redist/linmath.o redist/jsmn.o redist/os_generic.
 ifeq ($(UNAME), Darwin)
 REDISTS:=$(REDISTS) redist/hid-osx.c
 endif
-LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o
+LIBSURVIVE_CORE:=src/survive.o src/survive_usb.o src/survive_data.o src/survive_process.o src/ootx_decoder.o src/survive_driverman.o src/survive_default_devices.o src/survive_vive.o src/survive_playback.o src/survive_config.o src/survive_cal.o src/survive_reproject.o src/poser.o
 
 
 #If you want to use HIDAPI on Linux.
diff --git a/include/libsurvive/poser.h b/include/libsurvive/poser.h
index 7e05184..57a8599 100644
--- a/include/libsurvive/poser.h
+++ b/include/libsurvive/poser.h
@@ -17,11 +17,20 @@ typedef enum PoserType_t
 	POSERDATA_DISASSOCIATE, //If you get this, it doesn't contain data.  It just tells you to please disassociate from the current SurviveObject and delete your poserdata.
 } PoserType;
 
+typedef void (*poser_raw_pose_func)(SurviveObject *so, uint8_t lighthouse, FLT *pose, void *user);
+typedef void (*poser_lighthouse_pose_func)(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose, void *user);
+
 typedef struct
 {
 	PoserType pt;
+	poser_raw_pose_func rawposeproc;
+	poser_lighthouse_pose_func lighthouseposeproc;
+	void *userdata;
 } PoserData;
 
+void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, FLT *pose);
+void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose);
+
 typedef struct
 {
 	PoserData hdr;
diff --git a/src/poser.c b/src/poser.c
new file mode 100644
index 0000000..73fc7e8
--- /dev/null
+++ b/src/poser.c
@@ -0,0 +1,20 @@
+#include <stdint.h>
+
+#include "poser.h"
+#include "survive_internal.h"
+
+void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, FLT *pose) {
+	if (poser_data->rawposeproc) {
+		poser_data->rawposeproc(so, lighthouse, pose, poser_data->userdata);
+	} else {
+		so->ctx->rawposeproc(so, lighthouse, pose);
+	}
+}
+
+void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
+	if (poser_data->lighthouseposeproc) {
+		poser_data->lighthouseposeproc(so, lighthouse, pose, poser_data->userdata);
+	} else {
+		so->ctx->lighthouseposeproc(so->ctx, lighthouse, pose);
+	}
+}
diff --git a/src/poser_charlesslow.c b/src/poser_charlesslow.c
index 30fc677..0839d59 100644
--- a/src/poser_charlesslow.c
+++ b/src/poser_charlesslow.c
@@ -174,9 +174,7 @@ int PoserCharlesSlow( SurviveObject * so, PoserData * pd )
 			quatrotateabout(tmp, lighthousePose.Rot, rt);
 			memcpy(lighthousePose.Rot, tmp, sizeof(FLT) * 4);
 
-			if (ctx->lighthouseposeproc) {
-				ctx->lighthouseposeproc(ctx, lh, &lighthousePose);
-			}
+			PoserData_lighthouse_pose_func(pd, so, lh, &lighthousePose);
 #define ALT_COORDS
 
 #ifdef ALT_COORDS
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index ddc4ad2..db46b09 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1234,9 +1234,8 @@ void SolveForRotationQuat(FLT rotOut[4], TrackedObject *obj, Point lh)
 
 }
 
-
-static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, char doLogOutput,const int lh,const int setLhCalibration)
-{
+static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *obj, SurviveObject *so, PoserData *pd,
+								char doLogOutput, const int lh, const int setLhCalibration) {
 	ToriData *toriData = so->PoserData;
 
 	//printf("Solving for Lighthouse\n");
@@ -1444,9 +1443,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 		lighthousePose.Pos[1] = refinedEstimateGd.y;
 		lighthousePose.Pos[2] = refinedEstimateGd.z;
 
-		if (so->ctx->lighthouseposeproc) {
-			so->ctx->lighthouseposeproc(so->ctx, lh, &lighthousePose);
-		}
+		PoserData_lighthouse_pose_func(pd, so, lh, &lighthousePose);
 	}
 
 
@@ -1531,16 +1528,7 @@ static Point SolveForLighthouse(FLT posOut[3], FLT quatOut[4], TrackedObject *ob
 	return refinedEstimateGd;
 }
 
-
-
-
-
-
-
-
-static void QuickPose(SurviveObject *so, int lh)
-{
-
+static void QuickPose(SurviveObject *so, PoserData *pd, int lh) {
 
 	ToriData * td = so->PoserData;
 
@@ -1634,7 +1622,7 @@ static void QuickPose(SurviveObject *so, int lh)
 			//	SolveForLighthouse(pos, quat, to, so, 0, lh, 0);
 			//}
 
-			SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, 0, lh, 0);
+	                SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 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]);
 			if (so->ctx->rawposeproc)
@@ -1717,7 +1705,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 
 				// let's just do this occasionally for now...
 				if (counter % 4 == 0)
-					QuickPose(so, 0);
+					QuickPose(so, poserData, 0);
 			}
 			if (1 == l->lh && axis) // only once per full cycle...
 			{
@@ -1727,7 +1715,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 
 				// let's just do this occasionally for now...
 				if (counter % 4 == 0)
-					QuickPose(so, 1);
+					QuickPose(so, poserData, 1);
 			}
 			// axis changed, time to increment the circular buffer index.
 			td->angleIndex[l->lh][axis]++;
@@ -1802,7 +1790,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 
 			FLT pos[3], quat[4];
 
-			SolveForLighthouse(pos, quat, to, so, 0, 0, 1);
+			SolveForLighthouse(pos, quat, to, so, poserData, 0, 0, 1);
 		}
 		{
 			int sensorCount = 0;
@@ -1838,7 +1826,7 @@ int PoserTurveyTori( SurviveObject * so, PoserData * poserData )
 
 			FLT pos[3], quat[4];
 
-			SolveForLighthouse(pos, quat, to, so, 0, 1, 1);
+			SolveForLighthouse(pos, quat, to, so, poserData, 0, 1, 1);
 		}
 
 
diff --git a/src/survive_cal.c b/src/survive_cal.c
index 26ed1ad..fb3b240 100755
--- a/src/survive_cal.c
+++ b/src/survive_cal.c
@@ -603,7 +603,7 @@ static void handle_calibration( struct SurviveCalData *cd )
 	for( obj = 0; obj < cd->numPoseObjects; obj++ )
 	{
 		int i, j;
-		PoserDataFullScene fsd;
+		PoserDataFullScene fsd = {};
 		fsd.hdr.pt = POSERDATA_FULL_SCENE;
 		for( j = 0; j < NUM_LIGHTHOUSES; j++ )
 		for( i = 0; i < SENSORS_PER_OBJECT; i++ )