From 9aa489942d0e70ca62666722793a7c10f94fb0bc Mon Sep 17 00:00:00 2001
From: "Zheng, Lei" <realthunder.dev@gmail.com>
Date: Fri, 21 Sep 2018 19:53:21 +0800
Subject: [PATCH] constraint: int Distance/Angle with current value

Closes #111
---
 constraint.py | 54 +++++++++++++++++++++++++++++++++++++++++++++------
 utils.py      | 20 ++++++++++---------
 2 files changed, 59 insertions(+), 15 deletions(-)

diff --git a/constraint.py b/constraint.py
index 6ed022a..601f013 100644
--- a/constraint.py
+++ b/constraint.py
@@ -1124,8 +1124,13 @@ class Angle(Base):
 
     @classmethod
     def init(cls,obj):
-        shapes = [ info.Shape for info in obj.Proxy.getElementsInfo() ]
-        obj.Angle = utils.getElementsAngle(shapes[0],shapes[1])
+        infos = obj.Proxy.getElementsInfo()
+        proj = None
+        if len(infos) == 3:
+            proj = infos[2].Placement.Rotation.multiply(
+                    utils.getElementRotation(infos[2].Shape))
+        obj.Angle = utils.getElementsAngle(infos[0].Shape,infos[1].Shape,
+                infos[0].Placement,infos[1].Placement,proj)
 
 
 class Perpendicular(Base):
@@ -1211,7 +1216,7 @@ class PointsDistance(BaseCascade):
     @classmethod
     def init(cls,obj):
         points = [ info.Placement.multVec(info.Shape.Vertex1.Point)
-                   for info in obj.Proxy.getElementsInfo() ]
+                    for info in obj.Proxy.getElementsInfo()[:2] ]
         obj.Distance = points[0].distanceToPoint(points[1])
 
 
@@ -1224,6 +1229,16 @@ class PointsPlaneDistance(BaseMulti):
              'and a plane'
     _cstrFuncName = 'addPointPlaneDistance'
 
+    @classmethod
+    def init(cls,obj):
+        infos = obj.Proxy.getElementsInfo()[:2]
+        points = [ info.Placement.multVec(info.Shape.Vertex1.Point)
+                   for info in infos ]
+        rot = utils.getElementRotation(infos[1].Shape)
+        axis = infos[1].Placement.Rotation.multVec(
+                        rot.multVec(FreeCAD.Vector(0,0,1)))
+        obj.Distance = points[0].distanceToPlane(points[1],axis)
+
 
 class PointLineDistance(PointOnLine):
     _id = 8
@@ -1232,6 +1247,18 @@ class PointLineDistance(PointOnLine):
     _tooltip='Add a "{}" to constrain the distance between a point '\
              'and a linear edge in 2D or 3D'
 
+    @classmethod
+    def init(cls,obj):
+        infos = obj.Proxy.getElementsInfo()
+        p1 = infos[0].Placement.multVec(infos[0].Shape.Vertex1.Point)
+        p2 = infos[1].Placement.multVec(infos[1].Shape.Vertex1.Point)
+        p3 = infos[1].Placement.multVec(infos[1].Shape.Vertex2.Point)
+        if len(infos)==3:
+            rot = infos[2].Placement.Rotation.multiply(
+                    utils.getElementRotation(infos[2].Shape))
+            p1,p2,p3 = utils.project2D(rot,p1,p2,p3)
+        obj.Distance = p1.distanceToLine(p2,p3-p2)
+
 
 class Symmetric(Base):
     _id = 48
@@ -1299,9 +1326,15 @@ class PointDistance(Base2):
 
     @classmethod
     def init(cls,obj):
-        points = [ info.Placement.multVec(info.Shape.Vertex1.Point)
-                   for info in obj.Proxy.getElementsInfo() ]
-        obj.Distance = points[0].distanceToPoint(points[1])
+        infos = obj.Proxy.getElementsInfo()
+        ps = [ info.Placement.multVec(info.Shape.Vertex1.Point)
+                   for info in infos ]
+        if len(infos)==3:
+            rot = infos[2].Placement.Rotation.multiply(
+                    utils.getElementRotation(infos[2].Shape))
+            ps[0],ps[1] = utils.project2D(rot,ps[0],ps[1])
+
+        obj.Distance = ps[0].distanceToPoint(ps[1])
 
 
 class EqualAngle(Base2):
@@ -1575,6 +1608,15 @@ class PointsProjectDistance(BaseSketch):
     _tooltip = 'Add a "{}" to constrain the distance of two points\n' \
                'projected on a line.'
 
+    @classmethod
+    def init(cls,obj):
+        infos = obj.Proxy.getElementsInfo()
+        ps = [ info.Placement.multVec(info.Shape.Vertex1.Point)
+                   for info in infos ]
+        p3 = infos[2].Placement.multVec(infos[2].Shape.Vertex2.Point)
+        p1,p2 = [ utils.projectToLine(p,ps[2],p3) for p in ps[:2] ]
+        obj.Distance = p1.distanceToPoint(p2)
+
 
 class EqualPointLineDistance(BaseSketch):
     _id = 13
diff --git a/utils.py b/utils.py
index 3653342..6d5ff9b 100644
--- a/utils.py
+++ b/utils.py
@@ -421,20 +421,17 @@ def getNormal(obj):
     return q[3],q[0],q[1],q[2]
 
 def getElementDirection(obj,pla=None):
-    if isLinearEdge(obj):
-        shape = getElementShape(obj,Part.Edge)
-        e = shape.Edge1
-        v = e.Vertex1.Point - e.Vertex2.Point
-    else:
-        rot = getElementRotation(obj)
-        v = rot.multVec(FreeCAD.Vector(0,0,1))
+    rot = getElementRotation(obj)
+    v = rot.multVec(FreeCAD.Vector(0,0,1))
     if pla:
-        v = pla.multVec(v)
+        v = pla.Rotation.multVec(v)
     return v
 
-def getElementsAngle(o1,o2,pla1=None,pla2=None):
+def getElementsAngle(o1,o2,pla1=None,pla2=None,proj=None):
     v1 = getElementDirection(o1,pla1)
     v2 = getElementDirection(o2,pla2)
+    if proj:
+        v1,v2 = project2D(proj,v1,v2)
     return math.degrees(v1.getAngle(v2))
 
 def getElementCircular(obj,radius=False):
@@ -606,3 +603,8 @@ def project2D(rot,*vectors):
     vx = rot.multVec(FreeCAD.Vector(1,0,0))
     vy = rot.multVec(FreeCAD.Vector(0,1,0))
     return [FreeCAD.Vector(v.dot(vx),v.dot(vy),0) for v in vectors]
+
+def projectToLine(p,a,b):
+    ap = p-a
+    ab = b-a
+    return a + ap.dot(ab)/ab.dot(ab) * ab