43 #include <visp3/core/vpThetaUVector.h>
45 const double vpThetaUVector::minimum = 0.0001;
124 for (
unsigned int i = 0; i < 3; ++i) {
138 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])) +
139 ((R[2][1] - R[1][2]) * (R[2][1] - R[1][2]));
141 c = ((R[0][0] + R[1][1] + R[2][2]) - 1.0) / 2.0;
145 if ((1 + c) > minimum)
149 data[0] = (R[2][1] - R[1][2]) / (2 * sinc);
150 data[1] = (R[0][2] - R[2][0]) / (2 * sinc);
151 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));
161 if ((R[1][1] - c) > std::numeric_limits<double>::epsilon()) {
162 y = sqrt((R[1][1] - c) / (1 - c));
166 if ((R[2][2] - c) > std::numeric_limits<double>::epsilon()) {
167 z = sqrt((R[2][2] - c) / (1 - c));
170 if ((x > y) && (x > z)) {
171 if ((R[2][1] - R[1][2]) < 0) {
182 if ((R[0][2] - R[2][0]) < 0) {
193 if ((R[1][0] - R[0][1]) < 0) {
257 if (tu.size() != 3) {
261 for (
unsigned int i = 0; i < 3; ++i) {
273 if (tu.
size() != 3) {
277 for (
unsigned int i = 0; i < 3; ++i) {
306 for (
unsigned int i = 0; i <
dsize; ++i) {
342 unsigned int l_size =
size();
343 for (
unsigned int i = 0; i < l_size; ++i) {
384 if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
388 for (
unsigned int i = 0; i < 3; ++i) {
389 u[i] =
data[i] / theta;
446 if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
450 for (
unsigned int i = 0; i < 3; ++i) {
451 u[i] =
data[i] / theta;
479 double c = 2 * std::acos((std::cos(a_2) * std::cos(b_2)) - (
vpColVector::dotProd(a_hat_sin_2, b_hat_sin_2)));
480 vpColVector d = ((std::sin(a_2) * std::cos(b_2) * a_hat) + (std::cos(a_2) * std::sin(b_2) * b_hat)) +
482 d = (c * d) / std::sin(c / 2);
487 #if (VISP_CXX_STANDARD >= VISP_CXX_STANDARD_11)
507 if (list.size() >
size()) {
510 "Cannot set theta u vector out of bounds. It has only %d values while you try to initialize with %d values",
511 size(), list.size()));
513 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
void extract(double &theta, vpColVector &u) const
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
vpThetaUVector & operator=(const vpColVector &tu)