Finalized reading in and transforming of accel/gyro data

6 files changed, 95 insertions, 18 deletions

diff --git a/redist/json_helpers.c b/redist/json_helpers.c
index 0741c84..fa76c16 100644
--- a/redist/json_helpers.c
+++ b/redist/json_helpers.c
@@ -213,27 +213,18 @@ void json_load_file(const char* path) {
 	free(JSON_STRING);
 }
 
-int parse_float_array(char* str, jsmntok_t* token, FLT** f, uint8_t count) {
-	uint16_t i = 0;
-
-	if (count==0) return 0;
-
-	if (*f!=NULL) free(*f);
-	*f = malloc(sizeof(FLT) * count);
-
-	for(i=0;i<count;++i) {
+int parse_float_array_in_place(char *str, jsmntok_t *token, FLT *f, uint8_t count) {
+	for (int i = 0; i < count; ++i) {
 		char* end = str + token->end;
 		char* s = str+token->start;
 
 		#ifdef USE_DOUBLE
-		(*f)[i] = strtod(s, &end);
+		f[i] = strtod(s, &end);
 		#else
-		(*f)[i] = strtof(s, &end);
+		f[i] = strtof(s, &end);
 		#endif
 
 		if (s == end) {
-			free(*f);
-			*f=NULL;
 			return 0; //not a float
 		}
 		token++;
@@ -242,3 +233,21 @@ int parse_float_array(char* str, jsmntok_t* token, FLT** f, uint8_t count) {
 
 	return count;
 }
+int parse_float_array(char *str, jsmntok_t *token, FLT **f, uint8_t count) {
+	uint16_t i = 0;
+
+	if (count == 0)
+		return 0;
+
+	if (*f != NULL)
+		free(*f);
+	*f = malloc(sizeof(FLT) * count);
+
+	int rtn = parse_float_array_in_place(str, token, *f, count);
+	if (rtn == 0) {
+		free(*f);
+		*f = 0;
+	}
+
+	return count;
+}
diff --git a/redist/json_helpers.h b/redist/json_helpers.h
index 3ebf66b..2152d85 100644
--- a/redist/json_helpers.h
+++ b/redist/json_helpers.h
@@ -3,9 +3,10 @@
 #ifndef JSON_HELPERS_H
 #define JSON_HELPERS_H
 
-#include <stdint.h>
-#include <jsmn.h>
 #include "survive_types.h"
+#include <jsmn.h>
+#include <stdint.h>
+#include <stdio.h>
 
 void json_write_float_array(FILE* f, const char* tag, float* v, uint8_t count);
 void json_write_double_array(FILE* f, const char* tag, double* v, uint8_t count);
@@ -13,6 +14,7 @@ void json_write_uint32(FILE* f, const char* tag, uint32_t v);
 void json_write_float(FILE* f, const char* tag, float v);
 void json_write_str(FILE* f, const char* tag, const char* v);
 
+int parse_float_array_in_place(char *str, jsmntok_t *token, FLT *values, uint8_t count);
 int parse_float_array(char* str, jsmntok_t* token, FLT** values, uint8_t count);
 
 void json_load_file(const char* path);
@@ -20,5 +22,4 @@ extern void (*json_begin_object)(char* tag);
 extern void (*json_end_object)();
 extern void (*json_tag_value)(char* tag, char** values, uint8_t count);
 
-
-#endif
\ No newline at end of file
+#endif
diff --git a/redist/linmath.c b/redist/linmath.c
index f4c3635..80eeaa5 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -5,6 +5,8 @@
 #include <math.h>
 #include <string.h>
 
+#include "minimal_opencv.h"
+
 inline void cross3d(FLT *out, const FLT *a, const FLT *b) {
 	out[0] = a[1] * b[2] - a[2] * b[1];
 	out[1] = a[2] * b[0] - a[0] * b[2];
@@ -286,7 +288,6 @@ inline void quattomatrix(FLT *matrix44, const LinmathQuat qin) {
 	matrix44[14] = 0;
 	matrix44[15] = 1;
 }
-
 inline void quatfrommatrix33(FLT *q, const FLT *m) {
 	FLT m00 = m[0], m01 = m[1], m02 = m[2], m10 = m[3], m11 = m[4], m12 = m[5], m20 = m[6], m21 = m[7], m22 = m[8];
 
@@ -641,5 +642,41 @@ inline void PoseToMatrix(FLT *matrix44, const LinmathPose *pose_in) {
 	matrix44[11] = pose_in->Pos[2];
 }
 
+#include "stdio.h"
+
+void KabschCentered(LinmathQuat qout, const FLT *ptsA, const FLT *ptsB, int num_pts) {
+	// Note: The following follows along with https://en.wikipedia.org/wiki/Kabsch_algorithm
+	// for the most part but we use some transpose identities to let avoid unneeded transposes
+	CvMat A = cvMat(num_pts, 3, CV_64F, (FLT *)ptsA);
+	CvMat B = cvMat(num_pts, 3, CV_64F, (FLT *)ptsB);
+
+	double _C[9] = {};
+	CvMat C = cvMat(3, 3, CV_64F, _C);
+	cvGEMM(&B, &A, 1, 0, 0, &C, GEMM_1_T);
+
+	double _U[9] = {};
+	double _W[9] = {};
+	double _VT[9] = {};
+	CvMat U = cvMat(3, 3, CV_64F, _U);
+	CvMat W = cvMat(3, 3, CV_64F, _W);
+	CvMat VT = cvMat(3, 3, CV_64F, _VT);
+
+	cvSVD(&C, &W, &U, &VT, CV_SVD_V_T | CV_SVD_MODIFY_A);
+
+	double _R[9] = {};
+	CvMat R = cvMat(3, 3, CV_64F, _R);
+	cvGEMM(&U, &VT, 1, 0, 0, &R, 0);
+
+	// Enforce RH rule
+	if (cvDet(&R) < 0.) {
+		_U[2] *= -1;
+		_U[5] *= -1;
+		_U[8] *= -1;
+		cvGEMM(&U, &VT, 1, 0, 0, &R, 0);
+	}
+
+	quatfrommatrix33(qout, _R);
+}
+
 LinmathQuat LinmathQuat_Identity = {1.0};
 LinmathPose LinmathPose_Identity = {.Rot = {1.0}};
diff --git a/redist/linmath.h b/redist/linmath.h
index aff46a3..f961568 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -139,6 +139,12 @@ LINMATH_EXPORT void ApplyPoseToPose(LinmathPose *pout, const LinmathPose *lhs_po
 LINMATH_EXPORT void InvertPose(LinmathPose *poseout, const LinmathPose *pose_in);
 
 LINMATH_EXPORT void PoseToMatrix(FLT *mat44, const LinmathPose *pose_in);
+
+// Given num_pts in coordinate system B, and in coordinate system A, find the optimal RHS rotation
+// which transforms from B to A.
+//
+// This assumes that the space A and B share an origin.
+LINMATH_EXPORT void KabschCentered(LinmathQuat B2Atx, const FLT *ptsA, const FLT *ptsB, int num_pts);
 // Matrix Stuff
 
 typedef struct {
diff --git a/redist/minimal_opencv.c b/redist/minimal_opencv.c
index c9cacf3..da7681c 100644
--- a/redist/minimal_opencv.c
+++ b/redist/minimal_opencv.c
@@ -374,3 +374,26 @@ SURVIVE_LOCAL_ONLY void cvReleaseMat(CvMat **mat) {
 	free(*mat);
 	*mat = 0;
 }
+
+SURVIVE_LOCAL_ONLY double cvDet(const CvMat *M) {
+	assert(M->rows == M->cols);
+	assert(M->rows <= 3 && "cvDet unimplemented for matrices >3");
+	assert(CV_64F == CV_MAT_TYPE(M->type) && "cvDet unimplemented for float");
+	double *m = M->data.db;
+
+	switch (M->rows) {
+	case 1:
+		return m[0];
+	case 2: {
+		return m[0] * m[3] - m[1] * m[2];
+	}
+	case 3: {
+		double m00 = m[0], m01 = m[1], m02 = m[2], m10 = m[3], m11 = m[4], m12 = m[5], m20 = m[6], m21 = m[7],
+			   m22 = m[8];
+
+		return m00 * (m11 * m22 - m12 * m21) - m01 * (m10 * m22 - m12 * m20) + m02 * (m10 * m21 - m11 * m20);
+	}
+	default:
+		abort();
+	}
+}
diff --git a/redist/minimal_opencv.h b/redist/minimal_opencv.h
index fa07c84..59b80eb 100644
--- a/redist/minimal_opencv.h
+++ b/redist/minimal_opencv.h
@@ -198,6 +198,7 @@ void cvSVD(CvMat *aarr, CvMat *warr, CvMat *uarr, CvMat *varr, int flags);
 void cvMulTransposed(const CvMat *src, CvMat *dst, int order, const CvMat *delta, double scale);
 void cvTranspose(const CvMat *M, CvMat *dst);
 void print_mat(const CvMat *M);
+double cvDet(const CvMat *M);
 
 #define CV_SVD 1
 #define CV_SVD_MODIFY_A 1