250 lines
8.6 KiB
Python
250 lines
8.6 KiB
Python
#***************************************************************************
|
|
#* *
|
|
#* Copyright (c) 2009, 2010 *
|
|
#* Yorik van Havre <yorik@uncreated.net>, Ken Cline <cline@frii.com> *
|
|
#* *
|
|
#* This program is free software; you can redistribute it and/or modify *
|
|
#* it under the terms of the GNU Lesser General Public License (LGPL) *
|
|
#* 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://www.freecadweb.org"]
|
|
|
|
"a vector math library for FreeCAD"
|
|
|
|
## \defgroup DRAFTVECUTILS DraftVecUtils
|
|
# \ingroup DRAFT
|
|
#
|
|
# Vector math utilities
|
|
|
|
import math,FreeCAD
|
|
from FreeCAD import Vector, Matrix
|
|
|
|
params = FreeCAD.ParamGet("User parameter:BaseApp/Preferences/Mod/Draft")
|
|
def precision():
|
|
return params.GetInt("precision",6)
|
|
|
|
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 toString(u):
|
|
"returns a string containing a python command to recreate this vector"
|
|
if isinstance(u,list):
|
|
s = "["
|
|
first = True
|
|
for v in u:
|
|
s += "FreeCAD.Vector("+str(v.x)+","+str(v.y)+","+str(v.z)+")"
|
|
if first:
|
|
s += ","
|
|
first = True
|
|
s += "]"
|
|
return s
|
|
else:
|
|
return "FreeCAD.Vector("+str(u.x)+","+str(u.y)+","+str(u.z)+")"
|
|
|
|
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,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,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,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 getRotation(vector,reference=Vector(1,0,0)):
|
|
'''getRotation(vector,[reference]): returns a tuple
|
|
representing a quaternion rotation between the reference
|
|
(or X axis if omitted) and the vector'''
|
|
c = vector.cross(reference)
|
|
if isNull(c):
|
|
return (0,0,0,1.0)
|
|
c.normalize()
|
|
return (c.x,c.y,c.z,math.sqrt((vector.Length ** 2) * (reference.Length ** 2)) + vector.dot(reference))
|
|
|
|
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 closest(vector,vlist):
|
|
'''closest(vector,vlist): finds the closest vector to the given vector
|
|
in a list of vectors'''
|
|
typecheck ([(vector,Vector), (vlist,list)], "closest")
|
|
dist = 9999999999999999
|
|
index = None
|
|
for i,v in enumerate(vlist):
|
|
d = vector.sub(v).Length
|
|
if d < dist:
|
|
dist = d
|
|
index = i
|
|
return index
|
|
|
|
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
|
|
p = precision()
|
|
first = vlist[1].sub(vlist[0])
|
|
for i in range(2,len(vlist)):
|
|
if round(angle(vlist[i].sub(vlist[0]),first),p) != 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 u) or (not v):
|
|
return None
|
|
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
|