39 #include <visp3/core/vpCircle.h>
41 #include <visp3/core/vpFeatureDisplay.h>
117 vpCircle::vpCircle(
double A,
double B,
double C,
double X0,
double Y0,
double Z0,
double R)
169 double s = X0 * X0 + Y0 * Y0 + Z0 * Z0 - r * r;
170 double det = A * X0 + B * Y0 + C * Z0;
175 K[0] = 1 - 2 * A * X0 + A * A * s;
176 K[1] = 1 - 2 * B * Y0 + B * B * s;
177 K[2] = -A * Y0 - B * X0 + A * B * s;
178 K[3] = -C * X0 - A * Z0 + A * C * s;
179 K[4] = -C * Y0 - B * Z0 + B * C * s;
180 K[5] = 1 - 2 * C * Z0 + C * C * s;
188 double det = K[2] * K[2] - K[0] * K[1];
189 if (fabs(det) < 1e-8) {
194 double xc = (K[1] * K[3] - K[2] * K[4]) / det;
195 double yc = (K[0] * K[4] - K[2] * K[3]) / det;
197 double c = sqrt((K[0] - K[1]) * (K[0] - K[1]) + 4 * K[2] * K[2]);
198 double s = 2 * (K[0] * xc * xc + 2 * K[2] * xc * yc + K[1] * yc * yc - K[5]);
203 if (fabs(K[2]) < std::numeric_limits<double>::epsilon()) {
206 A = sqrt(s / (K[0] + K[1] + c));
207 B = sqrt(s / (K[0] + K[1] - c));
209 A = sqrt(s / (K[0] + K[1] - c));
210 B = sqrt(s / (K[0] + K[1] + c));
213 E = (K[1] - K[0] + c) / (2 * K[2]);
215 A = sqrt(s / (K[0] + K[1] + c));
216 B = sqrt(s / (K[0] + K[1] - c));
218 A = sqrt(s / (K[0] + K[1] - c));
219 B = sqrt(s / (K[0] + K[1] + c));
241 A = cMo[0][0] *
oP[0] + cMo[0][1] *
oP[1] + cMo[0][2] *
oP[2];
242 B = cMo[1][0] *
oP[0] + cMo[1][1] *
oP[1] + cMo[1][2] *
oP[2];
243 C = cMo[2][0] *
oP[0] + cMo[2][1] *
oP[1] + cMo[2][2] *
oP[2];
246 X0 = cMo[0][3] + cMo[0][0] *
oP[3] + cMo[0][1] *
oP[4] + cMo[0][2] *
oP[5];
247 Y0 = cMo[1][3] + cMo[1][0] *
oP[3] + cMo[1][1] *
oP[4] + cMo[1][2] *
oP[5];
248 Z0 = cMo[2][3] + cMo[2][0] *
oP[3] + cMo[2][1] *
oP[4] + cMo[2][2] *
oP[5];
269 A = cMo[0][0] *
oP[0] + cMo[0][1] *
oP[1] + cMo[0][2] *
oP[2];
270 B = cMo[1][0] *
oP[0] + cMo[1][1] *
oP[1] + cMo[1][2] *
oP[2];
271 C = cMo[2][0] *
oP[0] + cMo[2][1] *
oP[1] + cMo[2][2] *
oP[2];
274 X0 = cMo[0][3] + cMo[0][0] *
oP[3] + cMo[0][1] *
oP[4] + cMo[0][2] *
oP[5];
275 Y0 = cMo[1][3] + cMo[1][0] *
oP[3] + cMo[1][1] *
oP[4] + cMo[1][2] *
oP[5];
276 Z0 = cMo[2][3] + cMo[2][0] *
oP[3] + cMo[2][1] *
oP[4] + cMo[2][2] *
oP[5];
293 unsigned int thickness)
300 const vpColor &color,
unsigned int thickness)
331 const double &theta,
double &i,
double &j)
339 double mu11 = circle.
p[3];
340 double mu02 = circle.
p[4];
341 double mu20 = circle.
p[2];
342 double Xg = u0 + circle.
p[0] * px;
343 double Yg = v0 + circle.
p[1] * py;
348 double stheta = sin(theta);
349 double ctheta = cos(theta);
350 double sctheta = stheta * ctheta;
351 double m11yg = mu11 * Yg;
353 double m02xg = mu02 * Xg;
354 double m11stheta = mu11 * stheta;
355 j = ((mu11 * Xg * sctheta - mu20 * Yg * sctheta + mu20 * rho * ctheta - m11yg + m11yg * ctheta2 + m02xg -
356 m02xg * ctheta2 + m11stheta * rho) /
357 (mu20 * ctheta2 + 2.0 * m11stheta * ctheta + mu02 - mu02 * ctheta2));
359 double rhom02 = rho * mu02;
360 double sctheta2 = stheta * ctheta2;
361 double ctheta3 = ctheta2 * ctheta;
362 i = (-(-rho * mu11 * stheta * ctheta - rhom02 + rhom02 * ctheta2 + mu11 * Xg * sctheta2 - mu20 * Yg * sctheta2 -
363 ctheta * mu11 * Yg + ctheta3 * mu11 * Yg + ctheta * mu02 * Xg - ctheta3 * mu02 * Xg) /
364 (mu20 * ctheta2 + 2.0 * mu11 * stheta * ctheta + mu02 - mu02 * ctheta2) / stheta);