self.SIMPSON = 100 # number of sub-intervals of composite simpson integration, must be even, usually 100 is good enough
self.NEWTON = 0.0001 # epsilon, effective digits of the root calculated by Newton's method
# calculate the value at t
def __call__(self, t):
if t < 0 or t >= 1:
return None
else: # find the curve segment
for i in range(3, self.m - 3):
if self.u[i] <= t and t < self.u[i + 1]:
break
idx = i - 3 # get coefficients
a1 = self.a1[idx]
a2 = self.a2[idx]
a3 = self.a3[idx]
a4 = self.a4[idx]
b1 = self.b1[idx]
b2 = self.b2[idx]
b3 = self.b3[idx]
b4 = self.b4[idx]
t = (t - self.u[i]) / (self.u[i + 1] - self.u[i]) # map to the current curve segment
return (a1 * t * t * t + a2 * t * t + a3 * t + a4,
b1 * t * t * t + b2 * t * t + b3 * t + b4, 255) # 255 is the point's alpha value, to meet the TCAX's point structure requirement ((x, y, a), (x, y, a), ...)
# total arc-length
def length(self):
from math import sqrt
sqr = lambda x: x * x
l = 0
for i in range(self.num):
a1 = self.a1[i]
a2 = self.a2[i]
a3 = self.a3[i]
a4 = self.a4[i]
b1 = self.b1[i]
b2 = self.b2[i]
b3 = self.b3[i]
b4 = self.b4[i]
f_grand = lambda t: sqrt(sqr(3 * a1 * t * t + 2 * a2 * t + a3) + sqr(3 * b1 * t * t + 2 * b2 * t + b3)) # integrand of UCB
l += CompositeSimpson(f_grand, 0, 1, self.SIMPSON)
return l
# find t2 according to a specific arc-length s and a starting point t1
def t2OfLength(self, t1, s):
if t1 < 0 or t1 >= 1:
return None
else: # find the curve segment
for i in range(3, self.m - 3):
if self.u[i] <= t1 and t1 < self.u[i + 1]:
break
idx = i - 3 # get coefficients
a1 = self.a1[idx]
a2 = self.a2[idx]
a3 = self.a3[idx]
a4 = self.a4[idx]
b1 = self.b1[idx]
b2 = self.b2[idx]
b3 = self.b3[idx]
b4 = self.b4[idx]
t1 = (t1 - self.u[i]) / (self.u[i + 1] - self.u[i]) # map to the current curve segment
# Assumption: t1 and t are in the same piece of curve
from math import sqrt
sqr = lambda x: x * x
f_grand = lambda t: sqrt(sqr(3 * a1 * t * t + 2 * a2 * t + a3) + sqr(3 * b1 * t * t + 2 * b2 * t + b3)) # integrand of UCB
s1 = CompositeSimpson(f_grand, t1, 1, self.SIMPSON) # length from the starting point t1 to the end of the current curve segment
if s1 >= s: # t1 and t are in the same segment of curve
f_grand_d = lambda t: ((3 * a1 * t * t + 2 * a2 * t + a3) * (6 * a1 * t + 2 * a2) + (3 * b1 * t * t + 2 * b2 * t + b3) * (6 * b1 * t + 2 * b2 * t)) / f_grand(t) # derivative of f_grand
# using Newton's method to find the root of function f
f = lambda t: CompositeSimpson(f_grand, t1, t, self.SIMPSON) - s
fd = lambda t: CompositeSimpsonDerivative(f_grand, f_grand_d, t1, t, self.SIMPSON) # derivative of f
return NewtonMethod(f, fd, t1, self.NEWTON) * (self.u[i + 1] - self.u[i]) + self.u[i] # find the t and map to the whole curve
# otherwise, t is in next segment(s)
else:
if idx + 1 == self.num: # no more curve segment
return 1
for i in range(idx + 1, self.num):
a1 = self.a1[i]
a2 = self.a2[i]
a3 = self.a3[i]
a4 = self.a4[i]
b1 = self.b1[i]
b2 = self.b2[i]
b3 = self.b3[i]
b4 = self.b4[i]
f_grand = lambda t: sqrt(sqr(3 * a1 * t * t + 2 * a2 * t + a3) + sqr(3 * b1 * t * t + 2 * b2 * t + b3)) # integrand of UCB
f_grand_d = lambda t: ((3 * a1 * t * t + 2 * a2 * t + a3) * (6 * a1 * t + 2 * a2) + (3 * b1 * t * t + 2 * b2 * t + b3) * (6 * b1 * t + 2 * b2 * t)) / f_grand(t) # derivative of f_grand
# using Newton's method to find the root of function f
f = lambda t: CompositeSimpson(f_grand, 0, t, self.SIMPSON) - (s - s0)
fd = lambda t: CompositeSimpsonDerivative(f_grand, f_grand_d, 0, t, self.SIMPSON) # derivative of f
return NewtonMethod(f, fd, 0, self.NEWTON) * (self.u[i + 3 + 1] - self.u[i + 3]) + self.u[i + 3] # find the t and map to the whole curve, note, here i is actually idx, should + 3 to be real i
# f=function, a=initial value, b=end value, n=number of intervals of size h, n must be even
def CompositeSimpson(f, a, b, n):
h = (b - a) / n
S = f(a)
for i in range(1, n, 2):
x = a + h * i
S += 4 * f(x)
for i in range(2, n - 1, 2):
x = a + h * i
S += 2 * f(x)
S += f(b)
return h * S / 3
# f=function, fd=derivative of f, t1=initial value (constant), t=end value (variable), n=number of intervals of size h, n must be even
def CompositeSimpsonDerivative(f, fd, t1, t, n):
h = (t - t1) / n
hd = 1 / n
S = f(t1)
for i in range(1, n, 2):
x = t1 + h * i
S += 4 * f(x)
for i in range(2, n - 1, 2):
x = t1 + h * i
S += 2 * f(x)
S += f(t)
part1 = hd * S / 3
# part2
S = 0
for i in range(1, n, 2):
x = t1 + h * i
xd = hd * i
S += 4 * fd(x) * xd
for i in range(2, n - 1, 2):
x = t1 + h * i
xd = hd * i
S += 2 * fd(x) * xd
S += fd(t)
part2 = h * S / 3
return part1 + part2
# f is the function whose root needs to be found, fd is the derivative of function f, x0 is the starting point, e is an epsilon, i.e., effective digits
return (a1 * t * t * t + a2 * t * t + a3 * t + a4,
b1 * t * t * t + b2 * t * t + b3 * t + b4, 255) # 255 is the point's alpha value, to meet the TCAX's point structure requirement ((x, y, a), (x, y, a), ...)
def UniformCubicBSpline_Calc2(p1, p2, p3, p4, t):
A1 = (1 - t) * (1 - t) * (1 - t) / 6.0
A2 = (3 * t * t * t - 6 * t * t + 4) / 6.0
A3 = (-3 * t * t * t + 3 * t * t + 3 * t + 1) / 6.0