diff options
author | Justin Berger <jdavidberger@gmail.com> | 2018-07-01 05:26:29 +0000 |
---|---|---|
committer | Justin Berger <jdavidberger@gmail.com> | 2018-07-01 05:26:29 +0000 |
commit | caa3f1f1c57b7876156eedbc3738e90765f72d14 (patch) | |
tree | 8e1a6a6e880f4df63a2fb5281de3a978fc9e0c1e /redist/lintest.c | |
parent | 47141c873ffd3d7af62bfba40b1adbcce0df6574 (diff) | |
download | libsurvive-caa3f1f1c57b7876156eedbc3738e90765f72d14.tar.gz libsurvive-caa3f1f1c57b7876156eedbc3738e90765f72d14.tar.bz2 |
Added kabsch / test for kabsch
Diffstat (limited to 'redist/lintest.c')
-rw-r--r-- | redist/lintest.c | 28 |
1 files changed, 28 insertions, 0 deletions
diff --git a/redist/lintest.c b/redist/lintest.c index 4c53da7..ee3082a 100644 --- a/redist/lintest.c +++ b/redist/lintest.c @@ -102,11 +102,39 @@ void testApplyPoseToPoint() { } } +void testKabsch() { + FLT pts[] = {0, 0, 0, 100, 100, 100, 10, 0, 10, 50, 50, 0, 0, 0, 1000, -100, 0, 100}; + + LinmathPose tx = {.Pos = {}, .Rot = {4, 3, 2, 1}}; + + LinmathPose tx2 = {.Pos = {1, 2, 3}, .Rot = {1, 2, 3, 4}}; + + quatnormalize(tx.Rot, tx.Rot); + quatnormalize(tx2.Rot, tx2.Rot); + + int N = sizeof(pts) / sizeof(FLT) / 3; + FLT txPts[N * 3]; + FLT txPts2[N * 3]; + for (int i = 0; i < N; i++) { + ApplyPoseToPoint(txPts + i * 3, &tx, pts + i * 3); + ApplyPoseToPoint(txPts2 + i * 3, &tx2, pts + i * 3); + } + + LinmathQuat should_be_tx = {}; + KabschCentered(should_be_tx, pts, txPts, N); + ASSERT_FLTA_EQUALS(should_be_tx, tx.Rot, 4); + + LinmathPose should_be_tx2 = {}; + Kabsch(&should_be_tx2, pts, txPts2, N); + ASSERT_FLTA_EQUALS(should_be_tx2.Pos, tx2.Pos, 7); +} + int main() { testInvertPose(); testApplyPoseToPoint(); testApplyPoseToPose(); + testKabsch(); #if 1 #define NONTRANSPOSED_DAVE |