Visual Servoing Platform  version 3.5.1 under development (2022-05-22)
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 <cassert>
41 #include <stdio.h>
42 #include <string.h>
43 #include <visp3/core/vpMath.h>
44 #include <visp3/core/vpQuaternionVector.h>
45 
46 // minimum value of sine
47 const double vpQuaternionVector::minimum = 0.0001;
48 
56 
59 
61 vpQuaternionVector::vpQuaternionVector(double x_, double y_, double z_, double w_) : vpRotationVector(4)
62 {
63  set(x_, y_, z_, w_);
64 }
65 
68 
70 vpQuaternionVector::vpQuaternionVector(const std::vector<double> &q) : vpRotationVector(4) { buildFrom(q); }
71 
78 
86 
94 void vpQuaternionVector::set(double qx, double qy, double qz, double qw)
95 {
96  data[0] = qx;
97  data[1] = qy;
98  data[2] = qz;
99  data[3] = qw;
100 }
110 vpQuaternionVector vpQuaternionVector::buildFrom(double qx, double qy, double qz, double qw)
111 {
112  set(qx, qy, qz, qw);
113  return *this;
114 }
115 
123 {
124  vpRotationMatrix R(tu);
125  buildFrom(R);
126 
127  return *this;
128 }
129 
134 {
135  if (q.size() != 4) {
137  "Cannot construct a quaternion vector from a %d-dimension col vector", q.size()));
138  }
139  for (unsigned int i = 0; i < 4; i++)
140  data[i] = q[i];
141 
142  return *this;
143 }
144 
149 {
150  if (q.size() != 4) {
152  "Cannot construct a quaternion vector from a %d-dimension std::vector", q.size()));
153  }
154  for (unsigned int i = 0; i < 4; i++)
155  data[i] = q[i];
156 
157  return *this;
158 }
159 
168 {
169  return vpQuaternionVector(x() + q.x(), y() + q.y(), z() + q.z(), w() + q.w());
170 }
179 {
180  return vpQuaternionVector(x() - q.x(), y() - q.y(), z() - q.z(), w() - q.w());
181 }
182 
185 
188 {
189  return vpQuaternionVector(l * x(), l * y(), l * z(), l * w());
190 }
191 
194 {
195  return vpQuaternionVector(w() * rq.x() + x() * rq.w() + y() * rq.z() - z() * rq.y(),
196  w() * rq.y() + y() * rq.w() + z() * rq.x() - x() * rq.z(),
197  w() * rq.z() + z() * rq.w() + x() * rq.y() - y() * rq.x(),
198  w() * rq.w() - x() * rq.x() - y() * rq.y() - z() * rq.z());
199 }
200 
203 {
204  if (vpMath::nul(l, std::numeric_limits<double>::epsilon())) {
205  throw vpException(vpException::fatalError, "Division by scalar l==0 !");
206  }
207 
208  return vpQuaternionVector(x() / l, y() / l, z() / l, w() / l);
209 }
235 {
236  if (q.size() != 4) {
237  throw(vpException(vpException::dimensionError, "Cannot set a quaternion vector from a %d-dimension col vector",
238  q.size()));
239  }
240  for (unsigned int i = 0; i < 4; i++)
241  data[i] = q[i];
242 
243  return *this;
244 }
245 
252 {
253  vpThetaUVector tu(R);
254  vpColVector u;
255  double theta;
256  tu.extract(theta, u);
257 
258  theta *= 0.5;
259 
260  double sinTheta_2 = sin(theta);
261  set(u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2, cos(theta));
262  return *this;
263 }
264 
271 
278 {
279  vpQuaternionVector q_inv;
280 
281  double mag_square = w() * w() + x() * x() + y() * y() + z() * z();
282  if (!vpMath::nul(mag_square, std::numeric_limits<double>::epsilon())) {
283  q_inv = this->conjugate() / mag_square;
284  } else {
285  std::cerr << "The current quaternion is null ! The inverse cannot be computed !" << std::endl;
286  }
287 
288  return q_inv;
289 }
290 
296 double vpQuaternionVector::magnitude() const { return sqrt(w() * w() + x() * x() + y() * y() + z() * z()); }
297 
302 {
303  double mag = magnitude();
304  if (!vpMath::nul(mag, std::numeric_limits<double>::epsilon())) {
305  set(x() / mag, y() / mag, z() / mag, w() / mag);
306  }
307 }
308 
318 {
319  return q0.x() * q1.x() + q0.y() * q1.y() + q0.z() * q1.z() + q0.w() * q1.w();
320 }
321 
323 const double& vpQuaternionVector::x() const { return data[0]; }
325 const double& vpQuaternionVector::y() const { return data[1]; }
327 const double& vpQuaternionVector::z() const { return data[2]; }
329 const double& vpQuaternionVector::w() const { return data[3]; }
330 
332 double& vpQuaternionVector::x() { return data[0]; }
334 double& vpQuaternionVector::y() { return data[1]; }
336 double& vpQuaternionVector::z() { return data[2]; }
338 double& vpQuaternionVector::w() { return data[3]; }
339 
340 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
341 
358 vpQuaternionVector &vpQuaternionVector::operator=(const std::initializer_list<double> &list)
359 {
360  if (list.size() > size()) {
361  throw(vpException(
363  "Cannot set quaternion vector out of bounds. It has only %d values while you try to initialize with %d values",
364  size(), list.size()));
365  }
366  std::copy(list.begin(), list.end(), data);
367  return *this;
368 }
369 #endif
370 
386 {
387  assert(t >= 0 && t <= 1);
388 
389  double cosHalfTheta = dot(q0, q1);
390  vpQuaternionVector q1_ = q1;
391  if (cosHalfTheta < 0) {
392  cosHalfTheta = -cosHalfTheta;
393  q1_ = -q1;
394  }
395 
396  vpQuaternionVector qLerp;
397  qLerp.x() = q0.x() - t * (q0.x() - q1.x());
398  qLerp.y() = q0.y() - t * (q0.y() - q1.y());
399  qLerp.z() = q0.z() - t * (q0.z() - q1.z());
400  qLerp.w() = q0.w() - t * (q0.w() - q1.w());
401 
402  return qLerp;
403 }
404 
420 {
421  assert(t >= 0 && t <= 1);
422 
423  vpQuaternionVector qLerp = lerp(q0, q1, t);
424  qLerp.normalize();
425 
426  return qLerp;
427 }
428 
444 {
445  assert(t >= 0 && t <= 1);
446  // Some additional references:
447  // https://splines.readthedocs.io/en/latest/rotation/slerp.html
448  // https://zeux.io/2015/07/23/approximating-slerp/
449  // https://github.com/eigenteam/eigen-git-mirror/blob/36b95962756c1fce8e29b1f8bc45967f30773c00/Eigen/src/Geometry/Quaternion.h#L753-L790
450  // https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
451  // http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/
452  // https://www.3dgep.com/understanding-quaternions/
453  // https://blog.magnum.graphics/backstage/the-unnecessarily-short-ways-to-do-a-quaternion-slerp/
454 
455  double cosHalfTheta = dot(q0, q1);
456  vpQuaternionVector q1_ = q1;
457  if (cosHalfTheta < 0) {
458  cosHalfTheta = -cosHalfTheta;
459  q1_ = -q1;
460  }
461 
462  double scale0 = 1 - t;
463  double scale1 = t;
464 
465  if (1 - cosHalfTheta > 0.1) {
466  double theta = std::acos(cosHalfTheta);
467  double invSinTheta = 1 / std::sin(theta);
468 
469  scale0 = std::sin((1 - t) * theta) * invSinTheta;
470  scale1 = std::sin((t * theta)) * invSinTheta;
471  }
472 
473  vpQuaternionVector qSlerp;
474  qSlerp.x() = (scale0 * q0.x()) + (scale1 * q1_.x());
475  qSlerp.y() = (scale0 * q0.y()) + (scale1 * q1_.y());
476  qSlerp.z() = (scale0 * q0.z()) + (scale1 * q1_.z());
477  qSlerp.w() = (scale0 * q0.w()) + (scale1 * q1_.w());
478  qSlerp.normalize();
479 
480  return qSlerp;
481 }
Implementation of a generic rotation vector.
vpRowVector t() const
static vpQuaternionVector nlerp(const vpQuaternionVector &q0, const vpQuaternionVector &q1, double t)
const double & y() const
Returns the y-component of the quaternion.
static double dot(const vpQuaternionVector &q0, const vpQuaternionVector &q1)
void extract(double &theta, vpColVector &u) const
vpQuaternionVector conjugate() const
const double & x() const
Returns the x-component of the quaternion.
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:293
static vpQuaternionVector slerp(const vpQuaternionVector &q0, const vpQuaternionVector &q1, double t)
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.
const double & z() const
Returns the z-component of the quaternion.
vpQuaternionVector inverse() const
static bool nul(double x, double s=0.001)
Definition: vpMath.h:358
vpQuaternionVector operator+(const vpQuaternionVector &q) const
static vpQuaternionVector lerp(const vpQuaternionVector &q0, const vpQuaternionVector &q1, double t)
Implementation of a rotation vector as quaternion angle minimal representation.
const double & w() const
Returns the w-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).
void set(double x, double y, double z, double w)
Implementation of a rotation vector as axis-angle minimal representation.