Visual Servoing Platform  version 3.6.1 under development (2024-03-29)
vpRzyxVector.cpp
1 /****************************************************************************
2  *
3  * ViSP, open source Visual Servoing Platform software.
4  * Copyright (C) 2005 - 2023 by Inria. All rights reserved.
5  *
6  * This software is free software; you can redistribute it and/or modify
7  * it under the terms of the GNU General Public License as published by
8  * the Free Software Foundation; either version 2 of the License, or
9  * (at your option) any later version.
10  * See the file LICENSE.txt at the root directory of this source
11  * distribution for additional information about the GNU GPL.
12  *
13  * For using ViSP with software that can not be combined with the GNU
14  * GPL, please contact Inria about acquiring a ViSP Professional
15  * Edition License.
16  *
17  * See https://visp.inria.fr for more information.
18  *
19  * This software was developed at:
20  * Inria Rennes - Bretagne Atlantique
21  * Campus Universitaire de Beaulieu
22  * 35042 Rennes Cedex
23  * France
24  *
25  * If you have questions regarding the use of this file, please contact
26  * Inria at visp@inria.fr
27  *
28  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
29  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
30  *
31  * Description:
32  * Rzyx angle parameterization for the rotation.
33  * Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(x,psi)
34  *
35 *****************************************************************************/
36 
37 #include <math.h>
38 #include <visp3/core/vpRzyxVector.h>
48 
51 
58 vpRzyxVector::vpRzyxVector(double phi, double theta, double psi) : vpRotationVector(3) { buildFrom(phi, theta, psi); }
59 
66 
74 
77 
79 vpRzyxVector::vpRzyxVector(const std::vector<double> &rzyx) : vpRotationVector(3) { buildFrom(rzyx); }
80 
92 {
93  double nx = R[0][0];
94  double ny = R[1][0];
95 
96  double COEF_MIN_ROT = 1e-6;
97  double phi;
98 
99  if ((fabs(nx) < COEF_MIN_ROT) && (fabs(ny) < COEF_MIN_ROT)) {
100  phi = 0;
101  }
102  else {
103  phi = atan2(ny, nx);
104  }
105  double si = sin(phi);
106  double co = cos(phi);
107 
108  double nz = R[2][0];
109  double theta = atan2(-nz, co * nx + si * ny);
110 
111  double ax = R[0][2];
112  double ay = R[1][2];
113  double ox = R[0][1];
114  double oy = R[1][1];
115 
116  double psi = atan2(si * ax - co * ay, -si * ox + co * oy);
117 
118  buildFrom(phi, theta, psi);
119 
120  return *this;
121 }
122 
130 {
132  R.buildFrom(tu);
133  buildFrom(R);
134 
135  return *this;
136 }
137 
144 void vpRzyxVector::buildFrom(double phi, double theta, double psi)
145 {
146  data[0] = phi;
147  data[1] = theta;
148  data[2] = psi;
149 }
150 
155 {
156  if (rzyx.size() != 3) {
157  throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension col vector",
158  rzyx.size()));
159  }
160  for (unsigned int i = 0; i < 3; i++)
161  data[i] = rzyx[i];
162 
163  return *this;
164 }
165 
169 vpRzyxVector vpRzyxVector::buildFrom(const std::vector<double> &rzyx)
170 {
171  if (rzyx.size() != 3) {
172  throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension std::vector",
173  rzyx.size()));
174  }
175  for (unsigned int i = 0; i < 3; i++)
176  data[i] = rzyx[i];
177 
178  return *this;
179 }
180 
201 {
202  for (unsigned int i = 0; i < dsize; i++)
203  data[i] = v;
204 
205  return *this;
206 }
207 
231 {
232  if (rzyx.size() != 3) {
233  throw(vpException(vpException::dimensionError, "Cannot set a R-zyx vector from a %d-dimension col vector",
234  rzyx.size()));
235  }
236  for (unsigned int i = 0; i < 3; i++)
237  data[i] = rzyx[i];
238 
239  return *this;
240 }
241 
242 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
260 vpRzyxVector &vpRzyxVector::operator=(const std::initializer_list<double> &list)
261 {
262  if (list.size() > size()) {
263  throw(vpException(
265  "Cannot set Euler x-y-z vector out of bounds. It has only %d values while you try to initialize with %d values",
266  size(), list.size()));
267  }
268  std::copy(list.begin(), list.end(), data);
269  return *this;
270 }
271 #endif
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:138
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:134
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:286
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:83
Implementation of a rotation matrix and operations on such kind of matrices.
vpRotationMatrix buildFrom(const vpHomogeneousMatrix &M)
Implementation of a generic rotation vector.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRzyxVector.h:177
vpRzyxVector & operator=(const vpColVector &rzyx)
vpRzyxVector buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as axis-angle minimal representation.