aboutsummaryrefslogtreecommitdiff
diff options
context:
space:
mode:
authorJustin Berger <j.david.berger@gmail.com>2018-03-11 14:48:08 -0600
committerJustin Berger <j.david.berger@gmail.com>2018-03-11 14:48:08 -0600
commit5153a32da9793945f72d60185d1929cd72ceeea3 (patch)
treec82aa5d46518ba04279325ef41ab9b50ecf606e2
parentc3235ace3df4fae68c11dcd449ab3a32afca72aa (diff)
downloadlibsurvive-5153a32da9793945f72d60185d1929cd72ceeea3.tar.gz
libsurvive-5153a32da9793945f72d60185d1929cd72ceeea3.tar.bz2
Added missing functions into lintest
-rw-r--r--redist/Makefile9
-rw-r--r--redist/linmath.c14
-rw-r--r--redist/linmath.h17
-rw-r--r--redist/lintest.c104
-rw-r--r--simple_pose_test.c2
5 files changed, 137 insertions, 9 deletions
diff --git a/redist/Makefile b/redist/Makefile
index 5ba30dc..2aa5ef1 100644
--- a/redist/Makefile
+++ b/redist/Makefile
@@ -1,8 +1,11 @@
-all : jsmntest
+all : jsmntest lintest
jsmntest : jsmntest.c jsmn.c
- gcc -o $@ $^
+ gcc -g -O0 -o $@ $^
+
+lintest : lintest.c linmath.c linmath.h
+ gcc -g -O0 -o $@ $^ -lm
clean :
- rm -rf *.o *~ jsmntest
+ rm -rf *.o *~ jsmntest lintest
diff --git a/redist/linmath.c b/redist/linmath.c
index eb74271..76a723d 100644
--- a/redist/linmath.c
+++ b/redist/linmath.c
@@ -634,9 +634,19 @@ void matrix44transpose(FLT * mout, const FLT * minm )
}
-void ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose )
-{
+void ApplyPoseToPoint(LINMATH_POINT pout, const LINMATH_POSE pose, const LINMATH_POINT pin) {
quatrotatevector( pout, &pose[3], pin );
add3d( pout, pout, &pose[0] );
}
+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 InvertPose(LINMATH_POSE poseout, const LINMATH_POSE pose) {
+ quatgetreciprocal(&poseout[3], &pose[3]);
+
+ quatrotatevector(&poseout[0], &poseout[3], &pose[0]);
+ scale3d(&poseout[0], &poseout[0], -1);
+}
diff --git a/redist/linmath.h b/redist/linmath.h
index 1876b34..ff00c94 100644
--- a/redist/linmath.h
+++ b/redist/linmath.h
@@ -102,10 +102,21 @@ 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]
-//XXX TODO Write these!
-void ApplyPoseToPoint( FLT * pout, const FLT * pin, const FLT * pose );
-void InvertPose( FLT * poseout, const FLT * pose );
+typedef FLT *LINMATH_POSE;
+// Points are [x, y, z]
+typedef FLT *LINMATH_POINT;
+
+// 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);
+
+// 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);
+
+// 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);
// Matrix Stuff
diff --git a/redist/lintest.c b/redist/lintest.c
index fa5a9d7..4765e68 100644
--- a/redist/lintest.c
+++ b/redist/lintest.c
@@ -1,8 +1,112 @@
#include "linmath.h"
+#include <assert.h>
+#include <math.h>
+#include <stdbool.h>
#include <stdio.h>
+bool assertFLTEquals(FLT a, FLT b) { return fabs(a - b) < 0.0001; }
+
+int assertFLTAEquals(FLT *a, FLT *b, int length) {
+ for (int i = 0; i < length; i++) {
+ if (assertFLTEquals(a[i], b[i]) != true) {
+ return i;
+ }
+ }
+ return -1;
+}
+
+void printFLTA(FLT *a, int length) {
+ for (int i = 0; i < length; i++)
+ fprintf(stderr, "%.7f ", a[i]);
+}
+
+#define ASSERT_FLTA_EQUALS(a, b, l) \
+ if (assertFLTAEquals(a, b, l) != -1) { \
+ fprintf(stderr, "Where '" #a "'= "); \
+ printFLTA(a, l); \
+ fprintf(stderr, "\nWhere '" #b "'= "); \
+ printFLTA(b, l); \
+ fprintf(stderr, "\n"); \
+ assert(assertFLTAEquals(a, b, l) == -1); \
+ }
+
+void testInvertPose() {
+
+ FLT rotAroundYOffset[7] = {1, 1, 1, .5, 0, .5, 0};
+ FLT pose_out[7];
+
+ {
+ FLT expected[] = {1, -1, -1, 0.7071068, 0, -0.7071068, 0};
+ InvertPose(pose_out, rotAroundYOffset);
+ ASSERT_FLTA_EQUALS(pose_out, expected, 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);
+ }
+}
+
+void testApplyPoseToPose() {
+ FLT rotAroundYOffset[7] = {1, 1, 1, 0, 0, 1, 0};
+ FLT pose_out[7];
+
+ {
+ 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);
+ }
+
+ {
+ 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);
+ }
+}
+
+void testApplyPoseToPoint() {
+ FLT rotAroundY[7] = {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);
+ ASSERT_FLTA_EQUALS(pt_out, expected, 3);
+ }
+
+ {
+ FLT pt[3] = {1, 1, 0};
+ FLT expected[3] = {-1, 1, 0};
+ ApplyPoseToPoint(pt_out, rotAroundY, pt);
+ ASSERT_FLTA_EQUALS(pt_out, expected, 3);
+ }
+
+ FLT rotAroundYOffset[7] = {1, 1, 1, 0, 0, 1, 0};
+
+ {
+ FLT pt[3] = {0, 1, 0};
+ FLT expected[3] = {1, 2, 1};
+ 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);
+ ASSERT_FLTA_EQUALS(pt_out, expected, 3);
+ }
+}
+
int main()
{
+ testInvertPose();
+ testApplyPoseToPoint();
+ testApplyPoseToPose();
#if 1
#define NONTRANSPOSED_DAVE
diff --git a/simple_pose_test.c b/simple_pose_test.c
index aef3de5..71b6ae9 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, FLT *pose
hpos[1] = pose[1];
hpos[2] = pose[2];
FLT hposin[3] = { 0, 0, 1 };
- ApplyPoseToPoint( hpos2, hposin, pose );
+ ApplyPoseToPoint(hpos2, pose, hposin);
fflush(stdout);
}