Moved pose to linmath to support stronger typing

5 files changed, 16 insertions, 72 deletions

diff --git a/src/poser.c b/src/poser.c
index 1ed63da..3fb4fe8 100644
--- a/src/poser.c
+++ b/src/poser.c
@@ -38,17 +38,17 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui
 
 		// Start by just moving from whatever arbitrary space into object space.
 		SurvivePose arb2object;
-		InvertPose(arb2object.Pos, object2arb.Pos);
+		InvertPose(&arb2object, &object2arb);
 
 		SurvivePose lighthouse2obj;
-		ApplyPoseToPose(lighthouse2obj.Pos, arb2object.Pos, lighthouse2arb.Pos);
+		ApplyPoseToPose(&lighthouse2obj, &arb2object, &lighthouse2arb);
 
 		// Now find the space with the same origin, but rotated so that gravity is up
 		SurvivePose lighthouse2objUp = {}, object2objUp = {};
 		quatfrom2vectors(object2objUp.Rot, so->activations.accel, up);
 
 		// Calculate the pose of the lighthouse in this space
-		ApplyPoseToPose(lighthouse2objUp.Pos, object2objUp.Pos, lighthouse2obj.Pos);
+		ApplyPoseToPose(&lighthouse2objUp, &object2objUp, &lighthouse2obj);
 
 		// Purposefully only set this once. It should only depend on the first (calculated) lighthouse
 		if (quatmagnitude(objUp2world->Rot) == 0) {
@@ -61,8 +61,8 @@ void PoserData_lighthouse_pose_func(PoserData *poser_data, SurviveObject *so, ui
 
 		// Find find the poses that map to the above
 		SurvivePose obj2world, lighthouse2world;
-		ApplyPoseToPose(obj2world.Pos, objUp2world->Pos, object2objUp.Pos);
-		ApplyPoseToPose(lighthouse2world.Pos, objUp2world->Pos, lighthouse2objUp.Pos);
+		ApplyPoseToPose(&obj2world, objUp2world, &object2objUp);
+		ApplyPoseToPose(&lighthouse2world, objUp2world, &lighthouse2objUp);
 
 		so->ctx->lighthouseposeproc(so->ctx, lighthouse, &lighthouse2world, &obj2world);
 	}
diff --git a/src/poser_daveortho.c b/src/poser_daveortho.c
index a7ff691..c922b2e 100644
--- a/src/poser_daveortho.c
+++ b/src/poser_daveortho.c
@@ -105,7 +105,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 				SurvivePose lh2world = so->ctx->bsd[lhid].Pose;
 
 				SurvivePose obj2world;
-				ApplyPoseToPose(obj2world.Pos, lh2world.Pos, objpose.Pos);
+				ApplyPoseToPose(&obj2world, &lh2world, &objpose);
 				PoserData_poser_raw_pose_func(pd, so, lhid, &obj2world);
 
 				if (0) {
@@ -195,7 +195,7 @@ int PoserDaveOrtho( SurviveObject * so, PoserData * pd )
 			*/
 
 			SurvivePose poseout;
-			InvertPose(poseout.Pos, objpose.Pos);
+			InvertPose(&poseout, &objpose);
 			printf("INQUAT: %f %f %f %f = %f [%f %f %f]\n", objpose.Rot[0], objpose.Rot[1], objpose.Rot[2],
 				   objpose.Rot[3], quatmagnitude(objpose.Rot), objpose.Pos[0], objpose.Pos[1], objpose.Pos[2]);
 
diff --git a/src/poser_epnp.c b/src/poser_epnp.c
index cf4294f..401ea2a 100644
--- a/src/poser_epnp.c
+++ b/src/poser_epnp.c
@@ -39,13 +39,13 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer
 		cvGEMM(&R, &Tmp, -1, 0, 0, &T, 0);
 	}
 
-	FLT tmp[4];
+	LinmathQuat tmp;
 	quatfrommatrix33(tmp, r[0]);
 
 	// Typical camera applications have Z facing forward; the vive is contrarian and has Z going out of the
 	// back of the lighthouse. Think of this as a rotation on the Y axis a full 180 degrees -- the quat for that is
 	// [0 0x 1y 0z]
-	const FLT rt[4] = {0, 0, 1, 0};
+	const LinmathQuat rt = {0, 0, 1, 0};
 	quatrotateabout(rtn.Rot, tmp, rt);
 	if (!cameraToWorld) {
 		// We have to pre-multiply the rt transform here, which means we have to also offset our position by
@@ -56,50 +56,9 @@ static SurvivePose solve_correspondence(SurviveObject *so, epnp *pnp, bool camer
 
 	return rtn;
 }
-/*
-static int survive_standardize_calibration(SurviveObject* so,
-										   FLT upvec[3],
-										   SurvivePose* _object2world,
-										   SurvivePose* _lighthouses2world0,
-										   SurvivePose* _lighthouses2world1) {
-	SurvivePose object2world = {};
-	SurvivePose lighthouses2world0 = *_lighthouses2world0;
-	SurvivePose lighthouses2world1 = *_lighthouses2world1;
-
-	const FLT up[3] = {0, 0, 1};
-	quatfrom2vectors(object2world.Rot, so->activations.accel, _object2world->Pos);
-
-	FLT tx[4][4];
-	quattomatrix(tx, object2world.Rot);
 
-	SurvivePose additionalTx = {0};
-
-	SurvivePose lighthouse2world = {};
-	// Lighthouse is now a tx from camera -> object
-	ApplyPoseToPose(lighthouse2world.Pos,  object2world.Pos, lighthouse2object.Pos);
-
-	if(quatmagnitude(additionalTx.Rot) == 0) {
-		SurvivePose desiredPose = lighthouse2world;
-		desiredPose.Pos[0] = 0.;
-
-		quatfrom2vectors(additionalTx.Rot, lighthouse2world.Pos, desiredPose.Pos);
-	}
-	SurvivePose finalTx = {};
-	ApplyPoseToPose(finalTx.Pos,  additionalTx.Pos, lighthouse2world.Pos);
-
-}
-*/
 static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs) {
-	SurvivePose object2world = {//.Rot = { 0.7325378, 0.4619398, 0.1913417, 0.4619398}
-								.Rot = {1.}};
-	const FLT up[3] = {0, 0, 1};
-
-	SurvivePose actual;
-	// quatfrom2vectors(object2world.Rot, so->activations.accel, up);
-	// quatfrom2vectors(object2world.Rot, up, so->activations.accel);
-
 	SurvivePose additionalTx = {0};
-
 	for (int lh = 0; lh < 2; lh++) {
 		epnp pnp = {.fu = 1, .fv = 1};
 		epnp_set_maximum_number_of_correspondences(&pnp, so->sensor_ct);
@@ -122,22 +81,7 @@ static int opencv_solver_fullscene(SurviveObject *so, PoserDataFullScene *pdfs)
 		}
 
 		SurvivePose lighthouse2object = solve_correspondence(so, &pnp, true);
-		FLT euler[3];
-		quattoeuler(euler, lighthouse2object.Rot);
-		SurvivePose lighthouse2world = {};
-		// Lighthouse is now a tx from camera -> object
-		ApplyPoseToPose(lighthouse2world.Pos, object2world.Pos, lighthouse2object.Pos);
-
-		if (false && quatmagnitude(additionalTx.Rot) == 0) {
-			SurvivePose desiredPose = lighthouse2world;
-			desiredPose.Pos[0] = 0.;
-
-			quatfrom2vectors(additionalTx.Rot, lighthouse2world.Pos, desiredPose.Pos);
-		}
-		SurvivePose finalTx = lighthouse2world;
-		// ApplyPoseToPose(finalTx.Pos, additionalTx.Pos, lighthouse2world.Pos);
-
-		PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &finalTx, &object2world);
+		PoserData_lighthouse_pose_func(&pdfs->hdr, so, lh, &additionalTx, &lighthouse2object, 0);
 
 		epnp_dtor(&pnp);
 	}
diff --git a/src/poser_sba.c b/src/poser_sba.c
index b5962f8..870f541 100644
--- a/src/poser_sba.c
+++ b/src/poser_sba.c
@@ -36,7 +36,7 @@ void metric_function(int j, int i, double *aj, double *xij, void *adata) {
 
 	SurvivePose obj2world = ctx->obj_pose;
 	FLT sensorInWorld[3] = {};
-	ApplyPoseToPoint(sensorInWorld, obj2world.Pos, &so->sensor_locations[i * 3]);
+	ApplyPoseToPoint(sensorInWorld, &obj2world, &so->sensor_locations[i * 3]);
 	survive_reproject_from_pose_with_config(so->ctx, &ctx->calibration_config, j, (SurvivePose *)aj, sensorInWorld,
 											xij);
 }
@@ -134,7 +134,7 @@ void str_metric_function_single_sweep(int j, int i, double *bi, double *xij, voi
 
 	quatnormalize(obj.Rot, obj.Rot);
 	FLT xyz[3];
-	ApplyPoseToPoint(xyz, obj.Pos, &so->sensor_locations[sensor_idx * 3]);
+	ApplyPoseToPoint(xyz, &obj, &so->sensor_locations[sensor_idx * 3]);
 
 	// std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl;
 	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
@@ -157,7 +157,7 @@ void str_metric_function(int j, int i, double *bi, double *xij, void *adata) {
 
 	quatnormalize(obj.Rot, obj.Rot);
 	FLT xyz[3];
-	ApplyPoseToPoint(xyz, obj.Pos, &so->sensor_locations[sensor_idx * 3]);
+	ApplyPoseToPoint(xyz, &obj, &so->sensor_locations[sensor_idx * 3]);
 
 	// std::cerr << "Processing " << sensor_idx << ", " << lh << std::endl;
 	SurvivePose *camera = &so->ctx->bsd[lh].Pose;
diff --git a/src/survive_reproject.c b/src/survive_reproject.c
index 7bfc7d7..eabdb07 100644
--- a/src/survive_reproject.c
+++ b/src/survive_reproject.c
@@ -75,13 +75,13 @@ static FLT gibf(bool useSin, FLT v) {
 void survive_reproject_from_pose_with_config(
 	const SurviveContext *ctx, const survive_calibration_config *config,
 	int lighthouse, const SurvivePose *pose, const FLT *pt, FLT *out) {
-	FLT invq[4];
+	LinmathQuat invq;
 	quatgetreciprocal(invq, pose->Rot);
 
-	FLT tvec[3];
+	LinmathPoint3d tvec;
 	quatrotatevector(tvec, invq, pose->Pos);
 
-	FLT t_pt[3];
+	LinmathPoint3d t_pt;
 	quatrotatevector(t_pt, invq, pt);
 	for (int i = 0; i < 3; i++)
 		t_pt[i] = t_pt[i] - tvec[i];