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) {
153 "Cannot construct a quaternion vector from a %d-dimension std::vector", q.size()));
155 for (
unsigned int i = 0; i < 4; ++i) {
198 ((
w() * rq.
y()) + (
y() * rq.
w()) + (
z() * rq.
x())) - (
x() * rq.
z()),
199 ((
w() * rq.
z()) + (
z() * rq.
w()) + (
x() * rq.
y())) - (
y() * rq.
x()),
200 ((
w() * rq.
w()) - (
x() * rq.
x()) - (
y() * rq.
y())) - (
z() * rq.
z()));
206 if (
vpMath::nul(l, std::numeric_limits<double>::epsilon())) {
242 for (
unsigned int i = 0; i < 4; ++i) {
263 double sinTheta_2 = sin(theta);
264 set(u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2, cos(theta));
284 double mag_square = (
w() *
w()) + (
x() *
x()) + (
y() *
y()) + (
z() *
z());
285 if (!
vpMath::nul(mag_square, std::numeric_limits<double>::epsilon())) {
289 std::cerr <<
"The current quaternion is null ! The inverse cannot be computed !" << std::endl;
308 if (!
vpMath::nul(mag, std::numeric_limits<double>::epsilon())) {
309 set(
x() / mag,
y() / mag,
z() / mag,
w() / mag);
323 return (q0.
x() * q1.
x()) + (q0.
y() * q1.
y()) + (q0.
z() * q1.
z()) + (q0.
w() * q1.
w());
344 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
364 if (list.size() >
size()) {
367 "Cannot set quaternion vector out of bounds. It has only %d values while you try to initialize with %d values",
368 size(), list.size()));
370 std::copy(list.begin(), list.end(),
data);
391 assert(
t >= 0 &&
t <= 1);
393 double cosHalfTheta =
dot(q0, q1);
395 if (cosHalfTheta < 0) {
396 cosHalfTheta = -cosHalfTheta;
401 qLerp.
x() = q0.
x() - (
t * (q0.
x() - q1.
x()));
402 qLerp.
y() = q0.
y() - (
t * (q0.
y() - q1.
y()));
403 qLerp.
z() = q0.
z() - (
t * (q0.
z() - q1.
z()));
404 qLerp.
w() = q0.
w() - (
t * (q0.
w() - q1.
w()));
425 assert(
t >= 0 &&
t <= 1);
449 assert(
t >= 0 &&
t <= 1);
459 double cosHalfTheta =
dot(q0, q1);
461 if (cosHalfTheta < 0) {
462 cosHalfTheta = -cosHalfTheta;
466 double scale0 = 1 -
t;
469 if ((1 - cosHalfTheta) > 0.1) {
470 double theta = std::acos(cosHalfTheta);
471 double invSinTheta = 1 / std::sin(theta);
473 scale0 = std::sin((1 -
t) * theta) * invSinTheta;
474 scale1 = std::sin(
t * theta) * invSinTheta;
478 qSlerp.
x() = (scale0 * q0.
x()) + (scale1 * q1_.
x());
479 qSlerp.
y() = (scale0 * q0.
y()) + (scale1 * q1_.
y());
480 qSlerp.
z() = (scale0 * q0.
z()) + (scale1 * q1_.
z());
481 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 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)
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