From ffedc87d0f92f8942169ce53ee8006f71f3688eb Mon Sep 17 00:00:00 2001
From: Christophe Geuzaine <cgeuzaine@ulg.ac.be>
Date: Mon, 11 Oct 2004 17:21:16 +0000
Subject: [PATCH] compute Euler angles from rotation matrix
---
Common/Context.cpp | 130 ++++++++++++++-------------------------------
1 file changed, 41 insertions(+), 89 deletions(-)
diff --git a/Common/Context.cpp b/Common/Context.cpp
index 512aef0e3a..bbf955e848 100644
--- a/Common/Context.cpp
+++ b/Common/Context.cpp
@@ -1,4 +1,4 @@
-// $Id: Context.cpp,v 1.50 2004-02-07 01:40:17 geuzaine Exp $
+// $Id: Context.cpp,v 1.51 2004-10-11 17:21:16 geuzaine Exp $
//
// Copyright (C) 1997-2004 C. Geuzaine, J.-F. Remacle
//
@@ -29,102 +29,54 @@
#include "DefaultOptions.h"
#include "Trackball.h"
-/*
- 3 successive rotations along x, y and z:
-
- c(y)c(z) s(x)s(y)c(z)+c(x)s(z) -c(x)s(y)c(z)+s(x)s(z)
- t[][] = -c(y)s(z) -s(x)s(y)s(z)+c(x)c(z) c(x)s(y)s(z)+s(x)c(z)
- s(y) -s(x)c(y) c(x)c(y)
-
- get the position angles:
-
- y = asin(t31)
- Pi - asin(t31)
-
- si y != +- Pi/2 :
-
- x = atan(-t32/t33) si t33 cos y > 0
- atan(-t32/t33)+Pi si t33 cos y < 0
-
- z = atan(-t21/t11) si t11 cos y > 0
- atan(-t21/t11)+Pi si t11 cos y < 0
-
-*/
-
void Context_T::buildRotmatrix(void)
{
- double x, y, z;
-
if(useTrackball) {
build_rotmatrix(rot, quaternion);
-#if defined(HAVE_FLTK)
- // We should reconstruct the Euler angles from the rotation
- // matrix. I'm too lazy to do it :-(
- extern void set_r(int i, double val);
- set_r(0, 0.);
- set_r(1, 0.);
- set_r(2, 0.);
- /*
- double x=0., y=0., z=0.
-
- y = asin(rot[2][0]) ; y = Pi - asin(rot[2][0]) ; // choix ???
-
- if(fabs(y) != Pi/2.){
- if(rot[2][2]*cos(y) > 0.) x = atan2(-rot[2][1],rot[2][2]);
- else x = atan2(-rot[2][1],rot[2][2]) + Pi;
- if(rot[0][0]*cos(y) > 0.) z = atan2(-rot[1][0],rot[0][0]);
- else z = atan2(-rot[1][0],rot[0][0]) + Pi;
- }
- set_r(0, x * 180./Pi);
- set_r(1, y * 180./Pi);
- set_r(2, z * 180./Pi);
- */
- /*
- double r0, r1, r2;
-
- r1 = atan2(-rot[0][2],sqrt(rot[1][2]*rot[1][2] + rot[2][2]*rot[2][2]));
-
- double c = cos(r1);
- if(c != 0.0){
- r0 = atan2(rot[1][2]/c,rot[2][2]/c) ;
- r2 = atan2(-rot[1][0]/c,rot[0][0]/c) ;
- r0 *= 180./(Pi);
- r2 *= 180./(Pi);
- }
- set_r(0, r0);
- set_r(1, r1 * 180./(Pi)); // lazyyyyyy
- set_r(2, r2);
- */
-#endif
-
+ // get Euler angles from rotation matrix
+ r[1] = asin(rot[2][0]); // Calculate Y-axis angle
+ double C = cos(r[1]);
+ r[1] *= 180. / Pi;
+ if(fabs(C) > 0.005){ // Gimball lock?
+ double tmpx = rot[2][2] / C; // No, so get X-axis angle
+ double tmpy = -rot[2][1] / C;
+ r[0] = atan2(tmpy, tmpx) * 180. / Pi;
+ tmpx = rot[0][0] / C; // Get Z-axis angle
+ tmpy = -rot[1][0] / C;
+ r[2] = atan2(tmpy, tmpx) * 180. / Pi;
+ }
+ else{ // Gimball lock has occurred
+ r[0] = 0.; // Set X-axis angle to zero
+ double tmpx = rot[1][1]; // And calculate Z-axis angle
+ double tmpy = rot[0][1];
+ r[2] = atan2(tmpy, tmpx) * 180. / Pi;
+ }
+ // return only positive angles in [0,360]
+ if(r[0] < 0.) r[0] += 360.;
+ if(r[1] < 0.) r[1] += 360.;
+ if(r[2] < 0.) r[2] += 360.;
}
else {
- x = r[0] * Pi / 180.;
- y = r[1] * Pi / 180.;
- z = r[2] * Pi / 180.;
-
- rot[0][0] = cos(y) * cos(z);
- rot[0][1] = sin(x) * sin(y) * cos(z) + cos(x) * sin(z);
- rot[0][2] = -cos(x) * sin(y) * cos(z) + sin(x) * sin(z);
- rot[0][3] = 0.0;
-
- rot[1][0] = -cos(y) * sin(z);
- rot[1][1] = -sin(x) * sin(y) * sin(z) + cos(x) * cos(z);
- rot[1][2] = cos(x) * sin(y) * sin(z) + sin(x) * cos(z);
- rot[1][3] = 0.0;
-
- rot[2][0] = sin(y);
- rot[2][1] = -sin(x) * cos(y);
- rot[2][2] = cos(x) * cos(y);
- rot[2][3] = 0.0;
-
- rot[3][0] = 0.0;
- rot[3][1] = 0.0;
- rot[3][2] = 0.0;
- rot[3][3] = 1.0;
+ double x = r[0] * Pi / 180.;
+ double y = r[1] * Pi / 180.;
+ double z = r[2] * Pi / 180.;
+ double A = cos(x);
+ double B = sin(x);
+ double C = cos(y);
+ double D = sin(y);
+ double E = cos(z);
+ double F = sin(z);
+ double AD = A * D;
+ double BD = B * D;
+ rot[0][0] = C*E; rot[0][1] = BD*E+A*F; rot[0][2] =-AD*E+B*F; rot[0][3] = 0.;
+ rot[1][0] =-C*F; rot[1][1] =-BD*F+A*E; rot[1][2] = AD*F+B*E; rot[1][3] = 0.;
+ rot[2][0] = D; rot[2][1] =-B*C; rot[2][2] = A*C; rot[2][3] = 0.;
+ rot[3][0] = 0.; rot[3][1] = 0.; rot[3][2] = 0.; rot[3][3] = 1.;
+
+ // get the quaternion from the Euler angles
+ // todo
}
-
}
void Context_T::addQuaternion(float p1x, float p1y, float p2x, float p2y)
--
GitLab