From 6bf09c512dc42f407efeb7eaf71c776067df6878 Mon Sep 17 00:00:00 2001 From: Uwe Date: Mon, 20 Mar 2023 16:32:27 +0100 Subject: [PATCH] [Sketch] Constraints.cpp: formatting fixes - fix too long lines - also let clang reformat the file to uniform the formatting --- src/Mod/Sketcher/App/planegcs/Constraints.cpp | 1272 +++++++++-------- 1 file changed, 712 insertions(+), 560 deletions(-) diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index 3a628de3a6..e7674f639a 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -20,15 +20,16 @@ * * ***************************************************************************/ -#include -#include "Constraints.h" #include - +#include #define DEBUG_DERIVS 0 #if DEBUG_DERIVS -#include +# include #endif +#include "Constraints.h" + + namespace GCS { @@ -37,20 +38,24 @@ namespace GCS /////////////////////////////////////// Constraint::Constraint() -: origpvec(0), pvec(0), scale(1.), tag(0), pvecChangedFlag(true), driving(true), internalAlignment(Alignment::NoInternalAlignment) -{ -} + : origpvec(0), + pvec(0), + scale(1.), + tag(0), + pvecChangedFlag(true), + driving(true), + internalAlignment(Alignment::NoInternalAlignment) +{} -void Constraint::redirectParams(const MAP_pD_pD & redirectionmap) +void Constraint::redirectParams(const MAP_pD_pD& redirectionmap) { - int i=0; - for (VEC_pD::iterator param=origpvec.begin(); - param != origpvec.end(); ++param, i++) { + int i = 0; + for (VEC_pD::iterator param = origpvec.begin(); param != origpvec.end(); ++param, i++) { MAP_pD_pD::const_iterator it = redirectionmap.find(*param); if (it != redirectionmap.end()) pvec[i] = it->second; } - pvecChangedFlag=true; + pvecChangedFlag = true; } void Constraint::revertParams() @@ -66,17 +71,17 @@ ConstraintType Constraint::getTypeId() void Constraint::rescale(double coef) { - scale = coef * 1.; + scale = coef * 1.0; } double Constraint::error() { - return 0.; + return 0.0; } double Constraint::grad(double * /*param*/) { - return 0.; + return 0.0; } double Constraint::maxStep(MAP_pD_D & /*dir*/, double lim) @@ -84,11 +89,11 @@ double Constraint::maxStep(MAP_pD_D & /*dir*/, double lim) return lim; } -int Constraint::findParamInPvec(double *param) +int Constraint::findParamInPvec(double* param) { int ret = -1; - for( std::size_t i=0 ; i(i); break; } @@ -96,6 +101,8 @@ int Constraint::findParamInPvec(double *param) return ret; } + +// -------------------------------------------------------- // Equal ConstraintEqual::ConstraintEqual(double *p1, double *p2, double p1p2ratio) { @@ -129,14 +136,17 @@ double ConstraintEqual::grad(double *param) return scale * deriv; } -// Weighted Linear Combination -ConstraintWeightedLinearCombination::ConstraintWeightedLinearCombination(size_t givennumpoles, const std::vector& givenpvec, const std::vector& givenfactors) - : factors(givenfactors) - , numpoles(givennumpoles) +// -------------------------------------------------------- +// Weighted Linear Combination +ConstraintWeightedLinearCombination::ConstraintWeightedLinearCombination( + size_t givennumpoles, const std::vector& givenpvec, + const std::vector& givenfactors) + : factors(givenfactors), + numpoles(givennumpoles) { pvec = givenpvec; - assert(pvec.size() == 2*numpoles + 1); + assert(pvec.size() == 2 * numpoles + 1); assert(factors.size() == numpoles); origpvec = pvec; rescale(); @@ -169,12 +179,12 @@ double ConstraintWeightedLinearCombination::error() return scale * ((*thepoint()) * wsum - sum); } -double ConstraintWeightedLinearCombination::grad(double *param) +double ConstraintWeightedLinearCombination::grad(double* param) { // Equations are from here: // https://forum.freecadweb.org/viewtopic.php?f=9&t=71130&start=120#p635538 - double deriv=0.; + double deriv = 0.; if (param == thepoint()) { // Eq. (11) @@ -202,9 +212,11 @@ double ConstraintWeightedLinearCombination::grad(double *param) return scale * deriv; } -// Center of Gravity -ConstraintCenterOfGravity::ConstraintCenterOfGravity(const std::vector& givenpvec, const std::vector& givenweights) +// -------------------------------------------------------- +// Center of Gravity +ConstraintCenterOfGravity::ConstraintCenterOfGravity(const std::vector& givenpvec, + const std::vector& givenweights) : weights(givenweights) { pvec = givenpvec; @@ -247,8 +259,9 @@ double ConstraintCenterOfGravity::grad(double *param) return scale * deriv; } -// Slope at B-spline knot +// -------------------------------------------------------- +// Slope at B-spline knot ConstraintSlopeAtBSplineKnot::ConstraintSlopeAtBSplineKnot(BSpline& b, Line& l, size_t knotindex) { // set up pvec: pole x-coords, pole y-coords, pole weights, @@ -258,7 +271,7 @@ ConstraintSlopeAtBSplineKnot::ConstraintSlopeAtBSplineKnot(BSpline& b, Line& l, // slope at knot doesn't make sense if there's only C0 continuity assert(numpoles >= 2); - pvec.reserve(3*numpoles + 4); + pvec.reserve(3 * numpoles + 4); // `startpole` is the first pole affecting the knot with `knotindex` size_t startpole = 0; @@ -285,13 +298,13 @@ ConstraintSlopeAtBSplineKnot::ConstraintSlopeAtBSplineKnot(BSpline& b, Line& l, slopefactors.resize(numpoles); for (size_t i = 0; i < numpoles + 1; ++i) { tempfactors[i] = - b.getLinCombFactor(*(b.knots[knotindex]), startpole + b.degree, startpole + i, b.degree - 1) / - (b.flattenedknots[startpole + b.degree + i] - b.flattenedknots[startpole + i]); + b.getLinCombFactor( + *(b.knots[knotindex]), startpole + b.degree, startpole + i, b.degree - 1) + / (b.flattenedknots[startpole + b.degree + i] - b.flattenedknots[startpole + i]); } for (size_t i = 0; i < numpoles; ++i) { - factors[i] = - b.getLinCombFactor(*(b.knots[knotindex]), startpole + b.degree, startpole + i); - slopefactors[i] = b.degree * (tempfactors[i] - tempfactors[i+1]); + factors[i] = b.getLinCombFactor(*(b.knots[knotindex]), startpole + b.degree, startpole + i); + slopefactors[i] = b.degree * (tempfactors[i] - tempfactors[i + 1]); } origpvec = pvec; @@ -312,7 +325,7 @@ void ConstraintSlopeAtBSplineKnot::rescale(double coef) slopey += *poleyat(i) * slopefactors[i]; } - scale = coef / sqrt((slopex*slopex + slopey*slopey)); + scale = coef / sqrt((slopex * slopex + slopey * slopey)); } double ConstraintSlopeAtBSplineKnot::error() @@ -335,29 +348,29 @@ double ConstraintSlopeAtBSplineKnot::error() // This is actually wsum^2 * the respective slopes // See Eq (19) from: // https://forum.freecadweb.org/viewtopic.php?f=9&t=71130&start=120#p635538 - double slopex = wsum*xslopesum - wslopesum*xsum; - double slopey = wsum*yslopesum - wslopesum*ysum; + double slopex = wsum * xslopesum - wslopesum * xsum; + double slopey = wsum * yslopesum - wslopesum * ysum; // Normalizing it ensures that the cross product is not zero just because // one vector is zero. double linex = *linep2x() - *linep1x(); double liney = *linep2y() - *linep1y(); - double dirx = linex / sqrt(linex*linex + liney*liney); - double diry = liney / sqrt(linex*linex + liney*liney); + double dirx = linex / sqrt(linex * linex + liney * liney); + double diry = liney / sqrt(linex * linex + liney * liney); // error is the cross product - return scale * (slopex*diry - slopey*dirx); + return scale * (slopex * diry - slopey * dirx); } -double ConstraintSlopeAtBSplineKnot::grad(double *param) +double ConstraintSlopeAtBSplineKnot::grad(double* param) { // Equations are from here: // https://forum.freecadweb.org/viewtopic.php?f=9&t=71130&start=120#p635538 double result = 0.0; double linex = *linep2x() - *linep1x(); double liney = *linep2y() - *linep1y(); - double dirx = linex / sqrt(linex*linex + liney*liney); - double diry = liney / sqrt(linex*linex + liney*liney); + double dirx = linex / sqrt(linex * linex + liney * liney); + double diry = liney / sqrt(linex * linex + liney * liney); for (size_t i = 0; i < numpoles; ++i) { if (param == polexat(i)) { @@ -369,7 +382,7 @@ double ConstraintSlopeAtBSplineKnot::grad(double *param) wsum += wcontrib; wslopesum += wslopecontrib; } - result = (wsum*slopefactors[i] - wslopesum*factors[i]) * diry; + result = (wsum * slopefactors[i] - wslopesum * factors[i]) * diry; return scale * result; } if (param == poleyat(i)) { @@ -381,7 +394,7 @@ double ConstraintSlopeAtBSplineKnot::grad(double *param) wsum += wcontrib; wslopesum += wslopecontrib; } - result = - (wsum*slopefactors[i] - wslopesum*factors[i]) * dirx; + result = -(wsum * slopefactors[i] - wslopesum * factors[i]) * dirx; return scale * result; } if (param == weightat(i)) { @@ -396,9 +409,8 @@ double ConstraintSlopeAtBSplineKnot::grad(double *param) ysum += wcontrib * (*poleyat(j) - *poleyat(i)); yslopesum += wslopecontrib * (*poleyat(j) - *poleyat(i)); } - result = - (factors[i]*xslopesum - slopefactors[i]*xsum) * diry - - (factors[i]*yslopesum - slopefactors[i]*ysum) * dirx; + result = (factors[i] * xslopesum - slopefactors[i] * xsum) * diry + - (factors[i] * yslopesum - slopefactors[i] * ysum) * dirx; return scale * result; } } @@ -422,55 +434,57 @@ double ConstraintSlopeAtBSplineKnot::grad(double *param) } // This is actually wsum^2 * the respective slopes - slopex = wsum*xslopesum - wslopesum*xsum; - slopey = wsum*yslopesum - wslopesum*ysum; + slopex = wsum * xslopesum - wslopesum * xsum; + slopey = wsum * yslopesum - wslopesum * ysum; }; if (param == linep1x()) { getSlopes(); - double dDirxDLinex = (liney*liney) / pow(linex*linex + liney*liney, 1.5); - double dDiryDLinex = -(linex*liney) / pow(linex*linex + liney*liney, 1.5); + double dDirxDLinex = (liney * liney) / pow(linex * linex + liney * liney, 1.5); + double dDiryDLinex = -(linex * liney) / pow(linex * linex + liney * liney, 1.5); // NOTE: d(linex)/d(x1) = -1 - result = slopex*(-dDiryDLinex) - slopey*(-dDirxDLinex); + result = slopex * (-dDiryDLinex) - slopey * (-dDirxDLinex); return scale * result; } if (param == linep2x()) { getSlopes(); - double dDirxDLinex = (liney*liney) / pow(linex*linex + liney*liney, 1.5); - double dDiryDLinex = -(linex*liney) / pow(linex*linex + liney*liney, 1.5); + double dDirxDLinex = (liney * liney) / pow(linex * linex + liney * liney, 1.5); + double dDiryDLinex = -(linex * liney) / pow(linex * linex + liney * liney, 1.5); // NOTE: d(linex)/d(x2) = 1 - result = slopex*dDiryDLinex - slopey*dDirxDLinex; + result = slopex * dDiryDLinex - slopey * dDirxDLinex; return scale * result; } if (param == linep1y()) { getSlopes(); - double dDirxDLiney = -(linex*liney) / pow(linex*linex + liney*liney, 1.5); - double dDiryDLiney = (linex*linex) / pow(linex*linex + liney*liney, 1.5); + double dDirxDLiney = -(linex * liney) / pow(linex * linex + liney * liney, 1.5); + double dDiryDLiney = (linex * linex) / pow(linex * linex + liney * liney, 1.5); // NOTE: d(liney)/d(y1) = -1 - result = slopex*(-dDiryDLiney) - slopey*(-dDirxDLiney); + result = slopex * (-dDiryDLiney) - slopey * (-dDirxDLiney); return scale * result; } if (param == linep2y()) { getSlopes(); - double dDirxDLiney = -(linex*liney) / pow(linex*linex + liney*liney, 1.5); - double dDiryDLiney = (linex*linex) / pow(linex*linex + liney*liney, 1.5); + double dDirxDLiney = -(linex * liney) / pow(linex * linex + liney * liney, 1.5); + double dDiryDLiney = (linex * linex) / pow(linex * linex + liney * liney, 1.5); // NOTE: d(liney)/d(y2) = 1 - result = slopex*dDiryDLiney - slopey*dDirxDLiney; + result = slopex * dDiryDLiney - slopey * dDirxDLiney; return scale * result; } return scale * result; } -// Point On BSpline -ConstraintPointOnBSpline::ConstraintPointOnBSpline(double* point, double* initparam, int coordidx, BSpline& b) +// -------------------------------------------------------- +// Point On BSpline +ConstraintPointOnBSpline::ConstraintPointOnBSpline(double* point, double* initparam, int coordidx, + BSpline& b) : bsp(b) { // This is always going to be true numpoints = bsp.degree + 1; - pvec.reserve(2 + 2*b.poles.size()); + pvec.reserve(2 + 2 * b.poles.size()); pvec.push_back(point); pvec.push_back(initparam); @@ -516,8 +530,8 @@ void ConstraintPointOnBSpline::rescale(double coef) double ConstraintPointOnBSpline::error() { - if (*theparam() < bsp.flattenedknots[startpole + bsp.degree] || - *theparam() > bsp.flattenedknots[startpole + bsp.degree + 1]) + if (*theparam() < bsp.flattenedknots[startpole + bsp.degree] + || *theparam() > bsp.flattenedknots[startpole + bsp.degree + 1]) setStartPole(*theparam()); double sum = 0; @@ -527,51 +541,58 @@ double ConstraintPointOnBSpline::error() VEC_D d(numpoints); for (size_t i = 0; i < numpoints; ++i) d[i] = *poleat(i) * *weightat(i); - sum = BSpline::splineValue(*theparam(), startpole + bsp.degree, bsp.degree, d, bsp.flattenedknots); + sum = BSpline::splineValue( + *theparam(), startpole + bsp.degree, bsp.degree, d, bsp.flattenedknots); for (size_t i = 0; i < numpoints; ++i) d[i] = *weightat(i); - wsum = BSpline::splineValue(*theparam(), startpole + bsp.degree, bsp.degree, d, bsp.flattenedknots); + wsum = BSpline::splineValue( + *theparam(), startpole + bsp.degree, bsp.degree, d, bsp.flattenedknots); // TODO: Change the poles as the point moves between pieces return scale * (*thepoint() * wsum - sum); } -double ConstraintPointOnBSpline::grad(double *gcsparam) +double ConstraintPointOnBSpline::grad(double* gcsparam) { - double deriv=0.; + double deriv = 0.; if (gcsparam == thepoint()) { VEC_D d(numpoints); for (size_t i = 0; i < numpoints; ++i) d[i] = *weightat(i); - double wsum = BSpline::splineValue(*theparam(), startpole + bsp.degree, bsp.degree, d, bsp.flattenedknots); + double wsum = BSpline::splineValue( + *theparam(), startpole + bsp.degree, bsp.degree, d, bsp.flattenedknots); deriv += wsum; } if (gcsparam == theparam()) { VEC_D d(numpoints - 1); for (size_t i = 1; i < numpoints; ++i) { - d[i-1] = - (*poleat(i) * *weightat(i) - *poleat(i-1) * *weightat(i-1)) / - (bsp.flattenedknots[startpole+i+bsp.degree] - bsp.flattenedknots[startpole+i]); + d[i - 1] = (*poleat(i) * *weightat(i) - *poleat(i - 1) * *weightat(i - 1)) + / (bsp.flattenedknots[startpole + i + bsp.degree] + - bsp.flattenedknots[startpole + i]); } - double slopevalue = BSpline::splineValue(*theparam(), startpole + bsp.degree, bsp.degree-1, d, bsp.flattenedknots); + double slopevalue = BSpline::splineValue( + *theparam(), startpole + bsp.degree, bsp.degree - 1, d, bsp.flattenedknots); for (size_t i = 1; i < numpoints; ++i) { - d[i-1] = - (*weightat(i) - *weightat(i-1)) / - (bsp.flattenedknots[startpole+i+bsp.degree] - bsp.flattenedknots[startpole+i]); + d[i - 1] = (*weightat(i) - *weightat(i - 1)) + / (bsp.flattenedknots[startpole + i + bsp.degree] + - bsp.flattenedknots[startpole + i]); } - double wslopevalue = BSpline::splineValue(*theparam(), startpole + bsp.degree, bsp.degree-1, d, bsp.flattenedknots); + double wslopevalue = BSpline::splineValue( + *theparam(), startpole + bsp.degree, bsp.degree - 1, d, bsp.flattenedknots); deriv += (*thepoint() * wslopevalue - slopevalue) * bsp.degree; } for (size_t i = 0; i < numpoints; ++i) { if (gcsparam == poleat(i)) { - auto factorsI = bsp.getLinCombFactor(*theparam(), startpole + bsp.degree, startpole + i); + auto factorsI = + bsp.getLinCombFactor(*theparam(), startpole + bsp.degree, startpole + i); deriv += -(*weightat(i) * factorsI); } if (gcsparam == weightat(i)) { - auto factorsI = bsp.getLinCombFactor(*theparam(), startpole + bsp.degree, startpole + i); + auto factorsI = + bsp.getLinCombFactor(*theparam(), startpole + bsp.degree, startpole + i); deriv += (*thepoint() - *poleat(i)) * factorsI; } } @@ -604,15 +625,20 @@ double ConstraintDifference::error() return scale * (*param2() - *param1() - *difference()); } -double ConstraintDifference::grad(double *param) +double ConstraintDifference::grad(double* param) { - double deriv=0.; - if (param == param1()) deriv += -1; - if (param == param2()) deriv += 1; - if (param == difference()) deriv += -1; + double deriv = 0.; + if (param == param1()) + deriv += -1; + if (param == param2()) + deriv += 1; + if (param == difference()) + deriv += -1; return scale * deriv; } + +// -------------------------------------------------------- // P2PDistance ConstraintP2PDistance::ConstraintP2PDistance(Point &p1, Point &p2, double *d) { @@ -639,30 +665,34 @@ double ConstraintP2PDistance::error() { double dx = (*p1x() - *p2x()); double dy = (*p1y() - *p2y()); - double d = sqrt(dx*dx + dy*dy); - double dist = *distance(); + double d = sqrt(dx * dx + dy * dy); + double dist = *distance(); return scale * (d - dist); } -double ConstraintP2PDistance::grad(double *param) +double ConstraintP2PDistance::grad(double* param) { - double deriv=0.; - if (param == p1x() || param == p1y() || - param == p2x() || param == p2y()) { + double deriv = 0.; + if (param == p1x() || param == p1y() || param == p2x() || param == p2y()) { double dx = (*p1x() - *p2x()); double dy = (*p1y() - *p2y()); - double d = sqrt(dx*dx + dy*dy); - if (param == p1x()) deriv += dx/d; - if (param == p1y()) deriv += dy/d; - if (param == p2x()) deriv += -dx/d; - if (param == p2y()) deriv += -dy/d; + double d = sqrt(dx * dx + dy * dy); + if (param == p1x()) + deriv += dx / d; + if (param == p1y()) + deriv += dy / d; + if (param == p2x()) + deriv += -dx / d; + if (param == p2y()) + deriv += -dy / d; } - if (param == distance()) deriv += -1.; + if (param == distance()) + deriv += -1.; return scale * deriv; } -double ConstraintP2PDistance::maxStep(MAP_pD_D &dir, double lim) +double ConstraintP2PDistance::maxStep(MAP_pD_D& dir, double lim) { MAP_pD_D::iterator it; // distance() >= 0 @@ -672,27 +702,33 @@ double ConstraintP2PDistance::maxStep(MAP_pD_D &dir, double lim) lim = std::min(lim, -(*distance()) / it->second); } // restrict actual distance change - double ddx=0.,ddy=0.; + double ddx = 0., ddy = 0.; it = dir.find(p1x()); - if (it != dir.end()) ddx += it->second; + if (it != dir.end()) + ddx += it->second; it = dir.find(p1y()); - if (it != dir.end()) ddy += it->second; + if (it != dir.end()) + ddy += it->second; it = dir.find(p2x()); - if (it != dir.end()) ddx -= it->second; + if (it != dir.end()) + ddx -= it->second; it = dir.find(p2y()); - if (it != dir.end()) ddy -= it->second; - double dd = sqrt(ddx*ddx+ddy*ddy); - double dist = *distance(); + if (it != dir.end()) + ddy -= it->second; + double dd = sqrt(ddx * ddx + ddy * ddy); + double dist = *distance(); if (dd > dist) { double dx = (*p1x() - *p2x()); double dy = (*p1y() - *p2y()); - double d = sqrt(dx*dx + dy*dy); + double d = sqrt(dx * dx + dy * dy); if (dd > d) - lim = std::min(lim, std::max(d,dist)/dd); + lim = std::min(lim, std::max(d, dist) / dd); } return lim; } + +// -------------------------------------------------------- // P2PAngle ConstraintP2PAngle::ConstraintP2PAngle(Point &p1, Point &p2, double *a, double da_) : da(da_) @@ -723,48 +759,54 @@ double ConstraintP2PAngle::error() double a = *angle() + da; double ca = cos(a); double sa = sin(a); - double x = dx*ca + dy*sa; - double y = -dx*sa + dy*ca; - return scale * atan2(y,x); + double x = dx * ca + dy * sa; + double y = -dx * sa + dy * ca; + return scale * atan2(y, x); } -double ConstraintP2PAngle::grad(double *param) +double ConstraintP2PAngle::grad(double* param) { - double deriv=0.; - if (param == p1x() || param == p1y() || - param == p2x() || param == p2y()) { + double deriv = 0.; + if (param == p1x() || param == p1y() || param == p2x() || param == p2y()) { double dx = (*p2x() - *p1x()); double dy = (*p2y() - *p1y()); double a = *angle() + da; double ca = cos(a); double sa = sin(a); - double x = dx*ca + dy*sa; - double y = -dx*sa + dy*ca; - double r2 = dx*dx+dy*dy; - dx = -y/r2; - dy = x/r2; - if (param == p1x()) deriv += (-ca*dx + sa*dy); - if (param == p1y()) deriv += (-sa*dx - ca*dy); - if (param == p2x()) deriv += ( ca*dx - sa*dy); - if (param == p2y()) deriv += ( sa*dx + ca*dy); + double x = dx * ca + dy * sa; + double y = -dx * sa + dy * ca; + double r2 = dx * dx + dy * dy; + dx = -y / r2; + dy = x / r2; + if (param == p1x()) + deriv += (-ca * dx + sa * dy); + if (param == p1y()) + deriv += (-sa * dx - ca * dy); + if (param == p2x()) + deriv += (ca * dx - sa * dy); + if (param == p2y()) + deriv += (sa * dx + ca * dy); } - if (param == angle()) deriv += -1; + if (param == angle()) + deriv += -1; return scale * deriv; } -double ConstraintP2PAngle::maxStep(MAP_pD_D &dir, double lim) +double ConstraintP2PAngle::maxStep(MAP_pD_D& dir, double lim) { // step(angle()) <= pi/18 = 10° MAP_pD_D::iterator it = dir.find(angle()); if (it != dir.end()) { double step = std::abs(it->second); - if (step > M_PI/18.) - lim = std::min(lim, (M_PI/18.) / step); + if (step > M_PI / 18.0) + lim = std::min(lim, (M_PI / 18.0) / step); } return lim; } + +// -------------------------------------------------------- // P2LDistance ConstraintP2LDistance::ConstraintP2LDistance(Point &p, Line &l, double *d) { @@ -791,47 +833,55 @@ void ConstraintP2LDistance::rescale(double coef) double ConstraintP2LDistance::error() { - double x0=*p0x(), x1=*p1x(), x2=*p2x(); - double y0=*p0y(), y1=*p1y(), y2=*p2y(); + double x0 = *p0x(), x1 = *p1x(), x2 = *p2x(); + double y0 = *p0y(), y1 = *p1y(), y2 = *p2y(); double dist = *distance(); - double dx = x2-x1; - double dy = y2-y1; - double d = sqrt(dx*dx+dy*dy); - double area = std::abs(-x0*dy+y0*dx+x1*y2-x2*y1); // = x1y2 - x2y1 - x0y2 + x2y0 + x0y1 - x1y0 = 2*(triangle area) - return scale * (area/d - dist); + double dx = x2 - x1; + double dy = y2 - y1; + double d = sqrt(dx * dx + dy * dy); + double area = + std::abs(-x0 * dy + y0 * dx + x1 * y2 + - x2 * y1);// = x1y2 - x2y1 - x0y2 + x2y0 + x0y1 - x1y0 = 2*(triangle area) + return scale * (area / d - dist); } -double ConstraintP2LDistance::grad(double *param) +double ConstraintP2LDistance::grad(double* param) { - double deriv=0.; + double deriv = 0.; // darea/dx0 = (y1-y2) darea/dy0 = (x2-x1) // darea/dx1 = (y2-y0) darea/dy1 = (x0-x2) // darea/dx2 = (y0-y1) darea/dy2 = (x1-x0) - if (param == p0x() || param == p0y() || - param == p1x() || param == p1y() || - param == p2x() || param == p2y()) { - double x0=*p0x(), x1=*p1x(), x2=*p2x(); - double y0=*p0y(), y1=*p1y(), y2=*p2y(); - double dx = x2-x1; - double dy = y2-y1; - double d2 = dx*dx+dy*dy; + if (param == p0x() || param == p0y() || param == p1x() || param == p1y() || param == p2x() + || param == p2y()) { + double x0 = *p0x(), x1 = *p1x(), x2 = *p2x(); + double y0 = *p0y(), y1 = *p1y(), y2 = *p2y(); + double dx = x2 - x1; + double dy = y2 - y1; + double d2 = dx * dx + dy * dy; double d = sqrt(d2); - double area = -x0*dy+y0*dx+x1*y2-x2*y1; - if (param == p0x()) deriv += (y1-y2) / d; - if (param == p0y()) deriv += (x2-x1) / d ; - if (param == p1x()) deriv += ((y2-y0)*d + (dx/d)*area) / d2; - if (param == p1y()) deriv += ((x0-x2)*d + (dy/d)*area) / d2; - if (param == p2x()) deriv += ((y0-y1)*d - (dx/d)*area) / d2; - if (param == p2y()) deriv += ((x1-x0)*d - (dy/d)*area) / d2; + double area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1; + if (param == p0x()) + deriv += (y1 - y2) / d; + if (param == p0y()) + deriv += (x2 - x1) / d; + if (param == p1x()) + deriv += ((y2 - y0) * d + (dx / d) * area) / d2; + if (param == p1y()) + deriv += ((x0 - x2) * d + (dy / d) * area) / d2; + if (param == p2x()) + deriv += ((y0 - y1) * d - (dx / d) * area) / d2; + if (param == p2y()) + deriv += ((x1 - x0) * d - (dy / d) * area) / d2; if (area < 0) deriv *= -1; } - if (param == distance()) deriv += -1; + if (param == distance()) + deriv += -1; return scale * deriv; } -double ConstraintP2LDistance::maxStep(MAP_pD_D &dir, double lim) +double ConstraintP2LDistance::maxStep(MAP_pD_D& dir, double lim) { MAP_pD_D::iterator it; // distance() >= 0 @@ -841,36 +891,44 @@ double ConstraintP2LDistance::maxStep(MAP_pD_D &dir, double lim) lim = std::min(lim, -(*distance()) / it->second); } // restrict actual area change - double darea=0.; - double x0=*p0x(), x1=*p1x(), x2=*p2x(); - double y0=*p0y(), y1=*p1y(), y2=*p2y(); + double darea = 0.; + double x0 = *p0x(), x1 = *p1x(), x2 = *p2x(); + double y0 = *p0y(), y1 = *p1y(), y2 = *p2y(); it = dir.find(p0x()); - if (it != dir.end()) darea += (y1-y2) * it->second; + if (it != dir.end()) + darea += (y1 - y2) * it->second; it = dir.find(p0y()); - if (it != dir.end()) darea += (x2-x1) * it->second; + if (it != dir.end()) + darea += (x2 - x1) * it->second; it = dir.find(p1x()); - if (it != dir.end()) darea += (y2-y0) * it->second; + if (it != dir.end()) + darea += (y2 - y0) * it->second; it = dir.find(p1y()); - if (it != dir.end()) darea += (x0-x2) * it->second; + if (it != dir.end()) + darea += (x0 - x2) * it->second; it = dir.find(p2x()); - if (it != dir.end()) darea += (y0-y1) * it->second; + if (it != dir.end()) + darea += (y0 - y1) * it->second; it = dir.find(p2y()); - if (it != dir.end()) darea += (x1-x0) * it->second; + if (it != dir.end()) + darea += (x1 - x0) * it->second; darea = std::abs(darea); if (darea > 0.) { - double dx = x2-x1; - double dy = y2-y1; - double area = 0.3*(*distance())*sqrt(dx*dx+dy*dy); + double dx = x2 - x1; + double dy = y2 - y1; + double area = 0.3 * (*distance()) * sqrt(dx * dx + dy * dy); if (darea > area) { - area = std::max(area, 0.3*std::abs(-x0*dy+y0*dx+x1*y2-x2*y1)); + area = std::max(area, 0.3 * std::abs(-x0 * dy + y0 * dx + x1 * y2 - x2 * y1)); if (darea > area) - lim = std::min(lim, area/darea); + lim = std::min(lim, area / darea); } } return lim; } + +// -------------------------------------------------------- // PointOnLine ConstraintPointOnLine::ConstraintPointOnLine(Point &p, Line &l) { @@ -908,41 +966,49 @@ void ConstraintPointOnLine::rescale(double coef) double ConstraintPointOnLine::error() { - double x0=*p0x(), x1=*p1x(), x2=*p2x(); - double y0=*p0y(), y1=*p1y(), y2=*p2y(); - double dx = x2-x1; - double dy = y2-y1; - double d = sqrt(dx*dx+dy*dy); - double area = -x0*dy+y0*dx+x1*y2-x2*y1; // = x1y2 - x2y1 - x0y2 + x2y0 + x0y1 - x1y0 = 2*(triangle area) - return scale * area/d; + double x0 = *p0x(), x1 = *p1x(), x2 = *p2x(); + double y0 = *p0y(), y1 = *p1y(), y2 = *p2y(); + double dx = x2 - x1; + double dy = y2 - y1; + double d = sqrt(dx * dx + dy * dy); + double area = -x0 * dy + y0 * dx + x1 * y2 + - x2 * y1;// = x1y2 - x2y1 - x0y2 + x2y0 + x0y1 - x1y0 = 2*(triangle area) + return scale * area / d; } -double ConstraintPointOnLine::grad(double *param) +double ConstraintPointOnLine::grad(double* param) { - double deriv=0.; + double deriv = 0.; // darea/dx0 = (y1-y2) darea/dy0 = (x2-x1) // darea/dx1 = (y2-y0) darea/dy1 = (x0-x2) // darea/dx2 = (y0-y1) darea/dy2 = (x1-x0) - if (param == p0x() || param == p0y() || - param == p1x() || param == p1y() || - param == p2x() || param == p2y()) { - double x0=*p0x(), x1=*p1x(), x2=*p2x(); - double y0=*p0y(), y1=*p1y(), y2=*p2y(); - double dx = x2-x1; - double dy = y2-y1; - double d2 = dx*dx+dy*dy; + if (param == p0x() || param == p0y() || param == p1x() || param == p1y() || param == p2x() + || param == p2y()) { + double x0 = *p0x(), x1 = *p1x(), x2 = *p2x(); + double y0 = *p0y(), y1 = *p1y(), y2 = *p2y(); + double dx = x2 - x1; + double dy = y2 - y1; + double d2 = dx * dx + dy * dy; double d = sqrt(d2); - double area = -x0*dy+y0*dx+x1*y2-x2*y1; - if (param == p0x()) deriv += (y1-y2) / d; - if (param == p0y()) deriv += (x2-x1) / d ; - if (param == p1x()) deriv += ((y2-y0)*d + (dx/d)*area) / d2; - if (param == p1y()) deriv += ((x0-x2)*d + (dy/d)*area) / d2; - if (param == p2x()) deriv += ((y0-y1)*d - (dx/d)*area) / d2; - if (param == p2y()) deriv += ((x1-x0)*d - (dy/d)*area) / d2; + double area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1; + if (param == p0x()) + deriv += (y1 - y2) / d; + if (param == p0y()) + deriv += (x2 - x1) / d; + if (param == p1x()) + deriv += ((y2 - y0) * d + (dx / d) * area) / d2; + if (param == p1y()) + deriv += ((x0 - x2) * d + (dy / d) * area) / d2; + if (param == p2x()) + deriv += ((y0 - y1) * d - (dx / d) * area) / d2; + if (param == p2y()) + deriv += ((x1 - x0) * d - (dy / d) * area) / d2; } return scale * deriv; } + +// -------------------------------------------------------- // PointOnPerpBisector ConstraintPointOnPerpBisector::ConstraintPointOnPerpBisector(Point &p, Line &l) { @@ -978,11 +1044,11 @@ void ConstraintPointOnPerpBisector::rescale(double coef) scale = coef; } -void ConstraintPointOnPerpBisector::errorgrad(double *err, double *grad, double *param) +void ConstraintPointOnPerpBisector::errorgrad(double* err, double* grad, double* param) { - DeriVector2 p0(Point(p0x(),p0y()), param); - DeriVector2 p1(Point(p1x(),p1y()), param); - DeriVector2 p2(Point(p2x(),p2y()), param); + DeriVector2 p0(Point(p0x(), p0y()), param); + DeriVector2 p1(Point(p1x(), p1y()), param); + DeriVector2 p2(Point(p2x(), p2y()), param); DeriVector2 d1 = p0.subtr(p1); DeriVector2 d2 = p0.subtr(p2); @@ -995,15 +1061,15 @@ void ConstraintPointOnPerpBisector::errorgrad(double *err, double *grad, double projd2 = d2.scalarProd(D, &dprojd2); if (err) - *err = projd1+projd2; + *err = projd1 + projd2; if (grad) - *grad = dprojd1+dprojd2; + *grad = dprojd1 + dprojd2; } double ConstraintPointOnPerpBisector::error() { double err; - errorgrad(&err,nullptr,nullptr); + errorgrad(&err, nullptr, nullptr); return scale * err; } @@ -1019,6 +1085,8 @@ double ConstraintPointOnPerpBisector::grad(double *param) return deriv*scale; } + +// -------------------------------------------------------- // Parallel ConstraintParallel::ConstraintParallel(Line &l1, Line &l2) { @@ -1045,7 +1113,7 @@ void ConstraintParallel::rescale(double coef) double dy1 = (*l1p1y() - *l1p2y()); double dx2 = (*l2p1x() - *l2p2x()); double dy2 = (*l2p1y() - *l2p2y()); - scale = coef / sqrt((dx1*dx1+dy1*dy1)*(dx2*dx2+dy2*dy2)); + scale = coef / sqrt((dx1 * dx1 + dy1 * dy1) * (dx2 * dx2 + dy2 * dy2)); } double ConstraintParallel::error() @@ -1054,25 +1122,35 @@ double ConstraintParallel::error() double dy1 = (*l1p1y() - *l1p2y()); double dx2 = (*l2p1x() - *l2p2x()); double dy2 = (*l2p1y() - *l2p2y()); - return scale * (dx1*dy2 - dy1*dx2); + return scale * (dx1 * dy2 - dy1 * dx2); } -double ConstraintParallel::grad(double *param) +double ConstraintParallel::grad(double* param) { - double deriv=0.; - if (param == l1p1x()) deriv += (*l2p1y() - *l2p2y()); // = dy2 - if (param == l1p2x()) deriv += -(*l2p1y() - *l2p2y()); // = -dy2 - if (param == l1p1y()) deriv += -(*l2p1x() - *l2p2x()); // = -dx2 - if (param == l1p2y()) deriv += (*l2p1x() - *l2p2x()); // = dx2 + double deriv = 0.; + if (param == l1p1x()) + deriv += (*l2p1y() - *l2p2y());// = dy2 + if (param == l1p2x()) + deriv += -(*l2p1y() - *l2p2y());// = -dy2 + if (param == l1p1y()) + deriv += -(*l2p1x() - *l2p2x());// = -dx2 + if (param == l1p2y()) + deriv += (*l2p1x() - *l2p2x());// = dx2 - if (param == l2p1x()) deriv += -(*l1p1y() - *l1p2y()); // = -dy1 - if (param == l2p2x()) deriv += (*l1p1y() - *l1p2y()); // = dy1 - if (param == l2p1y()) deriv += (*l1p1x() - *l1p2x()); // = dx1 - if (param == l2p2y()) deriv += -(*l1p1x() - *l1p2x()); // = -dx1 + if (param == l2p1x()) + deriv += -(*l1p1y() - *l1p2y());// = -dy1 + if (param == l2p2x()) + deriv += (*l1p1y() - *l1p2y());// = dy1 + if (param == l2p1y()) + deriv += (*l1p1x() - *l1p2x());// = dx1 + if (param == l2p2y()) + deriv += -(*l1p1x() - *l1p2x());// = -dx1 return scale * deriv; } + +// -------------------------------------------------------- // Perpendicular ConstraintPerpendicular::ConstraintPerpendicular(Line &l1, Line &l2) { @@ -1114,7 +1192,7 @@ void ConstraintPerpendicular::rescale(double coef) double dy1 = (*l1p1y() - *l1p2y()); double dx2 = (*l2p1x() - *l2p2x()); double dy2 = (*l2p1y() - *l2p2y()); - scale = coef / sqrt((dx1*dx1+dy1*dy1)*(dx2*dx2+dy2*dy2)); + scale = coef / sqrt((dx1 * dx1 + dy1 * dy1) * (dx2 * dx2 + dy2 * dy2)); } double ConstraintPerpendicular::error() @@ -1123,25 +1201,35 @@ double ConstraintPerpendicular::error() double dy1 = (*l1p1y() - *l1p2y()); double dx2 = (*l2p1x() - *l2p2x()); double dy2 = (*l2p1y() - *l2p2y()); - return scale * (dx1*dx2 + dy1*dy2); + return scale * (dx1 * dx2 + dy1 * dy2); } -double ConstraintPerpendicular::grad(double *param) +double ConstraintPerpendicular::grad(double* param) { - double deriv=0.; - if (param == l1p1x()) deriv += (*l2p1x() - *l2p2x()); // = dx2 - if (param == l1p2x()) deriv += -(*l2p1x() - *l2p2x()); // = -dx2 - if (param == l1p1y()) deriv += (*l2p1y() - *l2p2y()); // = dy2 - if (param == l1p2y()) deriv += -(*l2p1y() - *l2p2y()); // = -dy2 + double deriv = 0.; + if (param == l1p1x()) + deriv += (*l2p1x() - *l2p2x());// = dx2 + if (param == l1p2x()) + deriv += -(*l2p1x() - *l2p2x());// = -dx2 + if (param == l1p1y()) + deriv += (*l2p1y() - *l2p2y());// = dy2 + if (param == l1p2y()) + deriv += -(*l2p1y() - *l2p2y());// = -dy2 - if (param == l2p1x()) deriv += (*l1p1x() - *l1p2x()); // = dx1 - if (param == l2p2x()) deriv += -(*l1p1x() - *l1p2x()); // = -dx1 - if (param == l2p1y()) deriv += (*l1p1y() - *l1p2y()); // = dy1 - if (param == l2p2y()) deriv += -(*l1p1y() - *l1p2y()); // = -dy1 + if (param == l2p1x()) + deriv += (*l1p1x() - *l1p2x());// = dx1 + if (param == l2p2x()) + deriv += -(*l1p1x() - *l1p2x());// = -dx1 + if (param == l2p1y()) + deriv += (*l1p1y() - *l1p2y());// = dy1 + if (param == l2p2y()) + deriv += -(*l1p1y() - *l1p2y());// = -dy1 return scale * deriv; } + +// -------------------------------------------------------- // L2LAngle ConstraintL2LAngle::ConstraintL2LAngle(Line &l1, Line &l2, double *a) { @@ -1190,63 +1278,72 @@ double ConstraintL2LAngle::error() double dy1 = (*l1p2y() - *l1p1y()); double dx2 = (*l2p2x() - *l2p1x()); double dy2 = (*l2p2y() - *l2p1y()); - double a = atan2(dy1,dx1) + *angle(); + double a = atan2(dy1, dx1) + *angle(); double ca = cos(a); double sa = sin(a); - double x2 = dx2*ca + dy2*sa; - double y2 = -dx2*sa + dy2*ca; - return scale * atan2(y2,x2); + double x2 = dx2 * ca + dy2 * sa; + double y2 = -dx2 * sa + dy2 * ca; + return scale * atan2(y2, x2); } -double ConstraintL2LAngle::grad(double *param) +double ConstraintL2LAngle::grad(double* param) { - double deriv=0.; - if (param == l1p1x() || param == l1p1y() || - param == l1p2x() || param == l1p2y()) { + double deriv = 0.; + if (param == l1p1x() || param == l1p1y() || param == l1p2x() || param == l1p2y()) { double dx1 = (*l1p2x() - *l1p1x()); double dy1 = (*l1p2y() - *l1p1y()); - double r2 = dx1*dx1+dy1*dy1; - if (param == l1p1x()) deriv += -dy1/r2; - if (param == l1p1y()) deriv += dx1/r2; - if (param == l1p2x()) deriv += dy1/r2; - if (param == l1p2y()) deriv += -dx1/r2; + double r2 = dx1 * dx1 + dy1 * dy1; + if (param == l1p1x()) + deriv += -dy1 / r2; + if (param == l1p1y()) + deriv += dx1 / r2; + if (param == l1p2x()) + deriv += dy1 / r2; + if (param == l1p2y()) + deriv += -dx1 / r2; } - if (param == l2p1x() || param == l2p1y() || - param == l2p2x() || param == l2p2y()) { + if (param == l2p1x() || param == l2p1y() || param == l2p2x() || param == l2p2y()) { double dx1 = (*l1p2x() - *l1p1x()); double dy1 = (*l1p2y() - *l1p1y()); double dx2 = (*l2p2x() - *l2p1x()); double dy2 = (*l2p2y() - *l2p1y()); - double a = atan2(dy1,dx1) + *angle(); + double a = atan2(dy1, dx1) + *angle(); double ca = cos(a); double sa = sin(a); - double x2 = dx2*ca + dy2*sa; - double y2 = -dx2*sa + dy2*ca; - double r2 = dx2*dx2+dy2*dy2; - dx2 = -y2/r2; - dy2 = x2/r2; - if (param == l2p1x()) deriv += (-ca*dx2 + sa*dy2); - if (param == l2p1y()) deriv += (-sa*dx2 - ca*dy2); - if (param == l2p2x()) deriv += ( ca*dx2 - sa*dy2); - if (param == l2p2y()) deriv += ( sa*dx2 + ca*dy2); + double x2 = dx2 * ca + dy2 * sa; + double y2 = -dx2 * sa + dy2 * ca; + double r2 = dx2 * dx2 + dy2 * dy2; + dx2 = -y2 / r2; + dy2 = x2 / r2; + if (param == l2p1x()) + deriv += (-ca * dx2 + sa * dy2); + if (param == l2p1y()) + deriv += (-sa * dx2 - ca * dy2); + if (param == l2p2x()) + deriv += (ca * dx2 - sa * dy2); + if (param == l2p2y()) + deriv += (sa * dx2 + ca * dy2); } - if (param == angle()) deriv += -1; + if (param == angle()) + deriv += -1; return scale * deriv; } -double ConstraintL2LAngle::maxStep(MAP_pD_D &dir, double lim) +double ConstraintL2LAngle::maxStep(MAP_pD_D& dir, double lim) { // step(angle()) <= pi/18 = 10° MAP_pD_D::iterator it = dir.find(angle()); if (it != dir.end()) { double step = std::abs(it->second); - if (step > M_PI/18.) - lim = std::min(lim, (M_PI/18.) / step); + if (step > M_PI / 18.0) + lim = std::min(lim, (M_PI / 18.0) / step); } return lim; } + +// -------------------------------------------------------- // MidpointOnLine ConstraintMidpointOnLine::ConstraintMidpointOnLine(Line &l1, Line &l2) { @@ -1262,7 +1359,8 @@ ConstraintMidpointOnLine::ConstraintMidpointOnLine(Line &l1, Line &l2) rescale(); } -ConstraintMidpointOnLine::ConstraintMidpointOnLine(Point &l1p1, Point &l1p2, Point &l2p1, Point &l2p2) +ConstraintMidpointOnLine::ConstraintMidpointOnLine(Point& l1p1, Point& l1p2, Point& l2p1, + Point& l2p2) { pvec.push_back(l1p1.x); pvec.push_back(l1p1.y); @@ -1288,51 +1386,60 @@ void ConstraintMidpointOnLine::rescale(double coef) double ConstraintMidpointOnLine::error() { - double x0=((*l1p1x())+(*l1p2x()))/2; - double y0=((*l1p1y())+(*l1p2y()))/2; - double x1=*l2p1x(), x2=*l2p2x(); - double y1=*l2p1y(), y2=*l2p2y(); - double dx = x2-x1; - double dy = y2-y1; - double d = sqrt(dx*dx+dy*dy); - double area = -x0*dy+y0*dx+x1*y2-x2*y1; // = x1y2 - x2y1 - x0y2 + x2y0 + x0y1 - x1y0 = 2*(triangle area) - return scale * area/d; + double x0 = ((*l1p1x()) + (*l1p2x())) / 2; + double y0 = ((*l1p1y()) + (*l1p2y())) / 2; + double x1 = *l2p1x(), x2 = *l2p2x(); + double y1 = *l2p1y(), y2 = *l2p2y(); + double dx = x2 - x1; + double dy = y2 - y1; + double d = sqrt(dx * dx + dy * dy); + double area = -x0 * dy + y0 * dx + x1 * y2 + - x2 * y1;// = x1y2 - x2y1 - x0y2 + x2y0 + x0y1 - x1y0 = 2*(triangle area) + return scale * area / d; } -double ConstraintMidpointOnLine::grad(double *param) +double ConstraintMidpointOnLine::grad(double* param) { - double deriv=0.; + double deriv = 0.; // darea/dx0 = (y1-y2) darea/dy0 = (x2-x1) // darea/dx1 = (y2-y0) darea/dy1 = (x0-x2) // darea/dx2 = (y0-y1) darea/dy2 = (x1-x0) - if (param == l1p1x() || param == l1p1y() || - param == l1p2x() || param == l1p2y()|| - param == l2p1x() || param == l2p1y() || - param == l2p2x() || param == l2p2y()) { - double x0=((*l1p1x())+(*l1p2x()))/2; - double y0=((*l1p1y())+(*l1p2y()))/2; - double x1=*l2p1x(), x2=*l2p2x(); - double y1=*l2p1y(), y2=*l2p2y(); - double dx = x2-x1; - double dy = y2-y1; - double d2 = dx*dx+dy*dy; + if (param == l1p1x() || param == l1p1y() || param == l1p2x() || param == l1p2y() + || param == l2p1x() || param == l2p1y() || param == l2p2x() || param == l2p2y()) { + double x0 = ((*l1p1x()) + (*l1p2x())) / 2; + double y0 = ((*l1p1y()) + (*l1p2y())) / 2; + double x1 = *l2p1x(), x2 = *l2p2x(); + double y1 = *l2p1y(), y2 = *l2p2y(); + double dx = x2 - x1; + double dy = y2 - y1; + double d2 = dx * dx + dy * dy; double d = sqrt(d2); - double area = -x0*dy+y0*dx+x1*y2-x2*y1; - if (param == l1p1x()) deriv += (y1-y2) / (2*d); - if (param == l1p1y()) deriv += (x2-x1) / (2*d); - if (param == l1p2x()) deriv += (y1-y2) / (2*d); - if (param == l1p2y()) deriv += (x2-x1) / (2*d); - if (param == l2p1x()) deriv += ((y2-y0)*d + (dx/d)*area) / d2; - if (param == l2p1y()) deriv += ((x0-x2)*d + (dy/d)*area) / d2; - if (param == l2p2x()) deriv += ((y0-y1)*d - (dx/d)*area) / d2; - if (param == l2p2y()) deriv += ((x1-x0)*d - (dy/d)*area) / d2; + double area = -x0 * dy + y0 * dx + x1 * y2 - x2 * y1; + if (param == l1p1x()) + deriv += (y1 - y2) / (2 * d); + if (param == l1p1y()) + deriv += (x2 - x1) / (2 * d); + if (param == l1p2x()) + deriv += (y1 - y2) / (2 * d); + if (param == l1p2y()) + deriv += (x2 - x1) / (2 * d); + if (param == l2p1x()) + deriv += ((y2 - y0) * d + (dx / d) * area) / d2; + if (param == l2p1y()) + deriv += ((x0 - x2) * d + (dy / d) * area) / d2; + if (param == l2p2x()) + deriv += ((y0 - y1) * d - (dx / d) * area) / d2; + if (param == l2p2y()) + deriv += ((x1 - x0) * d - (dy / d) * area) / d2; } return scale * deriv; } + +// -------------------------------------------------------- // TangentCircumf -ConstraintTangentCircumf::ConstraintTangentCircumf(Point &p1, Point &p2, - double *rad1, double *rad2, bool internal_) +ConstraintTangentCircumf::ConstraintTangentCircumf(Point& p1, Point& p2, double* rad1, double* rad2, + bool internal_) { internal = internal_; pvec.push_back(p1.x); @@ -1360,36 +1467,45 @@ double ConstraintTangentCircumf::error() double dx = (*c1x() - *c2x()); double dy = (*c1y() - *c2y()); if (internal) - return scale * (sqrt(dx*dx + dy*dy) - std::abs(*r1() - *r2())); + return scale * (sqrt(dx * dx + dy * dy) - std::abs(*r1() - *r2())); else - return scale * (sqrt(dx*dx + dy*dy) - (*r1() + *r2())); + return scale * (sqrt(dx * dx + dy * dy) - (*r1() + *r2())); } -double ConstraintTangentCircumf::grad(double *param) +double ConstraintTangentCircumf::grad(double* param) { - double deriv=0.; - if (param == c1x() || param == c1y() || - param == c2x() || param == c2y()|| - param == r1() || param == r2()) { + double deriv = 0.; + if (param == c1x() || param == c1y() || param == c2x() || param == c2y() || param == r1() + || param == r2()) { double dx = (*c1x() - *c2x()); double dy = (*c1y() - *c2y()); - double d = sqrt(dx*dx + dy*dy); - if (param == c1x()) deriv += dx/d; - if (param == c1y()) deriv += dy/d; - if (param == c2x()) deriv += -dx/d; - if (param == c2y()) deriv += -dy/d; + double d = sqrt(dx * dx + dy * dy); + if (param == c1x()) + deriv += dx / d; + if (param == c1y()) + deriv += dy / d; + if (param == c2x()) + deriv += -dx / d; + if (param == c2y()) + deriv += -dy / d; if (internal) { - if (param == r1()) deriv += (*r1() > *r2()) ? -1 : 1; - if (param == r2()) deriv += (*r1() > *r2()) ? 1 : -1; + if (param == r1()) + deriv += (*r1() > *r2()) ? -1 : 1; + if (param == r2()) + deriv += (*r1() > *r2()) ? 1 : -1; } else { - if (param == r1()) deriv += -1; - if (param == r2()) deriv += -1; + if (param == r1()) + deriv += -1; + if (param == r2()) + deriv += -1; } } return scale * deriv; } + +// -------------------------------------------------------- // ConstraintPointOnEllipse ConstraintPointOnEllipse::ConstraintPointOnEllipse(Point &p, Ellipse &e) { @@ -1424,19 +1540,17 @@ double ConstraintPointOnEllipse::error() double Y_F1 = *f1y(); double b = *rmin(); - double err=sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + sqrt(pow(X_0 + - X_F1 - 2*X_c, 2) + pow(Y_0 + Y_F1 - 2*Y_c, 2)) - 2*sqrt(pow(b, 2) + - pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); + double err = sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)) + - 2 * sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); return scale * err; } -double ConstraintPointOnEllipse::grad(double *param) +double ConstraintPointOnEllipse::grad(double* param) { - double deriv=0.; - if (param == p1x() || param == p1y() || - param == f1x() || param == f1y() || - param == cx() || param == cy() || - param == rmin()) { + double deriv = 0.; + if (param == p1x() || param == p1y() || param == f1x() || param == f1y() || param == cx() + || param == cy() || param == rmin()) { double X_0 = *p1x(); double Y_0 = *p1y(); @@ -1447,38 +1561,39 @@ double ConstraintPointOnEllipse::grad(double *param) double b = *rmin(); if (param == p1x()) - deriv += (X_0 - X_F1)/sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + - (X_0 + X_F1 - 2*X_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + Y_F1 - - 2*Y_c, 2)); + deriv += (X_0 - X_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + (X_0 + X_F1 - 2 * X_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); if (param == p1y()) - deriv += (Y_0 - Y_F1)/sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + - (Y_0 + Y_F1 - 2*Y_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + Y_F1 - - 2*Y_c, 2)); + deriv += (Y_0 - Y_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + (Y_0 + Y_F1 - 2 * Y_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); if (param == f1x()) - deriv += -(X_0 - X_F1)/sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) - - 2*(X_F1 - X_c)/sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) - + (X_0 + X_F1 - 2*X_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + Y_F1 - - 2*Y_c, 2)); + deriv += -(X_0 - X_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + - 2 * (X_F1 - X_c) / sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + + (X_0 + X_F1 - 2 * X_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); if (param == f1y()) - deriv +=-(Y_0 - Y_F1)/sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) - - 2*(Y_F1 - Y_c)/sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) - + (Y_0 + Y_F1 - 2*Y_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + Y_F1 - - 2*Y_c, 2)); + deriv += -(Y_0 - Y_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + - 2 * (Y_F1 - Y_c) / sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + + (Y_0 + Y_F1 - 2 * Y_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); if (param == cx()) - deriv += 2*(X_F1 - X_c)/sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2)) - 2*(X_0 + X_F1 - 2*X_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + - pow(Y_0 + Y_F1 - 2*Y_c, 2)); + deriv += 2 * (X_F1 - X_c) / sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + - 2 * (X_0 + X_F1 - 2 * X_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); if (param == cy()) - deriv +=2*(Y_F1 - Y_c)/sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2)) - 2*(Y_0 + Y_F1 - 2*Y_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + - pow(Y_0 + Y_F1 - 2*Y_c, 2)); + deriv += 2 * (Y_F1 - Y_c) / sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + - 2 * (Y_0 + Y_F1 - 2 * Y_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); if (param == rmin()) - deriv += -2*b/sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2)); + deriv += -2 * b / sqrt(pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); } return scale * deriv; } + +// -------------------------------------------------------- // ConstraintEllipseTangentLine ConstraintEllipseTangentLine::ConstraintEllipseTangentLine(Line &l, Ellipse &e) { @@ -1510,57 +1625,58 @@ void ConstraintEllipseTangentLine::rescale(double coef) scale = coef * 1; } -void ConstraintEllipseTangentLine::errorgrad(double *err, double *grad, double *param) +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 + 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 + // 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 + 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 + // calculate distance form f1m to f2 double distF1mF2, ddistF1mF2; distF1mF2 = f2.subtr(f1m).length(ddistF1mF2); - //calculate major radius (to compare the distance to) + // 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); + radmaj = e.getRadMaj(c, f1, *e.radmin, dradmin, dradmaj); if (err) - *err = distF1mF2 - 2*radmaj; + *err = distF1mF2 - 2 * radmaj; if (grad) - *grad = ddistF1mF2 - 2*dradmaj; + *grad = ddistF1mF2 - 2 * dradmaj; } double ConstraintEllipseTangentLine::error() { double err; - errorgrad(&err,nullptr,nullptr); + errorgrad(&err, nullptr, nullptr); return scale * err; } -double ConstraintEllipseTangentLine::grad(double *param) +double ConstraintEllipseTangentLine::grad(double* param) { - //first of all, check that we need to compute anything. - if ( findParamInPvec(param) == -1 ) + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) return 0.0; double deriv; errorgrad(nullptr, &deriv, param); - //use numeric for testing - #if 0 +// use numeric for testing +#if 0 double const eps = 0.00001; double oldparam = *param; double v0 = this->error(); @@ -1570,18 +1686,20 @@ double ConstraintEllipseTangentLine::grad(double *param) 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 + 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; + return deriv * scale; } + +// -------------------------------------------------------- // ConstraintInternalAlignmentPoint2Ellipse -ConstraintInternalAlignmentPoint2Ellipse::ConstraintInternalAlignmentPoint2Ellipse(Ellipse &e, Point &p1, InternalAlignmentType alignmentType) +ConstraintInternalAlignmentPoint2Ellipse::ConstraintInternalAlignmentPoint2Ellipse( + Ellipse& e, Point& p1, InternalAlignmentType alignmentType) { this->p = p1; pvec.push_back(p.x); @@ -1596,8 +1714,10 @@ ConstraintInternalAlignmentPoint2Ellipse::ConstraintInternalAlignmentPoint2Ellip void ConstraintInternalAlignmentPoint2Ellipse::ReconstructGeomPointers() { int i = 0; - p.x = pvec[i]; i++; - p.y = pvec[i]; i++; + p.x = pvec[i]; + i++; + p.y = pvec[i]; + i++; e.ReconstructOnNewPvec(pvec, i); pvecChangedFlag = false; } @@ -1612,82 +1732,84 @@ void ConstraintInternalAlignmentPoint2Ellipse::rescale(double coef) scale = coef * 1; } -void ConstraintInternalAlignmentPoint2Ellipse::errorgrad(double *err, double *grad, double *param) +void ConstraintInternalAlignmentPoint2Ellipse::errorgrad(double* err, double* grad, double* param) { - if (pvecChangedFlag) ReconstructGeomPointers(); + if (pvecChangedFlag) + ReconstructGeomPointers(); - //todo: prefill only what's needed, not everything + // 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; + DeriVector2 pv(p, param); + double b, db;// minor radius + b = *e.radmin; + db = (e.radmin == param) ? 1.0 : 0.0; - //major radius + // major radius double a, da; - a = e.getRadMaj(c,f1,b,db,da); + a = e.getRadMaj(c, f1, b, db, da); - DeriVector2 poa;//point to align to - bool by_y_not_by_x = false;//a flag to indicate if the alignment error function is for y (false - x, true - y). + DeriVector2 poa;// point to align to + bool by_y_not_by_x = + false;// a flag to indicate if the alignment error function is for y (false - x, true - y) - switch(AlignmentType){ + switch (AlignmentType) { case EllipsePositiveMajorX: case EllipsePositiveMajorY: poa = c.sum(emaj.multD(a, da)); by_y_not_by_x = AlignmentType == EllipsePositiveMajorY; - break; + break; case EllipseNegativeMajorX: case EllipseNegativeMajorY: poa = c.sum(emaj.multD(-a, -da)); by_y_not_by_x = AlignmentType == EllipseNegativeMajorY; - break; + break; case EllipsePositiveMinorX: case EllipsePositiveMinorY: poa = c.sum(emin.multD(b, db)); by_y_not_by_x = AlignmentType == EllipsePositiveMinorY; - break; + break; case EllipseNegativeMinorX: case EllipseNegativeMinorY: poa = c.sum(emin.multD(-b, -db)); by_y_not_by_x = AlignmentType == EllipseNegativeMinorY; - break; + break; case EllipseFocus2X: case EllipseFocus2Y: poa = c.linCombi(2.0, f1, -1.0); by_y_not_by_x = AlignmentType == EllipseFocus2Y; - break; + break; default: - //shouldn't happen - poa = pv;//align to the point itself, doing nothing essentially + // shouldn't happen + poa = pv;// align to the point itself, doing nothing essentially } - if(err) + if (err) *err = by_y_not_by_x ? pv.y - poa.y : pv.x - poa.x; - if(grad) + if (grad) *grad = by_y_not_by_x ? pv.dy - poa.dy : pv.dx - poa.dx; } double ConstraintInternalAlignmentPoint2Ellipse::error() { double err; - errorgrad(&err,nullptr,nullptr); + errorgrad(&err, nullptr, nullptr); return scale * err; - } -double ConstraintInternalAlignmentPoint2Ellipse::grad(double *param) +double ConstraintInternalAlignmentPoint2Ellipse::grad(double* param) { - //first of all, check that we need to compute anything. - if ( findParamInPvec(param) == -1 ) + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) return 0.0; double deriv; errorgrad(nullptr, &deriv, param); - //use numeric for testing - #if 0 +// use numeric for testing +#if 0 double const eps = 0.00001; double oldparam = *param; double v0 = this->error(); @@ -1697,18 +1819,20 @@ double ConstraintInternalAlignmentPoint2Ellipse::grad(double *param) 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 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; } + +// -------------------------------------------------------- // ConstraintInternalAlignmentPoint2Hyperbola -ConstraintInternalAlignmentPoint2Hyperbola::ConstraintInternalAlignmentPoint2Hyperbola(Hyperbola &e, Point &p1, InternalAlignmentType alignmentType) +ConstraintInternalAlignmentPoint2Hyperbola::ConstraintInternalAlignmentPoint2Hyperbola( + Hyperbola& e, Point& p1, InternalAlignmentType alignmentType) { this->p = p1; pvec.push_back(p.x); @@ -1739,29 +1863,32 @@ void ConstraintInternalAlignmentPoint2Hyperbola::rescale(double coef) scale = coef * 1; } -void ConstraintInternalAlignmentPoint2Hyperbola::errorgrad(double *err, double *grad, double *param) +void ConstraintInternalAlignmentPoint2Hyperbola::errorgrad(double* err, double* grad, double* param) { - if (pvecChangedFlag) ReconstructGeomPointers(); + if (pvecChangedFlag) + ReconstructGeomPointers(); - //todo: prefill only what's needed, not everything + // 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); + DeriVector2 pv(p, param); - double b, db;//minor radius - b = *e.radmin; db = (e.radmin == param) ? 1.0 : 0.0; + double b, db;// minor radius + b = *e.radmin; + db = (e.radmin == param) ? 1.0 : 0.0; - //major radius + // major radius double a, da; - a = e.getRadMaj(c,f1,b,db,da); + a = e.getRadMaj(c, f1, b, db, da); - DeriVector2 poa;//point to align to - bool by_y_not_by_x = false;//a flag to indicate if the alignment error function is for y (false - x, true - y). + DeriVector2 poa;// point to align to + bool by_y_not_by_x = + false;// a flag to indicate if the alignment error function is for y (false - x, true - y) - switch(AlignmentType){ + switch (AlignmentType) { case HyperbolaPositiveMajorX: case HyperbolaPositiveMajorY: poa = c.sum(emaj.multD(a, da)); @@ -1773,59 +1900,58 @@ void ConstraintInternalAlignmentPoint2Hyperbola::errorgrad(double *err, double * by_y_not_by_x = AlignmentType == HyperbolaNegativeMajorY; break; case HyperbolaPositiveMinorX: - case HyperbolaPositiveMinorY: - { + case HyperbolaPositiveMinorY: { DeriVector2 pa = c.sum(emaj.multD(a, da)); - //DeriVector2 A(pa.x,pa.y); - //poa = A.sum(emin.multD(b, db)); + // DeriVector2 A(pa.x,pa.y); + // poa = A.sum(emin.multD(b, db)); poa = pa.sum(emin.multD(b, db)); by_y_not_by_x = AlignmentType == HyperbolaPositiveMinorY; break; } case HyperbolaNegativeMinorX: - case HyperbolaNegativeMinorY: - { + case HyperbolaNegativeMinorY: { DeriVector2 pa = c.sum(emaj.multD(a, da)); - //DeriVector2 A(pa.x,pa.y); - //poa = A.sum(emin.multD(-b, -db)); + // DeriVector2 A(pa.x,pa.y); + // poa = A.sum(emin.multD(-b, -db)); poa = pa.sum(emin.multD(-b, -db)); by_y_not_by_x = AlignmentType == HyperbolaNegativeMinorY; break; } default: - //shouldn't happen - poa = pv;//align to the point itself, doing nothing essentially + // shouldn't happen + poa = pv;// align to the point itself, doing nothing essentially } - if(err) + if (err) *err = by_y_not_by_x ? pv.y - poa.y : pv.x - poa.x; - if(grad) + if (grad) *grad = by_y_not_by_x ? pv.dy - poa.dy : pv.dx - poa.dx; } double ConstraintInternalAlignmentPoint2Hyperbola::error() { double err; - errorgrad(&err,nullptr,nullptr); + errorgrad(&err, nullptr, nullptr); return scale * err; - } -double ConstraintInternalAlignmentPoint2Hyperbola::grad(double *param) +double ConstraintInternalAlignmentPoint2Hyperbola::grad(double* param) { - //first of all, check that we need to compute anything. - if ( findParamInPvec(param) == -1 ) + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) return 0.0; double deriv; errorgrad(nullptr, &deriv, param); - return deriv*scale; - + return deriv * scale; } + +// -------------------------------------------------------- // ConstraintEqualMajorAxesEllipse -ConstraintEqualMajorAxesConic:: ConstraintEqualMajorAxesConic(MajorRadiusConic * a1, MajorRadiusConic * a2) +ConstraintEqualMajorAxesConic::ConstraintEqualMajorAxesConic(MajorRadiusConic* a1, + MajorRadiusConic* a2) { this->e1 = a1; this->e1->PushOwnParams(pvec); @@ -1838,7 +1964,7 @@ ConstraintEqualMajorAxesConic:: ConstraintEqualMajorAxesConic(MajorRadiusConic * void ConstraintEqualMajorAxesConic::ReconstructGeomPointers() { - int i =0; + int i = 0; e1->ReconstructOnNewPvec(pvec, i); e2->ReconstructOnNewPvec(pvec, i); pvecChangedFlag = false; @@ -1870,14 +1996,14 @@ void ConstraintEqualMajorAxesConic::errorgrad(double *err, double *grad, double double ConstraintEqualMajorAxesConic::error() { double err; - errorgrad(&err,nullptr,nullptr); + errorgrad(&err, nullptr, nullptr); return scale * err; } -double ConstraintEqualMajorAxesConic::grad(double *param) +double ConstraintEqualMajorAxesConic::grad(double* param) { - //first of all, check that we need to compute anything. - if ( findParamInPvec(param) == -1 ) + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) return 0.0; double deriv; @@ -1887,7 +2013,7 @@ double ConstraintEqualMajorAxesConic::grad(double *param) } // ConstraintEqualFocalDistance -ConstraintEqualFocalDistance:: ConstraintEqualFocalDistance(ArcOfParabola * a1, ArcOfParabola * a2) +ConstraintEqualFocalDistance::ConstraintEqualFocalDistance(ArcOfParabola* a1, ArcOfParabola* a2) { this->e1 = a1; this->e1->PushOwnParams(pvec); @@ -1951,10 +2077,10 @@ double ConstraintEqualFocalDistance::error() return scale * err; } -double ConstraintEqualFocalDistance::grad(double *param) +double ConstraintEqualFocalDistance::grad(double* param) { - //first of all, check that we need to compute anything. - if ( findParamInPvec(param) == -1 ) + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) return 0.0; double deriv; @@ -1963,6 +2089,8 @@ double ConstraintEqualFocalDistance::grad(double *param) return deriv * scale; } + +// -------------------------------------------------------- // ConstraintCurveValue ConstraintCurveValue::ConstraintCurveValue(Point &p, double* pcoord, Curve& crv, double *u) { @@ -1984,11 +2112,13 @@ ConstraintCurveValue::~ConstraintCurveValue() void ConstraintCurveValue::ReconstructGeomPointers() { - int i=0; - p.x=pvec[i]; i++; - p.y=pvec[i]; i++; - i++;//we have an inline function for point coordinate - i++;//we have an inline function for the parameterU + int i = 0; + p.x = pvec[i]; + i++; + p.y = pvec[i]; + i++; + i++;// we have an inline function for point coordinate + i++;// we have an inline function for the parameterU this->crv->ReconstructOnNewPvec(pvec, i); pvecChangedFlag = false; } @@ -2008,7 +2138,7 @@ void ConstraintCurveValue::errorgrad(double *err, double *grad, double *param) if (pvecChangedFlag) ReconstructGeomPointers(); double u, du; - u = *(this->u()); du = ( param == this->u() ) ? 1.0 : 0.0; + u = *(this->u()); du = ( param == this->u() ) ? 1.0 : 0.0; DeriVector2 P_to; //point of curve at parameter value of u, in global coordinates P_to = this->crv->Value(u,du,param); @@ -2040,16 +2170,16 @@ double ConstraintCurveValue::error() return scale * err; } -double ConstraintCurveValue::grad(double *param) +double ConstraintCurveValue::grad(double* param) { - //first of all, check that we need to compute anything. - if ( findParamInPvec(param) == -1 ) + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) return 0.0; double deriv; errorgrad(nullptr, &deriv, param); - return deriv*scale; + return deriv * scale; } double ConstraintCurveValue::maxStep(MAP_pD_D &/*dir*/, double lim) @@ -2066,6 +2196,8 @@ double ConstraintCurveValue::maxStep(MAP_pD_D &/*dir*/, double lim) return lim; } + +// -------------------------------------------------------- // ConstraintPointOnHyperbola ConstraintPointOnHyperbola::ConstraintPointOnHyperbola(Point &p, Hyperbola &e) { @@ -2126,19 +2258,17 @@ double ConstraintPointOnHyperbola::error() // show(a) // DM=sqrt((P-F2)*(P-F2))-sqrt((P-F1)*(P-F1))-2*a // show(DM.simplify_radical()) - double err=-sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + sqrt(pow(X_0 - + X_F1 - 2*X_c, 2) + pow(Y_0 + Y_F1 - 2*Y_c, 2)) - 2*sqrt(-pow(b, 2) + - pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); + double err = -sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)) + - 2 * sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); return scale * err; } -double ConstraintPointOnHyperbola::grad(double *param) +double ConstraintPointOnHyperbola::grad(double* param) { - double deriv=0.; - if (param == p1x() || param == p1y() || - param == f1x() || param == f1y() || - param == cx() || param == cy() || - param == rmin()) { + double deriv = 0.; + if (param == p1x() || param == p1y() || param == f1x() || param == f1y() || param == cx() + || param == cy() || param == rmin()) { double X_0 = *p1x(); double Y_0 = *p1y(); @@ -2149,37 +2279,39 @@ double ConstraintPointOnHyperbola::grad(double *param) double b = *rmin(); if (param == p1x()) - deriv += -(X_0 - X_F1)/sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + - (X_0 + X_F1 - 2*X_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + Y_F1 - - 2*Y_c, 2)); + deriv += -(X_0 - X_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + (X_0 + X_F1 - 2 * X_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); if (param == p1y()) - deriv += -(Y_0 - Y_F1)/sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + - (Y_0 + Y_F1 - 2*Y_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + Y_F1 - - 2*Y_c, 2)); + deriv += -(Y_0 - Y_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + + (Y_0 + Y_F1 - 2 * Y_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); if (param == f1x()) - deriv += (X_0 - X_F1)/sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) - - 2*(X_F1 - X_c)/sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2)) + (X_0 + X_F1 - 2*X_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + - Y_F1 - 2*Y_c, 2)); + deriv += (X_0 - X_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + - 2 * (X_F1 - X_c) / sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + + (X_0 + X_F1 - 2 * X_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); if (param == f1y()) - deriv +=(Y_0 - Y_F1)/sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) - - 2*(Y_F1 - Y_c)/sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, - 2)) + (Y_0 + Y_F1 - 2*Y_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + pow(Y_0 + - Y_F1 - 2*Y_c, 2)); + deriv += (Y_0 - Y_F1) / sqrt(pow(X_0 - X_F1, 2) + pow(Y_0 - Y_F1, 2)) + - 2 * (Y_F1 - Y_c) / sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + + (Y_0 + Y_F1 - 2 * Y_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); if (param == cx()) - deriv += 2*(X_F1 - X_c)/sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2)) - 2*(X_0 + X_F1 - 2*X_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + - pow(Y_0 + Y_F1 - 2*Y_c, 2)); + deriv += 2 * (X_F1 - X_c) / sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + - 2 * (X_0 + X_F1 - 2 * X_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); if (param == cy()) - deriv +=2*(Y_F1 - Y_c)/sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - - Y_c, 2)) - 2*(Y_0 + Y_F1 - 2*Y_c)/sqrt(pow(X_0 + X_F1 - 2*X_c, 2) + - pow(Y_0 + Y_F1 - 2*Y_c, 2)); + deriv += 2 * (Y_F1 - Y_c) / sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)) + - 2 * (Y_0 + Y_F1 - 2 * Y_c) + / sqrt(pow(X_0 + X_F1 - 2 * X_c, 2) + pow(Y_0 + Y_F1 - 2 * Y_c, 2)); if (param == rmin()) - deriv += 2*b/sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c,2)); - } - return scale * deriv; + deriv += 2 * b / sqrt(-pow(b, 2) + pow(X_F1 - X_c, 2) + pow(Y_F1 - Y_c, 2)); + } + return scale * deriv; } + +// -------------------------------------------------------- // ConstraintPointOnParabola ConstraintPointOnParabola::ConstraintPointOnParabola(Point &p, Parabola &e) { @@ -2267,20 +2399,22 @@ double ConstraintPointOnParabola::error() return scale * err; } -double ConstraintPointOnParabola::grad(double *param) +double ConstraintPointOnParabola::grad(double* param) { - //first of all, check that we need to compute anything. - if ( findParamInPvec(param) == -1 ) - return 0.0; + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) + return 0.0; double deriv; errorgrad(nullptr, &deriv, param); - return deriv*scale; + return deriv * scale; } + +// -------------------------------------------------------- // ConstraintAngleViaPoint -ConstraintAngleViaPoint::ConstraintAngleViaPoint(Curve &acrv1, Curve &acrv2, Point p, double* angle) +ConstraintAngleViaPoint::ConstraintAngleViaPoint(Curve& acrv1, Curve& acrv2, Point p, double* angle) { pvec.push_back(angle); pvec.push_back(p.x); @@ -2290,9 +2424,10 @@ ConstraintAngleViaPoint::ConstraintAngleViaPoint(Curve &acrv1, Curve &acrv2, Poi crv1 = acrv1.Copy(); crv2 = acrv2.Copy(); origpvec = pvec; - pvecChangedFlag=true; + pvecChangedFlag = true; rescale(); } + ConstraintAngleViaPoint::~ConstraintAngleViaPoint() { delete crv1; crv1 = nullptr; @@ -2322,26 +2457,28 @@ void ConstraintAngleViaPoint::rescale(double coef) double ConstraintAngleViaPoint::error() { - if (pvecChangedFlag) ReconstructGeomPointers(); - double ang=*angle(); + if (pvecChangedFlag) + ReconstructGeomPointers(); + double ang = *angle(); DeriVector2 n1 = crv1->CalculateNormal(poa); DeriVector2 n2 = crv2->CalculateNormal(poa); - //rotate n1 by angle - DeriVector2 n1r (n1.x*cos(ang) - n1.y*sin(ang), n1.x*sin(ang) + n1.y*cos(ang) ); + // rotate n1 by angle + DeriVector2 n1r(n1.x * cos(ang) - n1.y * sin(ang), n1.x * sin(ang) + n1.y * cos(ang)); - //calculate angle between n1r and n2. Since we have rotated the n1, the angle is the error function. - //for our atan2, y is a dot product (n2) * (n1r rotated ccw by 90 degrees). - // x is a dot product (n2) * (n1r) - double err = atan2(-n2.x*n1r.y+n2.y*n1r.x, n2.x*n1r.x + n2.y*n1r.y); - //essentially, the function is equivalent to atan2(n2)-(atan2(n1)+angle). The only difference is behavior when normals are zero (the intended result is also zero in this case). + // calculate angle between n1r and n2. Since we have rotated the n1, the angle is the error + // function. for our atan2, y is a dot product (n2) * (n1r rotated ccw by 90 degrees). + // x is a dot product (n2) * (n1r) + double err = atan2(-n2.x * n1r.y + n2.y * n1r.x, n2.x * n1r.x + n2.y * n1r.y); + // essentially, the function is equivalent to atan2(n2)-(atan2(n1)+angle). The only difference + // is behavior when normals are zero (the intended result is also zero in this case). return scale * err; } double ConstraintAngleViaPoint::grad(double *param) { - //first of all, check that we need to compute anything. - if ( findParamInPvec(param) == -1 ) + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) return 0.0; double deriv=0.; @@ -2351,11 +2488,11 @@ double ConstraintAngleViaPoint::grad(double *param) if (param == angle()) deriv += -1.0; DeriVector2 n1 = crv1->CalculateNormal(poa, param); DeriVector2 n2 = crv2->CalculateNormal(poa, param); - deriv -= ( (-n1.dx)*n1.y / pow(n1.length(),2) + n1.dy*n1.x / pow(n1.length(),2) ); - deriv += ( (-n2.dx)*n2.y / pow(n2.length(),2) + n2.dy*n2.x / pow(n2.length(),2) ); + deriv -= ( (-n1.dx) * n1.y / pow(n1.length(), 2) + n1.dy * n1.x / pow(n1.length(), 2) ); + deriv += ( (-n2.dx) * n2.y / pow(n2.length(), 2) + n2.dy * n2.x / pow(n2.length(), 2) ); -//use numeric for testing +// use numeric for testing #if 0 double const eps = 0.00001; double oldparam = *param; @@ -2375,9 +2512,11 @@ double ConstraintAngleViaPoint::grad(double *param) return scale * deriv; } -//ConstraintSnell -ConstraintSnell::ConstraintSnell(Curve &ray1, Curve &ray2, Curve &boundary, Point p, double* n1, double* n2, bool flipn1, bool flipn2) +// -------------------------------------------------------- +// ConstraintSnell +ConstraintSnell::ConstraintSnell(Curve& ray1, Curve& ray2, Curve& boundary, Point p, double* n1, + double* n2, bool flipn1, bool flipn2) { pvec.push_back(n1); pvec.push_back(n2); @@ -2390,13 +2529,14 @@ ConstraintSnell::ConstraintSnell(Curve &ray1, Curve &ray2, Curve &boundary, Poin this->ray2 = ray2.Copy(); this->boundary = boundary.Copy(); origpvec = pvec; - pvecChangedFlag=true; + pvecChangedFlag = true; this->flipn1 = flipn1; this->flipn2 = flipn2; rescale(); } + ConstraintSnell::~ConstraintSnell() { delete ray1; ray1 = nullptr; @@ -2406,14 +2546,17 @@ ConstraintSnell::~ConstraintSnell() void ConstraintSnell::ReconstructGeomPointers() { - int cnt=0; - cnt++; cnt++;//skip n1, n2 - we have an inline function for that - poa.x = pvec[cnt]; cnt++; - poa.y = pvec[cnt]; cnt++; - ray1->ReconstructOnNewPvec(pvec,cnt); - ray2->ReconstructOnNewPvec(pvec,cnt); - boundary->ReconstructOnNewPvec(pvec,cnt); - pvecChangedFlag=false; + int cnt = 0; + cnt++; + cnt++;// skip n1, n2 - we have an inline function for that + poa.x = pvec[cnt]; + cnt++; + poa.y = pvec[cnt]; + cnt++; + ray1->ReconstructOnNewPvec(pvec, cnt); + ray2->ReconstructOnNewPvec(pvec, cnt); + boundary->ReconstructOnNewPvec(pvec, cnt); + pvecChangedFlag = false; } ConstraintType ConstraintSnell::getTypeId() @@ -2426,25 +2569,32 @@ void ConstraintSnell::rescale(double coef) scale = coef * 1.; } -//error and gradient combined. Values are returned through pointers. -void ConstraintSnell::errorgrad(double *err, double *grad, double* param) +// error and gradient combined. Values are returned through pointers. +void ConstraintSnell::errorgrad(double* err, double* grad, double* param) { - if (pvecChangedFlag) ReconstructGeomPointers(); + if (pvecChangedFlag) + ReconstructGeomPointers(); DeriVector2 tang1 = ray1->CalculateNormal(poa, param).rotate90cw().getNormalized(); DeriVector2 tang2 = ray2->CalculateNormal(poa, param).rotate90cw().getNormalized(); DeriVector2 tangB = boundary->CalculateNormal(poa, param).rotate90cw().getNormalized(); double sin1, dsin1, sin2, dsin2; - sin1 = tang1.scalarProd(tangB, &dsin1);//sinus of angle of incidence + sin1 = tang1.scalarProd(tangB, &dsin1);// sinus of angle of incidence sin2 = tang2.scalarProd(tangB, &dsin2); - if (flipn1) {sin1 = -sin1; dsin1 = -dsin1;} - if (flipn2) {sin2 = -sin2; dsin2 = -dsin2;} + if (flipn1) { + sin1 = -sin1; + dsin1 = -dsin1; + } + if (flipn2) { + sin2 = -sin2; + dsin2 = -dsin2; + } double dn1 = (param == n1()) ? 1.0 : 0.0; double dn2 = (param == n2()) ? 1.0 : 0.0; if (err) - *err = *n1()*sin1 - *n2()*sin2; + *err = *n1() * sin1 - *n2() * sin2; if (grad) - *grad = dn1*sin1 + *n1()*dsin1 - dn2*sin2 - *n2()*dsin2; + *grad = dn1 * sin1 + *n1() * dsin1 - dn2 * sin2 - *n2() * dsin2; } double ConstraintSnell::error() @@ -2456,16 +2606,14 @@ double ConstraintSnell::error() double ConstraintSnell::grad(double *param) { - - //first of all, check that we need to compute anything. - if ( findParamInPvec(param) == -1 ) + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) return 0.0; double deriv; errorgrad(nullptr, &deriv, param); - -//use numeric for testing +// use numeric for testing #if 0 double const eps = 0.00001; double oldparam = *param; @@ -2476,17 +2624,19 @@ double ConstraintSnell::grad(double *param) 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) ); + double numretl = (v0 - vl) / eps; + double numretr = (vr - v0) / eps; + assert(deriv <= std::max(numretl, numretr)); + assert(deriv >= std::min(numretl, numretr)); #endif return scale * deriv; } + +// -------------------------------------------------------- // ConstraintEqualLineLength -ConstraintEqualLineLength::ConstraintEqualLineLength(Line &l1, Line &l2) +ConstraintEqualLineLength::ConstraintEqualLineLength(Line& l1, Line& l2) { this->l1 = l1; this->l1.PushOwnParams(pvec); @@ -2545,23 +2695,23 @@ void ConstraintEqualLineLength::errorgrad(double *err, double *grad, double *par // So here we maintain the very small derivative of 1e-10 when the gradient is under such value, such // that the diagnose function with pivot threshold of 1e-13 treats the value as non-zero and correctly // detects and can tell apart when a parameter is fully constrained or just locked into a maximum/minimum - if(fabs(*grad) < 1e-10) { + if (fabs(*grad) < 1e-10) { double surrogate = 1e-10; - if( param == l1.p1.x ) + if (param == l1.p1.x) *grad = v1.x > 0 ? surrogate : -surrogate; - if( param == l1.p1.y ) + if (param == l1.p1.y) *grad = v1.y > 0 ? surrogate : -surrogate; - if( param == l1.p2.x ) + if (param == l1.p2.x) *grad = v1.x > 0 ? -surrogate : surrogate; - if( param == l1.p2.y ) + if (param == l1.p2.y) *grad = v1.y > 0 ? -surrogate : surrogate; - if( param == l2.p1.x ) + if (param == l2.p1.x) *grad = v2.x > 0 ? surrogate : -surrogate; - if( param == l2.p1.y ) + if (param == l2.p1.y) *grad = v2.y > 0 ? surrogate : -surrogate; - if( param == l2.p2.x ) + if (param == l2.p2.x) *grad = v2.x > 0 ? -surrogate : surrogate; - if( param == l2.p2.y ) + if (param == l2.p2.y) *grad = v2.y > 0 ? -surrogate : surrogate; } } @@ -2570,23 +2720,25 @@ void ConstraintEqualLineLength::errorgrad(double *err, double *grad, double *par double ConstraintEqualLineLength::error() { double err; - errorgrad(&err,nullptr,nullptr); + errorgrad(&err, nullptr, nullptr); return scale * err; } -double ConstraintEqualLineLength::grad(double *param) +double ConstraintEqualLineLength::grad(double* param) { - if ( findParamInPvec(param) == -1 ) + if (findParamInPvec(param) == -1) return 0.0; double deriv; errorgrad(nullptr, &deriv, param); - return deriv*scale; + return deriv * scale; } + +// -------------------------------------------------------- // ConstraintC2CDistance -ConstraintC2CDistance::ConstraintC2CDistance(Circle &c1, Circle &c2, double *d) +ConstraintC2CDistance::ConstraintC2CDistance(Circle& c1, Circle& c2, double* d) { this->d = d; pvec.push_back(d); @@ -2604,8 +2756,8 @@ ConstraintC2CDistance::ConstraintC2CDistance(Circle &c1, Circle &c2, double *d) void ConstraintC2CDistance::ReconstructGeomPointers() { - int i=0; - i++; // skip the first parameter as there is the inline function distance for it + int i = 0; + i++;// skip the first parameter as there is the inline function distance for it c1.ReconstructOnNewPvec(pvec, i); c2.ReconstructOnNewPvec(pvec, i); pvecChangedFlag = false; @@ -2635,13 +2787,12 @@ void ConstraintC2CDistance::errorgrad(double *err, double *grad, double *param) // outer case (defined as the centers of the circles are outside the center of the other circles) // it may well be that the circles intersect. - if( length_ct12 >= *c1.rad && - length_ct12 >= *c2.rad ) { + if (length_ct12 >= *c1.rad && length_ct12 >= *c2.rad) { if (err) { *err = length_ct12 - (*c2.rad + *c1.rad + *distance()); } else if (grad) { - double drad = (param == c2.rad || param == c1.rad || param == distance())?-1.0:0.0; + double drad = (param == c2.rad || param == c1.rad || param == distance()) ? -1.0 : 0.0; *grad = dlength_ct12 + drad; } } @@ -2657,18 +2808,19 @@ void ConstraintC2CDistance::errorgrad(double *err, double *grad, double *param) else if (grad) { double drad = 0.0; - if(param == bigradius) { + if (param == bigradius) { drad = 1.0; } - else if(param == smallradius) { + else if (param == smallradius) { drad = -1.0; } - else if(param == distance()) { - drad = (*distance()<0.)?1.0:-1.0; + else if (param == distance()) { + drad = (*distance() < 0.) ? 1.0 : -1.0; } - if (length_ct12>1e-13) { + if (length_ct12 > 1e-13) { *grad = -dlength_ct12 + drad; - } else { // concentric case + } + else {// concentric case *grad = drad; } } @@ -2684,13 +2836,13 @@ double ConstraintC2CDistance::error() double ConstraintC2CDistance::grad(double *param) { - if ( findParamInPvec(param) == -1 ) + if (findParamInPvec(param) == -1) return 0.0; double deriv; errorgrad(nullptr, &deriv, param); - return deriv*scale; + return deriv * scale; } } //namespace GCS