Visual Servoing Platform version 3.6.0
Loading...
Searching...
No Matches
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
58vpRzyxVector::vpRzyxVector(double phi, double theta, double psi) : vpRotationVector(3) { buildFrom(phi, theta, psi); }
59
66
74
77
79vpRzyxVector::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 phi = atan2(ny, nx);
97 double si = sin(phi);
98 double co = cos(phi);
99
100 double nz = R[2][0];
101 double theta = atan2(-nz, co * nx + si * ny);
102
103 double ax = R[0][2];
104 double ay = R[1][2];
105 double ox = R[0][1];
106 double oy = R[1][1];
107
108 double psi = atan2(si * ax - co * ay, -si * ox + co * oy);
109
110 buildFrom(phi, theta, psi);
111
112 return *this;
113}
114
122{
124 R.buildFrom(tu);
125 buildFrom(R);
126
127 return *this;
128}
129
136void vpRzyxVector::buildFrom(double phi, double theta, double psi)
137{
138 data[0] = phi;
139 data[1] = theta;
140 data[2] = psi;
141}
142
147{
148 if (rzyx.size() != 3) {
149 throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension col vector",
150 rzyx.size()));
151 }
152 for (unsigned int i = 0; i < 3; i++)
153 data[i] = rzyx[i];
154
155 return *this;
156}
157
161vpRzyxVector vpRzyxVector::buildFrom(const std::vector<double> &rzyx)
162{
163 if (rzyx.size() != 3) {
164 throw(vpException(vpException::dimensionError, "Cannot construct a R-zyx vector from a %d-dimension std::vector",
165 rzyx.size()));
166 }
167 for (unsigned int i = 0; i < 3; i++)
168 data[i] = rzyx[i];
169
170 return *this;
171}
172
193{
194 for (unsigned int i = 0; i < dsize; i++)
195 data[i] = v;
196
197 return *this;
198}
199
224{
225 if (rzyx.size() != 3) {
226 throw(vpException(vpException::dimensionError, "Cannot set a R-zyx vector from a %d-dimension col vector",
227 rzyx.size()));
228 }
229 for (unsigned int i = 0; i < 3; i++)
230 data[i] = rzyx[i];
231
232 return *this;
233}
234
235#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
253vpRzyxVector &vpRzyxVector::operator=(const std::initializer_list<double> &list)
254{
255 if (list.size() > size()) {
256 throw(vpException(
258 "Cannot set Euler x-y-z vector out of bounds. It has only %d values while you try to initialize with %d values",
259 size(), list.size()));
260 }
261 std::copy(list.begin(), list.end(), data);
262 return *this;
263}
264#endif
unsigned int dsize
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.
vpRzyxVector & operator=(const vpColVector &rzyx)
vpRzyxVector buildFrom(const vpRotationMatrix &R)
Implementation of a rotation vector as axis-angle minimal representation.