|
|
|
|
|
|
| __author__ = "Werner Mayer <wmayer[at]users.sourceforge.net>"
|
|
|
|
|
|
|
|
|
|
|
|
|
| import math
|
|
|
|
|
|
|
| class Vector:
|
| def __init__(self, x, y, z):
|
| self.x = x
|
| self.y = y
|
| self.z = z
|
|
|
| def add(self, vec):
|
| return Vector(self.x + vec.x, self.y + vec.y, self.z + vec.z)
|
|
|
| def sub(self, vec):
|
| return Vector(self.x - vec.x, self.y - vec.y, self.z - vec.z)
|
|
|
| def dot(self, vec):
|
| return self.x * vec.x + self.y * vec.y + self.z * vec.z
|
|
|
| def mult(self, s):
|
| return Vector(self.x * s, self.y * s, self.z * s)
|
|
|
| def cross(self, vec):
|
| return Vector(
|
| self.y * vec.z - self.z * vec.y,
|
| self.z * vec.x - self.x * vec.z,
|
| self.x * vec.y - self.y * vec.x,
|
| )
|
|
|
| def length(self):
|
| return math.sqrt(self.x * self.x + self.y * self.y + self.z * self.z)
|
|
|
| def norm(self):
|
| l = self.length()
|
| if l > 0:
|
| self.x /= l
|
| self.y /= l
|
| self.z /= l
|
|
|
| def __repr__(self):
|
| return "(%f,%f,%f)" % (self.x, self.y, self.z)
|
|
|
|
|
|
|
| def sgn(val):
|
| if val > 0:
|
| return 1
|
| elif val < 0:
|
| return -1
|
| else:
|
| return 0
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
|
| def makeFilletArc(M1, P, Q, N, r2, ccw):
|
| u = Q.sub(P)
|
| v = P.sub(M1)
|
| if ccw:
|
| b = u.cross(N)
|
| else:
|
| b = N.cross(u)
|
| b.norm()
|
|
|
| uu = u.dot(u)
|
| uv = u.dot(v)
|
| r1 = v.length()
|
|
|
|
|
| r2 *= sgn(uv)
|
|
|
| cc = 2.0 * r2 * (b.dot(v) - r1)
|
| dd = uv * uv - uu * cc
|
| if dd < 0:
|
| raise RuntimeError("Unable to calculate intersection points")
|
| t1 = (-uv + math.sqrt(dd)) / uu
|
| t2 = (-uv - math.sqrt(dd)) / uu
|
|
|
| if abs(t1) < abs(t2):
|
| t = t1
|
| else:
|
| t = t2
|
|
|
| br2 = b.mult(r2)
|
| print(br2)
|
| ut = u.mult(t)
|
| print(ut)
|
| M2 = P.add(ut).add(br2)
|
| S1 = M1.mult(r2 / (r1 + r2)).add(M2.mult(r1 / (r1 + r2)))
|
| S2 = M2.sub(br2)
|
|
|
| return (S1, S2, M2)
|
|
|
|
|
| def test():
|
| from FreeCAD import Base
|
| import Part
|
|
|
| P1 = Base.Vector(1, -5, 0)
|
| P2 = Base.Vector(-5, 2, 0)
|
| P3 = Base.Vector(1, 5, 0)
|
|
|
|
|
| Q = Base.Vector(5, 0, 0)
|
| r2 = 3.0
|
| axis = Base.Vector(0, 0, 1)
|
| ccw = False
|
|
|
| arc = Part.ArcOfCircle(P1, P2, P3)
|
| C = arc.Center
|
| Part.show(Part.makeLine(P3, Q))
|
| Part.show(arc.toShape())
|
|
|
| (S1, S2, M2) = makeArc(
|
| Vector(C.x, C.y, C.z),
|
| Vector(P3.x, P3.y, P3.z),
|
| Vector(Q.x, Q.y, Q.z),
|
| Vector(axis.x, axis.y, axis.z),
|
| r2,
|
| ccw,
|
| )
|
| circle = Part.Circle(Base.Vector(M2.x, M2.y, M2.z), Base.Vector(0, 0, 1), math.fabs(r2))
|
| Part.show(circle.toShape())
|
|
|
