Visual Servoing Platform  version 3.6.1 under development (2024-07-15)
vpRxyzVector.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  * Rxyz angle parameterization for the rotation.
32  * Rxyz(phi,theta,psi) = Rot(x,phi)Rot(y,theta)Rot(z,psi).
33  */
34 
41 #include <math.h>
42 
43 #include <visp3/core/vpRxyzVector.h>
44 
48 
51 
58 vpRxyzVector::vpRxyzVector(double phi, double theta, double psi) : vpRotationVector(3) { build(phi, theta, psi); }
59 
66 
74 
77 
79 vpRxyzVector::vpRxyzVector(const std::vector<double> &rxyz) : vpRotationVector(3) { build(rxyz); }
80 
81 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
91 {
92  build(R);
93  return *this;
94 }
95 
104 {
105  build(tu);
106  return *this;
107 }
108 
116 void vpRxyzVector::buildFrom(double phi, double theta, double psi)
117 {
118  build(phi, theta, psi);
119 }
120 
126 {
127  build(rxyz);
128  return *this;
129 }
130 
135 vpRxyzVector vpRxyzVector::buildFrom(const std::vector<double> &rxyz)
136 {
137  build(rxyz);
138  return *this;
139 }
140 #endif
141 
150 {
151  double COEF_MIN_ROT = 1e-6;
152  double phi;
153  const unsigned int index_0 = 0;
154  const unsigned int index_1 = 1;
155  const unsigned int index_2 = 2;
156 
157  if ((fabs(R[index_1][index_2]) < COEF_MIN_ROT) && (fabs(R[index_2][index_2]) < COEF_MIN_ROT)) {
158  phi = 0;
159  }
160  else {
161  phi = atan2(-R[index_1][index_2], R[index_2][index_2]);
162  }
163 
164  double si = sin(phi);
165  double co = cos(phi);
166  double theta = atan2(R[index_0][index_2], (-si * R[index_1][index_2]) + (co * R[index_2][index_2]));
167  double psi = atan2((co * R[index_1][index_0]) + (si * R[index_2][index_0]), (co * R[index_1][index_1]) + (si * R[index_2][index_1]));
168 
169  build(phi, theta, psi);
170 
171  return *this;
172 }
173 
181 {
183  R.build(tu);
184  build(R);
185 
186  return *this;
187 }
188 
195 vpRxyzVector &vpRxyzVector::build(const double &phi, const double &theta, const double &psi)
196 {
197  const unsigned int index_0 = 0;
198  const unsigned int index_1 = 1;
199  const unsigned int index_2 = 2;
200  data[index_0] = phi;
201  data[index_1] = theta;
202  data[index_2] = psi;
203  return *this;
204 }
205 
210 {
211  if (rxyz.size() != 3) {
212  throw(vpException(vpException::dimensionError, "Cannot construct a R-xyz vector from a %d-dimension col vector",
213  rxyz.size()));
214  }
215  const unsigned int val_3 = 3;
216  for (unsigned int i = 0; i < val_3; ++i) {
217  data[i] = rxyz[i];
218  }
219 
220  return *this;
221 }
222 
226 vpRxyzVector &vpRxyzVector::build(const std::vector<double> &rxyz)
227 {
228  if (rxyz.size() != 3) {
229  throw(vpException(vpException::dimensionError, "Cannot construct a R-xyz vector from a %d-dimension std::vector",
230  rxyz.size()));
231  }
232  const unsigned int val_3 = 3;
233  for (unsigned int i = 0; i < val_3; ++i) {
234  data[i] = rxyz[i];
235  }
236 
237  return *this;
238 }
239 
264 {
265  for (unsigned int i = 0; i < dsize; ++i) {
266  data[i] = v;
267  }
268 
269  return *this;
270 }
271 
299 {
300  if (rxyz.size() != 3) {
301  throw(vpException(vpException::dimensionError, "Cannot set a R-xyz vector from a %d-dimension col vector",
302  rxyz.size()));
303  }
304  const unsigned int val_3 = 3;
305  for (unsigned int i = 0; i < val_3; ++i) {
306  data[i] = rxyz[i];
307  }
308 
309  return *this;
310 }
311 
312 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
334 vpRxyzVector &vpRxyzVector::operator=(const std::initializer_list<double> &list)
335 {
336  if (list.size() > size()) {
337  throw(vpException(
339  "Cannot set Euler x-y-z vector out of bounds. It has only %d values while you try to initialize with %d values",
340  size(), list.size()));
341  }
342  std::copy(list.begin(), list.end(), data);
343  return *this;
344 }
345 #endif
346 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:1104
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 & build(const vpHomogeneousMatrix &M)
Implementation of a generic rotation vector.
Implementation of a rotation vector as Euler angle minimal representation.
Definition: vpRxyzVector.h:183
VP_DEPRECATED vpRxyzVector buildFrom(const vpRotationMatrix &R)
vpRxyzVector & build(const vpRotationMatrix &R)
vpRxyzVector & operator=(const vpColVector &rxyz)
Implementation of a rotation vector as axis-angle minimal representation.