Refactor pose function to get timecode and not lh

7 files changed, 38 insertions, 20 deletions

diff --git a/src/poser.c b/src/poser.c
index 9a0de24..20334f7 100644
--- a/src/poser.c
+++ b/src/poser.c
@@ -7,11 +7,29 @@
 #define _USE_MATH_DEFINES // for C
 #include <math.h>
 
-void PoserData_poser_raw_pose_func(PoserData *poser_data, SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
-	if (poser_data->rawposeproc) {
-		poser_data->rawposeproc(so, lighthouse, pose, poser_data->userdata);
+static uint32_t PoserData_timecode(PoserData *poser_data) {
+	switch (poser_data->pt) {
+	case POSERDATA_LIGHT: {
+		PoserDataLight *lightData = (PoserDataLight *)poser_data;
+		return lightData->timecode;
+	}
+	case POSERDATA_FULL_SCENE: {
+		PoserDataFullScene* pdfs = (PoserDataFullScene *)(poser_data);
+		return -1;
+	}
+	case POSERDATA_IMU: {
+		PoserDataIMU *imuData = (PoserDataIMU *)poser_data;
+		return imuData->timecode;
+	}
+	}
+	return -1;
+}
+
+void PoserData_poser_pose_func(PoserData *poser_data, SurviveObject *so, SurvivePose *pose) {
+	if (poser_data->poseproc) {
+		poser_data->poseproc(so, PoserData_timecode(poser_data), pose, poser_data->userdata);
 	} else {
-		so->ctx->rawposeproc(so, lighthouse, pose);
+		so->ctx->poseproc(so, PoserData_timecode(poser_data), pose);
 	}
 }
 
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index 9cdab45..330e7e8 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -107,7 +107,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 
 				SurvivePose obj2world;
 				ApplyPoseToPose(&obj2world, &lh2world, &objpose);
-				PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world);
+				PoserData_poser_pose_func(pd, so, &obj2world);
 
 				if (0) {
 					fprintf(stderr,"INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2],
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index c05450a..eaa1659 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -164,7 +164,7 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
 
 			if (winnerTakesAll) {
 				int winner = meas[0] > meas[1] ? 0 : 1;
-				PoserData_poser_raw_pose_func(pd, so, winner, &posers[winner]);
+				PoserData_poser_pose_func(pd, so, &posers[winner]);
 			} else {
 				double a, b;
 				a = meas[0] * meas[0];
@@ -175,11 +175,11 @@ int PoserEPNP(SurviveObject *so, PoserData *pd) {
 					interpolate.Pos[i] = (posers[0].Pos[i] * a + posers[1].Pos[i] * b) / (t);
 				}
 				quatslerp(interpolate.Rot, posers[0].Rot, posers[1].Rot, b / (t));
-				PoserData_poser_raw_pose_func(pd, so, lightData->lh, &interpolate);
+				PoserData_poser_pose_func(pd, so, &interpolate);
 			}
 		} else {
 			if (meas[lightData->lh])
-				PoserData_poser_raw_pose_func(pd, so, lightData->lh, &posers[lightData->lh]);
+				PoserData_poser_pose_func(pd, so, &posers[lightData->lh]);
 		}
 		return 0;
 	}
diff --git a/src/poser_sba.c b/src/poser_sba.c
index bd7d520..e74bb20 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -129,7 +129,7 @@ typedef struct {
 	SurvivePose poses;
 } sba_set_position_t;
 
-static void sba_set_position(SurviveObject *so, uint8_t lighthouse, SurvivePose *new_pose, void *_user) {
+static void sba_set_position(SurviveObject *so, uint32_t timecode, SurvivePose *new_pose, void *_user) {
 	sba_set_position_t *user = _user;
 	assert(user->hasInfo == false);
 	user->hasInfo = 1;
@@ -220,7 +220,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 			PoserData hdr = pdl->hdr;
 			memset(&pdl->hdr, 0, sizeof(pdl->hdr)); // Clear callback functions
 			pdl->hdr.pt = hdr.pt;
-			pdl->hdr.rawposeproc = sba_set_position;
+			pdl->hdr.poseproc = sba_set_position;
 
 			sba_set_position_t locations = {0};
 			pdl->hdr.userdata = &locations;
@@ -278,7 +278,7 @@ static double run_sba_find_3d_structure(SBAData *d, PoserDataLight *pdl, Survive
 	if (status > 0 && (info[1] / meas_size * 2) < d->max_error) {
 		d->failures_to_reset_cntr = d->failures_to_reset;
 		quatnormalize(soLocation.Rot, soLocation.Rot);
-		PoserData_poser_raw_pose_func(&pdl->hdr, so, 1, &soLocation);
+		PoserData_poser_pose_func(&pdl->hdr, so, &soLocation);
 	}
 
 	{
diff --git a/src/poser_turveytori.c b/src/poser_turveytori.c
index 4628207..243051e 100644
--- a/src/poser_turveytori.c
+++ b/src/poser_turveytori.c
@@ -1631,9 +1631,9 @@ static void QuickPose(SurviveObject *so, PoserData *pd, SurvivePose *additionalT
 			SolveForLighthouse(&pose.Pos[0], &pose.Rot[0], to, so, pd, 0, additionalTx, 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)
+			if (so->ctx->poseproc)
 			{
-				so->ctx->rawposeproc(so, lh, &pose);
+				so->ctx->poseproc(so, lh, &pose);
 			}
 
 			if (ttDebug) printf("!\n");
diff --git a/src/survive.c b/src/survive.c
index f74de85..f6f98fc 100644
--- a/src/survive.c
+++ b/src/survive.c
@@ -217,7 +217,7 @@ SurviveContext *survive_init_internal(int argc, char *const *argv) {
 	ctx->angleproc = survive_default_angle_process;
 	ctx->lighthouseposeproc = survive_default_lighthouse_pose_process;
 	ctx->configfunction = survive_default_htc_config_process;
-	ctx->rawposeproc = survive_default_raw_pose_process;
+	ctx->poseproc = survive_default_raw_pose_process;
 
 	ctx->calibration_config = survive_calibration_config_ctor();
 	ctx->calibration_config.use_flag = (enum SurviveCalFlag)survive_configi(ctx, "bsd-cal", SC_GET, SVCal_All);
@@ -381,11 +381,11 @@ void survive_install_button_fn(SurviveContext *ctx, button_process_func fbp) {
 		ctx->buttonproc = survive_default_button_process;
 }
 
-void survive_install_raw_pose_fn(SurviveContext *ctx, raw_pose_func fbp) {
+void survive_install_pose_fn(SurviveContext *ctx, pose_func fbp) {
 	if (fbp)
-		ctx->rawposeproc = fbp;
+		ctx->poseproc = fbp;
 	else
-		ctx->rawposeproc = survive_default_raw_pose_process;
+		ctx->poseproc = survive_default_raw_pose_process;
 }
 
 void survive_install_lighthouse_pose_fn(SurviveContext *ctx, lighthouse_pose_func fbp) {
diff --git a/src/survive_process.c b/src/survive_process.c
index 62459f2..e013327 100644
--- a/src/survive_process.c
+++ b/src/survive_process.c
@@ -139,12 +139,12 @@ void survive_default_button_process(SurviveObject * so, uint8_t eventType, uint8
 	//}
 }
 
-void survive_default_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePose *pose) {
+void survive_default_raw_pose_process(SurviveObject *so, uint32_t timecode, SurvivePose *pose) {
 	// print the pose;
 	//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]);
 	so->OutPose = *pose;
-	so->FromLHPose[lighthouse] = *pose;
-	survive_recording_raw_pose_process(so, lighthouse, pose);
+	so->OutPose_timecode = timecode;
+	survive_recording_raw_pose_process(so, timecode, pose);
 }
 
 void survive_default_lighthouse_pose_process(SurviveContext *ctx, uint8_t lighthouse, SurvivePose *lighthouse_pose,