39 #include <visp3/core/vpCylinder.h>
40 #include <visp3/core/vpFeatureDisplay.h>
188 double co, si, e, x0, y0, z0;
189 double A, B, C, X0, Y0, Z0, R;
190 double s, a, b, c, zero;
199 zero = A * X0 + B * Y0 + C * Z0;
201 s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - R * R - zero * zero;
209 x0 = C * Y0 - B * Z0;
210 y0 = A * Z0 - C * X0;
211 z0 = B * X0 - A * Y0;
216 e = sqrt(co * co + si * si);
217 p_[0] = -(R * c * s - z0) / e;
218 p_[1] = atan2(si, co);
223 e = sqrt(co * co + si * si);
224 p_[2] = -(R * c * s + z0) / e;
225 p_[3] = atan2(si, co);
257 double oA, oB, oC, oX0, oY0, oZ0;
265 X1 = cMo[0][0] * oA + cMo[0][1] * oB + cMo[0][2] * oC;
266 Y1 = cMo[1][0] * oA + cMo[1][1] * oB + cMo[1][2] * oC;
267 Z1 = cMo[2][0] * oA + cMo[2][1] * oB + cMo[2][2] * oC;
268 s = sqrt(X1 * X1 + Y1 * Y1 + Z1 * Z1);
278 X2 = cMo[0][3] + cMo[0][0] * oX0 + cMo[0][1] * oY0 + cMo[0][2] * oZ0;
279 Y2 = cMo[1][3] + cMo[1][0] * oX0 + cMo[1][1] * oY0 + cMo[1][2] * oZ0;
280 Z2 = cMo[2][3] + cMo[2][0] * oX0 + cMo[2][1] * oY0 + cMo[2][2] * oZ0;
284 cP_[3] = (1 - a * a) * X2 - a * b * Y2 - a * c * Z2;
285 cP_[4] = -a * b * X2 + (1 - b * b) * Y2 - b * c * Z2;
286 cP_[5] = -a * c * X2 - b * c * Y2 + (1 - c * c) * Z2;
350 return (B - std::sqrt(B * B - A * C)) / A;
373 const vpColor &color,
unsigned int thickness)
395 const vpColor &color,
unsigned int thickness)
413 unsigned int thickness)
427 unsigned int thickness)
Generic class defining intrinsic camera parameters.
Implementation of column vector and the associated operations.
void resize(unsigned int i, bool flagNullify=true)
Class to define RGB colors available for display functionnalities.
Class that defines a 3D cylinder in the object frame and allows forward projection of a 3D cylinder i...
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1)
vpCylinder * duplicate() const
For memory issue (used by the vpServo class only).
double computeZ(double x, double y) const
void setWorldCoordinates(const vpColVector &oP)
error that can be emited by ViSP classes.
static void displayCylinder(double rho1, double theta1, double rho2, double theta2, const vpCameraParameters &cam, const vpImage< unsigned char > &I, const vpColor &color=vpColor::green, unsigned int thickness=1)
Implementation of an homogeneous matrix and operations on such kind of matrices.