ViSP  2.8.0
vpRzyzVector.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRzyzVector.cpp 4056 2013-01-05 13:04:42Z fspindle $
4  *
5  * This file is part of the ViSP software.
6  * Copyright (C) 2005 - 2013 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  * Euler angles parameterization for the rotation.
36  * Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi)
37  *
38  * Authors:
39  * Eric Marchand
40  * Fabien Spindler
41  *
42  *****************************************************************************/
43 
44 
45 #include <visp/vpRzyzVector.h>
46 #include <math.h>
47 
58 vpRzyzVector &
59 vpRzyzVector::operator=(const vpRzyzVector &m)
60 {
61 
62  for (int i=0; i<3; i++)
63  {
64  r[i] = m.r[i] ;
65  }
66  return *this;
67 }
68 
69 
71 vpRzyzVector::vpRzyzVector(const vpRzyzVector &m) : vpRotationVector()
72 {
73  *this = m ;
74 }
75 
81 vpRzyzVector::vpRzyzVector(const vpRotationMatrix& R)
82 {
83  buildFrom(R) ;
84 }
85 
86 
93 vpRzyzVector::vpRzyzVector(const vpThetaUVector& tu)
94 {
95  buildFrom(tu) ;
96 }
97 
98 
106 vpRzyzVector
107 vpRzyzVector::buildFrom(const vpRotationMatrix& R)
108 {
109  double phi ;
110  if ((fabs(R[1][2]) < 1e-6) &&(fabs(R[0][2]) < 1e-6))
111  phi = 0 ;
112  else
113  phi = atan2(R[1][2],R[0][2]) ;
114  double cphi = cos(phi) ;
115  double sphi = sin(phi) ;
116 
117  double theta = atan2(cphi*R[0][2]+sphi*R[1][2],R[2][2]);
118 
119  double psi = atan2(-sphi*R[0][0]+cphi*R[1][0],-sphi*R[0][1]+cphi*R[1][1]) ;
120 
121  r[0] = phi ;
122  r[1] = theta ;
123  r[2] = psi ;
124 
125  return *this ;
126 }
127 
128 
136 vpRzyzVector
137 vpRzyzVector::buildFrom(const vpThetaUVector& tu)
138 {
139 
140  vpRotationMatrix R ;
141  R.buildFrom(tu) ;
142  buildFrom(R) ;
143 
144  return *this ;
145 }
146 
147 
148 
149 /*
150  * Local variables:
151  * c-basic-offset: 2
152  * End:
153  */
154 
vpRotationVector
Class that consider the case of a generic rotation vector (cannot be used as is !) consisting in thre...
Definition: vpRotationVector.h:95
vpRzyzVector::operator=
vpRzyzVector & operator=(const vpRzyzVector &m)
Definition: vpRzyzVector.cpp:59
vpRotationMatrix
The vpRotationMatrix considers the particular case of a rotation matrix.
Definition: vpRotationMatrix.h:72
vpRotationMatrix::buildFrom
vpRotationMatrix buildFrom(const vpThetaUVector &v)
Transform a vector vpThetaUVector into an rotation matrix.
Definition: vpRotationMatrix.cpp:497
vpRzyzVector
Class that consider the case of the Euler angles using the z-y-z convention, where are respectively...
Definition: vpRzyzVector.h:150
vpRzyzVector::buildFrom
void buildFrom(const double phi, const double theta, const double psi)
Definition: vpRzyzVector.h:186
vpThetaUVector
Class that consider the case of the parameterization for the rotation.
Definition: vpThetaUVector.h:140
vpRzyzVector::vpRzyzVector
vpRzyzVector()
Default constructor. Initialize the angles to zero.
Definition: vpRzyzVector.h:157