From 5153a32da9793945f72d60185d1929cd72ceeea3 Mon Sep 17 00:00:00 2001 From: Justin Berger Date: Sun, 11 Mar 2018 14:48:08 -0600 Subject: Added missing functions into lintest --- redist/Makefile | 9 +++-- redist/linmath.c | 14 ++++++-- redist/linmath.h | 17 +++++++-- redist/lintest.c | 104 +++++++++++++++++++++++++++++++++++++++++++++++++++++ simple_pose_test.c | 2 +- 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 +#include +#include #include +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); } -- cgit v1.2.3