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/linmath.c | |
parent | 47141c873ffd3d7af62bfba40b1adbcce0df6574 (diff) | |
download | libsurvive-caa3f1f1c57b7876156eedbc3738e90765f72d14.tar.gz libsurvive-caa3f1f1c57b7876156eedbc3738e90765f72d14.tar.bz2 |
Added kabsch / test for kabsch
Diffstat (limited to 'redist/linmath.c')
-rw-r--r-- | redist/linmath.c | 32 |
1 files changed, 31 insertions, 1 deletions
diff --git a/redist/linmath.c b/redist/linmath.c index 80eeaa5..7d21e5c 100644 --- a/redist/linmath.c +++ b/redist/linmath.c @@ -1,5 +1,4 @@ // Copyright 2013,2016 <>< C. N. Lohr. This file licensed under the terms of the MIT license. - #include "linmath.h" #include <float.h> #include <math.h> @@ -678,5 +677,36 @@ void KabschCentered(LinmathQuat qout, const FLT *ptsA, const FLT *ptsB, int num_ quatfrommatrix33(qout, _R); } +LINMATH_EXPORT void Kabsch(LinmathPose *B2Atx, const FLT *_ptsA, const FLT *_ptsB, int num_pts) { + FLT centerA[3] = {}; + FLT centerB[3] = {}; + + for (int i = 0; i < num_pts; i++) { + for (int j = 0; j < 3; j++) { + centerA[j] += _ptsA[i * 3 + j]; + centerB[j] += _ptsB[i * 3 + j]; + } + } + + for (int j = 0; j < 3; j++) { + centerA[j] = centerA[j] / (FLT)num_pts; + centerB[j] = centerB[j] / (FLT)num_pts; + } + + FLT ptsA[num_pts * 3]; + FLT ptsB[num_pts * 3]; + + for (int i = 0; i < num_pts; i++) { + for (int j = 0; j < 3; j++) { + ptsA[i * 3 + j] = _ptsA[i * 3 + j] - centerA[j]; + ptsB[i * 3 + j] = _ptsB[i * 3 + j] - centerB[j]; + } + } + + KabschCentered(B2Atx->Rot, ptsA, ptsB, num_pts); + quatrotatevector(centerA, B2Atx->Rot, centerA); + sub3d(B2Atx->Pos, centerB, centerA); +} + LinmathQuat LinmathQuat_Identity = {1.0}; LinmathPose LinmathPose_Identity = {.Rot = {1.0}}; |