summaryrefslogtreecommitdiffhomepage
diff options
context:
space:
mode:
authorStanislaw Halik <sthalik@misaki.pl>2019-03-02 00:22:32 +0100
committerStanislaw Halik <sthalik@misaki.pl>2019-03-02 00:22:39 +0100
commite842451d8e8f77f2ab2b4537d4619611b61c8c48 (patch)
tree6a7813530339039b36d23e85362010b6f916b1e4
parent7cc7876eda30779ab7a9c7a4fbb58cc675fa3f9b (diff)
tracker/pt: compute intermediates in double precision
-rw-r--r--tracker-pt/ftnoir_tracker_pt.cpp11
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;