39 #include <visp3/core/vpCylinder.h>
40 #include <visp3/core/vpFeatureDisplay.h>
199 double co, si, e, x0, y0, z0;
200 double A, B, C, X0, Y0, Z0, R;
201 double s, a, b, c, zero;
210 zero = A * X0 + B * Y0 + C * Z0;
212 s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - R * R - zero * zero;
214 printf(
"The camera is inside the cylinder with s=%f !\n", s);
221 x0 = C * Y0 - B * Z0;
222 y0 = A * Z0 - C * X0;
223 z0 = B * X0 - A * Y0;
228 e = sqrt(co * co + si * si);
229 p_[0] = -(R * c * s - z0) / e;
230 p_[1] = atan2(si, co);
238 e = sqrt(co * co + si * si);
239 p_[2] = -(R * c * s + z0) / e;
240 p_[3] = atan2(si, co);
276 double oA, oB, oC, oX0, oY0, oZ0;
284 X1 = cMo[0][0] * oA + cMo[0][1] * oB + cMo[0][2] * oC;
285 Y1 = cMo[1][0] * oA + cMo[1][1] * oB + cMo[1][2] * oC;
286 Z1 = cMo[2][0] * oA + cMo[2][1] * oB + cMo[2][2] * oC;
287 s = sqrt(X1 * X1 + Y1 * Y1 + Z1 * Z1);
297 X2 = cMo[0][3] + cMo[0][0] * oX0 + cMo[0][1] * oY0 + cMo[0][2] * oZ0;
298 Y2 = cMo[1][3] + cMo[1][0] * oX0 + cMo[1][1] * oY0 + cMo[1][2] * oZ0;
299 Z2 = cMo[2][3] + cMo[2][0] * oX0 + cMo[2][1] * oY0 + cMo[2][2] * oZ0;
303 cP_[3] = (1 - a * a) * X2 - a * b * Y2 - a * c * Z2;
304 cP_[4] = -a * b * X2 + (1 - b * b) * Y2 - b * c * Z2;
305 cP_[5] = -a * c * X2 - b * c * Y2 + (1 - c * c) * Z2;
369 return (B - std::sqrt(B * B - A * C)) / A;
383 const vpColor &color,
unsigned int thickness)
396 unsigned int thickness)