Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
vpRzyzVector.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 * Euler angles parameterization for the rotation.
33 * Rzyz(phi,theta,psi) = Rot(z,phi)Rot(y,theta)Rot(z,psi)
34 *
35*****************************************************************************/
36
37#include <math.h>
38#include <visp3/core/vpRzyzVector.h>
39
50
57vpRzyzVector::vpRzyzVector(double phi, double theta, double psi) : vpRotationVector(3) { buildFrom(phi, theta, psi); }
58
65
73
76
78vpRzyzVector::vpRzyzVector(const std::vector<double> &rzyz) : vpRotationVector(3) { buildFrom(rzyz); }
79
88{
89 double phi;
90 if ((fabs(R[1][2]) < 1e-6) && (fabs(R[0][2]) < 1e-6))
91 phi = 0;
92 else
93 phi = atan2(R[1][2], R[0][2]);
94 double cphi = cos(phi);
95 double sphi = sin(phi);
96
97 double theta = atan2(cphi * R[0][2] + sphi * R[1][2], R[2][2]);
98
99 double psi = atan2(-sphi * R[0][0] + cphi * R[1][0], -sphi * R[0][1] + cphi * R[1][1]);
100
101 buildFrom(phi, theta, psi);
102
103 return *this;
104}
105
113{
115 R.buildFrom(tu);
116 buildFrom(R);
117
118 return *this;
119}
120
125{
126 if (rzyz.size() != 3) {
127 throw(vpException(vpException::dimensionError, "Cannot construct a R-zyz vector from a %d-dimension col vector",
128 rzyz.size()));
129 }
130 for (unsigned int i = 0; i < 3; i++)
131 data[i] = rzyz[i];
132
133 return *this;
134}
135
139vpRzyzVector vpRzyzVector::buildFrom(const std::vector<double> &rzyz)
140{
141 if (rzyz.size() != 3) {
142 throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension std::vector",
143 rzyz.size()));
144 }
145 for (unsigned int i = 0; i < 3; i++)
146 data[i] = rzyz[i];
147
148 return *this;
149}
150
171{
172 for (unsigned int i = 0; i < dsize; i++)
173 data[i] = v;
174
175 return *this;
176}
177
184void vpRzyzVector::buildFrom(double phi, double theta, double psi)
185{
186 data[0] = phi;
187 data[1] = theta;
188 data[2] = psi;
189}
190
215{
216 if (rzyz.size() != 3) {
217 throw(vpException(vpException::dimensionError, "Cannot set a R-zyz vector from a %d-dimension col vector",
218 rzyz.size()));
219 }
220 for (unsigned int i = 0; i < 3; i++)
221 data[i] = rzyz[i];
222
223 return *this;
224}
225
226#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
244vpRzyzVector &vpRzyzVector::operator=(const std::initializer_list<double> &list)
245{
246 if (list.size() > size()) {
247 throw(vpException(
249 "Cannot set Euler x-y-z vector out of bounds. It has only %d values while you try to initialize with %d values",
250 size(), list.size()));
251 }
252 std::copy(list.begin(), list.end(), data);
253 return *this;
254}
255#endif
double * data
Address of the first element of the data array.
Definition vpArray2D.h:144
unsigned int dsize
Current array size (rowNum * colNum)
Definition vpArray2D.h:140
unsigned int size() const
Return the number of elements of the 2D array.
Definition vpArray2D.h:292
Implementation of column vector and the associated operations.
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.
vpRzyzVector buildFrom(const vpRotationMatrix &R)
vpRzyzVector & operator=(const vpColVector &rzyz)
Implementation of a rotation vector as axis-angle minimal representation.