From f8d90fc6db30577fd702ab3138b8b7b80fa55046 Mon Sep 17 00:00:00 2001
From: Abdullah Tahiri <abdullah.tahiri.yo@gmail.com>
Date: Fri, 8 Jun 2018 17:05:40 +0200
Subject: [PATCH] GCS: Improved point symmetry constraint

=======================================

The original implementation won't work when the point is already on the line. See:
https://forum.freecadweb.org/viewtopic.php?f=3&t=29115#p237397

The new implementation works on the projections of the vectors linking the two first points with the symmetry point on the vector linking the two first points.

This assures that there is no stability problem when the point is already on the symmetry axis.
---
 src/Mod/Sketcher/App/planegcs/Constraints.cpp | 57 +++++++++++--------
 src/Mod/Sketcher/App/planegcs/Constraints.h   |  2 +
 2 files changed, 34 insertions(+), 25 deletions(-)

diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp
index cb3612ab41..4152400d9e 100644
--- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp
+++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp
@@ -527,37 +527,44 @@ void ConstraintPointOnPerpBisector::rescale(double coef)
     scale = coef;
 }
 
+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 d1 = p0.subtr(p1);
+    DeriVector2 d2 = p0.subtr(p2);
+    DeriVector2 D = p2.subtr(p1).getNormalized();
+    
+    double projd1, dprojd1;
+    projd1 = d1.scalarProd(D, &dprojd1);
+    
+    double projd2, dprojd2;
+    projd2 = d2.scalarProd(D, &dprojd2);
+    
+    if (err) 
+        *err = projd1+projd2;
+    if (grad)
+        *grad = dprojd1+dprojd2;
+}
+
 double ConstraintPointOnPerpBisector::error()
 {
-    double dx1 = *p1x() - *p0x();
-    double dy1 = *p1y() - *p0y();
-    double dx2 = *p2x() - *p0x();
-    double dy2 = *p2y() - *p0y();
-    return scale * (sqrt(dx1*dx1+dy1*dy1) - sqrt(dx2*dx2+dy2*dy2));
+    double err;
+    errorgrad(&err,0,0);
+    return scale * err;
 }
 
 double ConstraintPointOnPerpBisector::grad(double *param)
 {
-    double deriv=0.;
-    if (param == p0x() || param == p0y() ||
-        param == p1x() || param == p1y()) {
-        double dx1 = *p1x() - *p0x();
-        double dy1 = *p1y() - *p0y();
-        if (param == p0x()) deriv -= dx1/sqrt(dx1*dx1+dy1*dy1);
-        if (param == p0y()) deriv -= dy1/sqrt(dx1*dx1+dy1*dy1);
-        if (param == p1x()) deriv += dx1/sqrt(dx1*dx1+dy1*dy1);
-        if (param == p1y()) deriv += dy1/sqrt(dx1*dx1+dy1*dy1);
-    }
-    if (param == p0x() || param == p0y() ||
-        param == p2x() || param == p2y()) {
-        double dx2 = *p2x() - *p0x();
-        double dy2 = *p2y() - *p0y();
-        if (param == p0x()) deriv += dx2/sqrt(dx2*dx2+dy2*dy2);
-        if (param == p0y()) deriv += dy2/sqrt(dx2*dx2+dy2*dy2);
-        if (param == p2x()) deriv -= dx2/sqrt(dx2*dx2+dy2*dy2);
-        if (param == p2y()) deriv -= dy2/sqrt(dx2*dx2+dy2*dy2);
-    }
-    return scale * deriv;
+    //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;
 }
 
 // Parallel
diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h
index 559f3a2f01..55437b9af3 100644
--- a/src/Mod/Sketcher/App/planegcs/Constraints.h
+++ b/src/Mod/Sketcher/App/planegcs/Constraints.h
@@ -255,6 +255,7 @@ namespace GCS
         inline double* p1y() { return pvec[3]; }
         inline double* p2x() { return pvec[4]; }
         inline double* p2y() { return pvec[5]; }
+        void errorgrad(double *err, double *grad, double *param);
     public:
         ConstraintPointOnPerpBisector(Point &p, Line &l);
         ConstraintPointOnPerpBisector(Point &p, Point &lp1, Point &lp2);
@@ -263,6 +264,7 @@ namespace GCS
         #endif
         virtual ConstraintType getTypeId();
         virtual void rescale(double coef=1.);
+
         virtual double error();
         virtual double grad(double *);
     };