From 4087f1e50898ec223e6cc04a9a2f528467ca3cd3 Mon Sep 17 00:00:00 2001 From: Ajinkya Dahale Date: Sun, 25 Feb 2024 21:38:25 +0530 Subject: [PATCH] [planegcs] Add `ConstraintAngleViaTwoPoints` Needed for B-spline to B-spline end to end tangent. The end point of one is for now not usable in to get normal at the other. --- src/Mod/Sketcher/App/planegcs/Constraints.cpp | 102 ++++++++++++++++++ src/Mod/Sketcher/App/planegcs/Constraints.h | 38 ++++++- src/Mod/Sketcher/App/planegcs/GCS.cpp | 14 +++ src/Mod/Sketcher/App/planegcs/GCS.h | 7 ++ 4 files changed, 160 insertions(+), 1 deletion(-) diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index f99bfbb69d..5a66be5bee 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -2650,6 +2650,108 @@ double ConstraintAngleViaPoint::grad(double* param) return scale * deriv; } +// -------------------------------------------------------- +// ConstraintAngleViaTwoPoints +ConstraintAngleViaTwoPoints::ConstraintAngleViaTwoPoints(Curve& acrv1, + Curve& acrv2, + Point p1, + Point p2, + double* angle) +{ + pvec.push_back(angle); + pvec.push_back(p1.x); + pvec.push_back(p1.y); + pvec.push_back(p2.x); + pvec.push_back(p2.y); + acrv1.PushOwnParams(pvec); + acrv2.PushOwnParams(pvec); + crv1 = acrv1.Copy(); + crv2 = acrv2.Copy(); + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +ConstraintAngleViaTwoPoints::~ConstraintAngleViaTwoPoints() +{ + delete crv1; + crv1 = nullptr; + delete crv2; + crv2 = nullptr; +} + +void ConstraintAngleViaTwoPoints::ReconstructGeomPointers() +{ + int cnt = 0; + cnt++; // skip angle - we have an inline function for that + poa1.x = pvec[cnt]; + cnt++; + poa1.y = pvec[cnt]; + cnt++; + poa2.x = pvec[cnt]; + cnt++; + poa2.y = pvec[cnt]; + cnt++; + crv1->ReconstructOnNewPvec(pvec, cnt); + crv2->ReconstructOnNewPvec(pvec, cnt); + pvecChangedFlag = false; +} + +ConstraintType ConstraintAngleViaTwoPoints::getTypeId() +{ + return AngleViaTwoPoints; +} + +void ConstraintAngleViaTwoPoints::rescale(double coef) +{ + scale = coef * 1.; +} + +double ConstraintAngleViaTwoPoints::error() +{ + if (pvecChangedFlag) { + ReconstructGeomPointers(); + } + double ang = *angle(); + DeriVector2 n1 = crv1->CalculateNormal(poa1); + DeriVector2 n2 = crv2->CalculateNormal(poa2); + + // 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 ConstraintAngleViaTwoPoints::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(poa1, param); + DeriVector2 n2 = crv2->CalculateNormal(poa2, 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)); + + return scale * deriv; +} + // -------------------------------------------------------- // ConstraintAngleViaPointAndParam ConstraintAngleViaPointAndParam::ConstraintAngleViaPointAndParam(Curve& acrv1, diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h index d092789e96..017165ca6f 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/src/Mod/Sketcher/App/planegcs/Constraints.h @@ -80,7 +80,8 @@ enum ConstraintType C2LDistance = 31, P2CDistance = 32, AngleViaPointAndParam = 33, - AngleViaPointAndTwoParams = 34 + AngleViaPointAndTwoParams = 34, + AngleViaTwoPoints = 35 }; enum InternalAlignmentType @@ -1126,6 +1127,41 @@ public: double grad(double*) override; }; +class ConstraintAngleViaTwoPoints: public Constraint +{ +private: + inline double* angle() + { + return pvec[0]; + }; + 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!) + // poa=point of angle //needs to be reconstructed if pvec was redirected/reverted. The points + // are easily shallow-copied by C++, so no pointer type here and no delete is necessary. We use + // two points in this method as a workaround for B-splines (and friends). There, normals at + // general points are not implemented, just at their stored start/end points. + Point poa1; + Point poa2; + // writes pointers in pvec to the parameters of crv1, crv2 and poa + void ReconstructGeomPointers(); + +public: + ConstraintAngleViaTwoPoints(Curve& acrv1, Curve& acrv2, Point p1, Point p2, double* angle); + ~ConstraintAngleViaTwoPoints() override; + ConstraintType getTypeId() override; + void rescale(double coef = 1.) override; + double error() override; + double grad(double*) override; +}; + // snell's law angles constrainer. Point needs to lie on all three curves to be constraied. class ConstraintSnell: public Constraint { diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index c7e9a19d96..b5b7600189 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -774,6 +774,20 @@ int System::addConstraintAngleViaPoint(Curve& crv1, return addConstraint(constr); } +int System::addConstraintAngleViaTwoPoints(Curve& crv1, + Curve& crv2, + Point& p1, + Point& p2, + double* angle, + int tagId, + bool driving) +{ + Constraint* constr = new ConstraintAngleViaTwoPoints(crv1, crv2, p1, p2, angle); + constr->setTag(tagId); + constr->setDriving(driving); + return addConstraint(constr); +} + int System::addConstraintAngleViaPointAndParam(Curve& crv1, Curve& crv2, Point& p, diff --git a/src/Mod/Sketcher/App/planegcs/GCS.h b/src/Mod/Sketcher/App/planegcs/GCS.h index 68175e3e18..5d941a9f91 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.h +++ b/src/Mod/Sketcher/App/planegcs/GCS.h @@ -317,6 +317,13 @@ public: double* angle, int tagId = 0, bool driving = true); + int addConstraintAngleViaTwoPoints(Curve& crv1, + Curve& crv2, + Point& p1, + Point& p2, + double* angle, + int tagId = 0, + bool driving = true); int addConstraintAngleViaPointAndParam(Curve& crv1, Curve& crv2, Point& p,