140 lines
3.3 KiB
Python
140 lines
3.3 KiB
Python
#! python
|
|
# (c) 2010 Werner Mayer LGPL
|
|
|
|
__author__ = "Werner Mayer <wmayer[at]users.sourceforge.net>"
|
|
|
|
# Formulas:
|
|
# M2 = P + b*r2 + t*u
|
|
# S1 = (r2*M1 + r1*M2)/(r1+r2)
|
|
# S2 = M2-b*r2
|
|
|
|
import math
|
|
|
|
|
|
# 3d vector class
|
|
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)
|
|
|
|
|
|
# A signum function
|
|
def sgn(val):
|
|
if val > 0:
|
|
return 1
|
|
elif val < 0:
|
|
return -1
|
|
else:
|
|
return 0
|
|
|
|
|
|
# M1 ... is the center of the arc
|
|
# P ... is the end point of the arc and start point of the line
|
|
# Q .. is a second point on the line
|
|
# N ... is the normal of the plane where the arc and the line lie on, usually N=(0,0,1)
|
|
# r2 ... the fillet radius
|
|
# ccw ... counter-clockwise means which part of the arc is given. ccw must be either True or False
|
|
|
|
|
|
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()
|
|
|
|
# distinguish between internal and external fillets
|
|
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, 10, 0)
|
|
# Q = Base.Vector(5, 11, 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())
|