48 #include <visp3/core/vpThetaUVector.h>
50 const double vpThetaUVector::minimum = 0.0001;
129 for (
unsigned int i = 0; i < 3; i++)
142 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]) +
143 (R[2][1] - R[1][2]) * (R[2][1] - R[1][2]);
145 c = (R[0][0] + R[1][1] + R[2][2] - 1.0) / 2.0;
149 if ((1 + c) > minimum)
153 data[0] = (R[2][1] - R[1][2]) / (2 * sinc);
154 data[1] = (R[0][2] - R[2][0]) / (2 * sinc);
155 data[2] = (R[1][0] - R[0][1]) / (2 * sinc);
159 if ((R[0][0] - c) > std::numeric_limits<double>::epsilon())
160 x = sqrt((R[0][0] - c) / (1 - c));
163 if ((R[1][1] - c) > std::numeric_limits<double>::epsilon())
164 y = sqrt((R[1][1] - c) / (1 - c));
167 if ((R[2][2] - c) > std::numeric_limits<double>::epsilon())
168 z = sqrt((R[2][2] - c) / (1 - c));
170 if (x > y && x > z) {
171 if ((R[2][1] - R[1][2]) < 0)
178 if ((R[0][2] - R[2][0]) < 0)
185 if ((R[1][0] - R[0][1]) < 0)
246 if (tu.size() != 3) {
250 for (
unsigned int i = 0; i < 3; i++)
261 if (tu.
size() != 3) {
265 for (
unsigned int i = 0; i < 3; i++)
294 for (
unsigned int i = 0; i <
dsize; i++)
329 for (
unsigned int i = 0; i <
size(); i++)
369 if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
373 for (
unsigned int i = 0; i < 3; i++)
374 u[i] =
data[i] / theta;
430 if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
434 for (
unsigned int i = 0; i < 3; i++)
435 u[i] =
data[i] / theta;
462 double c = 2 * std::acos(std::cos(a_2) * std::cos(b_2) -
vpColVector::dotProd(a_hat_sin_2, b_hat_sin_2));
463 vpColVector d = std::sin(a_2) * std::cos(b_2) * a_hat + std::cos(a_2) * std::sin(b_2) * b_hat +
465 d = c * d / std::sin(c / 2);
470 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
490 if (list.size() >
size()) {
493 "Cannot set theta u vector out of bounds. It has only %d values while you try to initialize with %d values",
494 size(), list.size()));
496 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 emited 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
void extract(double &theta, vpColVector &u) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
vpThetaUVector & operator=(const vpColVector &tu)