Sketcher: Parabola solver equality constraint

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) {

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)
 {

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:

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)

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,