ViSP  2.10.0
vpRxyzVector.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRxyzVector.cpp 4632 2014-02-03 17:06:40Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2014 by INRIA. All rights reserved.
7  *
8  * This software is free software; you can redistribute it and/or
9  * modify it under the terms of the GNU General Public License
10  * ("GPL") version 2 as published by the Free Software Foundation.
11  * See the file LICENSE.txt at the root directory of this source
12  * distribution for additional information about the GNU GPL.
13  *
14  * For using ViSP with software that can not be combined with the GNU
15  * GPL, please contact INRIA about acquiring a ViSP Professional
16  * Edition License.
17  *
18  * See http://www.irisa.fr/lagadic/visp/visp.html for more information.
19  *
20  * This software was developed at:
21  * INRIA Rennes - Bretagne Atlantique
22  * Campus Universitaire de Beaulieu
23  * 35042 Rennes Cedex
24  * France
25  * http://www.irisa.fr/lagadic
26  *
27  * If you have questions regarding the use of this file, please contact
28  * INRIA at visp@inria.fr
29  *
30  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
31  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
32  *
33  *
34  * Description:
35  * Rxyz angle parameterization for the rotation.
36  * Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi).
37  *
38  * Authors:
39  * Eric Marchand
40  * Fabien Spindler
41  *
42  *****************************************************************************/
43 
44 
45 #include <visp/vpRxyzVector.h>
46 #include <math.h>
60 {
61  buildFrom(R) ;
62 }
63 
71 {
72  buildFrom(tu) ;
73 }
74 
84 {
85 
86  double COEF_MIN_ROT = 1e-6;
87  double phi ;
88 
89  if ((fabs(R[1][2]) < COEF_MIN_ROT) && (fabs(R[2][2]) < COEF_MIN_ROT)) phi = 0 ;
90  else phi = atan2(-R[1][2], R[2][2]) ;
91 
92  double si = sin(phi) ;
93  double co = cos(phi) ;
94  double theta = atan2(R[0][2], -si*R[1][2] + co*R[2][2]) ;
95  double psi = atan2(co*R[1][0] + si*R[2][0], co*R[1][1] + si*R[2][1]);
96 
97  r[0] = phi ;
98  r[1] = theta ;
99  r[2] = psi ;
100 
101  if (0) // test new version wrt old version
102  {
103  // old version
104 
105  double v1;
106  double r2[3]; // has to be replaced by r below if good version
107 
108  v1 = R[0][2];
109  if (v1 > 1.0 ) v1 = 1.0;
110  if (v1 < -1.0 ) v1 = -1.0;
111  r2[1] = asin(v1);
112  if ( fabs(fabs(r2[1]) - M_PI_2) < 0.00001)
113  {
114  r2[0] = 0.0;
115  r2[2] = atan2(R[1][0],R[1][1]);
116  }
117  else
118  {
119  r2[0] = atan2(-R[1][2],R[2][2]);
120  r2[2] = atan2(-R[0][1],R[0][0]);
121  }
122  // verification of the new version
123  int pb = 0;
124  int i;
125  for (i=0;i<3;i++)
126  {
127  if (fabs(r[i] - r2[i]) > 1e-5) pb = 1;
128  }
129  if (pb == 1)
130  {
131  printf("vpRxyzVector::buildFrom(const vpRotationMatrix& R)\n");
132  printf(" r : %lf %lf %lf\n",r[0],r[1],r[2]);
133  printf(" r2 : %lf %lf %lf\n",r2[0],r2[1],r2[2]);
134  printf(" r - r2 : %lf %lf %lf\n",r[0]-r2[0],r[1]-r2[1],r[2]-r2[2]);
135  }
136  }
137 
138  // What is below corresponds to another representation, but which one???
139  /* double phi ;
140  if ((fabs(R[1][2]) < 1e-6)&&(fabs(R[2][2]) < 1e-6)) phi = 0 ;
141  else phi = atan2(R[1][2], R[2][2]) ;
142 
143  double si = sin(phi) ;
144  double co = cos(phi) ;
145  double theta = atan2(R[0][2], -si*R[1][2] + co*R[2][2]) ;
146  double psi = atan2(co*R[0][1]+si*R[0][2], co*R[1][1]+si*R[1][2]);
147 
148  r[0] = phi ;
149  r[1] = theta ;
150  r[2] = psi ;*/
151 
152  return *this ;
153 }
154 
164 {
165  vpRotationMatrix R ;
166  R.buildFrom(tu) ;
167  buildFrom(R) ;
168 
169  return *this ;
170 }
171 
172 /*
173  * Local variables:
174  * c-basic-offset: 2
175  * End:
176  */
The vpRotationMatrix considers the particular case of a rotation matrix.
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Build a rotation matrix from an homogeneous matrix.
Class that consider the case of the Euler angle using the x-y-z convention, where are respectively ...
Definition: vpRxyzVector.h:152
void buildFrom(const double phi, const double theta, const double psi)
Definition: vpRxyzVector.h:184
Class that consider the case of the parameterization for the rotation.