42 #include <visp3/core/vpMath.h> 43 #include <visp3/core/vpQuaternionVector.h> 46 const double vpQuaternionVector::minimum = 0.0001;
143 "Cannot construct a quaternion vector from a %d-dimension col vector", q.
size()));
145 for (
unsigned int i = 0; i < 4; i++)
158 "Cannot construct a quaternion vector from a %d-dimension std::vector", q.size()));
160 for (
unsigned int i = 0; i < 4; i++)
202 w() * rq.
y() +
y() * rq.
w() +
z() * rq.
x() -
x() * rq.
z(),
203 w() * rq.
z() +
z() * rq.
w() +
x() * rq.
y() -
y() * rq.
x(),
204 w() * rq.
w() -
x() * rq.
x() -
y() * rq.
y() -
z() * rq.
z());
210 if (
vpMath::nul(l, std::numeric_limits<double>::epsilon())) {
246 for (
unsigned int i = 0; i < 4; i++)
266 double sinTheta_2 = sin(theta);
267 set(u[0] * sinTheta_2, u[1] * sinTheta_2, u[2] * sinTheta_2, cos(theta));
287 double mag_square =
w() *
w() +
x() *
x() +
y() *
y() +
z() *
z();
288 if (!
vpMath::nul(mag_square, std::numeric_limits<double>::epsilon())) {
291 std::cerr <<
"The current quaternion is null ! The inverse cannot be computed !" << std::endl;
310 if (!
vpMath::nul(mag, std::numeric_limits<double>::epsilon())) {
311 set(
x() / mag,
y() / mag,
z() / mag,
w() / mag);
Implementation of a generic rotation vector.
void set(const double x, const double y, const double z, const double w)
void extract(double &theta, vpColVector &u) const
double z() const
Returns z-component of the quaternion.
vpQuaternionVector conjugate() const
error that can be emited by ViSP classes.
double * data
Address of the first element of the data array.
vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw)
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of a rotation matrix and operations on such kind of matrices.
vpQuaternionVector inverse() const
static bool nul(double x, double s=0.001)
vpQuaternionVector operator+(const vpQuaternionVector &q) const
Implementation of a rotation vector as quaternion angle minimal representation.
double y() const
Returns y-component of the quaternion.
vpQuaternionVector operator/(const double l) const
Division by scalar. Returns a quaternion defined by (x/l,y/l,z/l,w/l).
double x() const
Returns x-component of the quaternion.
vpQuaternionVector & operator=(const vpColVector &q)
Implementation of column vector and the associated operations.
vpQuaternionVector operator-() const
Negate operator. Returns a quaternion defined by (-x,-y,-z-,-w).
vpQuaternionVector operator*(const double l) const
Multiplication by scalar. Returns a quaternion defined by (lx,ly,lz,lw).
double w() const
Returns w-component of the quaternion.
Implementation of a rotation vector as axis-angle minimal representation.