Visual Servoing Platform  version 3.3.0 under development (2020-02-17)
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
46 const double vpQuaternionVector::minimum = 0.0001;
47 
55 
58 
60 vpQuaternionVector::vpQuaternionVector(double x_, double y_, double z_, double w_)
61  : vpRotationVector(4)
62 {
63  set(x_, y_, z_, w_);
64 }
65 
68 {
69  buildFrom(q);
70 }
71 
73 vpQuaternionVector::vpQuaternionVector(const std::vector<double> &q) : vpRotationVector(4)
74 {
75  buildFrom(q);
76 }
77 
84 
92 
100 void 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 }
116 vpQuaternionVector 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 
302 double 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 
316 double vpQuaternionVector::x() const { return data[0]; }
318 double vpQuaternionVector::y() const { return data[1]; }
320 double vpQuaternionVector::z() const { return data[2]; }
322 double vpQuaternionVector::w() const { return data[3]; }
323 
324 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
325 
342 vpQuaternionVector &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
Implementation of a generic rotation vector.
void extract(double &theta, vpColVector &u) const
double z() const
Returns z-component of the quaternion.
vpQuaternionVector conjugate() const
error that can be emited by ViSP classes.
Definition: vpException.h:71
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:145
vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw)
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:291
vpQuaternionVector operator*(double l) const
Multiplication by scalar. Returns a quaternion defined by (lx,ly,lz,lw).
Implementation of a rotation matrix and operations on such kind of matrices.
vpQuaternionVector inverse() const
static bool nul(double x, double s=0.001)
Definition: vpMath.h:287
vpQuaternionVector operator+(const vpQuaternionVector &q) const
Implementation of a rotation vector as quaternion angle minimal representation.
double y() const
Returns y-component of the quaternion.
double x() const
Returns x-component of the quaternion.
vpQuaternionVector & operator=(const vpColVector &q)
Implementation of column vector and the associated operations.
Definition: vpColVector.h:130
vpQuaternionVector operator/(double l) const
Division by scalar. Returns a quaternion defined by (x/l,y/l,z/l,w/l).
vpQuaternionVector operator-() const
Negate operator. Returns a quaternion defined by (-x,-y,-z-,-w).
double w() const
Returns w-component of the quaternion.
void set(double x, double y, double z, double w)
Implementation of a rotation vector as axis-angle minimal representation.