36#include <visp3/core/vpCylinder.h>
37#include <visp3/core/vpFeatureDisplay.h>
185 double co, si, e, x0, y0, z0;
186 double A, B, C, X0, Y0, Z0, R;
187 double s, a, b, c, zero;
196 zero = A * X0 + B * Y0 + C * Z0;
198 s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - R * R - zero * zero;
206 x0 = C * Y0 - B * Z0;
207 y0 = A * Z0 - C * X0;
208 z0 = B * X0 - A * Y0;
213 e = sqrt(co * co + si * si);
214 p_[0] = -(R * c * s - z0) / e;
215 p_[1] = atan2(si, co);
220 e = sqrt(co * co + si * si);
221 p_[2] = -(R * c * s + z0) / e;
222 p_[3] = atan2(si, co);
254 double oA, oB, oC, oX0, oY0, oZ0;
262 X1 = cMo[0][0] * oA + cMo[0][1] * oB + cMo[0][2] * oC;
263 Y1 = cMo[1][0] * oA + cMo[1][1] * oB + cMo[1][2] * oC;
264 Z1 = cMo[2][0] * oA + cMo[2][1] * oB + cMo[2][2] * oC;
265 s = sqrt(X1 * X1 + Y1 * Y1 + Z1 * Z1);
275 X2 = cMo[0][3] + cMo[0][0] * oX0 + cMo[0][1] * oY0 + cMo[0][2] * oZ0;
276 Y2 = cMo[1][3] + cMo[1][0] * oX0 + cMo[1][1] * oY0 + cMo[1][2] * oZ0;
277 Z2 = cMo[2][3] + cMo[2][0] * oX0 + cMo[2][1] * oY0 + cMo[2][2] * oZ0;
281 cP_[3] = (1 - a * a) * X2 - a * b * Y2 - a * c * Z2;
282 cP_[4] = -a * b * X2 + (1 - b * b) * Y2 - b * c * Z2;
283 cP_[5] = -a * c * X2 - b * c * Y2 + (1 - c * c) * Z2;
347 return (B - std::sqrt(B * B - A * C)) / A;
370 const vpColor &color,
unsigned int thickness)
392 const vpColor &color,
unsigned int thickness)
410 unsigned int thickness)
424 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 functionalities.
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 emitted 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.
Definition of the vpImage class member functions.