48 #include <visp3/core/vpThetaUVector.h> 50 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));
169 if ((R[2][1]-R[1][2]) < 0) x = -x;
175 if((R[0][2]-R[2][0]) < 0) y = -y;
181 if((R[1][0]-R[0][1]) < 0) z = -z;
239 if (tu.size() != 3) {
243 for (
unsigned int i = 0; i < 3; i++)
254 if (tu.
size() != 3) {
258 for (
unsigned int i = 0; i < 3; i++)
287 for (
unsigned int i = 0; i <
dsize; i++)
318 if (tu.
size() != 3) {
322 for (
unsigned int i = 0; i < 3; i++)
362 if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
366 for (
unsigned int i = 0; i < 3; i++)
367 u[i] =
data[i] / theta;
423 if (std::fabs(theta) <= std::numeric_limits<double>::epsilon()) {
427 for (
unsigned int i = 0; i < 3; i++)
428 u[i] =
data[i] / theta;
vpThetaUVector & operator=(const vpColVector &tu)
Implementation of a generic rotation vector.
Implementation of an homogeneous matrix and operations on such kind of matrices.
error that can be emited by ViSP classes.
double * data
Address of the first element of the data array.
unsigned int size() const
Return the number of elements of the 2D array.
Implementation of a rotation vector as Euler angle minimal representation.
vpThetaUVector buildFrom(const vpHomogeneousMatrix &M)
static double sinc(double x)
Implementation of a rotation matrix and operations on such kind of matrices.
void extract(double &theta, vpColVector &u) const
void extract(vpRotationMatrix &R) const
Implementation of a rotation vector as quaternion angle minimal representation.
Implementation of column vector and the associated operations.
Implementation of a pose vector and operations on poses.
Implementation of a rotation vector as Euler angle minimal representation.
unsigned int dsize
Current array size (rowNum * colNum)
Implementation of a rotation vector as Euler angle minimal representation.
static int() sign(double x)
Implementation of a rotation vector as axis-angle minimal representation.
void resize(const unsigned int i, const bool flagNullify=true)