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;