100 lines
2.7 KiB
Python
100 lines
2.7 KiB
Python
import numpy as np
|
|
import math
|
|
import mathutils
|
|
|
|
|
|
def catmullrom(P0, P1, P2, P3, a, nPoints=100):
|
|
"""
|
|
P0, P1, P2, and P3 should be (x,y,z) point pairs that define the Catmull-Rom spline.
|
|
nPoints is the number of points to include in this curve segment.
|
|
"""
|
|
# Convert the points to numpy so that we can do array multiplication
|
|
P0, P1, P2, P3 = map(np.array, [P0, P1, P2, P3])
|
|
|
|
# Calculate t0 to t4
|
|
alpha = a
|
|
|
|
def tj(ti, Pi, Pj):
|
|
xi, yi, zi = Pi
|
|
xj, yj, zj = Pj
|
|
|
|
# ( ( (xj-xi)**2 + (yj-yi)**2 )**0.5 )**alpha + ti
|
|
a = (xj - xi) ** 2 + (yj - yi) ** 2 + (zj - zi) ** 2
|
|
b = a ** 0.5
|
|
c = b ** alpha
|
|
return c + ti
|
|
|
|
t0 = 0
|
|
t1 = tj(t0, P0, P1)
|
|
t2 = tj(t1, P1, P2)
|
|
t3 = tj(t2, P2, P3)
|
|
|
|
# Only calculate points between P1 and P2
|
|
t = np.linspace(t1, t2, nPoints)
|
|
|
|
# Reshape so that we can multiply by the points P0 to P3
|
|
# and get a point for each value of t.
|
|
t = t.reshape(len(t), 1)
|
|
|
|
A1 = (t1 - t) / (t1 - t0) * P0 + (t - t0) / (t1 - t0) * P1
|
|
A2 = (t2 - t) / (t2 - t1) * P1 + (t - t1) / (t2 - t1) * P2
|
|
A3 = (t3 - t) / (t3 - t2) * P2 + (t - t2) / (t3 - t2) * P3
|
|
|
|
B1 = (t2 - t) / (t2 - t0) * A1 + (t - t0) / (t2 - t0) * A2
|
|
B2 = (t3 - t) / (t3 - t1) * A2 + (t - t1) / (t3 - t1) * A3
|
|
|
|
C = (t2 - t) / (t2 - t1) * B1 + (t - t1) / (t2 - t1) * B2
|
|
return C
|
|
|
|
|
|
def hermite_1d(y0, y1, y2, y3, mu, tension, bias):
|
|
mu2 = mu * mu
|
|
mu3 = mu2 * mu
|
|
|
|
m0 = (y1 - y0) * (1 + bias) * (1 - tension) / 2
|
|
m0 += (y2 - y1) * (1 - bias) * (1 - tension) / 2
|
|
m1 = (y2 - y1) * (1 + bias) * (1 - tension) / 2
|
|
m1 += (y3 - y2) * (1 - bias) * (1 - tension) / 2
|
|
a0 = 2 * mu3 - 3 * mu2 + 1
|
|
a1 = mu3 - 2 * mu2 + mu
|
|
a2 = mu3 - mu2
|
|
a3 = -2 * mu3 + 3 * mu2
|
|
|
|
return a0 * y1 + a1 * m0 + a2 * m1 + a3 * y2
|
|
|
|
|
|
def hermite_3d(p1, p2, p3, p4, mu, tension, bias):
|
|
'''
|
|
Mu: For interpolated values between p2 and p3 mu ranges between 0 and 1
|
|
Tension: 1 is high, 0 normal, -1 is low
|
|
Bias: 0 is even,
|
|
positive is towards first segment,
|
|
negative towards the other
|
|
|
|
:return: List
|
|
'''
|
|
x = hermite_1d(p1[0], p2[0], p3[0], p4[0], mu, tension, bias)
|
|
y = hermite_1d(p1[1], p2[1], p3[1], p4[1], mu, tension, bias)
|
|
z = hermite_1d(p1[2], p2[2], p3[2], p4[2], mu, tension, bias)
|
|
|
|
return [x, y, z]
|
|
|
|
|
|
def smooth_step(a, b, x):
|
|
'''
|
|
Perform Hermite interpolation between two values
|
|
'''
|
|
|
|
value = clamp((x - a) / (b - a))
|
|
return value * value * (3 - 2 * value)
|
|
|
|
|
|
def clamp(x, lowerlimit = 0.0, upperlimit = 1.0):
|
|
'''
|
|
Constrain a value to lie between two further values
|
|
'''
|
|
if (x < lowerlimit): return lowerlimit
|
|
if (x > upperlimit): return upperlimit
|
|
return x
|
|
|