39 #include <visp3/core/vpCylinder.h>
40 #include <visp3/core/vpFeatureDisplay.h>
82 const double Z0,
const double R)
136 const double Z0,
const double R)
201 double co, si, e, x0, y0, z0;
202 double A, B, C, X0, Y0, Z0, R;
203 double s, a, b, c, zero;
212 zero = A * X0 + B * Y0 + C * Z0;
214 s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - R * R - zero * zero;
216 printf(
"The camera is inside the cylinder with s=%f !\n", s);
223 x0 = C * Y0 - B * Z0;
224 y0 = A * Z0 - C * X0;
225 z0 = B * X0 - A * Y0;
230 e = sqrt(co * co + si * si);
231 p_[0] = -(R * c * s - z0) / e;
232 p_[1] = atan2(si, co);
240 e = sqrt(co * co + si * si);
241 p_[2] = -(R * c * s + z0) / e;
242 p_[3] = atan2(si, co);
278 double oA, oB, oC, oX0, oY0, oZ0;
286 X1 = cMo[0][0] * oA + cMo[0][1] * oB + cMo[0][2] * oC;
287 Y1 = cMo[1][0] * oA + cMo[1][1] * oB + cMo[1][2] * oC;
288 Z1 = cMo[2][0] * oA + cMo[2][1] * oB + cMo[2][2] * oC;
289 s = sqrt(X1 * X1 + Y1 * Y1 + Z1 * Z1);
299 X2 = cMo[0][3] + cMo[0][0] * oX0 + cMo[0][1] * oY0 + cMo[0][2] * oZ0;
300 Y2 = cMo[1][3] + cMo[1][0] * oX0 + cMo[1][1] * oY0 + cMo[1][2] * oZ0;
301 Z2 = cMo[2][3] + cMo[2][0] * oX0 + cMo[2][1] * oY0 + cMo[2][2] * oZ0;
305 cP_[3] = (1 - a * a) * X2 - a * b * Y2 - a * c * Z2;
306 cP_[4] = -a * b * X2 + (1 - b * b) * Y2 - b * c * Z2;
307 cP_[5] = -a * c * X2 - b * c * Y2 + (1 - c * c) * Z2;
371 return (B - std::sqrt(B * B - A * C)) / A;
385 const vpColor &color,
const unsigned int thickness)
398 const unsigned int thickness)