36 #include <visp3/core/vpCylinder.h>
37 #include <visp3/core/vpFeatureDisplay.h>
180 double co, si, e, x0, y0, z0;
181 double A, B, C, X0, Y0, Z0, R;
182 double s, a, b, c, zero;
191 zero = A * X0 + B * Y0 + C * Z0;
193 s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - R * R - zero * zero;
201 x0 = C * Y0 - B * Z0;
202 y0 = A * Z0 - C * X0;
203 z0 = B * X0 - A * Y0;
208 e = sqrt(co * co + si * si);
209 p_[0] = -(R * c * s - z0) / e;
210 p_[1] = atan2(si, co);
215 e = sqrt(co * co + si * si);
216 p_[2] = -(R * c * s + z0) / e;
217 p_[3] = atan2(si, co);
249 double oA, oB, oC, oX0, oY0, oZ0;
257 X1 = cMo[0][0] * oA + cMo[0][1] * oB + cMo[0][2] * oC;
258 Y1 = cMo[1][0] * oA + cMo[1][1] * oB + cMo[1][2] * oC;
259 Z1 = cMo[2][0] * oA + cMo[2][1] * oB + cMo[2][2] * oC;
260 s = sqrt(X1 * X1 + Y1 * Y1 + Z1 * Z1);
270 X2 = cMo[0][3] + cMo[0][0] * oX0 + cMo[0][1] * oY0 + cMo[0][2] * oZ0;
271 Y2 = cMo[1][3] + cMo[1][0] * oX0 + cMo[1][1] * oY0 + cMo[1][2] * oZ0;
272 Z2 = cMo[2][3] + cMo[2][0] * oX0 + cMo[2][1] * oY0 + cMo[2][2] * oZ0;
276 cP_[3] = (1 - a * a) * X2 - a * b * Y2 - a * c * Z2;
277 cP_[4] = -a * b * X2 + (1 - b * b) * Y2 - b * c * Z2;
278 cP_[5] = -a * c * X2 - b * c * Y2 + (1 - c * c) * Z2;
342 return (B - std::sqrt(B * B - A * C)) / A;
365 const vpColor &color,
unsigned int thickness)
387 const vpColor &color,
unsigned int thickness)
405 unsigned int thickness)
419 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 projection() override
vpCylinder * duplicate() const override
For memory issue (used by the vpServo class only).
void display(const vpImage< unsigned char > &I, const vpCameraParameters &cam, const vpColor &color=vpColor::green, unsigned int thickness=1) override
void setWorldCoordinates(const vpColVector &oP) override
double computeZ(double x, double y) const
void changeFrame(const vpHomogeneousMatrix &cMo, vpColVector &cP) const override
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.