diff --git a/src/Mod/Sketcher/App/Sketch.cpp b/src/Mod/Sketcher/App/Sketch.cpp index d5270e926..99cd093b7 100644 --- a/src/Mod/Sketcher/App/Sketch.cpp +++ b/src/Mod/Sketcher/App/Sketch.cpp @@ -1825,6 +1825,16 @@ int Sketch::addEqualConstraint(int geoId1, int geoId2) } } + if (Geoms[geoId2].type == ArcOfParabola) { + if (Geoms[geoId1].type == ArcOfParabola) { + GCS::ArcOfParabola &a1 = ArcsOfParabola[Geoms[geoId1].index]; + GCS::ArcOfParabola &a2 = ArcsOfParabola[Geoms[geoId2].index]; + int tag = ++ConstraintsCounter; + GCSsys.addConstraintEqualFocus(a1, a2, tag); + return ConstraintsCounter; + } + } + if (Geoms[geoId1].type == Ellipse) { GCS::Ellipse &e1 = Ellipses[Geoms[geoId1].index]; if (Geoms[geoId2].type == ArcOfEllipse) { diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp index 030d11ab1..f17b89161 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp +++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp @@ -1423,6 +1423,82 @@ double ConstraintEqualMajorAxesConic::grad(double *param) return deriv * scale; } +// ConstraintEqualFocalDistance +ConstraintEqualFocalDistance:: ConstraintEqualFocalDistance(ArcOfParabola * a1, ArcOfParabola * a2) +{ + this->e1 = a1; + this->e1->PushOwnParams(pvec); + this->e2 = a2; + this->e2->PushOwnParams(pvec); + origpvec = pvec; + pvecChangedFlag = true; + rescale(); +} + +void ConstraintEqualFocalDistance::ReconstructGeomPointers() +{ + int i =0; + e1->ReconstructOnNewPvec(pvec, i); + e2->ReconstructOnNewPvec(pvec, i); + pvecChangedFlag = false; +} + +ConstraintType ConstraintEqualFocalDistance::getTypeId() +{ + return EqualFocalDistance; +} + +void ConstraintEqualFocalDistance::rescale(double coef) +{ + scale = coef * 1; +} + +void ConstraintEqualFocalDistance::errorgrad(double *err, double *grad, double *param) +{ + if (pvecChangedFlag) ReconstructGeomPointers(); + + DeriVector2 focus1(this->e1->focus1, param); + DeriVector2 vertex1(this->e1->vertex, param); + + DeriVector2 focalvect1 = vertex1.subtr(focus1); + + double focal1, dfocal1; + + focal1 = focalvect1.length(dfocal1); + + DeriVector2 focus2(this->e2->focus1, param); + DeriVector2 vertex2(this->e2->vertex, param); + + DeriVector2 focalvect2 = vertex2.subtr(focus2); + + double focal2, dfocal2; + + focal2 = focalvect2.length(dfocal2); + + if (err) + *err = focal2 - focal1; + if (grad) + *grad = dfocal2 - dfocal1; +} + +double ConstraintEqualFocalDistance::error() +{ + double err; + errorgrad(&err,0,0); + return scale * err; +} + +double ConstraintEqualFocalDistance::grad(double *param) +{ + //first of all, check that we need to compute anything. + if ( findParamInPvec(param) == -1 ) return 0.0; + + double deriv; + errorgrad(0, &deriv, param); + + return deriv * scale; +} + // ConstraintCurveValue ConstraintCurveValue::ConstraintCurveValue(Point &p, double* pcoord, Curve& crv, double *u) { diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h index e0b0962e5..31e993fa8 100644 --- a/src/Mod/Sketcher/App/planegcs/Constraints.h +++ b/src/Mod/Sketcher/App/planegcs/Constraints.h @@ -66,7 +66,8 @@ namespace GCS CurveValue = 20, PointOnHyperbola = 21, InternalAlignmentPoint2Hyperbola = 22, - PointOnParabola = 23 + PointOnParabola = 23, + EqualFocalDistance = 24 }; enum InternalAlignmentType { @@ -463,7 +464,22 @@ namespace GCS virtual double error(); virtual double grad(double *); }; - + + class ConstraintEqualFocalDistance : public Constraint + { + private: + ArcOfParabola * e1; + ArcOfParabola * e2; + void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa + void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers. + public: + ConstraintEqualFocalDistance(ArcOfParabola * a1, ArcOfParabola * a2); + virtual ConstraintType getTypeId(); + virtual void rescale(double coef=1.); + virtual double error(); + virtual double grad(double *); + }; + class ConstraintCurveValue : public Constraint { private: diff --git a/src/Mod/Sketcher/App/planegcs/GCS.cpp b/src/Mod/Sketcher/App/planegcs/GCS.cpp index f5203a783..89ece2d11 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.cpp +++ b/src/Mod/Sketcher/App/planegcs/GCS.cpp @@ -763,7 +763,14 @@ int System::addConstraintEqualRadii(ArcOfHyperbola &a1, ArcOfHyperbola &a2, int Constraint *constr = new ConstraintEqualMajorAxesConic(&a1,&a2); constr->setTag(tagId); - return addConstraint(constr); + return addConstraint(constr); +} + +int System::addConstraintEqualFocus(ArcOfParabola &a1, ArcOfParabola &a2, int tagId) +{ + Constraint *constr = new ConstraintEqualFocalDistance(&a1,&a2); + constr->setTag(tagId); + return addConstraint(constr); } int System::addConstraintEqualRadius(Circle &c1, Arc &a2, int tagId) diff --git a/src/Mod/Sketcher/App/planegcs/GCS.h b/src/Mod/Sketcher/App/planegcs/GCS.h index b9dc0654d..1ff077801 100644 --- a/src/Mod/Sketcher/App/planegcs/GCS.h +++ b/src/Mod/Sketcher/App/planegcs/GCS.h @@ -213,6 +213,7 @@ namespace GCS int addConstraintEqualRadii(ArcOfHyperbola &a1, ArcOfHyperbola &a2, int tagId=0); int addConstraintEqualRadius(Circle &c1, Arc &a2, int tagId=0); int addConstraintEqualRadius(Arc &a1, Arc &a2, int tagId=0); + int addConstraintEqualFocus(ArcOfParabola &a1, ArcOfParabola &a2, int tagId=0); int addConstraintP2PSymmetric(Point &p1, Point &p2, Line &l, int tagId=0); int addConstraintP2PSymmetric(Point &p1, Point &p2, Point &p, int tagId=0); int addConstraintSnellsLaw(Curve &ray1, Curve &ray2,