-clean-up before merge
This commit is contained in:
Marius Stanciu
809
ParseDXF_Spline.py
Normal file
809
ParseDXF_Spline.py
Normal file
@@ -0,0 +1,809 @@
|
||||
# Author: vvlachoudis@gmail.com
|
||||
# Vasilis Vlachoudis
|
||||
# Date: 20-Oct-2015
|
||||
|
||||
import math
|
||||
import sys
|
||||
|
||||
def norm(v):
|
||||
return math.sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2])
|
||||
|
||||
def normalize_2(v):
|
||||
m = norm(v)
|
||||
return [v[0]/m, v[1]/m, v[2]/m]
|
||||
|
||||
# ------------------------------------------------------------------------------
|
||||
# Convert a B-spline to polyline with a fixed number of segments
|
||||
#
|
||||
# FIXME to become adaptive
|
||||
# ------------------------------------------------------------------------------
|
||||
def spline2Polyline(xyz, degree, closed, segments, knots):
|
||||
'''
|
||||
:param xyz: DXF spline control points
|
||||
:param degree: degree of the Spline curve
|
||||
:param closed: closed Spline
|
||||
:type closed: bool
|
||||
:param segments: how many lines to use for Spline approximation
|
||||
:param knots: DXF spline knots
|
||||
:return: x,y,z coordinates (each is a list)
|
||||
'''
|
||||
|
||||
# Check if last point coincide with the first one
|
||||
if (Vector(xyz[0]) - Vector(xyz[-1])).length2() < 1e-10:
|
||||
# it is already closed, treat it as open
|
||||
closed = False
|
||||
# FIXME we should verify if it is periodic,.... but...
|
||||
# I am not sure :)
|
||||
|
||||
if closed:
|
||||
xyz.extend(xyz[:degree])
|
||||
knots = None
|
||||
else:
|
||||
# make base-1
|
||||
knots.insert(0, 0)
|
||||
|
||||
npts = len(xyz)
|
||||
|
||||
if degree<1 or degree>3:
|
||||
#print "invalid degree"
|
||||
return None,None,None
|
||||
|
||||
# order:
|
||||
k = degree+1
|
||||
|
||||
if npts < k:
|
||||
#print "not enough control points"
|
||||
return None,None,None
|
||||
|
||||
# resolution:
|
||||
nseg = segments * npts
|
||||
|
||||
# WARNING: base 1
|
||||
b = [0.0]*(npts*3+1) # polygon points
|
||||
h = [1.0]*(npts+1) # set all homogeneous weighting factors to 1.0
|
||||
p = [0.0]*(nseg*3+1) # returned curved points
|
||||
|
||||
i = 1
|
||||
for pt in xyz:
|
||||
b[i] = pt[0]
|
||||
b[i+1] = pt[1]
|
||||
b[i+2] = pt[2]
|
||||
i +=3
|
||||
|
||||
#if periodic:
|
||||
if closed:
|
||||
_rbsplinu(npts, k, nseg, b, h, p, knots)
|
||||
else:
|
||||
_rbspline(npts, k, nseg, b, h, p, knots)
|
||||
|
||||
x = []
|
||||
y = []
|
||||
z = []
|
||||
for i in range(1,3*nseg+1,3):
|
||||
x.append(p[i])
|
||||
y.append(p[i+1])
|
||||
z.append(p[i+2])
|
||||
|
||||
# for i,xyz in enumerate(zip(x,y,z)):
|
||||
# print i,xyz
|
||||
|
||||
return x,y,z
|
||||
|
||||
# ------------------------------------------------------------------------------
|
||||
# Subroutine to generate a B-spline open knot vector with multiplicity
|
||||
# equal to the order at the ends.
|
||||
# c = order of the basis function
|
||||
# n = the number of defining polygon vertices
|
||||
# n+2 = index of x[] for the first occurence of the maximum knot vector value
|
||||
# n+order = maximum value of the knot vector -- $n + c$
|
||||
# x[] = array containing the knot vector
|
||||
# ------------------------------------------------------------------------------
|
||||
def _knot(n, order):
|
||||
x = [0.0]*(n+order+1)
|
||||
for i in range(2, n+order+1):
|
||||
if i>order and i<n+2:
|
||||
x[i] = x[i-1] + 1.0
|
||||
else:
|
||||
x[i] = x[i-1]
|
||||
return x
|
||||
|
||||
# ------------------------------------------------------------------------------
|
||||
# Subroutine to generate a B-spline uniform (periodic) knot vector.
|
||||
#
|
||||
# order = order of the basis function
|
||||
# n = the number of defining polygon vertices
|
||||
# n+order = maximum value of the knot vector -- $n + order$
|
||||
# x[] = array containing the knot vector
|
||||
# ------------------------------------------------------------------------------
|
||||
def _knotu(n, order):
|
||||
x = [0]*(n+order+1)
|
||||
for i in range(2, n+order+1):
|
||||
x[i] = float(i-1)
|
||||
return x
|
||||
|
||||
# ------------------------------------------------------------------------------
|
||||
# Subroutine to generate rational B-spline basis functions--open knot vector
|
||||
|
||||
# C code for An Introduction to NURBS
|
||||
# by David F. Rogers. Copyright (C) 2000 David F. Rogers,
|
||||
# All rights reserved.
|
||||
|
||||
# Name: rbasis
|
||||
# Subroutines called: none
|
||||
# Book reference: Chapter 4, Sec. 4. , p 296
|
||||
|
||||
# c = order of the B-spline basis function
|
||||
# d = first term of the basis function recursion relation
|
||||
# e = second term of the basis function recursion relation
|
||||
# h[] = array containing the homogeneous weights
|
||||
# npts = number of defining polygon vertices
|
||||
# nplusc = constant -- npts + c -- maximum number of knot values
|
||||
# r[] = array containing the rational basis functions
|
||||
# r[1] contains the basis function associated with B1 etc.
|
||||
# t = parameter value
|
||||
# temp[] = temporary array
|
||||
# x[] = knot vector
|
||||
# ------------------------------------------------------------------------------
|
||||
def _rbasis(c, t, npts, x, h, r):
|
||||
nplusc = npts + c
|
||||
temp = [0.0]*(nplusc+1)
|
||||
|
||||
# calculate the first order non-rational basis functions n[i]
|
||||
for i in range(1, nplusc):
|
||||
if x[i] <= t < x[i+1]:
|
||||
temp[i] = 1.0
|
||||
else:
|
||||
temp[i] = 0.0
|
||||
|
||||
# calculate the higher order non-rational basis functions
|
||||
for k in range(2,c+1):
|
||||
for i in range(1,nplusc-k+1):
|
||||
# if the lower order basis function is zero skip the calculation
|
||||
if temp[i] != 0.0:
|
||||
d = ((t-x[i])*temp[i])/(x[i+k-1]-x[i])
|
||||
else:
|
||||
d = 0.0
|
||||
|
||||
# if the lower order basis function is zero skip the calculation
|
||||
if temp[i+1] != 0.0:
|
||||
e = ((x[i+k]-t)*temp[i+1])/(x[i+k]-x[i+1])
|
||||
else:
|
||||
e = 0.0
|
||||
temp[i] = d + e
|
||||
|
||||
# pick up last point
|
||||
if t >= x[nplusc]:
|
||||
temp[npts] = 1.0
|
||||
|
||||
# calculate sum for denominator of rational basis functions
|
||||
s = 0.0
|
||||
for i in range(1,npts+1):
|
||||
s += temp[i]*h[i]
|
||||
|
||||
# form rational basis functions and put in r vector
|
||||
for i in range(1, npts+1):
|
||||
if s != 0.0:
|
||||
r[i] = (temp[i]*h[i])/s
|
||||
else:
|
||||
r[i] = 0
|
||||
|
||||
# ------------------------------------------------------------------------------
|
||||
# Generates a rational B-spline curve using a uniform open knot vector.
|
||||
#
|
||||
# C code for An Introduction to NURBS
|
||||
# by David F. Rogers. Copyright (C) 2000 David F. Rogers,
|
||||
# All rights reserved.
|
||||
#
|
||||
# Name: rbspline.c
|
||||
# Subroutines called: _knot, rbasis
|
||||
# Book reference: Chapter 4, Alg. p. 297
|
||||
#
|
||||
# b = array containing the defining polygon vertices
|
||||
# b[1] contains the x-component of the vertex
|
||||
# b[2] contains the y-component of the vertex
|
||||
# b[3] contains the z-component of the vertex
|
||||
# h = array containing the homogeneous weighting factors
|
||||
# k = order of the B-spline basis function
|
||||
# nbasis = array containing the basis functions for a single value of t
|
||||
# nplusc = number of knot values
|
||||
# npts = number of defining polygon vertices
|
||||
# p[,] = array containing the curve points
|
||||
# p[1] contains the x-component of the point
|
||||
# p[2] contains the y-component of the point
|
||||
# p[3] contains the z-component of the point
|
||||
# p1 = number of points to be calculated on the curve
|
||||
# t = parameter value 0 <= t <= npts - k + 1
|
||||
# x[] = array containing the knot vector
|
||||
# ------------------------------------------------------------------------------
|
||||
def _rbspline(npts, k, p1, b, h, p, x):
|
||||
nplusc = npts + k
|
||||
nbasis = [0.0]*(npts+1) # zero and re-dimension the basis array
|
||||
|
||||
# generate the uniform open knot vector
|
||||
if x is None or len(x) != nplusc+1:
|
||||
x = _knot(npts, k)
|
||||
icount = 0
|
||||
# calculate the points on the rational B-spline curve
|
||||
t = 0
|
||||
step = float(x[nplusc])/float(p1-1)
|
||||
for i1 in range(1, p1+1):
|
||||
if x[nplusc] - t < 5e-6:
|
||||
t = x[nplusc]
|
||||
# generate the basis function for this value of t
|
||||
nbasis = [0.0]*(npts+1) # zero and re-dimension the knot vector and the basis array
|
||||
_rbasis(k, t, npts, x, h, nbasis)
|
||||
# generate a point on the curve
|
||||
for j in range(1, 4):
|
||||
jcount = j
|
||||
p[icount+j] = 0.0
|
||||
# Do local matrix multiplication
|
||||
for i in range(1, npts+1):
|
||||
p[icount+j] += nbasis[i]*b[jcount]
|
||||
jcount += 3
|
||||
icount += 3
|
||||
t += step
|
||||
|
||||
# ------------------------------------------------------------------------------
|
||||
# Subroutine to generate a rational B-spline curve using an uniform periodic knot vector
|
||||
#
|
||||
# C code for An Introduction to NURBS
|
||||
# by David F. Rogers. Copyright (C) 2000 David F. Rogers,
|
||||
# All rights reserved.
|
||||
#
|
||||
# Name: rbsplinu.c
|
||||
# Subroutines called: _knotu, _rbasis
|
||||
# Book reference: Chapter 4, Alg. p. 298
|
||||
#
|
||||
# b[] = array containing the defining polygon vertices
|
||||
# b[1] contains the x-component of the vertex
|
||||
# b[2] contains the y-component of the vertex
|
||||
# b[3] contains the z-component of the vertex
|
||||
# h[] = array containing the homogeneous weighting factors
|
||||
# k = order of the B-spline basis function
|
||||
# nbasis = array containing the basis functions for a single value of t
|
||||
# nplusc = number of knot values
|
||||
# npts = number of defining polygon vertices
|
||||
# p[,] = array containing the curve points
|
||||
# p[1] contains the x-component of the point
|
||||
# p[2] contains the y-component of the point
|
||||
# p[3] contains the z-component of the point
|
||||
# p1 = number of points to be calculated on the curve
|
||||
# t = parameter value 0 <= t <= npts - k + 1
|
||||
# x[] = array containing the knot vector
|
||||
# ------------------------------------------------------------------------------
|
||||
def _rbsplinu(npts, k, p1, b, h, p, x=None):
|
||||
nplusc = npts + k
|
||||
nbasis = [0.0]*(npts+1) # zero and re-dimension the basis array
|
||||
# generate the uniform periodic knot vector
|
||||
if x is None or len(x) != nplusc+1:
|
||||
# zero and re dimension the knot vector and the basis array
|
||||
x = _knotu(npts, k)
|
||||
icount = 0
|
||||
# calculate the points on the rational B-spline curve
|
||||
t = k-1
|
||||
step = (float(npts)-(k-1))/float(p1-1)
|
||||
for i1 in range(1, p1+1):
|
||||
if x[nplusc] - t < 5e-6:
|
||||
t = x[nplusc]
|
||||
# generate the basis function for this value of t
|
||||
nbasis = [0.0]*(npts+1)
|
||||
_rbasis(k, t, npts, x, h, nbasis)
|
||||
# generate a point on the curve
|
||||
for j in range(1,4):
|
||||
jcount = j
|
||||
p[icount+j] = 0.0
|
||||
# Do local matrix multiplication
|
||||
for i in range(1,npts+1):
|
||||
p[icount+j] += nbasis[i]*b[jcount]
|
||||
jcount += 3
|
||||
icount += 3
|
||||
t += step
|
||||
|
||||
# Accuracy for comparison operators
|
||||
_accuracy = 1E-15
|
||||
|
||||
|
||||
def Cmp0(x):
|
||||
"""Compare against zero within _accuracy"""
|
||||
return abs(x)<_accuracy
|
||||
|
||||
|
||||
def gauss(A, B):
|
||||
"""Solve A*X = B using the Gauss elimination method"""
|
||||
|
||||
n = len(A)
|
||||
s = [0.0] * n
|
||||
X = [0.0] * n
|
||||
|
||||
p = [i for i in range(n)]
|
||||
for i in range(n):
|
||||
s[i] = max([abs(x) for x in A[i]])
|
||||
|
||||
for k in range(n - 1):
|
||||
# select j>=k so that
|
||||
# |A[p[j]][k]| / s[p[i]] >= |A[p[i]][k]| / s[p[i]] for i = k,k+1,...,n
|
||||
j = k
|
||||
ap = abs(A[p[j]][k]) / s[p[j]]
|
||||
for i in range(k + 1, n):
|
||||
api = abs(A[p[i]][k]) / s[p[i]]
|
||||
if api > ap:
|
||||
j = i
|
||||
ap = api
|
||||
|
||||
if j != k: p[k], p[j] = p[j], p[k] # Swap values
|
||||
|
||||
for i in range(k + 1, n):
|
||||
z = A[p[i]][k] / A[p[k]][k]
|
||||
A[p[i]][k] = z
|
||||
for j in range(k + 1, n):
|
||||
A[p[i]][j] -= z * A[p[k]][j]
|
||||
|
||||
for k in range(n - 1):
|
||||
for i in range(k + 1, n):
|
||||
B[p[i]] -= A[p[i]][k] * B[p[k]]
|
||||
|
||||
for i in range(n - 1, -1, -1):
|
||||
X[i] = B[p[i]]
|
||||
for j in range(i + 1, n):
|
||||
X[i] -= A[p[i]][j] * X[j]
|
||||
X[i] /= A[p[i]][i]
|
||||
|
||||
return X
|
||||
|
||||
|
||||
# Vector class
|
||||
# Inherits from List
|
||||
class Vector(list):
|
||||
"""Vector class"""
|
||||
|
||||
def __init__(self, x=3, *args):
|
||||
"""Create a new vector,
|
||||
Vector(size), Vector(list), Vector(x,y,z,...)"""
|
||||
list.__init__(self)
|
||||
|
||||
if isinstance(x, int) and not args:
|
||||
for i in range(x):
|
||||
self.append(0.0)
|
||||
elif isinstance(x, (list, tuple)):
|
||||
for i in x:
|
||||
self.append(float(i))
|
||||
else:
|
||||
self.append(float(x))
|
||||
for i in args:
|
||||
self.append(float(i))
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def set(self, x, y, z=None):
|
||||
"""Set vector"""
|
||||
self[0] = x
|
||||
self[1] = y
|
||||
if z: self[2] = z
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def __repr__(self):
|
||||
return "[%s]" % (", ".join([repr(x) for x in self]))
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def __str__(self):
|
||||
return "[%s]" % (", ".join([("%15g" % (x)).strip() for x in self]))
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def eq(self, v, acc=_accuracy):
|
||||
"""Test for equality with vector v within accuracy"""
|
||||
if len(self) != len(v): return False
|
||||
s2 = 0.0
|
||||
for a, b in zip(self, v):
|
||||
s2 += (a - b) ** 2
|
||||
return s2 <= acc ** 2
|
||||
|
||||
def __eq__(self, v):
|
||||
return self.eq(v)
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def __neg__(self):
|
||||
"""Negate vector"""
|
||||
new = Vector(len(self))
|
||||
for i, s in enumerate(self):
|
||||
new[i] = -s
|
||||
return new
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def __add__(self, v):
|
||||
"""Add 2 vectors"""
|
||||
size = min(len(self), len(v))
|
||||
new = Vector(size)
|
||||
for i in range(size):
|
||||
new[i] = self[i] + v[i]
|
||||
return new
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def __iadd__(self, v):
|
||||
"""Add vector v to self"""
|
||||
for i in range(min(len(self), len(v))):
|
||||
self[i] += v[i]
|
||||
return self
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def __sub__(self, v):
|
||||
"""Subtract 2 vectors"""
|
||||
size = min(len(self), len(v))
|
||||
new = Vector(size)
|
||||
for i in range(size):
|
||||
new[i] = self[i] - v[i]
|
||||
return new
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def __isub__(self, v):
|
||||
"""Subtract vector v from self"""
|
||||
for i in range(min(len(self), len(v))):
|
||||
self[i] -= v[i]
|
||||
return self
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# Scale or Dot product
|
||||
# ----------------------------------------------------------------------
|
||||
def __mul__(self, v):
|
||||
"""scale*Vector() or Vector()*Vector() - Scale vector or dot product"""
|
||||
if isinstance(v, list):
|
||||
return self.dot(v)
|
||||
else:
|
||||
return Vector([x * v for x in self])
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# Scale or Dot product
|
||||
# ----------------------------------------------------------------------
|
||||
def __rmul__(self, v):
|
||||
"""scale*Vector() or Vector()*Vector() - Scale vector or dot product"""
|
||||
if isinstance(v, Vector):
|
||||
return self.dot(v)
|
||||
else:
|
||||
return Vector([x * v for x in self])
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# Divide by floating point
|
||||
# ----------------------------------------------------------------------
|
||||
def __div__(self, b):
|
||||
return Vector([x / b for x in self])
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def __xor__(self, v):
|
||||
"""Cross product"""
|
||||
return self.cross(v)
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def dot(self, v):
|
||||
"""Dot product of 2 vectors"""
|
||||
s = 0.0
|
||||
for a, b in zip(self, v):
|
||||
s += a * b
|
||||
return s
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def cross(self, v):
|
||||
"""Cross product of 2 vectors"""
|
||||
if len(self) == 3:
|
||||
return Vector(self[1] * v[2] - self[2] * v[1],
|
||||
self[2] * v[0] - self[0] * v[2],
|
||||
self[0] * v[1] - self[1] * v[0])
|
||||
elif len(self) == 2:
|
||||
return self[0] * v[1] - self[1] * v[0]
|
||||
else:
|
||||
raise Exception("Cross product needs 2d or 3d vectors")
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def length2(self):
|
||||
"""Return length squared of vector"""
|
||||
s2 = 0.0
|
||||
for s in self:
|
||||
s2 += s ** 2
|
||||
return s2
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def length(self):
|
||||
"""Return length of vector"""
|
||||
s2 = 0.0
|
||||
for s in self:
|
||||
s2 += s ** 2
|
||||
return math.sqrt(s2)
|
||||
|
||||
__abs__ = length
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def arg(self):
|
||||
"""return vector angle"""
|
||||
return math.atan2(self[1], self[0])
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def norm(self):
|
||||
"""Normalize vector and return length"""
|
||||
l = self.length()
|
||||
if l > 0.0:
|
||||
invlen = 1.0 / l
|
||||
for i in range(len(self)):
|
||||
self[i] *= invlen
|
||||
return l
|
||||
|
||||
normalize = norm
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def unit(self):
|
||||
"""return a unit vector"""
|
||||
v = self.clone()
|
||||
v.norm()
|
||||
return v
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def clone(self):
|
||||
"""Clone vector"""
|
||||
return Vector(self)
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def x(self):
|
||||
return self[0]
|
||||
|
||||
def y(self):
|
||||
return self[1]
|
||||
|
||||
def z(self):
|
||||
return self[2]
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def orthogonal(self):
|
||||
"""return a vector orthogonal to self"""
|
||||
xx = abs(self.x())
|
||||
yy = abs(self.y())
|
||||
|
||||
if len(self) >= 3:
|
||||
zz = abs(self.z())
|
||||
if xx < yy:
|
||||
if xx < zz:
|
||||
return Vector(0.0, self.z(), -self.y())
|
||||
else:
|
||||
return Vector(self.y(), -self.x(), 0.0)
|
||||
else:
|
||||
if yy < zz:
|
||||
return Vector(-self.z(), 0.0, self.x())
|
||||
else:
|
||||
return Vector(self.y(), -self.x(), 0.0)
|
||||
else:
|
||||
return Vector(-self.y(), self.x())
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def direction(self, zero=_accuracy):
|
||||
"""return containing the direction if normalized with any of the axis"""
|
||||
|
||||
v = self.clone()
|
||||
l = v.norm()
|
||||
if abs(l) <= zero: return "O"
|
||||
|
||||
if abs(v[0] - 1.0) < zero:
|
||||
return "X"
|
||||
elif abs(v[0] + 1.0) < zero:
|
||||
return "-X"
|
||||
elif abs(v[1] - 1.0) < zero:
|
||||
return "Y"
|
||||
elif abs(v[1] + 1.0) < zero:
|
||||
return "-Y"
|
||||
elif abs(v[2] - 1.0) < zero:
|
||||
return "Z"
|
||||
elif abs(v[2] + 1.0) < zero:
|
||||
return "-Z"
|
||||
else:
|
||||
# nothing special about the direction, return N
|
||||
return "N"
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# Set the vector directly in polar coordinates
|
||||
# @param ma magnitude of vector
|
||||
# @param ph azimuthal angle in radians
|
||||
# @param th polar angle in radians
|
||||
# ----------------------------------------------------------------------
|
||||
def setPolar(self, ma, ph, th):
|
||||
"""Set the vector directly in polar coordinates"""
|
||||
sf = math.sin(ph)
|
||||
cf = math.cos(ph)
|
||||
st = math.sin(th)
|
||||
ct = math.cos(th)
|
||||
self[0] = ma * st * cf
|
||||
self[1] = ma * st * sf
|
||||
self[2] = ma * ct
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def phi(self):
|
||||
"""return the azimuth angle."""
|
||||
if Cmp0(self.x()) and Cmp0(self.y()):
|
||||
return 0.0
|
||||
return math.atan2(self.y(), self.x())
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def theta(self):
|
||||
"""return the polar angle."""
|
||||
if Cmp0(self.x()) and Cmp0(self.y()) and Cmp0(self.z()):
|
||||
return 0.0
|
||||
return math.atan2(self.perp(), self.z())
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def cosTheta(self):
|
||||
"""return cosine of the polar angle."""
|
||||
ptot = self.length()
|
||||
if Cmp0(ptot):
|
||||
return 1.0
|
||||
else:
|
||||
return self.z() / ptot
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def perp2(self):
|
||||
"""return the transverse component squared
|
||||
(R^2 in cylindrical coordinate system)."""
|
||||
return self.x() * self.x() + self.y() * self.y()
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
def perp(self):
|
||||
"""@return the transverse component
|
||||
(R in cylindrical coordinate system)."""
|
||||
return math.sqrt(self.perp2())
|
||||
|
||||
# ----------------------------------------------------------------------
|
||||
# Return a random 3D vector
|
||||
# ----------------------------------------------------------------------
|
||||
# @staticmethod
|
||||
# def random():
|
||||
# cosTheta = 2.0 * random.random() - 1.0
|
||||
# sinTheta = math.sqrt(1.0 - cosTheta ** 2)
|
||||
# phi = 2.0 * math.pi * random.random()
|
||||
# return Vector(math.cos(phi) * sinTheta, math.sin(phi) * sinTheta, cosTheta)
|
||||
|
||||
# #===============================================================================
|
||||
# # Cardinal cubic spline class
|
||||
# #===============================================================================
|
||||
# class CardinalSpline:
|
||||
# def __init__(self, A=0.5):
|
||||
# # The default matrix is the Catmull-Rom spline
|
||||
# # which is equal to Cardinal matrix
|
||||
# # for A = 0.5
|
||||
# #
|
||||
# # Note: Vasilis
|
||||
# # The A parameter should be the fraction in t where
|
||||
# # the second derivative is zero
|
||||
# self.setMatrix(A)
|
||||
#
|
||||
# #-----------------------------------------------------------------------
|
||||
# # Set the matrix according to Cardinal
|
||||
# #-----------------------------------------------------------------------
|
||||
# def setMatrix(self, A=0.5):
|
||||
# self.M = []
|
||||
# self.M.append([ -A, 2.-A, A-2., A ])
|
||||
# self.M.append([2.*A, A-3., 3.-2.*A, -A ])
|
||||
# self.M.append([ -A, 0., A, 0.])
|
||||
# self.M.append([ 0., 1., 0, 0.])
|
||||
#
|
||||
# #-----------------------------------------------------------------------
|
||||
# # Evaluate Cardinal spline at position t
|
||||
# # @param P list or tuple with 4 points y positions
|
||||
# # @param t [0..1] fraction of interval from points 1..2
|
||||
# # @param k index of starting 4 elements in P
|
||||
# # @return spline evaluation
|
||||
# #-----------------------------------------------------------------------
|
||||
# def __call__(self, P, t, k=1):
|
||||
# T = [t*t*t, t*t, t, 1.0]
|
||||
# R = [0.0]*4
|
||||
# for i in range(4):
|
||||
# for j in range(4):
|
||||
# R[i] += T[j] * self.M[j][i]
|
||||
# y = 0.0
|
||||
# for i in range(4):
|
||||
# y += R[i]*P[k+i-1]
|
||||
#
|
||||
# return y
|
||||
#
|
||||
# #-----------------------------------------------------------------------
|
||||
# # Return the coefficients of a 3rd degree polynomial
|
||||
# # f(x) = a t^3 + b t^2 + c t + d
|
||||
# # @return [a, b, c, d]
|
||||
# #-----------------------------------------------------------------------
|
||||
# def coefficients(self, P, k=1):
|
||||
# C = [0.0]*4
|
||||
# for i in range(4):
|
||||
# for j in range(4):
|
||||
# C[i] += self.M[i][j] * P[k+j-1]
|
||||
# return C
|
||||
#
|
||||
# #-----------------------------------------------------------------------
|
||||
# # Evaluate the value of the spline using the coefficients
|
||||
# #-----------------------------------------------------------------------
|
||||
# def evaluate(self, C, t):
|
||||
# return ((C[0]*t + C[1])*t + C[2])*t + C[3]
|
||||
#
|
||||
# #===============================================================================
|
||||
# # Cubic spline ensuring that the first and second derivative are continuous
|
||||
# # adapted from Penelope Manual Appending B.1
|
||||
# # It requires all the points (xi,yi) and the assumption on how to deal
|
||||
# # with the second derivative on the extremities
|
||||
# # Option 1: assume zero as second derivative on both ends
|
||||
# # Option 2: assume the same as the next or previous one
|
||||
# #===============================================================================
|
||||
# class CubicSpline:
|
||||
# def __init__(self, X, Y):
|
||||
# self.X = X
|
||||
# self.Y = Y
|
||||
# self.n = len(X)
|
||||
#
|
||||
# # Option #1
|
||||
# s1 = 0.0 # zero based = s0
|
||||
# sN = 0.0 # zero based = sN-1
|
||||
#
|
||||
# # Construct the tri-diagonal matrix
|
||||
# A = []
|
||||
# B = [0.0] * (self.n-2)
|
||||
# for i in range(self.n-2):
|
||||
# A.append([0.0] * (self.n-2))
|
||||
#
|
||||
# for i in range(1,self.n-1):
|
||||
# hi = self.h(i)
|
||||
# Hi = 2.0*(self.h(i-1) + hi)
|
||||
# j = i-1
|
||||
# A[j][j] = Hi
|
||||
# if i+1<self.n-1:
|
||||
# A[j][j+1] = A[j+1][j] = hi
|
||||
#
|
||||
# if i==1:
|
||||
# B[j] = 6.*(self.d(i) - self.d(j)) - hi*s1
|
||||
# elif i<self.n-2:
|
||||
# B[j] = 6.*(self.d(i) - self.d(j))
|
||||
# else:
|
||||
# B[j] = 6.*(self.d(i) - self.d(j)) - hi*sN
|
||||
#
|
||||
#
|
||||
# self.s = gauss(A,B)
|
||||
# self.s.insert(0,s1)
|
||||
# self.s.append(sN)
|
||||
# # print ">> s <<"
|
||||
# # pprint(self.s)
|
||||
#
|
||||
# #-----------------------------------------------------------------------
|
||||
# def h(self, i):
|
||||
# return self.X[i+1] - self.X[i]
|
||||
#
|
||||
# #-----------------------------------------------------------------------
|
||||
# def d(self, i):
|
||||
# return (self.Y[i+1] - self.Y[i]) / (self.X[i+1] - self.X[i])
|
||||
#
|
||||
# #-----------------------------------------------------------------------
|
||||
# def coefficients(self, i):
|
||||
# """return coefficients of cubic spline for interval i a*x**3+b*x**2+c*x+d"""
|
||||
# hi = self.h(i)
|
||||
# si = self.s[i]
|
||||
# si1 = self.s[i+1]
|
||||
# xi = self.X[i]
|
||||
# xi1 = self.X[i+1]
|
||||
# fi = self.Y[i]
|
||||
# fi1 = self.Y[i+1]
|
||||
#
|
||||
# a = 1./(6.*hi)*(si*xi1**3 - si1*xi**3 + 6.*(fi*xi1 - fi1*xi)) + hi/6.*(si1*xi - si*xi1)
|
||||
# b = 1./(2.*hi)*(si1*xi**2 - si*xi1**2 + 2*(fi1 - fi)) + hi/6.*(si - si1)
|
||||
# c = 1./(2.*hi)*(si*xi1 - si1*xi)
|
||||
# d = 1./(6.*hi)*(si1-si)
|
||||
#
|
||||
# return [d,c,b,a]
|
||||
#
|
||||
# #-----------------------------------------------------------------------
|
||||
# def __call__(self, i, x):
|
||||
# # FIXME should interpolate to find the interval
|
||||
# C = self.coefficients(i)
|
||||
# return ((C[0]*x + C[1])*x + C[2])*x + C[3]
|
||||
#
|
||||
# #-----------------------------------------------------------------------
|
||||
# # @return evaluation of cubic spline at x using coefficients C
|
||||
# #-----------------------------------------------------------------------
|
||||
# def evaluate(self, C, x):
|
||||
# return ((C[0]*x + C[1])*x + C[2])*x + C[3]
|
||||
#
|
||||
# #-----------------------------------------------------------------------
|
||||
# # Return evaluated derivative at x using coefficients C
|
||||
# #-----------------------------------------------------------------------
|
||||
# def derivative(self, C, x):
|
||||
# a = 3.0*C[0] # derivative coefficients
|
||||
# b = 2.0*C[1] # ... for sampling with rejection
|
||||
# c = C[2]
|
||||
# return (3.0*C[0]*x + 2.0*C[1])*x + C[2]
|
||||
#
|
||||
Reference in New Issue
Block a user