43 #include <visp3/core/vpMath.h>
44 #include <visp3/core/vpQuaternionVector.h>
47 const 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());
double * data
Address of the first element of the data array.
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 emited 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)
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