 |
Visual Servoing Platform
version 3.3.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);
324 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
344 if (list.size() >
size()) {
347 std::copy(list.begin(), list.end(),
data);
vpQuaternionVector inverse() const
double x() const
Returns x-component of the quaternion.
double w() const
Returns w-component of the quaternion.
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.
void set(double x, double y, double z, double w)
vpQuaternionVector buildFrom(const double qx, const double qy, const double qz, const double qw)
Implementation of column vector and the associated operations.
vpQuaternionVector operator*(double l) const
Multiplication by scalar. Returns a quaternion defined by (lx,ly,lz,lw).
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.
vpPoseVector buildFrom(double tx, double ty, double tz, double tux, double tuy, double tuz)
double * data
Address of the first element of the data array.
void set(double tx, double ty, double tz, double tux, double tuy, double tuz)
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/(double l) const
Division by scalar. Returns a quaternion defined by (x/l,y/l,z/l,w/l).
vpQuaternionVector & operator=(const vpColVector &q)