From 8660d9a91429b3013d2bfed027a726f75d0d5ee5 Mon Sep 17 00:00:00 2001 From: DeepSOIC Date: Wed, 17 Dec 2014 20:53:21 +0300 Subject: [PATCH] Sketcher Ellipse: porting to DeriVector2 Sketcher Ellipse: porting tangent-line to DeriVector2 Replacing a ton of unreadable, sage generated math code with easy-to-manage C++ code. Sketcher Ellipse: porting internal align-t to DeriVector2 Sketcher Ellipse: small math refactor; const members Moving the repeating code computing deriv+value of major radius to a method of GCS::Ellipse. Marking several methods of DeriVector2 as const member functions. Sketcher Ellipse: porting arc angle rules to DeriVector2 Just porting. Probably a complete remake of the concept is worth... Angles can be calculated explicitly, there's no need to load the solver. I see no benefits whatsoever on using the solver to keep track of angle values. Sketcher Ellipse: porting equality to DeriVector2 --- src/Mod/Sketcher/App/freegcs/Constraints.cpp | 1322 ++++-------------- src/Mod/Sketcher/App/freegcs/Constraints.h | 51 +- src/Mod/Sketcher/App/freegcs/Geo.cpp | 35 +- src/Mod/Sketcher/App/freegcs/Geo.h | 27 +- 4 files changed, 354 insertions(+), 1081 deletions(-) diff --git a/src/Mod/Sketcher/App/freegcs/Constraints.cpp b/src/Mod/Sketcher/App/freegcs/Constraints.cpp index 99a0d3772..6e6368f0c 100644 --- a/src/Mod/Sketcher/App/freegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/freegcs/Constraints.cpp @@ -1024,32 +1024,22 @@ double ConstraintPointOnEllipse::grad(double *param) // ConstraintEllipseTangentLine ConstraintEllipseTangentLine::ConstraintEllipseTangentLine(Line &l, Ellipse &e) { - pvec.push_back(l.p1.x); - pvec.push_back(l.p1.y); - pvec.push_back(l.p2.x); - pvec.push_back(l.p2.y); - pvec.push_back(e.center.x); - pvec.push_back(e.center.y); - pvec.push_back(e.focus1.x); - pvec.push_back(e.focus1.y); - pvec.push_back(e.radmin); + this->l = l; + this->l.PushOwnParams(pvec); + + this->e = e; + this->e.PushOwnParams(pvec);//DeepSOIC: hopefully, this won't push arc's parameters origpvec = pvec; + pvecChangedFlag = true; rescale(); } -ConstraintEllipseTangentLine::ConstraintEllipseTangentLine(Line &l, ArcOfEllipse &a) +void ConstraintEllipseTangentLine::ReconstructGeomPointers() { - pvec.push_back(l.p1.x); - pvec.push_back(l.p1.y); - pvec.push_back(l.p2.x); - pvec.push_back(l.p2.y); - pvec.push_back(a.center.x); - pvec.push_back(a.center.y); - pvec.push_back(a.focus1.x); - pvec.push_back(a.focus1.y); - pvec.push_back(a.radmin); - origpvec = pvec; - rescale(); + int i=0; + l.ReconstructOnNewPvec(pvec, i); + e.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; } ConstraintType ConstraintEllipseTangentLine::getTypeId() @@ -1062,318 +1052,99 @@ void ConstraintEllipseTangentLine::rescale(double coef) scale = coef * 1; } +void ConstraintEllipseTangentLine::errorgrad(double *err, double *grad, double *param) +{ + // DeepSOIC equation + // http://forum.freecadweb.org/viewtopic.php?f=10&t=7520&start=140 + + if (pvecChangedFlag) ReconstructGeomPointers(); + DeriVector2 p1 (l.p1, param); + DeriVector2 p2 (l.p2, param); + DeriVector2 f1 (e.focus1, param); + DeriVector2 c (e.center, param); + DeriVector2 f2 = c.linCombi(2.0, f1, -1.0); // 2*cv - f1v + + //mirror F1 against the line + DeriVector2 nl = l.CalculateNormal(l.p1, param).getNormalized(); + double distF1L = 0, ddistF1L = 0; //distance F1 to line + distF1L = f1.subtr(p1).scalarProd(nl,&ddistF1L); + DeriVector2 f1m = f1.sum(nl.multD(-2*distF1L,-2*ddistF1L));//f1m = f1 mirrored + + //calculate distance form f1m to f2 + double distF1mF2, ddistF1mF2; + distF1mF2 = f2.subtr(f1m).length(ddistF1mF2); + + //calculate major radius (to compare the distance to) + double dradmin = (param == e.radmin) ? 1.0 : 0.0; + double radmaj, dradmaj; + radmaj = e.getRadMaj(c,f1,*e.radmin, dradmin, dradmaj); + + if (err) + *err = distF1mF2 - 2*radmaj; + if (grad) + *grad = ddistF1mF2 - 2*dradmaj; +} + double ConstraintEllipseTangentLine::error() { - double X_1 = *p1x(); - double Y_1 = *p1y(); - double X_2 = *p2x(); - double Y_2 = *p2y(); - double X_c = *cx(); - double Y_c = *cy(); - double X_F1 = *f1x(); - double Y_F1 = *f1y(); - double b = *rmin(); - - double err=-2*(sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - - 2*Y_1*Y_2 + pow(Y_2, 2))*sqrt(pow(X_F1, 2) - 2*X_F1*X_c + pow(X_c, 2) + - pow(Y_F1, 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + pow(b, 2)) - sqrt(pow(X_1, - 2)*(pow(X_c, 2) + pow(Y_c, 2)) - 2*X_1*X_2*(pow(X_c, 2) + pow(Y_c, 2)) + - pow(X_2, 2)*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_F1, 2)*(pow(X_1, 2) - - 2*X_1*X_2 + pow(X_2, 2)) - 2*X_F1*(pow(X_1, 2)*X_c - 2*X_1*X_2*X_c + - pow(X_2, 2)*X_c) + pow(Y_1, 2)*(pow(X_2, 2) - 2*X_2*X_c + pow(X_c, 2) + - pow(Y_c, 2)) + 2*Y_1*(X_1*X_2*Y_c - pow(X_2, 2)*Y_c - X_F1*(X_1*Y_c - - X_2*Y_c)) + pow(Y_2, 2)*(pow(X_1, 2) - 2*X_1*X_c + pow(X_c, 2) + - pow(Y_c, 2)) - 2*Y_2*(pow(X_1, 2)*Y_c - X_1*X_2*Y_c - X_F1*(X_1*Y_c - - X_2*Y_c) + Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) + pow(Y_c, 2))) - + pow(Y_F1, 2)*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)) - - 2*Y_F1*(pow(Y_1, 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + - pow(Y_2, 2)*Y_c + Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - - 2*Y_1*Y_c))))/sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - - 2*Y_1*Y_2 + pow(Y_2, 2)); + double err; + errorgrad(&err,0,0); return scale * err; } double ConstraintEllipseTangentLine::grad(double *param) { - double deriv=0.; - if (param == p1x() || param == p1y() || - param == p2x() || param == p2y() || - param == f1x() || param == f1y() || - param == cx() || param == cy() || - param == rmin()) { - - double X_1 = *p1x(); - double Y_1 = *p1y(); - double X_2 = *p2x(); - double Y_2 = *p2y(); - double X_c = *cx(); - double Y_c = *cy(); - double X_F1 = *f1x(); - double Y_F1 = *f1y(); - double b = *rmin(); - - // DeepSOIC equation - // http://forum.freecadweb.org/viewtopic.php?f=10&t=7520&start=140 - // Partials: - - if (param == p1x()) - deriv += 2*(X_1 - X_2)*(sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + - pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2))*sqrt(pow(X_F1, 2) - 2*X_F1*X_c + - pow(X_c, 2) + pow(Y_F1, 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + pow(b, 2)) - - sqrt(pow(X_1, 2)*(pow(X_c, 2) + pow(Y_c, 2)) - 2*X_1*X_2*(pow(X_c, 2) + - pow(Y_c, 2)) + pow(X_2, 2)*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_F1, - 2)*(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2)) - 2*X_F1*(pow(X_1, 2)*X_c - - 2*X_1*X_2*X_c + pow(X_2, 2)*X_c) + pow(Y_1, 2)*(pow(X_2, 2) - 2*X_2*X_c - + pow(X_c, 2) + pow(Y_c, 2)) + 2*Y_1*(X_1*X_2*Y_c - pow(X_2, 2)*Y_c - - X_F1*(X_1*Y_c - X_2*Y_c)) + pow(Y_2, 2)*(pow(X_1, 2) - 2*X_1*X_c + - pow(X_c, 2) + pow(Y_c, 2)) - 2*Y_2*(pow(X_1, 2)*Y_c - X_1*X_2*Y_c - - X_F1*(X_1*Y_c - X_2*Y_c) + Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) - + pow(Y_c, 2))) + pow(Y_F1, 2)*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)) - - 2*Y_F1*(pow(Y_1, 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + - pow(Y_2, 2)*Y_c + Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - - 2*Y_1*Y_c))))/pow(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - - 2*Y_1*Y_2 + pow(Y_2, 2), 3.0/2.0) - 2*((X_1 - X_2)*sqrt(pow(X_F1, 2) - - 2*X_F1*X_c + pow(X_c, 2) + pow(Y_F1, 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + - pow(b, 2))/sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - - 2*Y_1*Y_2 + pow(Y_2, 2)) - (X_1*(pow(X_c, 2) + pow(Y_c, 2)) - - X_2*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_F1, 2)*(X_1 - X_2) - - 2*X_F1*(X_1*X_c - X_2*X_c) + Y_1*(X_2*Y_c - X_F1*Y_c) + pow(Y_2, 2)*(X_1 - - X_c) - Y_2*(2*X_1*Y_c - X_2*Y_c - X_F1*Y_c + Y_1*(X_2 - X_c)) + - Y_F1*(Y_1*(X_F1 - X_c) - Y_2*(X_F1 - X_c)))/sqrt(pow(X_1, 2)*(pow(X_c, - 2) + pow(Y_c, 2)) - 2*X_1*X_2*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_2, - 2)*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_F1, 2)*(pow(X_1, 2) - 2*X_1*X_2 + - pow(X_2, 2)) - 2*X_F1*(pow(X_1, 2)*X_c - 2*X_1*X_2*X_c + pow(X_2, - 2)*X_c) + pow(Y_1, 2)*(pow(X_2, 2) - 2*X_2*X_c + pow(X_c, 2) + pow(Y_c, - 2)) + 2*Y_1*(X_1*X_2*Y_c - pow(X_2, 2)*Y_c - X_F1*(X_1*Y_c - X_2*Y_c)) + - pow(Y_2, 2)*(pow(X_1, 2) - 2*X_1*X_c + pow(X_c, 2) + pow(Y_c, 2)) - - 2*Y_2*(pow(X_1, 2)*Y_c - X_1*X_2*Y_c - X_F1*(X_1*Y_c - X_2*Y_c) + - Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) + pow(Y_c, 2))) + - pow(Y_F1, 2)*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)) - 2*Y_F1*(pow(Y_1, - 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + pow(Y_2, 2)*Y_c + - Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - 2*Y_1*Y_c))))/sqrt(pow(X_1, - 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)); - if (param == p1y()) - deriv += 2*(Y_1 - Y_2)*(sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + - pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2))*sqrt(pow(X_F1, 2) - 2*X_F1*X_c + - pow(X_c, 2) + pow(Y_F1, 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + pow(b, 2)) - - sqrt(pow(X_1, 2)*(pow(X_c, 2) + pow(Y_c, 2)) - 2*X_1*X_2*(pow(X_c, 2) + - pow(Y_c, 2)) + pow(X_2, 2)*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_F1, - 2)*(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2)) - 2*X_F1*(pow(X_1, 2)*X_c - - 2*X_1*X_2*X_c + pow(X_2, 2)*X_c) + pow(Y_1, 2)*(pow(X_2, 2) - 2*X_2*X_c - + pow(X_c, 2) + pow(Y_c, 2)) + 2*Y_1*(X_1*X_2*Y_c - pow(X_2, 2)*Y_c - - X_F1*(X_1*Y_c - X_2*Y_c)) + pow(Y_2, 2)*(pow(X_1, 2) - 2*X_1*X_c + - pow(X_c, 2) + pow(Y_c, 2)) - 2*Y_2*(pow(X_1, 2)*Y_c - X_1*X_2*Y_c - - X_F1*(X_1*Y_c - X_2*Y_c) + Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) - + pow(Y_c, 2))) + pow(Y_F1, 2)*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)) - - 2*Y_F1*(pow(Y_1, 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + - pow(Y_2, 2)*Y_c + Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - - 2*Y_1*Y_c))))/pow(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - - 2*Y_1*Y_2 + pow(Y_2, 2), 3.0/2.0) - 2*((Y_1 - Y_2)*sqrt(pow(X_F1, 2) - - 2*X_F1*X_c + pow(X_c, 2) + pow(Y_F1, 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + - pow(b, 2))/sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - - 2*Y_1*Y_2 + pow(Y_2, 2)) - (X_1*X_2*Y_c - pow(X_2, 2)*Y_c - - X_F1*(X_1*Y_c - X_2*Y_c) + Y_1*(pow(X_2, 2) - 2*X_2*X_c + pow(X_c, 2) + - pow(Y_c, 2)) - Y_2*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) + pow(Y_c, - 2)) + pow(Y_F1, 2)*(Y_1 - Y_2) + Y_F1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - - X_2) - 2*Y_1*Y_c + 2*Y_2*Y_c))/sqrt(pow(X_1, 2)*(pow(X_c, 2) + pow(Y_c, - 2)) - 2*X_1*X_2*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_2, 2)*(pow(X_c, 2) + - pow(Y_c, 2)) + pow(X_F1, 2)*(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2)) - - 2*X_F1*(pow(X_1, 2)*X_c - 2*X_1*X_2*X_c + pow(X_2, 2)*X_c) + pow(Y_1, - 2)*(pow(X_2, 2) - 2*X_2*X_c + pow(X_c, 2) + pow(Y_c, 2)) + - 2*Y_1*(X_1*X_2*Y_c - pow(X_2, 2)*Y_c - X_F1*(X_1*Y_c - X_2*Y_c)) + - pow(Y_2, 2)*(pow(X_1, 2) - 2*X_1*X_c + pow(X_c, 2) + pow(Y_c, 2)) - - 2*Y_2*(pow(X_1, 2)*Y_c - X_1*X_2*Y_c - X_F1*(X_1*Y_c - X_2*Y_c) + - Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) + pow(Y_c, 2))) + - pow(Y_F1, 2)*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)) - 2*Y_F1*(pow(Y_1, - 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + pow(Y_2, 2)*Y_c + - Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - 2*Y_1*Y_c))))/sqrt(pow(X_1, - 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)); - if (param == p2x()) - deriv += -2*(X_1 - X_2)*(sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + - pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2))*sqrt(pow(X_F1, 2) - 2*X_F1*X_c + - pow(X_c, 2) + pow(Y_F1, 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + pow(b, 2)) - - sqrt(pow(X_1, 2)*(pow(X_c, 2) + pow(Y_c, 2)) - 2*X_1*X_2*(pow(X_c, 2) + - pow(Y_c, 2)) + pow(X_2, 2)*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_F1, - 2)*(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2)) - 2*X_F1*(pow(X_1, 2)*X_c - - 2*X_1*X_2*X_c + pow(X_2, 2)*X_c) + pow(Y_1, 2)*(pow(X_2, 2) - 2*X_2*X_c - + pow(X_c, 2) + pow(Y_c, 2)) + 2*Y_1*(X_1*X_2*Y_c - pow(X_2, 2)*Y_c - - X_F1*(X_1*Y_c - X_2*Y_c)) + pow(Y_2, 2)*(pow(X_1, 2) - 2*X_1*X_c + - pow(X_c, 2) + pow(Y_c, 2)) - 2*Y_2*(pow(X_1, 2)*Y_c - X_1*X_2*Y_c - - X_F1*(X_1*Y_c - X_2*Y_c) + Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) - + pow(Y_c, 2))) + pow(Y_F1, 2)*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)) - - 2*Y_F1*(pow(Y_1, 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + - pow(Y_2, 2)*Y_c + Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - - 2*Y_1*Y_c))))/pow(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - - 2*Y_1*Y_2 + pow(Y_2, 2), 3.0/2.0) + 2*((X_1 - X_2)*sqrt(pow(X_F1, 2) - - 2*X_F1*X_c + pow(X_c, 2) + pow(Y_F1, 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + - pow(b, 2))/sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - - 2*Y_1*Y_2 + pow(Y_2, 2)) - (X_1*(pow(X_c, 2) + pow(Y_c, 2)) - - X_2*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_F1, 2)*(X_1 - X_2) - - 2*X_F1*(X_1*X_c - X_2*X_c) - pow(Y_1, 2)*(X_2 - X_c) - Y_1*(X_1*Y_c - - 2*X_2*Y_c + X_F1*Y_c) + Y_2*(-X_1*Y_c + X_F1*Y_c + Y_1*(X_1 - X_c)) + - Y_F1*(Y_1*(X_F1 - X_c) - Y_2*(X_F1 - X_c)))/sqrt(pow(X_1, 2)*(pow(X_c, - 2) + pow(Y_c, 2)) - 2*X_1*X_2*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_2, - 2)*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_F1, 2)*(pow(X_1, 2) - 2*X_1*X_2 + - pow(X_2, 2)) - 2*X_F1*(pow(X_1, 2)*X_c - 2*X_1*X_2*X_c + pow(X_2, - 2)*X_c) + pow(Y_1, 2)*(pow(X_2, 2) - 2*X_2*X_c + pow(X_c, 2) + pow(Y_c, - 2)) + 2*Y_1*(X_1*X_2*Y_c - pow(X_2, 2)*Y_c - X_F1*(X_1*Y_c - X_2*Y_c)) + - pow(Y_2, 2)*(pow(X_1, 2) - 2*X_1*X_c + pow(X_c, 2) + pow(Y_c, 2)) - - 2*Y_2*(pow(X_1, 2)*Y_c - X_1*X_2*Y_c - X_F1*(X_1*Y_c - X_2*Y_c) + - Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) + pow(Y_c, 2))) + - pow(Y_F1, 2)*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)) - 2*Y_F1*(pow(Y_1, - 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + pow(Y_2, 2)*Y_c + - Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - 2*Y_1*Y_c))))/sqrt(pow(X_1, - 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)); - if (param == p2y()) - deriv += -2*(Y_1 - Y_2)*(sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + - pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2))*sqrt(pow(X_F1, 2) - 2*X_F1*X_c + - pow(X_c, 2) + pow(Y_F1, 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + pow(b, 2)) - - sqrt(pow(X_1, 2)*(pow(X_c, 2) + pow(Y_c, 2)) - 2*X_1*X_2*(pow(X_c, 2) + - pow(Y_c, 2)) + pow(X_2, 2)*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_F1, - 2)*(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2)) - 2*X_F1*(pow(X_1, 2)*X_c - - 2*X_1*X_2*X_c + pow(X_2, 2)*X_c) + pow(Y_1, 2)*(pow(X_2, 2) - 2*X_2*X_c - + pow(X_c, 2) + pow(Y_c, 2)) + 2*Y_1*(X_1*X_2*Y_c - pow(X_2, 2)*Y_c - - X_F1*(X_1*Y_c - X_2*Y_c)) + pow(Y_2, 2)*(pow(X_1, 2) - 2*X_1*X_c + - pow(X_c, 2) + pow(Y_c, 2)) - 2*Y_2*(pow(X_1, 2)*Y_c - X_1*X_2*Y_c - - X_F1*(X_1*Y_c - X_2*Y_c) + Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) - + pow(Y_c, 2))) + pow(Y_F1, 2)*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)) - - 2*Y_F1*(pow(Y_1, 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + - pow(Y_2, 2)*Y_c + Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - - 2*Y_1*Y_c))))/pow(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - - 2*Y_1*Y_2 + pow(Y_2, 2), 3.0/2.0) + 2*((Y_1 - Y_2)*sqrt(pow(X_F1, 2) - - 2*X_F1*X_c + pow(X_c, 2) + pow(Y_F1, 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + - pow(b, 2))/sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - - 2*Y_1*Y_2 + pow(Y_2, 2)) - (pow(X_1, 2)*Y_c - X_1*X_2*Y_c - - X_F1*(X_1*Y_c - X_2*Y_c) + Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) - + pow(Y_c, 2)) - Y_2*(pow(X_1, 2) - 2*X_1*X_c + pow(X_c, 2) + pow(Y_c, - 2)) + pow(Y_F1, 2)*(Y_1 - Y_2) + Y_F1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - - X_2) - 2*Y_1*Y_c + 2*Y_2*Y_c))/sqrt(pow(X_1, 2)*(pow(X_c, 2) + pow(Y_c, - 2)) - 2*X_1*X_2*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_2, 2)*(pow(X_c, 2) + - pow(Y_c, 2)) + pow(X_F1, 2)*(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2)) - - 2*X_F1*(pow(X_1, 2)*X_c - 2*X_1*X_2*X_c + pow(X_2, 2)*X_c) + pow(Y_1, - 2)*(pow(X_2, 2) - 2*X_2*X_c + pow(X_c, 2) + pow(Y_c, 2)) + - 2*Y_1*(X_1*X_2*Y_c - pow(X_2, 2)*Y_c - X_F1*(X_1*Y_c - X_2*Y_c)) + - pow(Y_2, 2)*(pow(X_1, 2) - 2*X_1*X_c + pow(X_c, 2) + pow(Y_c, 2)) - - 2*Y_2*(pow(X_1, 2)*Y_c - X_1*X_2*Y_c - X_F1*(X_1*Y_c - X_2*Y_c) + - Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) + pow(Y_c, 2))) + - pow(Y_F1, 2)*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)) - 2*Y_F1*(pow(Y_1, - 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + pow(Y_2, 2)*Y_c + - Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - 2*Y_1*Y_c))))/sqrt(pow(X_1, - 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)); - if (param == f1x()) - deriv += -2*((X_F1 - X_c)*sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + - pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2))/sqrt(pow(X_F1, 2) - 2*X_F1*X_c + - pow(X_c, 2) + pow(Y_F1, 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + pow(b, 2)) + - (pow(X_1, 2)*X_c - 2*X_1*X_2*X_c + pow(X_2, 2)*X_c - X_F1*(pow(X_1, 2) - - 2*X_1*X_2 + pow(X_2, 2)) + Y_1*(X_1*Y_c - X_2*Y_c) - Y_2*(X_1*Y_c - - X_2*Y_c) - Y_F1*(Y_1*(X_1 - X_2) - Y_2*(X_1 - X_2)))/sqrt(pow(X_1, - 2)*(pow(X_c, 2) + pow(Y_c, 2)) - 2*X_1*X_2*(pow(X_c, 2) + pow(Y_c, 2)) + - pow(X_2, 2)*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_F1, 2)*(pow(X_1, 2) - - 2*X_1*X_2 + pow(X_2, 2)) - 2*X_F1*(pow(X_1, 2)*X_c - 2*X_1*X_2*X_c + - pow(X_2, 2)*X_c) + pow(Y_1, 2)*(pow(X_2, 2) - 2*X_2*X_c + pow(X_c, 2) + - pow(Y_c, 2)) + 2*Y_1*(X_1*X_2*Y_c - pow(X_2, 2)*Y_c - X_F1*(X_1*Y_c - - X_2*Y_c)) + pow(Y_2, 2)*(pow(X_1, 2) - 2*X_1*X_c + pow(X_c, 2) + - pow(Y_c, 2)) - 2*Y_2*(pow(X_1, 2)*Y_c - X_1*X_2*Y_c - X_F1*(X_1*Y_c - - X_2*Y_c) + Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) + pow(Y_c, 2))) - + pow(Y_F1, 2)*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)) - - 2*Y_F1*(pow(Y_1, 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + - pow(Y_2, 2)*Y_c + Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - - 2*Y_1*Y_c))))/sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - - 2*Y_1*Y_2 + pow(Y_2, 2)); - if (param == f1y()) - deriv +=-2*((Y_F1 - Y_c)*sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + - pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2))/sqrt(pow(X_F1, 2) - 2*X_F1*X_c + - pow(X_c, 2) + pow(Y_F1, 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + pow(b, 2)) + - (pow(Y_1, 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + - pow(Y_2, 2)*Y_c + Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - - 2*Y_1*Y_c) - Y_F1*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)))/sqrt(pow(X_1, - 2)*(pow(X_c, 2) + pow(Y_c, 2)) - 2*X_1*X_2*(pow(X_c, 2) + pow(Y_c, 2)) + - pow(X_2, 2)*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_F1, 2)*(pow(X_1, 2) - - 2*X_1*X_2 + pow(X_2, 2)) - 2*X_F1*(pow(X_1, 2)*X_c - 2*X_1*X_2*X_c + - pow(X_2, 2)*X_c) + pow(Y_1, 2)*(pow(X_2, 2) - 2*X_2*X_c + pow(X_c, 2) + - pow(Y_c, 2)) + 2*Y_1*(X_1*X_2*Y_c - pow(X_2, 2)*Y_c - X_F1*(X_1*Y_c - - X_2*Y_c)) + pow(Y_2, 2)*(pow(X_1, 2) - 2*X_1*X_c + pow(X_c, 2) + - pow(Y_c, 2)) - 2*Y_2*(pow(X_1, 2)*Y_c - X_1*X_2*Y_c - X_F1*(X_1*Y_c - - X_2*Y_c) + Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) + pow(Y_c, 2))) - + pow(Y_F1, 2)*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)) - - 2*Y_F1*(pow(Y_1, 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + - pow(Y_2, 2)*Y_c + Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - - 2*Y_1*Y_c))))/sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - - 2*Y_1*Y_2 + pow(Y_2, 2)); - if (param == cx()) - deriv += 2*((X_F1 - X_c)*sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + - pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2))/sqrt(pow(X_F1, 2) - 2*X_F1*X_c + - pow(X_c, 2) + pow(Y_F1, 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + pow(b, 2)) + - (pow(X_1, 2)*X_c - 2*X_1*X_2*X_c + pow(X_2, 2)*X_c - X_F1*(pow(X_1, 2) - - 2*X_1*X_2 + pow(X_2, 2)) - pow(Y_1, 2)*(X_2 - X_c) + Y_1*Y_2*(X_1 + X_2 - - 2*X_c) - pow(Y_2, 2)*(X_1 - X_c) - Y_F1*(Y_1*(X_1 - X_2) - Y_2*(X_1 - - X_2)))/sqrt(pow(X_1, 2)*(pow(X_c, 2) + pow(Y_c, 2)) - - 2*X_1*X_2*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_2, 2)*(pow(X_c, 2) + - pow(Y_c, 2)) + pow(X_F1, 2)*(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2)) - - 2*X_F1*(pow(X_1, 2)*X_c - 2*X_1*X_2*X_c + pow(X_2, 2)*X_c) + pow(Y_1, - 2)*(pow(X_2, 2) - 2*X_2*X_c + pow(X_c, 2) + pow(Y_c, 2)) + - 2*Y_1*(X_1*X_2*Y_c - pow(X_2, 2)*Y_c - X_F1*(X_1*Y_c - X_2*Y_c)) + - pow(Y_2, 2)*(pow(X_1, 2) - 2*X_1*X_c + pow(X_c, 2) + pow(Y_c, 2)) - - 2*Y_2*(pow(X_1, 2)*Y_c - X_1*X_2*Y_c - X_F1*(X_1*Y_c - X_2*Y_c) + - Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) + pow(Y_c, 2))) + - pow(Y_F1, 2)*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)) - 2*Y_F1*(pow(Y_1, - 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + pow(Y_2, 2)*Y_c + - Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - 2*Y_1*Y_c))))/sqrt(pow(X_1, - 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)); - if (param == cy()) - deriv += 2*((Y_F1 - Y_c)*sqrt(pow(X_1, 2) - 2*X_1*X_2 + pow(X_2, 2) + - pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2))/sqrt(pow(X_F1, 2) - 2*X_F1*X_c + - pow(X_c, 2) + pow(Y_F1, 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + pow(b, 2)) + - (pow(X_1, 2)*Y_c - 2*X_1*X_2*Y_c + pow(X_2, 2)*Y_c + pow(Y_1, 2)*Y_c + - Y_1*(X_1*X_2 - pow(X_2, 2) - X_F1*(X_1 - X_2)) + pow(Y_2, 2)*Y_c - - Y_2*(pow(X_1, 2) - X_1*X_2 - X_F1*(X_1 - X_2) + 2*Y_1*Y_c) - - Y_F1*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)))/sqrt(pow(X_1, 2)*(pow(X_c, - 2) + pow(Y_c, 2)) - 2*X_1*X_2*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_2, - 2)*(pow(X_c, 2) + pow(Y_c, 2)) + pow(X_F1, 2)*(pow(X_1, 2) - 2*X_1*X_2 + - pow(X_2, 2)) - 2*X_F1*(pow(X_1, 2)*X_c - 2*X_1*X_2*X_c + pow(X_2, - 2)*X_c) + pow(Y_1, 2)*(pow(X_2, 2) - 2*X_2*X_c + pow(X_c, 2) + pow(Y_c, - 2)) + 2*Y_1*(X_1*X_2*Y_c - pow(X_2, 2)*Y_c - X_F1*(X_1*Y_c - X_2*Y_c)) + - pow(Y_2, 2)*(pow(X_1, 2) - 2*X_1*X_c + pow(X_c, 2) + pow(Y_c, 2)) - - 2*Y_2*(pow(X_1, 2)*Y_c - X_1*X_2*Y_c - X_F1*(X_1*Y_c - X_2*Y_c) + - Y_1*(-X_1*X_c + X_2*(X_1 - X_c) + pow(X_c, 2) + pow(Y_c, 2))) + - pow(Y_F1, 2)*(pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)) - 2*Y_F1*(pow(Y_1, - 2)*Y_c - Y_1*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2)) + pow(Y_2, 2)*Y_c + - Y_2*(-X_1*X_c + X_2*X_c + X_F1*(X_1 - X_2) - 2*Y_1*Y_c))))/sqrt(pow(X_1, - 2) - 2*X_1*X_2 + pow(X_2, 2) + pow(Y_1, 2) - 2*Y_1*Y_2 + pow(Y_2, 2)); - if (param == rmin()) - deriv += -2*b/sqrt(pow(X_F1, 2) - 2*X_F1*X_c + pow(X_c, 2) + pow(Y_F1, - 2) - 2*Y_F1*Y_c + pow(Y_c, 2) + pow(b, 2)); - } - return scale * deriv; + //first of all, check that we need to compute anything. + int i; + for( i=0 ; ierror(); + *param += eps; + double vr = this->error(); + *param = oldparam - eps; + double vl = this->error(); + *param = oldparam; + //If not nasty, real derivative should be between left one and right one + double numretl = (v0-vl)/eps; + double numretr = (vr-v0)/eps; + assert(deriv <= std::max(numretl,numretr) ); + assert(deriv >= std::min(numretl,numretr) ); + #endif + + + return deriv*scale; } // ConstraintInternalAlignmentPoint2Ellipse ConstraintInternalAlignmentPoint2Ellipse::ConstraintInternalAlignmentPoint2Ellipse(Ellipse &e, Point &p1, InternalAlignmentType alignmentType) { - pvec.push_back(p1.x); - pvec.push_back(p1.y); - pvec.push_back(e.center.x); - pvec.push_back(e.center.y); - pvec.push_back(e.focus1.x); - pvec.push_back(e.focus1.y); - pvec.push_back(e.radmin); + this->p = p1; + pvec.push_back(p.x); + pvec.push_back(p.y); + this->e = e; + this->e.PushOwnParams(pvec); + this->AlignmentType = alignmentType; origpvec = pvec; rescale(); - AlignmentType=alignmentType; } -ConstraintInternalAlignmentPoint2Ellipse::ConstraintInternalAlignmentPoint2Ellipse(ArcOfEllipse &a, Point &p1, InternalAlignmentType alignmentType) +void ConstraintInternalAlignmentPoint2Ellipse::ReconstructGeomPointers() { - pvec.push_back(p1.x); - pvec.push_back(p1.y); - pvec.push_back(a.center.x); - pvec.push_back(a.center.y); - pvec.push_back(a.focus1.x); - pvec.push_back(a.focus1.y); - pvec.push_back(a.radmin); - origpvec = pvec; - rescale(); - AlignmentType=alignmentType; + int i = 0; + p.x = pvec[i]; i++; + p.y = pvec[i]; i++; + e.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; } ConstraintType ConstraintInternalAlignmentPoint2Ellipse::getTypeId() @@ -1386,465 +1157,122 @@ void ConstraintInternalAlignmentPoint2Ellipse::rescale(double coef) scale = coef * 1; } +void ConstraintInternalAlignmentPoint2Ellipse::errorgrad(double *err, double *grad, double *param) +{ + if (pvecChangedFlag) ReconstructGeomPointers(); + + //todo: prefill only what's needed, not everything + + DeriVector2 c(e.center, param); + DeriVector2 f1(e.focus1, param); + DeriVector2 emaj = f1.subtr(c).getNormalized(); + DeriVector2 emin = emaj.rotate90ccw(); + DeriVector2 pv (p, param); + double b, db;//minor radius + b = *e.radmin; db = (e.radmin == param) ? 1.0 : 0.0; + + //major radius + double a, da; + a = e.getRadMaj(c,f1,b,db,da); + + DeriVector2 poa;//point to align to + bool by_y_not_by_x;//a flag to indicate if the alignment error function is for y (false - x, true - y). + + switch(AlignmentType){ + case EllipsePositiveMajorX: + case EllipsePositiveMajorY: + poa = c.sum(emaj.multD(a, da)); + by_y_not_by_x = AlignmentType == EllipsePositiveMajorY; + break; + case EllipseNegativeMajorX: + case EllipseNegativeMajorY: + poa = c.sum(emaj.multD(-a, -da)); + by_y_not_by_x = AlignmentType == EllipseNegativeMajorY; + break; + case EllipsePositiveMinorX: + case EllipsePositiveMinorY: + poa = c.sum(emin.multD(b, db)); + by_y_not_by_x = AlignmentType == EllipsePositiveMinorY; + break; + case EllipseNegativeMinorX: + case EllipseNegativeMinorY: + poa = c.sum(emin.multD(-b, -db)); + by_y_not_by_x = AlignmentType == EllipseNegativeMinorY; + break; + case EllipseFocus2X: + case EllipseFocus2Y: + poa = c.linCombi(2.0, f1, -1.0); + by_y_not_by_x = AlignmentType == EllipseFocus2Y; + break; + default: + //shouldn't happen + poa = pv;//align to the point itself, doing nothing essentially + } + if(err) + *err = by_y_not_by_x ? pv.y - poa.y : pv.x - poa.x; + if(grad) + *grad = by_y_not_by_x ? pv.dy - poa.dy : pv.dx - poa.dx; +} + double ConstraintInternalAlignmentPoint2Ellipse::error() { - // so first is to select the point (X_0,Y_0) in line to calculate - double X_1 = *p1x(); - double Y_1 = *p1y(); - double X_c = *cx(); - double Y_c = *cy(); - double X_F1 = *f1x(); - double Y_F1 = *f1y(); - double b = *rmin(); - - switch(AlignmentType) - { - case EllipsePositiveMajorX: - return scale * (X_1 - X_c - (X_F1 - X_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + - pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))); - break; - case EllipsePositiveMajorY: - return scale * (Y_1 - Y_c - (Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + - pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))); - break; - case EllipseNegativeMajorX: - return scale * (X_1 - X_c + (X_F1 - X_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + - pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))); - break; - case EllipseNegativeMajorY: - return scale * (Y_1 - Y_c + (Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + - pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))); - break; - case EllipsePositiveMinorX: - return scale * (X_1 - X_c + b*(Y_F1 - Y_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))); - break; - case EllipsePositiveMinorY: - return scale * (Y_1 - Y_c - b*(X_F1 - X_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))); - break; - case EllipseNegativeMinorX: - return scale * (X_1 - X_c - b*(Y_F1 - Y_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))); - break; - case EllipseNegativeMinorY: - return scale * (Y_1 - Y_c + b*(X_F1 - X_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))); - break; - case EllipseFocus2X: - return scale * (X_1 + X_F1 - 2*X_c); - break; - case EllipseFocus2Y: - return scale * (Y_1 + Y_F1 - 2*Y_c); - break; - default: - return 0; - } + double err; + errorgrad(&err,0,0); + return scale * err; + } double ConstraintInternalAlignmentPoint2Ellipse::grad(double *param) { - double deriv=0.; - if (param == p1x() || param == p1y() || - param == f1x() || param == f1y() || - param == cx() || param == cy() || - param == rmin()) { - - double X_1 = *p1x(); - double Y_1 = *p1y(); - double X_c = *cx(); - double Y_c = *cy(); - double X_F1 = *f1x(); - double Y_F1 = *f1y(); - double b = *rmin(); - - if (param == p1x()) - switch(AlignmentType) - { - case EllipsePositiveMajorX: - deriv += 1; - break; - case EllipsePositiveMajorY: - deriv += 0; - break; - case EllipseNegativeMajorX: - deriv += 1; - break; - case EllipseNegativeMajorY: - deriv += 0; - break; - case EllipsePositiveMinorX: - deriv += 1; - break; - case EllipsePositiveMinorY: - deriv += 0; - break; - case EllipseNegativeMinorX: - deriv += 1; - break; - case EllipseNegativeMinorY: - deriv += 0; - break; - case EllipseFocus2X: - deriv += 1; - break; - case EllipseFocus2Y: - deriv += 0; - break; - default: - deriv+=0; - } - if (param == p1y()) - switch(AlignmentType) - { - case EllipsePositiveMajorX: - deriv += 0; - break; - case EllipsePositiveMajorY: - deriv += 1; - break; - case EllipseNegativeMajorX: - deriv += 0; - break; - case EllipseNegativeMajorY: - deriv += 1; - break; - case EllipsePositiveMinorX: - deriv += 0; - break; - case EllipsePositiveMinorY: - deriv += 1; - break; - case EllipseNegativeMinorX: - deriv += 0; - break; - case EllipseNegativeMinorY: - deriv += 1; - break; - case EllipseFocus2X: - deriv += 0; - break; - case EllipseFocus2Y: - deriv += 1; - break; - default: - deriv+=0; - } - if (param == f1x()) - switch(AlignmentType) - { - case EllipsePositiveMajorX: - deriv += -pow(X_F1 - X_c, 2)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) + - pow(X_F1 - X_c, 2)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0) - - sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipsePositiveMajorY: - deriv += -(X_F1 - X_c)*(Y_F1 - Y_c)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) + - (X_F1 - X_c)*(Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0); - break; - case EllipseNegativeMajorX: - deriv += pow(X_F1 - X_c, 2)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) - - pow(X_F1 - X_c, 2)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0) + - sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipseNegativeMajorY: - deriv += (X_F1 - X_c)*(Y_F1 - Y_c)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) - - (X_F1 - X_c)*(Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0); - break; - case EllipsePositiveMinorX: - deriv += -b*(X_F1 - X_c)*(Y_F1 - Y_c)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2), 3.0/2.0); - break; - case EllipsePositiveMinorY: - deriv += b*pow(X_F1 - X_c, 2)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2), 3.0/2.0) - b/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipseNegativeMinorX: - deriv += b*(X_F1 - X_c)*(Y_F1 - Y_c)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2), 3.0/2.0); - break; - case EllipseNegativeMinorY: - deriv += -b*pow(X_F1 - X_c, 2)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2), 3.0/2.0) + b/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipseFocus2X: - deriv += 1; - break; - case EllipseFocus2Y: - deriv+=0; - break; - default: - deriv+=0; - } - if (param == f1y()) - switch(AlignmentType) - { - case EllipsePositiveMajorX: - deriv += -(X_F1 - X_c)*(Y_F1 - Y_c)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) + - (X_F1 - X_c)*(Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0); - break; - case EllipsePositiveMajorY: - deriv += -pow(Y_F1 - Y_c, 2)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) + - pow(Y_F1 - Y_c, 2)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0) - - sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipseNegativeMajorX: - deriv += (X_F1 - X_c)*(Y_F1 - Y_c)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) - - (X_F1 - X_c)*(Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0); - break; - case EllipseNegativeMajorY: - deriv += pow(Y_F1 - Y_c, 2)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) - - pow(Y_F1 - Y_c, 2)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0) + - sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipsePositiveMinorX: - deriv += -b*pow(Y_F1 - Y_c, 2)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2), 3.0/2.0) + b/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipsePositiveMinorY: - deriv += b*(X_F1 - X_c)*(Y_F1 - Y_c)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2), 3.0/2.0); - break; - case EllipseNegativeMinorX: - deriv += b*pow(Y_F1 - Y_c, 2)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2), 3.0/2.0) - b/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipseNegativeMinorY: - deriv += -b*(X_F1 - X_c)*(Y_F1 - Y_c)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2), 3.0/2.0); - break; - case EllipseFocus2X: - deriv += 0; - break; - case EllipseFocus2Y: - deriv += 1; - break; - default: - deriv+=0; - } - if (param == cx()) - switch(AlignmentType) - { - case EllipsePositiveMajorX: - deriv += pow(X_F1 - X_c, 2)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) - - pow(X_F1 - X_c, 2)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0) - 1 + - sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipsePositiveMajorY: - deriv += (X_F1 - X_c)*(Y_F1 - Y_c)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) - - (X_F1 - X_c)*(Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0); - break; - case EllipseNegativeMajorX: - deriv += -pow(X_F1 - X_c, 2)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) + - pow(X_F1 - X_c, 2)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0) - 1 - - sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipseNegativeMajorY: - deriv += -(X_F1 - X_c)*(Y_F1 - Y_c)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) + - (X_F1 - X_c)*(Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0); - break; - case EllipsePositiveMinorX: - deriv += b*(X_F1 - X_c)*(Y_F1 - Y_c)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2), 3.0/2.0) - 1; - break; - case EllipsePositiveMinorY: - deriv += -b*pow(X_F1 - X_c, 2)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2), 3.0/2.0) + b/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipseNegativeMinorX: - deriv += -b*(X_F1 - X_c)*(Y_F1 - Y_c)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2), 3.0/2.0) - 1; - break; - case EllipseNegativeMinorY: - deriv += b*pow(X_F1 - X_c, 2)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2), 3.0/2.0) - b/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipseFocus2X: - deriv += -2; - break; - case EllipseFocus2Y: - deriv+=0; - break; - default: - deriv+=0; - } - if (param == cy()) - switch(AlignmentType) - { - case EllipsePositiveMajorX: - deriv += (X_F1 - X_c)*(Y_F1 - Y_c)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) - - (X_F1 - X_c)*(Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0); - break; - case EllipsePositiveMajorY: - deriv += pow(Y_F1 - Y_c, 2)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) - - pow(Y_F1 - Y_c, 2)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0) - 1 + - sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipseNegativeMajorX: - deriv += -(X_F1 - X_c)*(Y_F1 - Y_c)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) + - (X_F1 - X_c)*(Y_F1 - Y_c)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0); - break; - case EllipseNegativeMajorY: - deriv += -pow(Y_F1 - Y_c, 2)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))) + - pow(Y_F1 - Y_c, 2)*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2), 3.0/2.0) - 1 - - sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))/sqrt(pow(X_F1 - - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipsePositiveMinorX: - deriv += b*pow(Y_F1 - Y_c, 2)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2), 3.0/2.0) - b/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipsePositiveMinorY: - deriv += -b*(X_F1 - X_c)*(Y_F1 - Y_c)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2), 3.0/2.0) - 1; - break; - case EllipseNegativeMinorX: - deriv += -b*pow(Y_F1 - Y_c, 2)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2), 3.0/2.0) + b/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipseNegativeMinorY: - deriv += b*(X_F1 - X_c)*(Y_F1 - Y_c)/pow(pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2), 3.0/2.0) - 1; - break; - case EllipseFocus2X: - deriv += 0; - break; - case EllipseFocus2Y: - deriv += -2; - break; - default: - deriv+=0; - } - if (param == rmin()) - switch(AlignmentType) - { - case EllipsePositiveMajorX: - deriv += -b*(X_F1 - X_c)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))); - break; - case EllipsePositiveMajorY: - deriv += -b*(Y_F1 - Y_c)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))); - break; - case EllipseNegativeMajorX: - deriv += b*(X_F1 - X_c)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))); - break; - case EllipseNegativeMajorY: - deriv += b*(Y_F1 - Y_c)/(sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2))*sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2))); - break; - case EllipsePositiveMinorX: - deriv += (Y_F1 - Y_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipsePositiveMinorY: - deriv += -(X_F1 - X_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipseNegativeMinorX: - deriv += -(Y_F1 - Y_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipseNegativeMinorY: - deriv += (X_F1 - X_c)/sqrt(pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); - break; - case EllipseFocus2X: - deriv += 0; - break; - case EllipseFocus2Y: - deriv += 0; - break; - default: - deriv+=0; - } - } - return scale * deriv; + //first of all, check that we need to compute anything. + int i; + for( i=0 ; ierror(); + *param += eps; + double vr = this->error(); + *param = oldparam - eps; + double vl = this->error(); + *param = oldparam; + //If not nasty, real derivative should be between left one and right one + double numretl = (v0-vl)/eps; + double numretr = (vr-v0)/eps; + assert(deriv <= std::max(numretl,numretr) ); + assert(deriv >= std::min(numretl,numretr) ); + #endif + + return deriv*scale; + } // ConstraintEqualMajorAxesEllipse ConstraintEqualMajorAxesEllipse:: ConstraintEqualMajorAxesEllipse(Ellipse &e1, Ellipse &e2) { - pvec.push_back(e1.center.x); - pvec.push_back(e1.center.y); - pvec.push_back(e1.focus1.x); - pvec.push_back(e1.focus1.y); - pvec.push_back(e1.radmin); - pvec.push_back(e2.center.x); - pvec.push_back(e2.center.y); - pvec.push_back(e2.focus1.x); - pvec.push_back(e2.focus1.y); - pvec.push_back(e2.radmin); + this->e1 = e1; + this->e1.PushOwnParams(pvec); + this->e2 = e2; + this->e2.PushOwnParams(pvec); origpvec = pvec; + pvecChangedFlag = true; rescale(); } -ConstraintEqualMajorAxesEllipse:: ConstraintEqualMajorAxesEllipse(ArcOfEllipse &a1, Ellipse &e2) +void ConstraintEqualMajorAxesEllipse::ReconstructGeomPointers() { - pvec.push_back(a1.center.x); - pvec.push_back(a1.center.y); - pvec.push_back(a1.focus1.x); - pvec.push_back(a1.focus1.y); - pvec.push_back(a1.radmin); - pvec.push_back(e2.center.x); - pvec.push_back(e2.center.y); - pvec.push_back(e2.focus1.x); - pvec.push_back(e2.focus1.y); - pvec.push_back(e2.radmin); - origpvec = pvec; - rescale(); -} - -ConstraintEqualMajorAxesEllipse:: ConstraintEqualMajorAxesEllipse(ArcOfEllipse &a1, ArcOfEllipse &a2) -{ - pvec.push_back(a1.center.x); - pvec.push_back(a1.center.y); - pvec.push_back(a1.focus1.x); - pvec.push_back(a1.focus1.y); - pvec.push_back(a1.radmin); - pvec.push_back(a2.center.x); - pvec.push_back(a2.center.y); - pvec.push_back(a2.focus1.x); - pvec.push_back(a2.focus1.y); - pvec.push_back(a2.radmin); - origpvec = pvec; - rescale(); + int i =0; + e1.ReconstructOnNewPvec(pvec, i); + e2.ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; } ConstraintType ConstraintEqualMajorAxesEllipse::getTypeId() @@ -1857,87 +1285,39 @@ void ConstraintEqualMajorAxesEllipse::rescale(double coef) scale = coef * 1; } +void ConstraintEqualMajorAxesEllipse::errorgrad(double *err, double *grad, double *param) +{ + if (pvecChangedFlag) ReconstructGeomPointers(); + double a1, da1; + a1 = e1.getRadMaj(param, da1); + double a2, da2; + a2 = e2.getRadMaj(param, da2); + if (err) + *err = a2 - a1; + if (grad) + *grad = da2 - da1; +} + double ConstraintEqualMajorAxesEllipse::error() { - double E1X_c = *e1cx(); - double E1Y_c = *e1cy(); - double E1X_F1 = *e1f1x(); - double E1Y_F1 = *e1f1y(); - double E1b = *e1rmin(); - double E2X_c = *e2cx(); - double E2Y_c = *e2cy(); - double E2X_F1 = *e2f1x(); - double E2Y_F1 = *e2f1y(); - double E2b = *e2rmin(); - - double err=sqrt(pow(E1X_F1, 2) - 2*E1X_F1*E1X_c + pow(E1X_c, 2) + - pow(E1Y_F1, 2) - 2*E1Y_F1*E1Y_c + pow(E1Y_c, 2) + pow(E1b, 2)) - - sqrt(pow(E2X_F1, 2) - 2*E2X_F1*E2X_c + pow(E2X_c, 2) + pow(E2Y_F1, 2) - - 2*E2Y_F1*E2Y_c + pow(E2Y_c, 2) + pow(E2b, 2)); + double err; + errorgrad(&err,0,0); return scale * err; } double ConstraintEqualMajorAxesEllipse::grad(double *param) { - double deriv=0.; - if (param == e1f1x() || param == e1f1y() || - param == e1cx() || param == e1cy() || - param == e1rmin() || - param == e2f1x() || param == e2f1y() || - param == e2cx() || param == e2cy() || - param == e2rmin()) { + //first of all, check that we need to compute anything. + int i; + for( i=0 ; ierror(); + *param += eps; + double vr = this->error(); + *param = oldparam - eps; + double vl = this->error(); + *param = oldparam; + //If not nasty, real derivative should be between left one and right one + double numretl = (v0-vl)/eps; + double numretr = (vr-v0)/eps; + assert(deriv <= std::max(numretl,numretr) ); + assert(deriv >= std::min(numretl,numretr) ); + #endif + + + return deriv*scale; } double ConstraintEllipticalArcRangeToEndPoints::maxStep(MAP_pD_D &dir, double lim) diff --git a/src/Mod/Sketcher/App/freegcs/Constraints.h b/src/Mod/Sketcher/App/freegcs/Constraints.h index 879d9a72f..40a004cbf 100644 --- a/src/Mod/Sketcher/App/freegcs/Constraints.h +++ b/src/Mod/Sketcher/App/freegcs/Constraints.h @@ -349,7 +349,8 @@ namespace GCS class ConstraintEllipseTangentLine : public Constraint { private: - inline double* p1x() { return pvec[0]; } + /*tbd + * inline double* p1x() { return pvec[0]; } inline double* p1y() { return pvec[1]; } inline double* p2x() { return pvec[2]; } inline double* p2y() { return pvec[3]; } @@ -357,10 +358,13 @@ namespace GCS inline double* cy() { return pvec[5]; } inline double* f1x() { return pvec[6]; } inline double* f1y() { return pvec[7]; } - inline double* rmin() { return pvec[8]; } + inline double* rmin() { return pvec[8]; }*/ + Line l; + Ellipse e; + void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa + void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. public: ConstraintEllipseTangentLine(Line &l, Ellipse &e); - ConstraintEllipseTangentLine(Line &l, ArcOfEllipse &a); virtual ConstraintType getTypeId(); virtual void rescale(double coef=1.); virtual double error(); @@ -369,42 +373,28 @@ namespace GCS class ConstraintInternalAlignmentPoint2Ellipse : public Constraint { - private: - inline double* p1x() { return pvec[0]; } - inline double* p1y() { return pvec[1]; } - inline double* cx() { return pvec[2]; } - inline double* cy() { return pvec[3]; } - inline double* f1x() { return pvec[4]; } - inline double* f1y() { return pvec[5]; } - inline double* rmin() { return pvec[6]; } public: ConstraintInternalAlignmentPoint2Ellipse(Ellipse &e, Point &p1, InternalAlignmentType alignmentType); - ConstraintInternalAlignmentPoint2Ellipse(ArcOfEllipse &e, Point &p1, InternalAlignmentType alignmentType); virtual ConstraintType getTypeId(); virtual void rescale(double coef=1.); virtual double error(); virtual double grad(double *); private: + void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. + void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa + Ellipse e; + Point p; InternalAlignmentType AlignmentType; }; class ConstraintEqualMajorAxesEllipse : public Constraint { - private: - inline double* e1cx() { return pvec[0]; } - inline double* e1cy() { return pvec[1]; } - inline double* e1f1x() { return pvec[2]; } - inline double* e1f1y() { return pvec[3]; } - inline double* e1rmin() { return pvec[4]; } - inline double* e2cx() { return pvec[5]; } - inline double* e2cy() { return pvec[6]; } - inline double* e2f1x() { return pvec[7]; } - inline double* e2f1y() { return pvec[8]; } - inline double* e2rmin() { return pvec[9]; } + private: + Ellipse e1, e2; + void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa + void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. public: ConstraintEqualMajorAxesEllipse(Ellipse &e1, Ellipse &e2); - ConstraintEqualMajorAxesEllipse(ArcOfEllipse &a1, Ellipse &e2); - ConstraintEqualMajorAxesEllipse(ArcOfEllipse &a1, ArcOfEllipse &a2); virtual ConstraintType getTypeId(); virtual void rescale(double coef=1.); virtual double error(); @@ -415,14 +405,11 @@ namespace GCS class ConstraintEllipticalArcRangeToEndPoints : public Constraint { private: - inline double* p1x() { return pvec[0]; } - inline double* p1y() { return pvec[1]; } inline double* angle() { return pvec[2]; } - inline double* cx() { return pvec[3]; } - inline double* cy() { return pvec[4]; } - inline double* f1x() { return pvec[5]; } - inline double* f1y() { return pvec[6]; } - inline double* rmin() { return pvec[7]; } + void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. + void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa + Ellipse e; + Point p; public: ConstraintEllipticalArcRangeToEndPoints(Point &p, ArcOfEllipse &a, double *angle_t); virtual ConstraintType getTypeId(); diff --git a/src/Mod/Sketcher/App/freegcs/Geo.cpp b/src/Mod/Sketcher/App/freegcs/Geo.cpp index de0ac170a..554085548 100644 --- a/src/Mod/Sketcher/App/freegcs/Geo.cpp +++ b/src/Mod/Sketcher/App/freegcs/Geo.cpp @@ -39,7 +39,7 @@ DeriVector2::DeriVector2(const Point &p, double *derivparam) dy = 1.0; } -double DeriVector2::length(double &dlength) +double DeriVector2::length(double &dlength) const { double l = length(); if(l==0){ @@ -51,7 +51,7 @@ double DeriVector2::length(double &dlength) } } -DeriVector2 DeriVector2::getNormalized() +DeriVector2 DeriVector2::getNormalized() const { double l=length(); if(l==0.0) { @@ -71,7 +71,7 @@ DeriVector2 DeriVector2::getNormalized() } } -double DeriVector2::scalarProd(const DeriVector2 &v2, double *dprd) +double DeriVector2::scalarProd(const DeriVector2 &v2, double *dprd) const { if (dprd) { *dprd = dx*v2.x + x*v2.dx + dy*v2.y + y*v2.dy; @@ -79,7 +79,8 @@ double DeriVector2::scalarProd(const DeriVector2 &v2, double *dprd) return x*v2.x + y*v2.y; } -DeriVector2 DeriVector2::divD(double val, double dval){ +DeriVector2 DeriVector2::divD(double val, double dval) const +{ return DeriVector2(x/val,y/val, dx/val - x*dval/(val*val), dy/val - y*dval/(val*val) @@ -178,6 +179,32 @@ Arc* Arc::Copy() //--------------ellipse + +//this function is exposed to allow reusing pre-filled derivectors in constraints code +double Ellipse::getRadMaj(const DeriVector2 ¢er, const DeriVector2 &f1, double b, double db, double &ret_dRadMaj) +{ + double cf, dcf; + cf = f1.subtr(center).length(dcf); + DeriVector2 hack (b, cf, + db, dcf);//hack = a nonsense vector to calculate major radius with derivatives, useful just because the calculation formula is the same as vector length formula + return hack.length(ret_dRadMaj); +} + +//returns major radius. The derivative by derivparam is returned into ret_dRadMaj argument. +double Ellipse::getRadMaj(double *derivparam, double &ret_dRadMaj) +{ + DeriVector2 c(center, derivparam); + DeriVector2 f1(focus1, derivparam); + return getRadMaj(c, f1, *radmin, radmin==derivparam ? 1.0 : 0.0, ret_dRadMaj); +} + +//returns the major radius (plain value, no derivatives) +double Ellipse::getRadMaj() +{ + double dradmaj;//dummy + return getRadMaj(0,dradmaj); +} + DeriVector2 Ellipse::CalculateNormal(Point &p, double* derivparam) { //fill some vectors in diff --git a/src/Mod/Sketcher/App/freegcs/Geo.h b/src/Mod/Sketcher/App/freegcs/Geo.h index 029198799..c736ca4d2 100644 --- a/src/Mod/Sketcher/App/freegcs/Geo.h +++ b/src/Mod/Sketcher/App/freegcs/Geo.h @@ -59,27 +59,27 @@ namespace GCS double x, dx; double y, dy; - double length() {return sqrt(x*x + y*y);} - double length(double &dlength); //returns length and writes length deriv into the dlength argument. + double length() const {return sqrt(x*x + y*y);} + double length(double &dlength) const; //returns length and writes length deriv into the dlength argument. //unlike other vectors in FreeCAD, this normalization creates a new vector instead of modifying existing one. - DeriVector2 getNormalized(); //returns zero vector if the original is zero. - double scalarProd(const DeriVector2 &v2, double* dprd=0);//calculates scalar product of two vectors and returns the result. The derivative of the result is written into argument dprd. - DeriVector2 sum(const DeriVector2 &v2){//adds two vectors and returns result + DeriVector2 getNormalized() const; //returns zero vector if the original is zero. + double scalarProd(const DeriVector2 &v2, double* dprd=0) const;//calculates scalar product of two vectors and returns the result. The derivative of the result is written into argument dprd. + DeriVector2 sum(const DeriVector2 &v2) const {//adds two vectors and returns result return DeriVector2(x + v2.x, y + v2.y, dx + v2.dx, dy + v2.dy);} - DeriVector2 subtr(const DeriVector2 &v2){//subtracts two vectors and returns result + DeriVector2 subtr(const DeriVector2 &v2) const {//subtracts two vectors and returns result return DeriVector2(x - v2.x, y - v2.y, dx - v2.dx, dy - v2.dy);} - DeriVector2 mult(double val){ + DeriVector2 mult(double val) const { return DeriVector2(x*val, y*val, dx*val, dy*val);}//multiplies the vector by a number. Derivatives are scaled. - DeriVector2 multD(double val, double dval){//multiply vector by a variable with a derivative. + DeriVector2 multD(double val, double dval) const {//multiply vector by a variable with a derivative. return DeriVector2(x*val, y*val, dx*val+x*dval, dy*val+y*dval);} - DeriVector2 divD(double val, double dval);//divide vector by a variable with a derivative - DeriVector2 rotate90ccw(){return DeriVector2(-y,x,-dy,dx);} - DeriVector2 rotate90cw(){return DeriVector2(y,-x,dy,-dx);} - DeriVector2 linCombi(double m1, const DeriVector2 &v2, double m2){//linear combination of two vectors + DeriVector2 divD(double val, double dval) const;//divide vector by a variable with a derivative + DeriVector2 rotate90ccw() const {return DeriVector2(-y,x,-dy,dx);} + DeriVector2 rotate90cw() const {return DeriVector2(y,-x,dy,-dx);} + DeriVector2 linCombi(double m1, const DeriVector2 &v2, double m2) const {//linear combination of two vectors return DeriVector2(x*m1 + v2.x*m2, y*m1 + v2.y*m2, dx*m1 + v2.dx*m2, dy*m1 + v2.dy*m2);} @@ -156,6 +156,9 @@ namespace GCS Point center; Point focus1; double *radmin; + double getRadMaj(const DeriVector2 ¢er, const DeriVector2 &f1, double b, double db, double &ret_dRadMaj); + double getRadMaj(double* derivparam, double &ret_dRadMaj); + double getRadMaj(); DeriVector2 CalculateNormal(Point &p, double* derivparam = 0); virtual int PushOwnParams(VEC_pD &pvec); virtual void ReconstructOnNewPvec (VEC_pD &pvec, int &cnt);