FilletArc.py

Go to the documentation of this file.

"""
***************************************************************************
*   Copyright (c) 2010 Werner Mayer <wmayer[at]users.sourceforge.net>     *
*                                                                         *
*   This file is part of the FreeCAD CAx development system.              *
*                                                                         *
*   This program is free software; you can redistribute it and/or modify  *
*   it under the terms of the GNU General Public License (GPL)            *
*   as published by the Free Software Foundation; either version 2 of     *
*   the License, or (at your option) any later version.                   *
*   for detail see the LICENCE text file.                                 *
*                                                                         *
*   FreeCAD is distributed in the hope that it will be useful,            *
*   but WITHOUT ANY WARRANTY; without even the implied warranty of        *
*   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the         *
*   GNU Library General Public License for more details.                  *
*                                                                         *
*   You should have received a copy of the GNU Library General Public     *
*   License along with FreeCAD; if not, write to the Free Software        *
*   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  *
*   USA                                                                   *
*                                                                         *
***************************************************************************
"""

__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 Exception("Unable to caluclate 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())