39 #include <visp3/core/vpCircle.h>
41 #include <visp3/core/vpFeatureDisplay.h>
76 const double Z0,
const double R)
118 vpCircle::vpCircle(
const double A,
const double B,
const double C,
const double X0,
const double Y0,
const double Z0,
171 double s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - r * r;
172 double det = A * X0 + B * Y0 + C * Z0;
177 K[0] = 1 - 2 * A * X0 + A * A * s;
178 K[1] = 1 - 2 * B * Y0 + B * B * s;
179 K[2] = -A * Y0 - B * X0 + A * B * s;
180 K[3] = -C * X0 - A * Z0 + A * C * s;
181 K[4] = -C * Y0 - B * Z0 + B * C * s;
182 K[5] = 1 - 2 * C * Z0 + C * C * s;
190 double det = K[2] * K[2] - K[0] * K[1];
191 if (fabs(det) < 1e-8) {
196 double xc = (K[1] * K[3] - K[2] * K[4]) / det;
197 double yc = (K[0] * K[4] - K[2] * K[3]) / det;
199 double c = sqrt((K[0] - K[1]) * (K[0] - K[1]) + 4 * K[2] * K[2]);
200 double s = 2 * (K[0] * xc * xc + 2 * K[2] * xc * yc + K[1] * yc * yc - K[5]);
205 if (fabs(K[2]) < std::numeric_limits<double>::epsilon()) {
208 A = sqrt(s / (K[0] + K[1] + c));
209 B = sqrt(s / (K[0] + K[1] - c));
211 A = sqrt(s / (K[0] + K[1] - c));
212 B = sqrt(s / (K[0] + K[1] + c));
215 E = (K[1] - K[0] + c) / (2 * K[2]);
217 A = sqrt(s / (K[0] + K[1] + c));
218 B = sqrt(s / (K[0] + K[1] - c));
220 A = sqrt(s / (K[0] + K[1] - c));
221 B = sqrt(s / (K[0] + K[1] + c));
243 A = cMo[0][0] *
oP[0] + cMo[0][1] *
oP[1] + cMo[0][2] *
oP[2];
244 B = cMo[1][0] *
oP[0] + cMo[1][1] *
oP[1] + cMo[1][2] *
oP[2];
245 C = cMo[2][0] *
oP[0] + cMo[2][1] *
oP[1] + cMo[2][2] *
oP[2];
248 X0 = cMo[0][3] + cMo[0][0] *
oP[3] + cMo[0][1] *
oP[4] + cMo[0][2] *
oP[5];
249 Y0 = cMo[1][3] + cMo[1][0] *
oP[3] + cMo[1][1] *
oP[4] + cMo[1][2] *
oP[5];
250 Z0 = cMo[2][3] + cMo[2][0] *
oP[3] + cMo[2][1] *
oP[4] + cMo[2][2] *
oP[5];
271 A = cMo[0][0] *
oP[0] + cMo[0][1] *
oP[1] + cMo[0][2] *
oP[2];
272 B = cMo[1][0] *
oP[0] + cMo[1][1] *
oP[1] + cMo[1][2] *
oP[2];
273 C = cMo[2][0] *
oP[0] + cMo[2][1] *
oP[1] + cMo[2][2] *
oP[2];
276 X0 = cMo[0][3] + cMo[0][0] *
oP[3] + cMo[0][1] *
oP[4] + cMo[0][2] *
oP[5];
277 Y0 = cMo[1][3] + cMo[1][0] *
oP[3] + cMo[1][1] *
oP[4] + cMo[1][2] *
oP[5];
278 Z0 = cMo[2][3] + cMo[2][0] *
oP[3] + cMo[2][1] *
oP[4] + cMo[2][2] *
oP[5];
295 const unsigned int thickness)
302 const vpColor &color,
const unsigned int thickness)
333 const double &theta,
double &i,
double &j)
341 double mu11 = circle.
p[3];
342 double mu02 = circle.
p[4];
343 double mu20 = circle.
p[2];
344 double Xg = u0 + circle.
p[0] * px;
345 double Yg = v0 + circle.
p[1] * py;
350 double stheta = sin(theta);
351 double ctheta = cos(theta);
352 double sctheta = stheta * ctheta;
353 double m11yg = mu11 * Yg;
355 double m02xg = mu02 * Xg;
356 double m11stheta = mu11 * stheta;
357 j = ((mu11 * Xg * sctheta - mu20 * Yg * sctheta + mu20 * rho * ctheta - m11yg + m11yg * ctheta2 + m02xg -
358 m02xg * ctheta2 + m11stheta * rho) /
359 (mu20 * ctheta2 + 2.0 * m11stheta * ctheta + mu02 - mu02 * ctheta2));
361 double rhom02 = rho * mu02;
362 double sctheta2 = stheta * ctheta2;
363 double ctheta3 = ctheta2 * ctheta;
364 i = (-(-rho * mu11 * stheta * ctheta - rhom02 + rhom02 * ctheta2 + mu11 * Xg * sctheta2 - mu20 * Yg * sctheta2 -
365 ctheta * mu11 * Yg + ctheta3 * mu11 * Yg + ctheta * mu02 * Xg - ctheta3 * mu02 * Xg) /
366 (mu20 * ctheta2 + 2.0 * mu11 * stheta * ctheta + mu02 - mu02 * ctheta2) / stheta);