 |
Visual Servoing Platform
version 3.2.0
|
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);
vpQuaternionVector operator/(const double l) const
Division by scalar. Returns a quaternion defined by (x/l,y/l,z/l,w/l).
vpQuaternionVector inverse() const
vpQuaternionVector operator*(const double l) const
Multiplication by scalar. Returns a quaternion defined by (lx,ly,lz,lw).
void set(const double x, const double y, const double z, const double w)
double x() const
Returns x-component of the quaternion.
double w() const
Returns w-component of the quaternion.
vpPoseVector buildFrom(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)
vpColVector extract(unsigned int r, unsigned int colsize) const
static bool nul(double x, double s=0.001)
double y() const
Returns y-component of the quaternion.
vpQuaternionVector operator-() const
Negate operator. Returns a quaternion defined by (-x,-y,-z-,-w).
Implementation of a rotation vector as axis-angle minimal representation.
vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw)
Implementation of column vector and the associated operations.
Implementation of a generic rotation vector.
Implementation of a rotation vector as quaternion angle minimal representation.
Implementation of a rotation matrix and operations on such kind of matrices.
double * data
Address of the first element of the data array.
double z() const
Returns z-component of the quaternion.
vpQuaternionVector operator+(const vpQuaternionVector &q) const
unsigned int size() const
Return the number of elements of the 2D array.
vpQuaternionVector conjugate() const
error that can be emited by ViSP classes.
vpQuaternionVector & operator=(const vpColVector &q)
void set(const double tx, const double ty, const double tz, const double tux, const double tuy, const double tuz)