Visual Servoing Platform  version 3.6.1 under development (2024-12-10)
vpRzyxVector.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Rzyx angle parameterization for the rotation.
32  * Rzyx(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(x,psi)
33  */
34 
42 #include <math.h>
43 #include <visp3/core/vpRzyxVector.h>
44 
45 BEGIN_VISP_NAMESPACE
46 const unsigned int vpRzyxVector::constr_val_3 = 3;
49 
52 
59 vpRzyxVector::vpRzyxVector(double phi, double theta, double psi) : vpRotationVector(constr_val_3) { buildFrom(phi, theta, psi); }
60 
67 
75 
77 vpRzyxVector::vpRzyxVector(const vpColVector &rzyx) : vpRotationVector(constr_val_3) { buildFrom(rzyx); }
78 
80 vpRzyxVector::vpRzyxVector(const std::vector<double> &rzyx) : vpRotationVector(constr_val_3) { buildFrom(rzyx); }
81 
93 {
94  const unsigned int index_0 = 0;
95  const unsigned int index_1 = 1;
96  const unsigned int index_2 = 2;
97  double nx = R[index_0][index_0];
98  double ny = R[index_1][index_0];
99 
100  double COEF_MIN_ROT = 1e-6;
101  double phi;
102 
103  if ((fabs(nx) < COEF_MIN_ROT) && (fabs(ny) < COEF_MIN_ROT)) {
104  phi = 0;
105  }
106  else {
107  phi = atan2(ny, nx);
108  }
109  double si = sin(phi);
110  double co = cos(phi);
111 
112  double nz = R[index_2][index_0];
113  double theta = atan2(-nz, (co * nx) + (si * ny));
114 
115  double ax = R[index_0][index_2];
116  double ay = R[index_1][index_2];
117  double ox = R[index_0][index_1];
118  double oy = R[index_1][index_1];
119 
120  double psi = atan2((si * ax) - (co * ay), (-si * ox) + (co * oy));
121 
122  buildFrom(phi, theta, psi);
123 
124  return *this;
125 }
126 
134 {
136  R.buildFrom(tu);
137  buildFrom(R);
138 
139  return *this;
140 }
141 
148 vpRzyxVector &vpRzyxVector::buildFrom(const double &phi, const double &theta, const double &psi)
149 {
150  const unsigned int index_0 = 0;
151  const unsigned int index_1 = 1;
152  const unsigned int index_2 = 2;
153  data[index_0] = phi;
154  data[index_1] = theta;
155  data[index_2] = psi;
156  return *this;
157 }
158 
163 {
164  const unsigned int val_3 = 3;
165  if (rzyx.size() != val_3) {
166  throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension col vector",
167  rzyx.size()));
168  }
169  for (unsigned int i = 0; i < val_3; ++i) {
170  data[i] = rzyx[i];
171  }
172 
173  return *this;
174 }
175 
179 vpRzyxVector &vpRzyxVector::buildFrom(const std::vector<double> &rzyx)
180 {
181  const unsigned int val_3 = 3;
182  if (rzyx.size() != val_3) {
183  throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension std::vector",
184  rzyx.size()));
185  }
186  for (unsigned int i = 0; i < val_3; ++i) {
187  data[i] = rzyx[i];
188  }
189 
190  return *this;
191 }
192 
217 {
218  for (unsigned int i = 0; i < dsize; ++i) {
219  data[i] = v;
220  }
221 
222  return *this;
223 }
224 
252 {
253  const unsigned int val_3 = 3;
254  if (rzyx.size() != val_3) {
255  throw(vpException(vpException::dimensionError, "Cannot set a R-zyx vector from a %d-dimension col vector",
256  rzyx.size()));
257  }
258  for (unsigned int i = 0; i < val_3; ++i) {
259  data[i] = rzyx[i];
260  }
261 
262  return *this;
263 }
264 
265 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
287 vpRzyxVector &vpRzyxVector::operator=(const std::initializer_list<double> &list)
288 {
289  if (list.size() > size()) {
290  throw(vpException(
292  "Cannot set Euler x-y-z vector out of bounds. It has only %d values while you try to initialize with %d values",
293  size(), list.size()));
294  }
295  std::copy(list.begin(), list.end(), data);
296  return *this;
297 }
298 #endif
299 END_VISP_NAMESPACE
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
unsigned int dsize
Current array size (rowNum * colNum)
Definition: vpArray2D.h:1107
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
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:184
vpRzyxVector & operator=(const vpColVector &rzyx)
vpRzyxVector & buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as axis-angle minimal representation.