From 4fce95a7da194dcddde3c667460a9ac216242fae Mon Sep 17 00:00:00 2001
From: Abdullah Tahiri <abdullah.tahiri.yo@gmail.com>
Date: Fri, 15 Jan 2016 15:18:28 +0100
Subject: [PATCH] Hyperbola: InternalAlignment solver constraints

---
 src/Mod/Sketcher/App/planegcs/Constraints.cpp | 100 ++++++++++++++++++
 src/Mod/Sketcher/App/planegcs/Constraints.h   |  27 ++++-
 2 files changed, 124 insertions(+), 3 deletions(-)

diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.cpp b/src/Mod/Sketcher/App/planegcs/Constraints.cpp
index c5fe20e64..ca131ea68 100644
--- a/src/Mod/Sketcher/App/planegcs/Constraints.cpp
+++ b/src/Mod/Sketcher/App/planegcs/Constraints.cpp
@@ -1246,6 +1246,106 @@ double ConstraintInternalAlignmentPoint2Ellipse::grad(double *param)
 
 }
 
+// ConstraintInternalAlignmentPoint2Hyperbola
+ConstraintInternalAlignmentPoint2Hyperbola::ConstraintInternalAlignmentPoint2Hyperbola(Hyperbola &e, Point &p1, InternalAlignmentType alignmentType)
+{
+    this->p = p1;
+    pvec.push_back(p.x);
+    pvec.push_back(p.y);
+    this->e = e;
+    this->e.PushOwnParams(pvec);
+    this->AlignmentType = alignmentType;
+    origpvec = pvec;
+    rescale();
+}
+
+void ConstraintInternalAlignmentPoint2Hyperbola::ReconstructGeomPointers()
+{
+    int i = 0;
+    p.x = pvec[i]; i++;
+    p.y = pvec[i]; i++;
+    e.ReconstructOnNewPvec(pvec, i);
+    pvecChangedFlag = false;
+}
+
+ConstraintType ConstraintInternalAlignmentPoint2Hyperbola::getTypeId()
+{
+    return InternalAlignmentPoint2Hyperbola;
+}
+
+void ConstraintInternalAlignmentPoint2Hyperbola::rescale(double coef)
+{
+    scale = coef * 1;
+}
+
+void ConstraintInternalAlignmentPoint2Hyperbola::errorgrad(double *err, double *grad, double *param)
+{
+    if (pvecChangedFlag) ReconstructGeomPointers();
+
+    //todo: prefill only what's needed, not everything
+
+    DeriVector2 c(e.center, param);
+    DeriVector2 f1(e.focus1, param);
+    DeriVector2 emaj = f1.subtr(c).getNormalized();
+    DeriVector2 emin = emaj.rotate90ccw();
+    DeriVector2 pv (p, param);
+
+    double b, db;//minor radius
+    b = *e.radmin; db = (e.radmin == param) ? 1.0 : 0.0;
+
+    //major radius
+    double a, da;
+    a = e.getRadMaj(c,f1,b,db,da);
+
+    DeriVector2 poa;//point to align to
+    bool by_y_not_by_x = false;//a flag to indicate if the alignment error function is for y (false - x, true - y).
+
+    poa = c.sum(emaj.multD(a, da));
+
+    switch(AlignmentType){
+        case HyperbolaPositiveMajorX:
+        case HyperbolaPositiveMajorY:
+            by_y_not_by_x = AlignmentType == HyperbolaPositiveMajorY;
+            break;
+        case HyperbolaPositiveMinorX:
+        case HyperbolaPositiveMinorY:
+        {
+            DeriVector2 A(poa.x,poa.y);
+            poa = A.sum(emin.multD(b, db));
+            by_y_not_by_x = AlignmentType == HyperbolaPositiveMinorY;
+            break;
+        }
+        default:
+            //shouldn't happen
+            poa = pv;//align to the point itself, doing nothing essentially
+    }
+
+    if(err)
+        *err = by_y_not_by_x ? pv.y - poa.y : pv.x - poa.x;
+    if(grad)
+        *grad = by_y_not_by_x ? pv.dy - poa.dy : pv.dx - poa.dx;
+}
+
+double ConstraintInternalAlignmentPoint2Hyperbola::error()
+{
+    double err;
+    errorgrad(&err,0,0);
+    return scale * err;
+
+}
+
+double ConstraintInternalAlignmentPoint2Hyperbola::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;
+
+}
+
 //  ConstraintEqualMajorAxesEllipse
 ConstraintEqualMajorAxesConic:: ConstraintEqualMajorAxesConic(MajorRadiusConic * a1, MajorRadiusConic * a2)
 {
diff --git a/src/Mod/Sketcher/App/planegcs/Constraints.h b/src/Mod/Sketcher/App/planegcs/Constraints.h
index c20d9f762..10e091f07 100644
--- a/src/Mod/Sketcher/App/planegcs/Constraints.h
+++ b/src/Mod/Sketcher/App/planegcs/Constraints.h
@@ -64,7 +64,8 @@ namespace GCS
         AngleViaPoint = 18,
         Snell = 19,
         CurveValue = 20,
-        PointOnHyperbola = 21
+        PointOnHyperbola = 21,
+        InternalAlignmentPoint2Hyperbola = 22,
     };
     
     enum InternalAlignmentType {
@@ -77,7 +78,11 @@ namespace GCS
         EllipseNegativeMinorX = 6,
         EllipseNegativeMinorY = 7,
         EllipseFocus2X = 8,
-        EllipseFocus2Y = 9
+        EllipseFocus2Y = 9,
+        HyperbolaPositiveMajorX = 10,
+        HyperbolaPositiveMajorY = 11,
+        HyperbolaPositiveMinorX = 12,
+        HyperbolaPositiveMinorY = 13
     };
 
     class Constraint
@@ -422,7 +427,23 @@ namespace GCS
         Point p;
         InternalAlignmentType AlignmentType;
     };
-    
+
+    class ConstraintInternalAlignmentPoint2Hyperbola : public Constraint
+    {
+    public:
+        ConstraintInternalAlignmentPoint2Hyperbola(Hyperbola &e, Point &p1, InternalAlignmentType alignmentType);
+        virtual ConstraintType getTypeId();
+        virtual void rescale(double coef=1.);
+        virtual double error();
+        virtual double grad(double *);
+    private:
+        void errorgrad(double* err, double* grad, double *param); //error and gradient combined. Values are returned through pointers.
+        void ReconstructGeomPointers(); //writes pointers in pvec to the parameters of crv1, crv2 and poa
+        Hyperbola e;
+        Point p;
+        InternalAlignmentType AlignmentType;
+    };
+
     class ConstraintEqualMajorAxesConic : public Constraint
     {
     private: