diff options
author | Stanislaw Halik <sthalik@misaki.pl> | 2019-03-02 00:22:32 +0100 |
---|---|---|
committer | Stanislaw Halik <sthalik@misaki.pl> | 2019-03-02 00:22:39 +0100 |
commit | e842451d8e8f77f2ab2b4537d4619611b61c8c48 (patch) | |
tree | 6a7813530339039b36d23e85362010b6f916b1e4 | |
parent | 7cc7876eda30779ab7a9c7a4fbb58cc675fa3f9b (diff) |
tracker/pt: compute intermediates in double precision
-rw-r--r-- | tracker-pt/ftnoir_tracker_pt.cpp | 11 |
1 files changed, 7 insertions, 4 deletions
diff --git a/tracker-pt/ftnoir_tracker_pt.cpp b/tracker-pt/ftnoir_tracker_pt.cpp index 97404ec4..c3b6c6e3 100644 --- a/tracker-pt/ftnoir_tracker_pt.cpp +++ b/tracker-pt/ftnoir_tracker_pt.cpp @@ -171,10 +171,13 @@ void Tracker_PT::data(double *data) const vec3& t = X_GH.t; // extract rotation angles - double alpha, beta, gamma; - beta = atan2( (double)-R(2,0), (double)sqrt(R(2,1)*R(2,1) + R(2,2)*R(2,2)) ); - alpha = atan2( (double)R(1,0), (double)R(0,0) ); - gamma = atan2( (double)R(2,1), (double)R(2,2) ); + auto r00 = (double)R(0, 0); + auto r10 = (double)R(1,0), r20 = (double)R(2,0); + auto r21 = (double)R(2,1), r22 = (double)R(2,2); + + double beta = atan2(-r20, sqrt(r21*r21 + r22*r22)); + double alpha = atan2(r10, r00); + double gamma = atan2(r21, r22); constexpr double rad2deg = 180/M_PI; |