Moved pose to linmath to support stronger typing

12 files changed, 126 insertions, 248 deletions

diff --git a/Makefile b/Makefile
index dc1c4e7..ace84d4 100644
--- a/Makefile
+++ b/Makefile
@@ -119,7 +119,8 @@ calibrate_tcc : $(LIBSURVIVE_C)
 clean :
 	rm -rf */*/*.o *.o src/*.o *~ src/*~ test simple_pose_test data_recorder calibrate testCocoa lib/libsurvive.so test_minimal_cv test_epnp test_epnp_ocv calibrate_client redist/*.o redist/*~ tools/data_server/data_server tools/lighthousefind/lighthousefind tools/lighthousefind_tori/lighthousefind-tori tools/plot_lighthouse/plot_lighthouse tools/process_rawcap/process_to_points redist/jsmntest redist/lintest
 
+.test_redist:
+	cd redist && make .run_tests;
 
-
-
+.run_tests: .test_redist
 
diff --git a/include/libsurvive/survive_types.h b/include/libsurvive/survive_types.h
index 7253df0..3ea6253 100644
--- a/include/libsurvive/survive_types.h
+++ b/include/libsurvive/survive_types.h
@@ -1,6 +1,7 @@
 #ifndef _SURVIVE_TYPES_H
 #define _SURVIVE_TYPES_H
 
+#include "linmath.h"
 #include "stdint.h"
 
 #ifdef __cplusplus
@@ -21,12 +22,8 @@ extern "C" {
 #define _FLT_format2(f) f##_format
 #define _FLT_format(f) _FLT_format2(f)
 #define FLT_format _FLT_format(FLT)
-  
-typedef struct SurvivePose
-{
-	FLT  Pos[3];
-	FLT  Rot[4]; //This is the [wxyz] quaternion, in wxyz format.
-} SurvivePose;
+
+typedef LinmathPose SurvivePose;
 
 //Careful with this, you can't just add another one right now, would take minor changes in survive_data.c and the cal tools.
 //It will also require a recompile.  TODO: revisit this and correct the comment once fixed.
diff --git a/redist/Makefile b/redist/Makefile
index 1437758..58536c9 100644
--- a/redist/Makefile
+++ b/redist/Makefile
@@ -9,6 +9,11 @@ lintest : lintest.c linmath.c linmath.h
 test_dcl : test_dcl.c dclhelpers.c minimal_opencv.c ../src/epnp/epnp.c
 	gcc -o $@ $^  os_generic.c -DFLT=double -lpthread -lcblas -lm -llapacke -O3 -msse2 -ftree-vectorize 
 
+.run_tests: clean all
+	./lintest
+	./test_dcl
+	./jsmntest
+
 clean :
 	rm -rf *.o *~ jsmntest lintest
 
diff --git a/redist/linmath.c b/redist/linmath.c
index c302f5b..16f771e 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -183,24 +183,18 @@ void axisanglefromquat(FLT *angle, FLT *axis, FLT *q)
 //Originally from Mercury (Copyright (C) 2009 by Joshua Allen, Charles Lohr, Adam Lowman)
 //Under the mit/X11 license.
 
-
-
-
-void quatsetnone(FLT * q)
-{
+void quatsetnone(LinmathQuat q) {
 	q[0] = 1; q[1] = 0; q[2] = 0; q[3] = 0;
 }
 
-void quatcopy(FLT * qout, const FLT * qin)
-{
+void quatcopy(LinmathQuat qout, const LinmathQuat qin) {
 	qout[0] = qin[0];
 	qout[1] = qin[1];
 	qout[2] = qin[2];
 	qout[3] = qin[3];
 }
 
-void quatfromeuler( FLT * q, const FLT * euler )
-{
+void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler) {
 	FLT X = euler[0]/2.0f; //roll
 	FLT Y = euler[1]/2.0f; //pitch
 	FLT Z = euler[2]/2.0f; //yaw
@@ -221,16 +215,14 @@ void quatfromeuler( FLT * q, const FLT * euler )
 	quatnormalize( q, q );
 }
 
-void quattoeuler( FLT * euler, const FLT * q )
-{
+void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q) {
 	//According to http://en.wikipedia.org/wiki/Conversion_between_quaternions_and_Euler_angles (Oct 26, 2009)
 	euler[0] = FLT_ATAN2(2 * (q[0] * q[1] + q[2] * q[3]), 1 - 2 * (q[1] * q[1] + q[2] * q[2]));
 	euler[1] = FLT_ASIN(2 * (q[0] * q[2] - q[3] * q[1]));
 	euler[2] = FLT_ATAN2(2 * (q[0] * q[3] + q[1] * q[2]), 1 - 2 * (q[2] * q[2] + q[3] * q[3]));
 }
 
-void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians )
-{
+void quatfromaxisangle(LinmathQuat q, const FLT *axis, FLT radians) {
 	FLT v[3];
 	normalize3d( v, axis );
 	
@@ -243,25 +235,20 @@ void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians )
 	quatnormalize( q, q );
 }
 
-FLT quatmagnitude( const FLT * q )
-{
+FLT quatmagnitude(const LinmathQuat q) {
 	return FLT_SQRT((q[0] * q[0]) + (q[1] * q[1]) + (q[2] * q[2]) + (q[3] * q[3]));
 }
 
-FLT quatinvsqmagnitude( const FLT * q )
-{
+FLT quatinvsqmagnitude(const LinmathQuat q) {
 	return ((FLT)1.)/FLT_SQRT((q[0]*q[0])+(q[1]*q[1])+(q[2]*q[2])+(q[3]*q[3]));
 }
 
-
-void quatnormalize( FLT * qout, const FLT * qin )
-{
+void quatnormalize(LinmathQuat qout, const LinmathQuat qin) {
 	FLT imag = quatinvsqmagnitude( qin );
 	quatscale( qout, qin, imag );
 }
 
-void quattomatrix(FLT * matrix44, const FLT * qin)
-{
+void quattomatrix(FLT *matrix44, const LinmathQuat qin) {
 	FLT q[4];
 	quatnormalize(q, qin);
 
@@ -332,8 +319,7 @@ void quatfrommatrix33(FLT *q, const FLT *m) {
 	}
 }
 
-void quatfrommatrix( FLT * q, const FLT * matrix44 )
-{
+void quatfrommatrix(LinmathQuat q, const FLT *matrix44) {
 	//Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/matrixToQuaternion/
 	FLT tr = matrix44[0] + matrix44[5] + matrix44[10];
 
@@ -366,8 +352,7 @@ void quatfrommatrix( FLT * q, const FLT * matrix44 )
 
 
 // Algorithm from http://www.euclideanspace.com/maths/geometry/rotations/conversions/quaternionToMatrix/
-void quattomatrix33(FLT * matrix33, const FLT * qin)
-{
+void quattomatrix33(FLT *matrix33, const LinmathQuat qin) {
 	FLT q[4];
 	quatnormalize(q, qin);
 
@@ -399,39 +384,34 @@ void quattomatrix33(FLT * matrix33, const FLT * qin)
 	matrix33[8] = 1 - xx - yy;
 }
 
-void quatgetconjugate(FLT * qout, const FLT * qin)
-{
+void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin) {
 	qout[0] = qin[0];
 	qout[1] = -qin[1];
 	qout[2] = -qin[2];
 	qout[3] = -qin[3];
 }
 
-void quatgetreciprocal( FLT * qout, const FLT * qin )
-{
+void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin) {
 	FLT m = quatinvsqmagnitude(qin);
 	quatgetconjugate( qout, qin );
 	quatscale( qout, qout, m );
 }
 
-void quatsub( FLT * qout, const FLT * a, const FLT * b )
-{
+void quatsub(LinmathQuat qout, const FLT *a, const FLT *b) {
 	qout[0] = a[0] - b[0];
 	qout[1] = a[1] - b[1];
 	qout[2] = a[2] - b[2];
 	qout[3] = a[3] - b[3];
 }
 
-void quatadd( FLT * qout, const FLT * a, const FLT * b )
-{
+void quatadd(LinmathQuat qout, const FLT *a, const FLT *b) {
 	qout[0] = a[0] + b[0];
 	qout[1] = a[1] + b[1];
 	qout[2] = a[2] + b[2];
 	qout[3] = a[3] + b[3];
 }
 
-void quatrotateabout( FLT * qout, const FLT * q1, const FLT * q2 )
-{
+void quatrotateabout(LinmathQuat qout, const LinmathQuat q1, const LinmathQuat q2) {
 	//NOTE: Does not normalize
 	qout[0] = (q1[0]*q2[0])-(q1[1]*q2[1])-(q1[2]*q2[2])-(q1[3]*q2[3]);
 	qout[1] = (q1[0]*q2[1])+(q1[1]*q2[0])+(q1[2]*q2[3])-(q1[3]*q2[2]);
@@ -439,44 +419,37 @@ void quatrotateabout( FLT * qout, const FLT * q1, const FLT * q2 )
 	qout[3] = (q1[0]*q2[3])+(q1[1]*q2[2])-(q1[2]*q2[1])+(q1[3]*q2[0]);
 }
 
-void quatscale( FLT * qout, const FLT * qin, FLT s )
-{
+void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s) {
 	qout[0] = qin[0] * s;
 	qout[1] = qin[1] * s;
 	qout[2] = qin[2] * s;
 	qout[3] = qin[3] * s;
 }
 
-
-FLT quatinnerproduct( const FLT * qa, const FLT * qb )
-{
+FLT quatinnerproduct(const LinmathQuat qa, const LinmathQuat qb) {
 	return (qa[0]*qb[0])+(qa[1]*qb[1])+(qa[2]*qb[2])+(qa[3]*qb[3]);
 }
 
-void quatouterproduct( FLT * outvec3, FLT * qa, FLT * qb )
-{
+void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb) {
 	outvec3[0] = (qa[0]*qb[1])-(qa[1]*qb[0])-(qa[2]*qb[3])+(qa[3]*qb[2]);
 	outvec3[1] = (qa[0]*qb[2])+(qa[1]*qb[3])-(qa[2]*qb[0])-(qa[3]*qb[1]);
 	outvec3[2] = (qa[0]*qb[3])-(qa[1]*qb[2])+(qa[2]*qb[1])-(qa[3]*qb[0]);
 }
 
-void quatevenproduct( FLT * q, FLT * qa, FLT * qb )
-{
+void quatevenproduct(LinmathQuat q, LinmathQuat qa, LinmathQuat qb) {
 	q[0] = (qa[0]*qb[0])-(qa[1]*qb[1])-(qa[2]*qb[2])-(qa[3]*qb[3]);
 	q[1] = (qa[0]*qb[1])+(qa[1]*qb[0]);
 	q[2] = (qa[0]*qb[2])+(qa[2]*qb[0]);
 	q[3] = (qa[0]*qb[3])+(qa[3]*qb[0]);
 }
 
-void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb )
-{
+void quatoddproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb) {
 	outvec3[0] = (qa[2]*qb[3])-(qa[3]*qb[2]);
 	outvec3[1] = (qa[3]*qb[1])-(qa[1]*qb[3]);
 	outvec3[2] = (qa[1]*qb[2])-(qa[2]*qb[1]);
 }
 
-void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t )
-{
+void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t) {
 	FLT an[4];
 	FLT bn[4];
 	quatnormalize( an, qa );
@@ -511,8 +484,7 @@ void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t )
 	}
 }
 
-void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in )
-{
+void quatrotatevector(FLT *vec3out, const LinmathQuat quat, const FLT *vec3in) {
 	//See: http://www.geeks3d.com/20141201/how-to-rotate-a-vertex-by-a-quaternion-in-glsl/
 
 	FLT tmp[3];
@@ -665,19 +637,19 @@ void matrix44transpose(FLT * mout, const FLT * minm )
 
 }
 
-void ApplyPoseToPoint(LINMATH_POINT pout, const LINMATH_POSE pose, const LINMATH_POINT pin) {
-	quatrotatevector( pout, &pose[3], pin );
-	add3d( pout, pout, &pose[0] );
+void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin) {
+	quatrotatevector(pout, pose->Rot, pin);
+	add3d(pout, pout, pose->Pos);
 }
 
-void ApplyPoseToPose(LINMATH_POSE pout, const LINMATH_POSE lhs_pose, const LINMATH_POSE rhs_pose) {
-	ApplyPoseToPoint(pout, lhs_pose, rhs_pose);
-	quatrotateabout(&pout[3], &lhs_pose[3], &rhs_pose[3]);
+void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_pose, const LinmathPose *rhs_pose) {
+	ApplyPoseToPoint(pout->Pos, lhs_pose, rhs_pose->Pos);
+	quatrotateabout(pout->Rot, lhs_pose->Rot, rhs_pose->Rot);
 }
 
-void InvertPose(LINMATH_POSE poseout, const LINMATH_POSE pose) {
-	quatgetreciprocal(&poseout[3], &pose[3]);
+void InvertPose(LinmathPose *poseout, const LinmathPose *pose) {
+	quatgetreciprocal(poseout->Rot, pose->Rot);
 
-	quatrotatevector(&poseout[0], &poseout[3], &pose[0]);
-	scale3d(&poseout[0], &poseout[0], -1);
+	quatrotatevector(poseout->Pos, poseout->Rot, pose->Pos);
+	scale3d(poseout->Pos, poseout->Pos, -1);
 }
diff --git a/redist/linmath.h b/redist/linmath.h
index 57b98d3..782a13d 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -47,6 +47,14 @@
 #define FLT_FABS FLT_FABS__
 #endif
 
+typedef FLT LinmathQuat[4]; // This is the [wxyz] quaternion, in wxyz format.
+typedef FLT LinmathPoint3d[3];
+typedef FLT linmathVec3d[3];
+
+typedef struct LinmathPose {
+	LinmathPoint3d Pos;
+	LinmathQuat Rot;
+} LinmathPose;
 
 //NOTE: Inputs may never be output with cross product.
 void cross3d( FLT * out, const FLT * a, const FLT * b );
@@ -72,52 +80,48 @@ 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);
-
+void axisanglefromquat(FLT *angle, FLT *axis, LinmathQuat quat);
 
 //Quaternion things...
 
-void quatsetnone( FLT * q );
-void quatcopy( FLT * qout, const FLT * qin );
-void quatfromeuler( FLT * q, const FLT * euler );
-void quattoeuler( FLT * euler, const FLT * q );
-void quatfromaxisangle( FLT * q, const FLT * axis, FLT radians );
-FLT quatmagnitude( const FLT * q );
-FLT quatinvsqmagnitude( const FLT * q );
-void quatnormalize( FLT * qout, const FLT * qin );  //Safe for in to be same as out.
-void quattomatrix( FLT * matrix44, const FLT * q );
-void quatfrommatrix( FLT * q, const FLT * matrix44 );
-void quatfrommatrix33(FLT *q, const FLT *matrix33);
-void quatgetconjugate( FLT * qout, const FLT * qin );
-void quatgetreciprocal( FLT * qout, const FLT * qin );
-void quatsub( FLT * qout, const FLT * a, const FLT * b );
-void quatadd( FLT * qout, const FLT * a, const FLT * b );
-void quatrotateabout( FLT * qout, const FLT * a, const FLT * b );  //same as quat multiply, not piecewise multiply.
-void quatscale( FLT * qout, const FLT * qin, FLT s );
-FLT quatinnerproduct( const FLT * qa, const FLT * qb );
-void quatouterproduct( FLT * outvec3, FLT * qa, FLT * qb );
-void quatevenproduct( FLT * q, FLT * qa, FLT * qb );
-void quatoddproduct( FLT * outvec3, FLT * qa, FLT * qb );
-void quatslerp( FLT * q, const FLT * qa, const FLT * qb, FLT t );
-void quatrotatevector( FLT * vec3out, const FLT * quat, const FLT * vec3in );
-void quatfrom2vectors(FLT *q, const FLT *src, const FLT *dest);
-
-//Poses are Position: [x, y, z]  Quaternion: [q, x, y, z]
-typedef FLT *LINMATH_POSE;
-
-// Points are [x, y, z]
-typedef FLT *LINMATH_POINT;
+typedef FLT LinmathEulerAngle[3];
+
+void quatsetnone(LinmathQuat q);
+void quatcopy(LinmathQuat q, const LinmathQuat qin);
+void quatfromeuler(LinmathQuat q, const LinmathEulerAngle euler);
+void quattoeuler(LinmathEulerAngle euler, const LinmathQuat q);
+void quatfromaxisangle(LinmathQuat q, const FLT *axis, FLT radians);
+FLT quatmagnitude(const LinmathQuat q);
+FLT quatinvsqmagnitude(const LinmathQuat q);
+void quatnormalize(LinmathQuat qout, const LinmathQuat qin); // Safe for in to be same as out.
+void quattomatrix(FLT *matrix44, const LinmathQuat q);
+void quatfrommatrix(LinmathQuat q, const FLT *matrix44);
+void quatfrommatrix33(LinmathQuat q, const FLT *matrix33);
+void quatgetconjugate(LinmathQuat qout, const LinmathQuat qin);
+void quatgetreciprocal(LinmathQuat qout, const LinmathQuat qin);
+void quatsub(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b);
+void quatadd(LinmathQuat qout, const LinmathQuat a, const LinmathQuat b);
+void quatrotateabout(LinmathQuat qout, const LinmathQuat a,
+					 const LinmathQuat b); // same as quat multiply, not piecewise multiply.
+void quatscale(LinmathQuat qout, const LinmathQuat qin, FLT s);
+FLT quatinnerproduct(const LinmathQuat qa, const LinmathQuat qb);
+void quatouterproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb);
+void quatevenproduct(LinmathQuat q, LinmathQuat qa, LinmathQuat qb);
+void quatoddproduct(FLT *outvec3, LinmathQuat qa, LinmathQuat qb);
+void quatslerp(LinmathQuat q, const LinmathQuat qa, const LinmathQuat qb, FLT t);
+void quatrotatevector(FLT *vec3out, const LinmathQuat quat, const FLT *vec3in);
+void quatfrom2vectors(LinmathQuat q, const FLT *src, const FLT *dest);
 
 // This is the quat equivalent of 'pout = pose * pin' if pose were a 4x4 matrix in homogenous space
-void ApplyPoseToPoint(LINMATH_POINT pout, const LINMATH_POSE pose, const LINMATH_POINT pin);
+void ApplyPoseToPoint(LinmathPoint3d pout, const LinmathPose *pose, const LinmathPoint3d pin);
 
 // This is the quat equivalent of 'pout = lhs_pose * rhs_pose' if poses were a 4x4 matrix in homogenous space
-void ApplyPoseToPose(LINMATH_POSE pout, const LINMATH_POSE lhs_pose, const LINMATH_POSE rhs_pose);
+void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_pose, const LinmathPose *rhs_pose);
 
 // This is the quat equivlant of 'pose_in^-1'; so that ApplyPoseToPose(..., InvertPose(..., pose_in), pose_in) ==
 // Identity ( [0, 0, 0], [1, 0, 0, 0] )
 // by definition.
-void InvertPose(LINMATH_POSE poseout, const LINMATH_POSE pose_in);
+void InvertPose(LinmathPose *poseout, const LinmathPose *pose_in);
 
 // Matrix Stuff
 
diff --git a/redist/lintest.c b/redist/lintest.c
index 4765e68..4c53da7 100644
--- a/redist/lintest.c
+++ b/redist/lintest.c
@@ -32,72 +32,72 @@ void printFLTA(FLT *a, int length) {
 
 void testInvertPose() {
 
-	FLT rotAroundYOffset[7] = {1, 1, 1, .5, 0, .5, 0};
-	FLT pose_out[7];
+	LinmathPose rotAroundYOffset = {1, 1, 1, .5, 0, .5, 0};
+	LinmathPose pose_out;
 
 	{
-		FLT expected[] = {1, -1, -1, 0.7071068, 0, -0.7071068, 0};
-		InvertPose(pose_out, rotAroundYOffset);
-		ASSERT_FLTA_EQUALS(pose_out, expected, 7);
+		LinmathPose expected = {1, -1, -1, 0.7071068, 0, -0.7071068, 0};
+		InvertPose(&pose_out, &rotAroundYOffset);
+		ASSERT_FLTA_EQUALS(pose_out.Pos, expected.Pos, 7);
 
 		FLT identity[] = {0, 0, 0, 1, 0, 0, 0};
-		ApplyPoseToPose(pose_out, expected, rotAroundYOffset);
-		quatnormalize(&pose_out[3], &pose_out[3]);
-		ASSERT_FLTA_EQUALS(pose_out, identity, 7);
+		ApplyPoseToPose(&pose_out, &expected, &rotAroundYOffset);
+		quatnormalize(pose_out.Rot, pose_out.Rot);
+		ASSERT_FLTA_EQUALS(pose_out.Pos, identity, 7);
 	}
 }
 
 void testApplyPoseToPose() {
-	FLT rotAroundYOffset[7] = {1, 1, 1, 0, 0, 1, 0};
-	FLT pose_out[7];
+	LinmathPose rotAroundYOffset = {1, 1, 1, 0, 0, 1, 0};
+	LinmathPose pose_out;
 
 	{
-		FLT pt[] = {0, 1, 0, 0, 0, 1, 0};
-		FLT expected[] = {1, 2, 1, -1, 0, 0, 0};
-		ApplyPoseToPose(pose_out, rotAroundYOffset, pt);
-		quatnormalize(&pose_out[3], &pose_out[3]);
-		ASSERT_FLTA_EQUALS(pose_out, expected, 7);
+		LinmathPose pt = {0, 1, 0, 0, 0, 1, 0};
+		LinmathPose expected = {1, 2, 1, -1, 0, 0, 0};
+		ApplyPoseToPose(&pose_out, &rotAroundYOffset, &pt);
+		quatnormalize(pose_out.Rot, pose_out.Rot);
+		ASSERT_FLTA_EQUALS(pose_out.Pos, expected.Pos, 7);
 	}
 
 	{
-		FLT pt[] = {0, 1, 0, 0, 1, 0, 0};
-		FLT expected[] = {1, 2, 1, 0, 0, 0, -1};
-		ApplyPoseToPose(pose_out, rotAroundYOffset, pt);
-		ASSERT_FLTA_EQUALS(pose_out, expected, 7);
+		LinmathPose pt = {0, 1, 0, 0, 1, 0, 0};
+		LinmathPose expected = {1, 2, 1, 0, 0, 0, -1};
+		ApplyPoseToPose(&pose_out, &rotAroundYOffset, &pt);
+		ASSERT_FLTA_EQUALS(pose_out.Pos, expected.Pos, 7);
 	}
 }
 
 void testApplyPoseToPoint() {
-	FLT rotAroundY[7] = {0, 0, 0, 0, 0, 1, 0};
+	LinmathPose rotAroundY = {0, 0, 0, 0, 0, 1, 0};
 	FLT pt_out[3];
 
 	{
 		FLT pt[3] = {0, 1, 0};
 		FLT expected[3] = {0, 1, 0};
-		ApplyPoseToPoint(pt_out, rotAroundY, pt);
+		ApplyPoseToPoint(pt_out, &rotAroundY, pt);
 		ASSERT_FLTA_EQUALS(pt_out, expected, 3);
 	}
 
 	{
 		FLT pt[3] = {1, 1, 0};
 		FLT expected[3] = {-1, 1, 0};
-		ApplyPoseToPoint(pt_out, rotAroundY, pt);
+		ApplyPoseToPoint(pt_out, &rotAroundY, pt);
 		ASSERT_FLTA_EQUALS(pt_out, expected, 3);
 	}
 
-	FLT rotAroundYOffset[7] = {1, 1, 1, 0, 0, 1, 0};
+	LinmathPose rotAroundYOffset = {1, 1, 1, 0, 0, 1, 0};
 
 	{
 		FLT pt[3] = {0, 1, 0};
 		FLT expected[3] = {1, 2, 1};
-		ApplyPoseToPoint(pt_out, rotAroundYOffset, pt);
+		ApplyPoseToPoint(pt_out, &rotAroundYOffset, pt);
 		ASSERT_FLTA_EQUALS(pt_out, expected, 3);
 	}
 
 	{
 		FLT pt[3] = {1, 1, 0};
 		FLT expected[3] = {0, 2, 1};
-		ApplyPoseToPoint(pt_out, rotAroundYOffset, pt);
+		ApplyPoseToPoint(pt_out, &rotAroundYOffset, pt);
 		ASSERT_FLTA_EQUALS(pt_out, expected, 3);
 	}
 }
@@ -163,53 +163,8 @@ int main()
 	printf( "%f %f %f\n", PFTHREE( pOut1 ) );
 	printf( "%f %f %f\n", PFTHREE( pOut2 ) );
 */
-	return -1;
-
-#endif
+	return 0;
 
-#if 0
-
-	FLT e[3] = { 1,1,3.14 };
-	FLT q[4];
-	FLT m[16];
-	FLT pt[3] = { 1, 1, 1 };
-
-	q[0] = 0;
-	q[1] = 0;
-	q[2] = 0;
-	q[3] = 1;
-
-	quatrotatevector( pt, q, pt );
-	printf( "%f %f %f\n", PFTHREE( pt ) );
-	printf( "\n" );
-
-	quatfromeuler( q, e );
-	printf( "%f %f %f %f\n\n", PFFOUR( q ) );
-	quattomatrix(m,q);
-	printf( "%f %f %f %f\n", PFFOUR( &m[0] ) );
-	printf( "%f %f %f %f\n", PFFOUR( &m[4] ) );
-	printf( "%f %f %f %f\n", PFFOUR( &m[8] ) );
-	printf( "%f %f %f %f\n\n", PFFOUR( &m[12] ) );
-	quatfrommatrix(q,m );
-	printf( "%f %f %f %f\n\n", PFFOUR( q ) );
-	quattoeuler( e,q );
-	printf( "E: %f %f %f\n", e[0], e[1], e[2] );
-
-
-	FLT pfromlh[3] = { 0, 1, 0 };
-	FLT p[3] = { 0, 1, 0 };
-	quatrotatevector( p, q, p );
-	printf( "%f %f %f\n", PFTHREE( p ) );
-	printf( "Flipping rotation\n" );
-	q[0] *= -1; //Wow that was easy.
-	quatrotatevector( p, q, p );
-	printf( "%f %f %f\n", PFTHREE( p ) );
-
-	//Try setting up a pose.
-//	FLT mypose[7] = { 0, 0, 10, q[0], q[1], q[2], q[3] );
-//	ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose );
-//void InvertPose( FLT * poseout, const FLT * pose );
-	
 #endif
 	
 }
diff --git a/simple_pose_test.c b/simple_pose_test.c
index 91d80d7..34c15de 100644
--- a/simple_pose_test.c
+++ b/simple_pose_test.c
@@ -71,7 +71,7 @@ void testprog_raw_pose_process(SurviveObject *so, uint8_t lighthouse, SurvivePos
 	hpos[1] = pose->Pos[1];
 	hpos[2] = pose->Pos[2];
 	FLT hposin[3] = { 0, 0, 1 };
-	ApplyPoseToPoint(hpos2, &pose->Pos[0], hposin);
+	ApplyPoseToPoint(hpos2, pose, hposin);
 
 	fflush(stdout);
 }
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];