45 #include <visp3/core/vpThetaUVector.h>
47 const double vpThetaUVector::minimum = 0.0001;
126 for (
unsigned int i = 0; i < 3; i++)
139 s = (R[1][0] - R[0][1]) * (R[1][0] - R[0][1]) + (R[2][0] - R[0][2]) * (R[2][0] - R[0][2]) +
140 (R[2][1] - R[1][2]) * (R[2][1] - R[1][2]);
142 c = (R[0][0] + R[1][1] + R[2][2] - 1.0) / 2.0;
146 if ((1 + c) > minimum)
150 data[0] = (R[2][1] - R[1][2]) / (2 * sinc);
151 data[1] = (R[0][2] - R[2][0]) / (2 * sinc);
152 data[2] = (R[1][0] - R[0][1]) / (2 * sinc);
156 if ((R[0][0] - c) > std::numeric_limits<double>::epsilon())
157 x = sqrt((R[0][0] - c) / (1 - c));
160 if ((R[1][1] - c) > std::numeric_limits<double>::epsilon())
161 y = sqrt((R[1][1] - c) / (1 - c));
164 if ((R[2][2] - c) > std::numeric_limits<double>::epsilon())
165 z = sqrt((R[2][2] - c) / (1 - c));
167 if (x > y && x > z) {
168 if ((R[2][1] - R[1][2]) < 0)
175 if ((R[0][2] - R[2][0]) < 0)
182 if ((R[1][0] - R[0][1]) < 0)
243 if (tu.size() != 3) {
247 for (
unsigned int i = 0; i < 3; i++)
258 if (tu.
size() != 3) {
262 for (
unsigned int i = 0; i < 3; i++)
291 for (
unsigned int i = 0; i <
dsize; i++)
326 for (
unsigned int i = 0; i <
size(); i++)
366 if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
370 for (
unsigned int i = 0; i < 3; i++)
371 u[i] =
data[i] / theta;
427 if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
431 for (
unsigned int i = 0; i < 3; i++)
432 u[i] =
data[i] / theta;
459 double c = 2 * std::acos(std::cos(a_2) * std::cos(b_2) -
vpColVector::dotProd(a_hat_sin_2, b_hat_sin_2));
460 vpColVector d = std::sin(a_2) * std::cos(b_2) * a_hat + std::cos(a_2) * std::sin(b_2) * b_hat +
462 d = c * d / std::sin(c / 2);
467 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
487 if (list.size() >
size()) {
490 "Cannot set theta u vector out of bounds. It has only %d values while you try to initialize with %d values",
491 size(), list.size()));
493 std::copy(list.begin(), list.end(),
data);
double * data
Address of the first element of the data array.
unsigned int dsize
Current array size (rowNum * colNum)
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of column vector and the associated operations.
static double dotProd(const vpColVector &a, const vpColVector &b)
static vpColVector crossProd(const vpColVector &a, const vpColVector &b)
void resize(unsigned int i, bool flagNullify=true)
error that can be emitted by ViSP classes.
@ dimensionError
Bad dimension.
Implementation of an homogeneous matrix and operations on such kind of matrices.
void extract(vpRotationMatrix &R) const
static double sinc(double x)
static int sign(double x)
Implementation of a pose vector and operations on poses.
Implementation of a rotation vector as quaternion angle minimal representation.
Implementation of a rotation matrix and operations on such kind of matrices.
Implementation of a generic rotation vector.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as Euler angle minimal representation.
Implementation of a rotation vector as axis-angle minimal representation.
vpThetaUVector operator*(const vpThetaUVector &tu_b) const
vpThetaUVector buildFrom(const vpRzyxVector &rzyx)
void extract(double &theta, vpColVector &u) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
vpThetaUVector & operator=(const vpColVector &tu)