gp_Circ LeastSquaresFitting::fitLeastSquaresCircleToPoints(const Handle_TColgp_HArray1OfPnt& points)
{
//Get initial guess for least squares fit
InitialGuessForLeastSquaresFitting initialGuess = findInitialGuess(points);
math_Vector startingPointForBFGS(1,3);
startingPointForBFGS(1) = initialGuess.myCenterPoint.X();
startingPointForBFGS(2) = initialGuess.myCenterPoint.Y();
startingPointForBFGS(3) = initialGuess.myRadius;
CircleFitCostFunction costFunction(points);
math_BFGS minimizer(costFunction,startingPointForBFGS);
math_Vector minimum(1,3);
if (minimizer.IsDone())
{
std::cout << "Minimizer is done, result is:" << std::endl;
minimizer.Location(minimum);
std::cout << minimum(1) << " " << minimum(2) << " " << minimum(3) << std::endl;
} else
{
std::cout << "Error: minimizer failed!" << std::endl;
}
gp_Ax2 axis = gp::XOY();
axis.SetLocation(gp_Pnt(minimum(1),minimum(2),0.0));
gp_Circ result(axis,minimum(3));
return result;
}
bool solve(const P3* ptArray, int n, Result *result) {
P3 const *p0 = NULL;
P3 const *p1 = NULL;
P3 const *p2 = NULL;
if (!findInitialGuess(ptArray, n, &p0, &p1, &p2)) {
result->u = std::numeric_limits<double>::infinity();
result->v = std::numeric_limits<double>::infinity();
return false;
}
#ifdef PARALLEL
static OpenMPReductor<const P3*, LetItFallBody> reductor;
LetItFallBody body;
body.init(p0, p1, p2);
#endif
int counter = 0;
bool c1;
do {
counter++;
release_assert(counter < 1000);
#ifndef PARALLEL
c1 = letItFall(ptArray, n, &p0, &p1, &p2);
#else
body.changed = false;
reductor.reduce(ptArray, ptArray + n, &body);
c1 = body.changed;
#endif
} while(c1);
#ifdef PARALLEL
p0 = body.p0;
p1 = body.p1;
p2 = body.p2;
#endif
performanceDataSingleton.add_scans_count_3d(counter);
dualSolutionToMinMaxSolution(*p0, *p1, *p2, &result->u, &result->v, &result->value);
internal_verification(assertDualSolution(ptArray, n, *p0, *p1, *p2));
return true;
}
