43#include <visp3/core/vpMath.h>
44#include <visp3/core/vpQuaternionVector.h>
47const double vpQuaternionVector::minimum = 0.0001;
137 "Cannot construct a quaternion vector from a %d-dimension col vector", q.
size()));
139 for (
unsigned int i = 0; i < 4; i++)
152 "Cannot construct a quaternion vector from a %d-dimension std::vector", q.size()));
154 for (
unsigned int i = 0; i < 4; i++)
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());
204 if (
vpMath::nul(l, std::numeric_limits<double>::epsilon())) {
240 for (
unsigned int i = 0; i < 4; i++)
260 double sinTheta_2 = sin(theta);
261 set(u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2, cos(theta));
281 double mag_square =
w() *
w() +
x() *
x() +
y() *
y() +
z() *
z();
282 if (!
vpMath::nul(mag_square, std::numeric_limits<double>::epsilon())) {
285 std::cerr <<
"The current quaternion is null ! The inverse cannot be computed !" << std::endl;
304 if (!
vpMath::nul(mag, std::numeric_limits<double>::epsilon())) {
305 set(
x() / mag,
y() / mag,
z() / mag,
w() / mag);
319 return q0.
x() * q1.
x() + q0.
y() * q1.
y() + q0.
z() * q1.
z() + q0.
w() * q1.
w();
340#if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
360 if (list.size() >
size()) {
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()));
366 std::copy(list.begin(), list.end(),
data);
387 assert(
t >= 0 &&
t <= 1);
389 double cosHalfTheta =
dot(q0, q1);
391 if (cosHalfTheta < 0) {
392 cosHalfTheta = -cosHalfTheta;
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());
421 assert(
t >= 0 &&
t <= 1);
445 assert(
t >= 0 &&
t <= 1);
455 double cosHalfTheta =
dot(q0, q1);
457 if (cosHalfTheta < 0) {
458 cosHalfTheta = -cosHalfTheta;
462 double scale0 = 1 -
t;
465 if (1 - cosHalfTheta > 0.1) {
466 double theta = std::acos(cosHalfTheta);
467 double invSinTheta = 1 / std::sin(theta);
469 scale0 = std::sin((1 -
t) * theta) * invSinTheta;
470 scale1 = std::sin((
t * theta)) * invSinTheta;
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());
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
static bool nul(double x, double threshold=0.001)
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 vpThetaUVector &tu)
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 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.
Implementation of a rotation vector as axis-angle minimal representation.
void extract(double &theta, vpColVector &u) const