Visual Servoing Platform  version 3.6.1 under development (2024-05-28)
vpQuaternionVector.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  * 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) { build(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 }
101 
102 #ifdef VISP_BUILD_DEPRECATED_FUNCTIONS
113 vpQuaternionVector vpQuaternionVector::buildFrom(const double qx, const double qy, const double qz, const double qw)
114 {
115  build(qx, qy, qz, qw);
116  return *this;
117 }
118 
127 {
128  build(tu);
129  return *this;
130 }
131 
137 {
138  build(q);
139  return *this;
140 }
141 
147 {
148  build(q);
149  return *this;
150 }
151 
159 {
160  build(R);
161  return *this;
162 }
163 #endif
164 
174 vpQuaternionVector &vpQuaternionVector::build(const double &qx, const double &qy, const double &qz, const double &qw)
175 {
176  set(qx, qy, qz, qw);
177  return *this;
178 }
179 
187 {
188  vpRotationMatrix R(tu);
189  build(R);
190 
191  return *this;
192 }
193 
198 {
199  if (q.size() != 4) {
201  "Cannot construct a quaternion vector from a %d-dimension col vector", q.size()));
202  }
203  for (unsigned int i = 0; i < 4; ++i) {
204  data[i] = q[i];
205  }
206 
207  return *this;
208 }
209 
213 vpQuaternionVector &vpQuaternionVector::build(const std::vector<double> &q)
214 {
215  if (q.size() != 4) {
217  "Cannot construct a quaternion vector from a %d-dimension std::vector", q.size()));
218  }
219  for (unsigned int i = 0; i < 4; ++i) {
220  data[i] = q[i];
221  }
222 
223  return *this;
224 }
225 
232 {
233  vpThetaUVector tu(R);
234  vpColVector u;
235  double theta;
236  tu.extract(theta, u);
237 
238  theta *= 0.5;
239 
240  double sinTheta_2 = sin(theta);
241  set(u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2, cos(theta));
242  return *this;
243 }
244 
253 {
254  return vpQuaternionVector(x() + q.x(), y() + q.y(), z() + q.z(), w() + q.w());
255 }
264 {
265  return vpQuaternionVector(x() - q.x(), y() - q.y(), z() - q.z(), w() - q.w());
266 }
267 
270 
273 {
274  return vpQuaternionVector(l * x(), l * y(), l * z(), l * w());
275 }
276 
279 {
280  return vpQuaternionVector(((w() * rq.x()) + (x() * rq.w()) + (y() * rq.z())) - (z() * rq.y()),
281  ((w() * rq.y()) + (y() * rq.w()) + (z() * rq.x())) - (x() * rq.z()),
282  ((w() * rq.z()) + (z() * rq.w()) + (x() * rq.y())) - (y() * rq.x()),
283  ((w() * rq.w()) - (x() * rq.x()) - (y() * rq.y())) - (z() * rq.z()));
284 }
285 
288 {
289  if (vpMath::nul(l, std::numeric_limits<double>::epsilon())) {
290  throw vpException(vpException::fatalError, "Division by scalar l==0 !");
291  }
292 
293  return vpQuaternionVector(x() / l, y() / l, z() / l, w() / l);
294 }
320 {
321  if (q.size() != 4) {
322  throw(vpException(vpException::dimensionError, "Cannot set a quaternion vector from a %d-dimension col vector",
323  q.size()));
324  }
325  for (unsigned int i = 0; i < 4; ++i) {
326  data[i] = q[i];
327  }
328 
329  return *this;
330 }
331 
332 
333 
340 
347 {
348  vpQuaternionVector q_inv;
349 
350  double mag_square = (w() * w()) + (x() * x()) + (y() * y()) + (z() * z());
351  if (!vpMath::nul(mag_square, std::numeric_limits<double>::epsilon())) {
352  q_inv = this->conjugate() / mag_square;
353  }
354  else {
355  std::cerr << "The current quaternion is null ! The inverse cannot be computed !" << std::endl;
356  }
357 
358  return q_inv;
359 }
360 
366 double vpQuaternionVector::magnitude() const { return sqrt((w() * w()) + (x() * x()) + (y() * y()) + (z() * z())); }
367 
372 {
373  double mag = magnitude();
374  if (!vpMath::nul(mag, std::numeric_limits<double>::epsilon())) {
375  set(x() / mag, y() / mag, z() / mag, w() / mag);
376  }
377 }
378 
388 {
389  return (q0.x() * q1.x()) + (q0.y() * q1.y()) + (q0.z() * q1.z()) + (q0.w() * q1.w());
390 }
391 
393 const double &vpQuaternionVector::x() const { return data[0]; }
395 const double &vpQuaternionVector::y() const { return data[1]; }
397 const double &vpQuaternionVector::z() const { return data[2]; }
399 const double &vpQuaternionVector::w() const { return data[3]; }
400 
402 double &vpQuaternionVector::x() { return data[0]; }
404 double &vpQuaternionVector::y() { return data[1]; }
406 double &vpQuaternionVector::z() { return data[2]; }
408 double &vpQuaternionVector::w() { return data[3]; }
409 
410 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
428 vpQuaternionVector &vpQuaternionVector::operator=(const std::initializer_list<double> &list)
429 {
430  if (list.size() > size()) {
431  throw(vpException(
433  "Cannot set quaternion vector out of bounds. It has only %d values while you try to initialize with %d values",
434  size(), list.size()));
435  }
436  std::copy(list.begin(), list.end(), data);
437  return *this;
438 }
439 #endif
440 
456 {
457  assert(t >= 0 && t <= 1);
458 
459  double cosHalfTheta = dot(q0, q1);
460  vpQuaternionVector q1_ = q1;
461  if (cosHalfTheta < 0) {
462  cosHalfTheta = -cosHalfTheta;
463  q1_ = -q1;
464  }
465 
466  vpQuaternionVector qLerp;
467  qLerp.x() = q0.x() - (t * (q0.x() - q1.x()));
468  qLerp.y() = q0.y() - (t * (q0.y() - q1.y()));
469  qLerp.z() = q0.z() - (t * (q0.z() - q1.z()));
470  qLerp.w() = q0.w() - (t * (q0.w() - q1.w()));
471 
472  return qLerp;
473 }
474 
490 {
491  assert(t >= 0 && t <= 1);
492 
493  vpQuaternionVector qLerp = lerp(q0, q1, t);
494  qLerp.normalize();
495 
496  return qLerp;
497 }
498 
514 {
515  assert(t >= 0 && t <= 1);
516  // Some additional references:
517  // https://splines.readthedocs.io/en/latest/rotation/slerp.html
518  // https://zeux.io/2015/07/23/approximating-slerp/
519  // https://github.com/eigenteam/eigen-git-mirror/blob/36b95962756c1fce8e29b1f8bc45967f30773c00/Eigen/src/Geometry/Quaternion.h#L753-L790
520  // https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
521  // http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/
522  // https://www.3dgep.com/understanding-quaternions/
523  // https://blog.magnum.graphics/backstage/the-unnecessarily-short-ways-to-do-a-quaternion-slerp/
524 
525  double cosHalfTheta = dot(q0, q1);
526  vpQuaternionVector q1_ = q1;
527  if (cosHalfTheta < 0) {
528  cosHalfTheta = -cosHalfTheta;
529  q1_ = -q1;
530  }
531 
532  double scale0 = 1 - t;
533  double scale1 = t;
534 
535  if ((1 - cosHalfTheta) > 0.1) {
536  double theta = std::acos(cosHalfTheta);
537  double invSinTheta = 1 / std::sin(theta);
538 
539  scale0 = std::sin((1 - t) * theta) * invSinTheta;
540  scale1 = std::sin(t * theta) * invSinTheta;
541  }
542 
543  vpQuaternionVector qSlerp;
544  qSlerp.x() = (scale0 * q0.x()) + (scale1 * q1_.x());
545  qSlerp.y() = (scale0 * q0.y()) + (scale1 * q1_.y());
546  qSlerp.z() = (scale0 * q0.z()) + (scale1 * q1_.z());
547  qSlerp.w() = (scale0 * q0.w()) + (scale1 * q1_.w());
548  qSlerp.normalize();
549 
550  return qSlerp;
551 }
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:139
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:341
Implementation of column vector and the associated operations.
Definition: vpColVector.h:163
error that can be emitted by ViSP classes.
Definition: vpException.h:59
@ dimensionError
Bad dimension.
Definition: vpException.h:70
@ fatalError
Fatal error.
Definition: vpException.h:71
static bool nul(double x, double threshold=0.001)
Definition: vpMath.h:445
Implementation of a rotation vector as quaternion angle minimal representation.
vpQuaternionVector operator*(double l) const
Multiplication by scalar. Returns a quaternion defined by (lx,ly,lz,lw).
const double & z() const
Returns the z-component of the quaternion.
vpQuaternionVector conjugate() const
vpQuaternionVector inverse() const
vpQuaternionVector & operator=(const vpColVector &q)
void set(double x, double y, double z, double w)
static vpQuaternionVector slerp(const vpQuaternionVector &q0, const vpQuaternionVector &q1, double t)
static vpQuaternionVector nlerp(const vpQuaternionVector &q0, const vpQuaternionVector &q1, double t)
vpQuaternionVector operator-() const
Negate operator. Returns a quaternion defined by (-x,-y,-z-,-w).
vpQuaternionVector & build(const double &qx, const double &qy, const double &qz, const double &qw)
const double & x() const
Returns the x-component of the quaternion.
static double dot(const vpQuaternionVector &q0, const vpQuaternionVector &q1)
const double & y() const
Returns the y-component of the quaternion.
const double & w() const
Returns the w-component of the quaternion.
vp_deprecated vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw)
vpQuaternionVector operator+(const vpQuaternionVector &q) const
static vpQuaternionVector lerp(const vpQuaternionVector &q0, const vpQuaternionVector &q1, double t)
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.
vpRowVector t() const
Implementation of a rotation vector as axis-angle minimal representation.
void extract(double &theta, vpColVector &u) const