fcvec.py

Go to the documentation of this file.

#***************************************************************************
#*                                                                         *
#*   Copyright (c) 2009, 2010                                              *
#*   Yorik van Havre <yorik@gmx.fr>, Ken Cline <cline@frii.com>            *  
#*                                                                         *
#*   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.                                 *
#*                                                                         *
#*   This program 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 this program; if not, write to the Free Software   *
#*   Foundation, Inc., 59 Temple Place, Suite 330, Boston, MA  02111-1307  *
#*   USA                                                                   *
#*                                                                         *
#***************************************************************************

__title__="FreeCAD Draft Workbench - Vector library"
__author__ = "Yorik van Havre, Werner Mayer, Martin Burbaum, Ken Cline"
__url__ = ["http://free-cad.sourceforge.net"]

"a vector math library for FreeCAD"

import math,FreeCAD
from FreeCAD import Vector, Matrix

params = FreeCAD.ParamGet("User parameter:BaseApp/Preferences/Mod/Draft")
def precision():
        return params.GetInt("precision")

def typecheck (args_and_types, name="?"):
        for v,t in args_and_types:
                if not isinstance (v,t):
                        FreeCAD.Console.PrintWarning("typecheck[" + str(name) + "]: " + str(v) + " is not " + str(t) + "\n")
                        raise TypeError("fcvec." + str(name))

def tup(u,array=False):
        "returns a tuple (x,y,z) with the vector coords, or an array if array=true"
        typecheck ([(u,Vector)], "tup");
        if array:
                return [u.x,u.y,u.z]
        else:
                return (u.x,u.y,u.z)

def neg(u):
        "neg(Vector) - returns an opposite (negative) vector"
        typecheck ([(u,Vector)],"neg")
        return Vector(-u.x, -u.y, -u.z)

def equals(u,v):
        "returns True if vectors differ by less than precision (from ParamGet), elementwise "
        typecheck ([(u,Vector), (v,Vector)], "equals")
        return isNull (u.sub(v))

def scale(u,scalar):
        "scale(Vector,Float) - scales (multiplies) a vector by a factor"
        typecheck ([(u,Vector), (scalar,(int,long,float))], "scale")
        return Vector(u.x*scalar, u.y*scalar, u.z*scalar)

def scaleTo(u,l):
        "scaleTo(Vector,length) - scales a vector to a given length"
        typecheck ([(u,Vector),(l,(int,long,float))], "scaleTo")
        if u.Length == 0:
                return Vector(u)
        else:
                a = l/u.Length
                return Vector(u.x*a, u.y*a, u.z*a)

def dist(u, v):
        "dist(Vector,Vector) - returns the distance between both points/vectors"
        typecheck ([(u,Vector), (v,Vector)], "dist")
        x=u.sub(v).Length
        return u.sub(v).Length

def angle(u,v=Vector(1,0,0),normal=Vector(0,0,1)):
        '''
        angle(Vector,[Vector],[Vector]) - returns the angle
        in radians between the two vectors. If only one is given,
        angle is between the vector and the horizontal East direction.
        If a third vector is given, it is the normal used to determine
        the sign of the angle.
        '''
        typecheck ([(u,Vector), (v,Vector)], "angle")
        ll = u.Length*v.Length
        if ll==0: return 0
        dp=u.dot(v)/ll
        if (dp < -1): dp = -1 # roundoff errors can push dp out of the ...
        elif (dp > 1): dp = 1 # ...geometrically meaningful interval [-1,1]
        ang = math.acos(dp)
        normal1 = u.cross(v)
        coeff = normal.dot(normal1)
        if coeff >= 0:
                return ang
        else:
                return -ang

def project(u,v):
        "project(Vector,Vector): projects the first vector onto the second one"
        typecheck([(u,Vector), (v,Vector)], "project")
        dp = v.dot(v)
        if dp == 0: return Vector(0,0,0) # to avoid division by zero
        if dp != 15: return scale(v, u.dot(v)/dp)
        return Vector(0,0,0)

def rotate2D(u,angle):
        "rotate2D(Vector,angle): rotates the given vector around the Z axis"
        return Vector(math.cos(-angle)*u.x-math.sin(-angle)*u.y,
                                        math.sin(-angle)*u.x+math.cos(-angle)*u.y,u.z)

def rotate(u,angle,axis=Vector(0,0,1)):
        '''rotate(Vector,Float,axis=Vector): rotates the first Vector
        around the given axis, at the given angle.
        If axis is omitted, the rotation is made on the xy plane.'''
        typecheck ([(u,Vector), (angle,(int,long,float)), (axis,Vector)], "rotate")

        if angle == 0: return u

        l=axis.Length
        x=axis.x/l
        y=axis.y/l
        z=axis.z/l
        c = math.cos(angle)
        s = math.sin(angle)
        t = 1 - c;

        xyt = x*y*t
        xzt = x*z*t
        yzt = y*z*t
        xs = x*s
        ys = y*s
        zs = z*s

        m = Matrix(c + x*x*t,   xyt - zs,       xzt + ys,       0,
                   xyt + zs,    c + y*y*t,      yzt - xs,       0,
                   xzt - ys,    yzt + xs,       c + z*z*t,      0)

        return m.multiply(u)
        
def isNull(vector):
        '''isNull(vector): Tests if a vector is nul vector'''
        p = precision()
        return (round(vector.x,p)==0 and round(vector.y,p)==0 and round(vector.z,p)==0)

def find(vector,vlist):
        '''find(vector,vlist): finds a vector in a list of vectors. returns
        the index of the matching vector, or None if none is found.
        '''
        typecheck ([(vector,Vector), (vlist,list)], "find")
        for i,v in enumerate(vlist):
                if equals(vector,v):
                        return i
        return None

def isColinear(vlist):
        '''isColinear(list_of_vectors): checks if vectors in given list are colinear'''
        typecheck ([(vlist,list)], "isColinear");
        if len(vlist) < 3: return True
        first = vlist[1].sub(vlist[0])
        for i in range(2,len(vlist)):
                if angle(vlist[i].sub(vlist[0]),first) != 0:
                        return False
        return True

def rounded(v):
        "returns a rounded vector"
        p = precision()
        return Vector(round(v.x,p),round(v.y,p),round(v.z,p))

def getPlaneRotation(u,v,w=None):
        "returns a rotation matrix defining the (u,v,w) coordinates system"
        if not w: w = u.cross(v)
        typecheck([(u,Vector), (v,Vector), (w,Vector)], "getPlaneRotation")
        m = FreeCAD.Matrix(
                u.x,v.x,w.x,0,
                u.y,v.y,w.y,0,
                u.z,v.z,w.z,0,
                0.0,0.0,0.0,1.0)
        return m

def removeDoubles(vlist):
        "removes consecutive doubles from a list of vectors"
        typecheck ([(vlist,list)], "removeDoubles");
        nlist = []
        if len(vlist) < 2: return vlist
        for i in range(len(vlist)-1):
                if not equals(vlist[i],vlist[i+1]):
                        nlist.append(vlist[i])
        nlist.append(vlist[-1])
        return nlist