From d8a050303a5971237a884ff89296bf740d66f138 Mon Sep 17 00:00:00 2001 From: Ajinkya Dahale Date: Thu, 30 Mar 2023 20:48:36 +0530 Subject: [PATCH] [Sketcher][planegcs] Support angle via point with params These are intended to use when calculating normal simply with points could be numerically expensive or otherwise nonviable. --- src/Mod/Sketcher/App/planegcs/Constraints.cpp | 235 ++++++++++++++++++ src/Mod/Sketcher/App/planegcs/Constraints.h | 89 ++++++- src/Mod/Sketcher/App/planegcs/Geo.h | 13 +- 3 files changed, 335 insertions(+), 2 deletions(-) diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index aee8375afa..b670dc1b3f 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -2650,6 +2650,241 @@ double ConstraintAngleViaPoint::grad(double* param) return scale * deriv; } +// -------------------------------------------------------- +// ConstraintAngleViaPointAndParam +ConstraintAngleViaPointAndParam::ConstraintAngleViaPointAndParam(Curve& acrv1, + Curve& acrv2, + Point p, + double* cparam, + double* angle) +{ + pvec.push_back(angle); + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(cparam); + acrv1.PushOwnParams(pvec); + acrv2.PushOwnParams(pvec); + crv1 = acrv1.Copy(); + crv2 = acrv2.Copy(); + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +ConstraintAngleViaPointAndParam::~ConstraintAngleViaPointAndParam() +{ + delete crv1; + crv1 = nullptr; + delete crv2; + crv2 = nullptr; +} + +void ConstraintAngleViaPointAndParam::ReconstructGeomPointers() +{ + int cnt = 0; + cnt++; // skip angle - we have an inline function for that + poa.x = pvec[cnt]; + cnt++; + poa.y = pvec[cnt]; + cnt++; + cnt++; // skip cparam + crv1->ReconstructOnNewPvec(pvec, cnt); + crv2->ReconstructOnNewPvec(pvec, cnt); + pvecChangedFlag = false; +} + +ConstraintType ConstraintAngleViaPointAndParam::getTypeId() +{ + return AngleViaPointAndParam; +} + +void ConstraintAngleViaPointAndParam::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintAngleViaPointAndParam::error() +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + double ang = *angle(); + DeriVector2 n1 = crv1->CalculateNormal(cparam()); + 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)); + + // 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 ConstraintAngleViaPointAndParam::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv = 0.; + + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + if (param == angle()) { + deriv += -1.0; + } + DeriVector2 n1 = crv1->CalculateNormal(cparam(), 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)); + + +// use numeric for testing +#if 0 + double const eps = 0.00001; + double oldparam = *param; + double v0 = this->error(); + *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 scale * deriv; +} + +// -------------------------------------------------------- +// ConstraintAngleViaPointAndTwoParams +ConstraintAngleViaPointAndTwoParams::ConstraintAngleViaPointAndTwoParams(Curve& acrv1, + Curve& acrv2, + Point p, + double* cparam1, + double* cparam2, + double* angle) +{ + pvec.push_back(angle); + pvec.push_back(p.x); + pvec.push_back(p.y); + pvec.push_back(cparam1); + pvec.push_back(cparam2); + acrv1.PushOwnParams(pvec); + acrv2.PushOwnParams(pvec); + crv1 = acrv1.Copy(); + crv2 = acrv2.Copy(); + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +ConstraintAngleViaPointAndTwoParams::~ConstraintAngleViaPointAndTwoParams() +{ + delete crv1; + crv1 = nullptr; + delete crv2; + crv2 = nullptr; +} + +void ConstraintAngleViaPointAndTwoParams::ReconstructGeomPointers() +{ + int cnt = 0; + cnt++; // skip angle - we have an inline function for that + poa.x = pvec[cnt]; + cnt++; + poa.y = pvec[cnt]; + cnt++; + cnt++; // skip cparam1 - we have an inline function for that + cnt++; // skip cparam2 - we have an inline function for that + crv1->ReconstructOnNewPvec(pvec, cnt); + crv2->ReconstructOnNewPvec(pvec, cnt); + pvecChangedFlag = false; +} + +ConstraintType ConstraintAngleViaPointAndTwoParams::getTypeId() +{ + return AngleViaPointAndTwoParams; +} + +void ConstraintAngleViaPointAndTwoParams::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintAngleViaPointAndTwoParams::error() +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + double ang = *angle(); + DeriVector2 n1 = crv1->CalculateNormal(cparam1()); + DeriVector2 n2 = crv2->CalculateNormal(cparam2()); + + // 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). + return scale * err; +} + +double ConstraintAngleViaPointAndTwoParams::grad(double* param) +{ + // first of all, check that we need to compute anything. + if (findParamInPvec(param) == -1) { + return 0.0; + } + + double deriv = 0.; + + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + + if (param == angle()) { + deriv += -1.0; + } + DeriVector2 n1 = crv1->CalculateNormal(cparam1(), param); + DeriVector2 n2 = crv2->CalculateNormal(cparam2(), 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)); + + +// use numeric for testing +#if 0 + double const eps = 0.00001; + double oldparam = *param; + double v0 = this->error(); + *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 scale * deriv; +} + // -------------------------------------------------------- // ConstraintSnell diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h index e4ff32edf9..d092789e96 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/src/Mod/Sketcher/App/planegcs/Constraints.h @@ -78,7 +78,9 @@ enum ConstraintType PointOnBSpline = 29, C2CDistance = 30, C2LDistance = 31, - P2CDistance = 32 + P2CDistance = 32, + AngleViaPointAndParam = 33, + AngleViaPointAndTwoParams = 34 }; enum InternalAlignmentType @@ -1173,6 +1175,91 @@ public: double grad(double*) override; }; +class ConstraintAngleViaPointAndParam: public Constraint +{ +private: + inline double* angle() + { + return pvec[0]; + }; + inline double* cparam() + { + return pvec[3]; + }; + Curve* crv1; + Curve* crv2; + // These two pointers hold copies of the curves that were passed on + // constraint creation. The curves must be deleted upon destruction of + // the constraint. It is necessary to have copies, since messing with + // original objects that were passed is a very bad idea (but messing is + // necessary, because we need to support redirectParams()/revertParams + // functions. + // The pointers in the curves need to be reconstructed if pvec was redirected + // (test pvecChangedFlag variable before use!) + Point poa; // poa=point of angle //needs to be reconstructed if pvec was redirected/reverted. + // The point is easily shallow-copied by C++, so no pointer type here and no delete + // is necessary. + void + ReconstructGeomPointers(); // writes pointers in pvec to the parameters of crv1, crv2 and poa +public: + // We assume first curve needs param1 + ConstraintAngleViaPointAndParam(Curve& acrv1, + Curve& acrv2, + Point p, + double* param1, + double* angle); + ~ConstraintAngleViaPointAndParam() override; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + +// TODO: Do we need point here at all? +class ConstraintAngleViaPointAndTwoParams: public Constraint +{ +private: + inline double* angle() + { + return pvec[0]; + }; + inline double* cparam1() + { + return pvec[3]; + }; + inline double* cparam2() + { + return pvec[4]; + }; + Curve* crv1; + Curve* crv2; + // These two pointers hold copies of the curves that were passed on + // constraint creation. The curves must be deleted upon destruction of + // the constraint. It is necessary to have copies, since messing with + // original objects that were passed is a very bad idea (but messing is + // necessary, because we need to support redirectParams()/revertParams + // functions. + // The pointers in the curves need to be reconstructed if pvec was redirected + // (test pvecChangedFlag variable before use!) + Point poa; // poa=point of angle //needs to be reconstructed if pvec was redirected/reverted. + // The point is easily shallow-copied by C++, so no pointer type here and no delete + // is necessary. + void + ReconstructGeomPointers(); // writes pointers in pvec to the parameters of crv1, crv2 and poa +public: + ConstraintAngleViaPointAndTwoParams(Curve& acrv1, + Curve& acrv2, + Point p, + double* param1, + double* param2, + double* angle); + ~ConstraintAngleViaPointAndTwoParams() override; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + class ConstraintEqualLineLength: public Constraint { private: diff --git a/src/Mod/Sketcher/App/planegcs/Geo.h b/src/Mod/Sketcher/App/planegcs/Geo.h index 91f0eb6f6f..fa7f6c0455 100644 --- a/src/Mod/Sketcher/App/planegcs/Geo.h +++ b/src/Mod/Sketcher/App/planegcs/Geo.h @@ -161,6 +161,14 @@ public: virtual DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const = 0; + // returns normal vector at parameter instead of at the given point. + virtual DeriVector2 CalculateNormal(const double* param, const double* derivparam = nullptr) + { + DeriVector2 pointDV = Value(*param, 0.0); + Point p(&pointDV.x, &pointDV.y); + return CalculateNormal(p, derivparam); + } + /** * @brief Value: returns point (vector) given the value of parameter * @param u: value of parameter @@ -414,6 +422,9 @@ public: // interface helpers VEC_D flattenedknots; DeriVector2 CalculateNormal(const Point& p, const double* derivparam = nullptr) const override; + // TODO: override parametric version + // DeriVector2 CalculateNormal(const double* param, const double* derivparam = nullptr) const + // override; DeriVector2 Value(double u, double du, const double* derivparam = nullptr) const override; int PushOwnParams(VEC_pD& pvec) override; void ReconstructOnNewPvec(VEC_pD& pvec, int& cnt) override; @@ -433,7 +444,7 @@ public: /// x is the point at which combination is needed /// k is the range in `flattenedknots` that contains x /// p is the degree - /// d is the vector of (relevant) poles (this will be changed) + /// d is the vector of (relevant) poles (note that this is not const and will be changed) /// flatknots is the vector of knots static double splineValue(double x, size_t k, unsigned int p, VEC_D& d, const VEC_D& flatknots); };