ViSP  2.8.0
vpRzyxVector.cpp
1 /****************************************************************************
2  *
3  * $Id: vpRzyxVector.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  * Rzyx angle parameterization for the rotation.
36  * Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(x,psi)
37  *
38  * Authors:
39  * Eric Marchand
40  * Fabien Spindler
41  *
42  *****************************************************************************/
43 
44 
45 #include <visp/vpRzyxVector.h>
46 #include <math.h>
58 vpRzyxVector &
59 vpRzyxVector::operator=(const vpRzyxVector &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 vpRzyxVector::vpRzyxVector(const vpRzyxVector &m) : vpRotationVector()
72 {
73  *this = m ;
74 }
75 
76 
82 vpRzyxVector::vpRzyxVector(const vpRotationMatrix& R)
83 {
84  buildFrom(R) ;
85 }
86 
87 
94 vpRzyxVector::vpRzyxVector(const vpThetaUVector& tu)
95 {
96  buildFrom(tu) ;
97 }
98 
109 vpRzyxVector
110 vpRzyxVector::buildFrom(const vpRotationMatrix& R)
111 {
112  double nx = R[0][0];
113  double ny = R[1][0];
114 
115  double phi = atan2(ny,nx) ;
116  double si = sin(phi) ;
117  double co = cos(phi) ;
118 
119  double nz = R[2][0];
120  double theta = atan2(-nz, co*nx+si*ny) ;
121 
122  double ax = R[0][2];
123  double ay = R[1][2];
124  double ox = R[0][1];
125  double oy = R[1][1];
126 
127  double psi = atan2(si*ax-co*ay,-si*ox+co*oy);
128 
129  r[0] = phi ;
130  r[1] = theta ;
131  r[2] = psi ;
132 
133  return *this ;
134 }
135 
136 
144 vpRzyxVector
145 vpRzyxVector::buildFrom(const vpThetaUVector& tu)
146 {
147  vpRotationMatrix R ;
148  R.buildFrom(tu) ;
149  buildFrom(R) ;
150 
151  return *this ;
152 }
153 
154 
155 /*
156  * Local variables:
157  * c-basic-offset: 2
158  * End:
159  */
vpRzyxVector::vpRzyxVector
vpRzyxVector()
Default constructor. Initialize the angles to zero.
Definition: vpRzyxVector.h:158
vpRotationVector
Class that consider the case of a generic rotation vector (cannot be used as is !) consisting in thre...
Definition: vpRotationVector.h:95
vpRzyxVector::operator=
vpRzyxVector & operator=(const vpRzyxVector &m)
Definition: vpRzyxVector.cpp:59
vpRzyxVector
Class that consider the case of the Euler angle using the z-y-x convention, where are respectively ...
Definition: vpRzyxVector.h:151
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
vpRzyxVector::buildFrom
void buildFrom(const double phi, const double theta, const double psi)
Definition: vpRzyxVector.h:187
vpThetaUVector
Class that consider the case of the parameterization for the rotation.
Definition: vpThetaUVector.h:140