Visual Servoing Platform  version 3.6.1 under development (2024-12-12)
vpQuaternionVector.cpp
1 /*
2  * ViSP, open source Visual Servoing Platform software.
3  * Copyright (C) 2005 - 2024 by Inria. All rights reserved.
4  *
5  * This software is free software; you can redistribute it and/or modify
6  * it under the terms of the GNU General Public License as published by
7  * the Free Software Foundation; either version 2 of the License, or
8  * (at your option) any later version.
9  * See the file LICENSE.txt at the root directory of this source
10  * distribution for additional information about the GNU GPL.
11  *
12  * For using ViSP with software that can not be combined with the GNU
13  * GPL, please contact Inria about acquiring a ViSP Professional
14  * Edition License.
15  *
16  * See https://visp.inria.fr for more information.
17  *
18  * This software was developed at:
19  * Inria Rennes - Bretagne Atlantique
20  * Campus Universitaire de Beaulieu
21  * 35042 Rennes Cedex
22  * France
23  *
24  * If you have questions regarding the use of this file, please contact
25  * Inria at visp@inria.fr
26  *
27  * This file is provided AS IS with NO WARRANTY OF ANY KIND, INCLUDING THE
28  * WARRANTY OF DESIGN, MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE.
29  *
30  * Description:
31  * Quaternion vector.
32  */
33 
34 #include <algorithm>
35 #include <cassert>
36 #include <stdio.h>
37 #include <string.h>
38 #include <visp3/core/vpMath.h>
39 #include <visp3/core/vpQuaternionVector.h>
40 
41 BEGIN_VISP_NAMESPACE
42 // minimum value of sine
43 const double vpQuaternionVector::minimum = 0.0001;
44 const unsigned int vpQuaternionVector::constr_val_4 = 4;
45 
53 
56 
58 vpQuaternionVector::vpQuaternionVector(double x_, double y_, double z_, double w_) : vpRotationVector(constr_val_4)
59 {
60  set(x_, y_, z_, w_);
61 }
62 
65 
67 vpQuaternionVector::vpQuaternionVector(const std::vector<double> &q) : vpRotationVector(constr_val_4) { buildFrom(q); }
68 
75 
83 
91 void vpQuaternionVector::set(double qx, double qy, double qz, double qw)
92 {
93  const unsigned int index_0 = 0;
94  const unsigned int index_1 = 1;
95  const unsigned int index_2 = 2;
96  const unsigned int index_3 = 3;
97  data[index_0] = qx;
98  data[index_1] = qy;
99  data[index_2] = qz;
100  data[index_3] = qw;
101 }
102 
112 vpQuaternionVector &vpQuaternionVector::buildFrom(const double &qx, const double &qy, const double &qz, const double &qw)
113 {
114  set(qx, qy, qz, qw);
115  return *this;
116 }
117 
125 {
126  vpRotationMatrix R(tu);
127  buildFrom(R);
128 
129  return *this;
130 }
131 
136 {
137  const unsigned int val_4 = 4;
138  if (q.size() != val_4) {
140  "Cannot construct a quaternion vector from a %d-dimension col vector", q.size()));
141  }
142  for (unsigned int i = 0; i < val_4; ++i) {
143  data[i] = q[i];
144  }
145 
146  return *this;
147 }
148 
152 vpQuaternionVector &vpQuaternionVector::buildFrom(const std::vector<double> &q)
153 {
154  const unsigned int val_4 = 4;
155  if (q.size() != val_4) {
157  "Cannot construct a quaternion vector from a %d-dimension std::vector", q.size()));
158  }
159 
160  for (unsigned int i = 0; i < val_4; ++i) {
161  data[i] = q[i];
162  }
163 
164  return *this;
165 }
166 
173 {
174  vpThetaUVector tu(R);
175  vpColVector u;
176  double theta;
177  tu.extract(theta, u);
178 
179  theta *= 0.5;
180 
181  double sinTheta_2 = sin(theta);
182  const unsigned int index_0 = 0;
183  const unsigned int index_1 = 1;
184  const unsigned int index_2 = 2;
185  set(u[index_0] * sinTheta_2, u[index_1] * sinTheta_2, u[index_2] * sinTheta_2, cos(theta));
186  return *this;
187 }
188 
197 {
198  return vpQuaternionVector(x() + q.x(), y() + q.y(), z() + q.z(), w() + q.w());
199 }
208 {
209  return vpQuaternionVector(x() - q.x(), y() - q.y(), z() - q.z(), w() - q.w());
210 }
211 
214 {
215  return vpQuaternionVector(-x(), -y(), -z(), -w());
216 }
217 
220 {
221  return vpQuaternionVector(l * x(), l * y(), l * z(), l * w());
222 }
223 
226 {
227  return vpQuaternionVector(((w() * rq.x()) + (x() * rq.w()) + (y() * rq.z())) - (z() * rq.y()),
228  ((w() * rq.y()) + (y() * rq.w()) + (z() * rq.x())) - (x() * rq.z()),
229  ((w() * rq.z()) + (z() * rq.w()) + (x() * rq.y())) - (y() * rq.x()),
230  ((w() * rq.w()) - (x() * rq.x()) - (y() * rq.y())) - (z() * rq.z()));
231 }
232 
235 {
236  if (vpMath::nul(l, std::numeric_limits<double>::epsilon())) {
237  throw vpException(vpException::fatalError, "Division by scalar l==0 !");
238  }
239 
240  return vpQuaternionVector(x() / l, y() / l, z() / l, w() / l);
241 }
269 {
270  const unsigned int val_4 = 4;
271  if (q.size() != val_4) {
272  throw(vpException(vpException::dimensionError, "Cannot set a quaternion vector from a %d-dimension col vector",
273  q.size()));
274  }
275  for (unsigned int i = 0; i < val_4; ++i) {
276  data[i] = q[i];
277  }
278 
279  return *this;
280 }
281 
282 
283 
290 
297 {
298  vpQuaternionVector q_inv;
299 
300  double mag_square = (w() * w()) + (x() * x()) + (y() * y()) + (z() * z());
301  if (!vpMath::nul(mag_square, std::numeric_limits<double>::epsilon())) {
302  q_inv = this->conjugate() / mag_square;
303  }
304  else {
305  std::cerr << "The current quaternion is null ! The inverse cannot be computed !" << std::endl;
306  }
307 
308  return q_inv;
309 }
310 
316 double vpQuaternionVector::magnitude() const { return sqrt((w() * w()) + (x() * x()) + (y() * y()) + (z() * z())); }
317 
322 {
323  double mag = magnitude();
324  if (!vpMath::nul(mag, std::numeric_limits<double>::epsilon())) {
325  set(x() / mag, y() / mag, z() / mag, w() / mag);
326  }
327 }
328 
338 {
339  return (q0.x() * q1.x()) + (q0.y() * q1.y()) + (q0.z() * q1.z()) + (q0.w() * q1.w());
340 }
341 
343 const double &vpQuaternionVector::x() const { const unsigned int index_0 = 0; return data[index_0]; }
345 const double &vpQuaternionVector::y() const { const unsigned int index_1 = 1; return data[index_1]; }
347 const double &vpQuaternionVector::z() const { const unsigned int index_2 = 2; return data[index_2]; }
349 const double &vpQuaternionVector::w() const { const unsigned int index_3 = 3; return data[index_3]; }
350 
352 double &vpQuaternionVector::x() { const unsigned int index_0 = 0; return data[index_0]; }
354 double &vpQuaternionVector::y() { const unsigned int index_1 = 1; return data[index_1]; }
356 double &vpQuaternionVector::z() { const unsigned int index_2 = 2; return data[index_2]; }
358 double &vpQuaternionVector::w() { const unsigned int index_3 = 3; return data[index_3]; }
359 
360 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
382 vpQuaternionVector &vpQuaternionVector::operator=(const std::initializer_list<double> &list)
383 {
384  if (list.size() > size()) {
385  throw(vpException(
387  "Cannot set quaternion vector out of bounds. It has only %d values while you try to initialize with %d values",
388  size(), list.size()));
389  }
390  std::copy(list.begin(), list.end(), data);
391  return *this;
392 }
393 #endif
394 
410 {
411  assert(t >= 0 && t <= 1);
412 
413  double cosHalfTheta = dot(q0, q1);
414  vpQuaternionVector q1_ = q1;
415  if (cosHalfTheta < 0) {
416  cosHalfTheta = -cosHalfTheta;
417  q1_ = -q1;
418  }
419 
420  vpQuaternionVector qLerp;
421  qLerp.x() = q0.x() - (t * (q0.x() - q1.x()));
422  qLerp.y() = q0.y() - (t * (q0.y() - q1.y()));
423  qLerp.z() = q0.z() - (t * (q0.z() - q1.z()));
424  qLerp.w() = q0.w() - (t * (q0.w() - q1.w()));
425 
426  return qLerp;
427 }
428 
444 {
445  assert(t >= 0 && t <= 1);
446 
447  vpQuaternionVector qLerp = lerp(q0, q1, t);
448  qLerp.normalize();
449 
450  return qLerp;
451 }
452 
468 {
469  assert(t >= 0 && t <= 1);
470  // Some additional references:
471  // https://splines.readthedocs.io/en/latest/rotation/slerp.html
472  // https://zeux.io/2015/07/23/approximating-slerp/
473  // https://github.com/eigenteam/eigen-git-mirror/blob/36b95962756c1fce8e29b1f8bc45967f30773c00/Eigen/src/Geometry/Quaternion.h#L753-L790
474  // https://www.euclideanspace.com/maths/algebra/realNormedAlgebra/quaternions/slerp/index.htm
475  // http://number-none.com/product/Understanding%20Slerp,%20Then%20Not%20Using%20It/
476  // https://www.3dgep.com/understanding-quaternions/
477  // https://blog.magnum.graphics/backstage/the-unnecessarily-short-ways-to-do-a-quaternion-slerp/
478 
479  double cosHalfTheta = dot(q0, q1);
480  vpQuaternionVector q1_ = q1;
481  if (cosHalfTheta < 0) {
482  cosHalfTheta = -cosHalfTheta;
483  q1_ = -q1;
484  }
485 
486  double scale0 = 1 - t;
487  double scale1 = t;
488 
489  if ((1 - cosHalfTheta) > 0.1) {
490  double theta = std::acos(cosHalfTheta);
491  double invSinTheta = 1 / std::sin(theta);
492 
493  scale0 = std::sin((1 - t) * theta) * invSinTheta;
494  scale1 = std::sin(t * theta) * invSinTheta;
495  }
496 
497  vpQuaternionVector qSlerp;
498  qSlerp.x() = (scale0 * q0.x()) + (scale1 * q1_.x());
499  qSlerp.y() = (scale0 * q0.y()) + (scale1 * q1_.y());
500  qSlerp.z() = (scale0 * q0.z()) + (scale1 * q1_.z());
501  qSlerp.w() = (scale0 * q0.w()) + (scale1 * q1_.w());
502  qSlerp.normalize();
503 
504  return qSlerp;
505 }
506 END_VISP_NAMESPACE
double * data
Address of the first element of the data array.
Definition: vpArray2D.h:148
unsigned int size() const
Return the number of elements of the 2D array.
Definition: vpArray2D.h:349
Implementation of column vector and the associated operations.
Definition: vpColVector.h:191
error that can be emitted by ViSP classes.
Definition: vpException.h:60
@ dimensionError
Bad dimension.
Definition: vpException.h:71
@ fatalError
Fatal error.
Definition: vpException.h:72
static bool nul(double x, double threshold=0.001)
Definition: vpMath.h:450
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)
vpQuaternionVector & buildFrom(const double &qx, const double &qy, const double &qz, const double &qw)
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).
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.
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