From e842451d8e8f77f2ab2b4537d4619611b61c8c48 Mon Sep 17 00:00:00 2001 From: Stanislaw Halik Date: Sat, 2 Mar 2019 00:22:32 +0100 Subject: tracker/pt: compute intermediates in double precision --- tracker-pt/ftnoir_tracker_pt.cpp | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) (limited to 'tracker-pt') 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; -- cgit v1.2.3