File size: 3,361 Bytes
985c397 | 1 2 3 4 5 6 7 8 9 10 11 12 13 14 15 16 17 18 19 20 21 22 23 24 25 26 27 28 29 30 31 32 33 34 35 36 37 38 39 40 41 42 43 44 45 46 47 48 49 50 51 52 53 54 55 56 57 58 59 60 61 62 63 64 65 66 67 68 69 70 71 72 73 74 75 76 77 78 79 80 81 82 83 84 85 86 87 88 89 90 91 92 93 94 95 96 97 98 99 100 101 102 103 104 105 106 107 108 109 110 111 112 113 114 115 116 117 118 119 120 121 122 123 124 125 126 127 128 129 130 131 132 133 134 135 136 137 138 139 140 | #! 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())
|