Visual Servoing Platform version 3.5.0
vpQuaternionVector.cpp
1/****************************************************************************
2 *
3 * ViSP, open source Visual Servoing Platform software.
4 * Copyright (C) 2005 - 2019 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 http://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 * Quaternion vector.
33 *
34 * Authors:
35 * Filip Novotny
36 *
37 *****************************************************************************/
38
39#include <algorithm>
40#include <stdio.h>
41#include <string.h>
42#include <visp3/core/vpMath.h>
43#include <visp3/core/vpQuaternionVector.h>
44
45// minimum value of sine
46const double vpQuaternionVector::minimum = 0.0001;
47
55
58
60vpQuaternionVector::vpQuaternionVector(double x_, double y_, double z_, double w_)
62{
63 set(x_, y_, z_, w_);
64}
65
68{
69 buildFrom(q);
70}
71
74{
75 buildFrom(q);
76}
77
84
92
100void vpQuaternionVector::set(double qx, double qy, double qz, double qw)
101{
102 data[0] = qx;
103 data[1] = qy;
104 data[2] = qz;
105 data[3] = qw;
106}
116vpQuaternionVector vpQuaternionVector::buildFrom(double qx, double qy, double qz, double qw)
117{
118 set(qx, qy, qz, qw);
119 return *this;
120}
121
129{
130 vpRotationMatrix R(tu);
131 buildFrom(R);
132
133 return *this;
134}
135
140{
141 if (q.size() != 4) {
143 "Cannot construct a quaternion vector from a %d-dimension col vector", q.size()));
144 }
145 for (unsigned int i = 0; i < 4; i++)
146 data[i] = q[i];
147
148 return *this;
149}
150
155{
156 if (q.size() != 4) {
158 "Cannot construct a quaternion vector from a %d-dimension std::vector", q.size()));
159 }
160 for (unsigned int i = 0; i < 4; i++)
161 data[i] = q[i];
162
163 return *this;
164}
165
174{
175 return vpQuaternionVector(x() + q.x(), y() + q.y(), z() + q.z(), w() + q.w());
176}
185{
186 return vpQuaternionVector(x() - q.x(), y() - q.y(), z() - q.z(), w() - q.w());
187}
188
191
194{
195 return vpQuaternionVector(l * x(), l * y(), l * z(), l * w());
196}
197
200{
201 return vpQuaternionVector(w() * rq.x() + x() * rq.w() + y() * rq.z() - z() * rq.y(),
202 w() * rq.y() + y() * rq.w() + z() * rq.x() - x() * rq.z(),
203 w() * rq.z() + z() * rq.w() + x() * rq.y() - y() * rq.x(),
204 w() * rq.w() - x() * rq.x() - y() * rq.y() - z() * rq.z());
205}
206
209{
210 if (vpMath::nul(l, std::numeric_limits<double>::epsilon())) {
211 throw vpException(vpException::fatalError, "Division by scalar l==0 !");
212 }
213
214 return vpQuaternionVector(x() / l, y() / l, z() / l, w() / l);
215}
241{
242 if (q.size() != 4) {
243 throw(vpException(vpException::dimensionError, "Cannot set a quaternion vector from a %d-dimension col vector",
244 q.size()));
245 }
246 for (unsigned int i = 0; i < 4; i++)
247 data[i] = q[i];
248
249 return *this;
250}
251
258{
259 vpThetaUVector tu(R);
260 vpColVector u;
261 double theta;
262 tu.extract(theta, u);
263
264 theta *= 0.5;
265
266 double sinTheta_2 = sin(theta);
267 set(u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2, cos(theta));
268 return *this;
269}
270
277
284{
285 vpQuaternionVector q_inv;
286
287 double mag_square = w() * w() + x() * x() + y() * y() + z() * z();
288 if (!vpMath::nul(mag_square, std::numeric_limits<double>::epsilon())) {
289 q_inv = this->conjugate() / mag_square;
290 } else {
291 std::cerr << "The current quaternion is null ! The inverse cannot be computed !" << std::endl;
292 }
293
294 return q_inv;
295}
296
302double vpQuaternionVector::magnitude() const { return sqrt(w() * w() + x() * x() + y() * y() + z() * z()); }
303
308{
309 double mag = magnitude();
310 if (!vpMath::nul(mag, std::numeric_limits<double>::epsilon())) {
311 set(x() / mag, y() / mag, z() / mag, w() / mag);
312 }
313}
314
316double vpQuaternionVector::x() const { return data[0]; }
318double vpQuaternionVector::y() const { return data[1]; }
320double vpQuaternionVector::z() const { return data[2]; }
322double vpQuaternionVector::w() const { return data[3]; }
323
324#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
342vpQuaternionVector &vpQuaternionVector::operator=(const std::initializer_list<double> &list)
343{
344 if (list.size() > size()) {
345 throw(vpException(vpException::dimensionError, "Cannot set quaternion vector out of bounds. It has only %d values while you try to initialize with %d values", size(), list.size()));
346 }
347 std::copy(list.begin(), list.end(), data);
348 return *this;
349}
350#endif
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
Implementation of column vector and the associated operations.
Definition: vpColVector.h:131
error that can be emited by ViSP classes.
Definition: vpException.h:72
@ dimensionError
Bad dimension.
Definition: vpException.h:95
@ fatalError
Fatal error.
Definition: vpException.h:96
static bool nul(double x, double s=0.001)
Definition: vpMath.h:286
Implementation of a rotation vector as quaternion angle minimal representation.
double w() const
Returns w-component of the quaternion.
double y() const
Returns y-component of the quaternion.
vpQuaternionVector operator*(double l) const
Multiplication by scalar. Returns a quaternion defined by (lx,ly,lz,lw).
vpQuaternionVector conjugate() const
vpQuaternionVector inverse() const
vpQuaternionVector & operator=(const vpColVector &q)
void set(double x, double y, double z, double w)
double z() const
Returns z-component of the quaternion.
vpQuaternionVector operator-() const
Negate operator. Returns a quaternion defined by (-x,-y,-z-,-w).
double x() const
Returns x-component of the quaternion.
vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw)
vpQuaternionVector operator+(const vpQuaternionVector &q) const
vpQuaternionVector operator/(double l) const
Division by scalar. Returns a quaternion defined by (x/l,y/l,z/l,w/l).
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a generic rotation vector.
Implementation of a rotation vector as axis-angle minimal representation.
void extract(double &theta, vpColVector &u) const