from math import floor, sqrt
[docs]class Spline:
[docs] def __init__(self, points_data=[], resolution=10000, sharpness=0.85):
self.duration = resolution
self.sharpness = sharpness
self.centers = []
self.controls = []
self.stepLength = 60
self.delay = 0
self.steps = []
self.points = points_data
self.length = len(self.points)
for i in range(0, self.length):
if "z" not in self.points[i]:
self.points[i]["z"] = 0
for i in range(0, self.length - 1):
p1 = self.points[i]
p2 = self.points[i + 1]
self.centers.append(
{
"x": (p1["x"] + p2["x"]) / 2,
"y": (p1["y"] + p2["y"]) / 2,
"z": (p1["z"] + p2["z"]) / 2,
}
)
self.controls.append([self.points[0], self.points[0]])
for i in range(0, len(self.centers) - 1):
dx = (
self.points[i + 1]["x"]
- (self.centers[i]["x"] + self.centers[i + 1]["x"]) / 2
) # noqa: E501
dy = (
self.points[i + 1]["y"]
- (self.centers[i]["y"] + self.centers[i + 1]["y"]) / 2
) # noqa: E501
dz = (
self.points[i + 1]["z"]
- (self.centers[i]["y"] + self.centers[i + 1]["z"]) / 2
) # noqa: E501
self.controls.append(
[
{
"x": (1.0 - self.sharpness) * self.points[i + 1]["x"]
+ self.sharpness * (self.centers[i]["x"] + dx), # noqa: E501
"y": (1.0 - self.sharpness) * self.points[i + 1]["y"]
+ self.sharpness * (self.centers[i]["y"] + dy), # noqa: E501
"z": (1.0 - self.sharpness) * self.points[i + 1]["z"]
+ self.sharpness * (self.centers[i]["z"] + dz),
}, # noqa: E501
{
"x": (1.0 - self.sharpness) * self.points[i + 1]["x"]
+ self.sharpness
* (self.centers[i + 1]["x"] + dx), # noqa: E501 # noqa: E126
"y": (1.0 - self.sharpness) * self.points[i + 1]["y"]
+ self.sharpness
* (self.centers[i + 1]["y"] + dy), # noqa: E501 # noqa: E126
"z": (1.0 - self.sharpness) * self.points[i + 1]["z"]
+ self.sharpness * (self.centers[i + 1]["z"] + dz), # noqa: E501
},
]
) # noqa: E126
self.controls.append(
[self.points[self.length - 1], self.points[self.length - 1]]
) # noqa: E501
self.steps = self.cache_steps(self.stepLength)
[docs] def cache_steps(self, mindist):
steps = []
laststep = self.pos(0)
steps.append(0)
t = 0
while t < self.duration:
step = self.pos(t)
dist = sqrt(
(step["x"] - laststep["x"]) * (step["x"] - laststep["x"])
+ (step["y"] - laststep["y"]) * (step["y"] - laststep["y"]) # noqa: W504
+ (step["z"] - laststep["z"]) * (step["z"] - laststep["z"]) # noqa: W504
)
if dist > mindist:
steps.append(t)
laststep = step
t += 10
return steps
[docs] def pos(self, time):
t = time - self.delay
if t < 0:
t = 0
if t > self.duration:
t = self.duration - 1
t2 = t / self.duration
if t2 >= 1:
return self.points[self.length - 1]
n = floor((len(self.points) - 1) * t2)
t1 = (self.length - 1) * t2 - n
return self.bezier(
t1,
self.points[n],
self.controls[n][1],
self.controls[n + 1][0],
self.points[n + 1],
) # noqa: E501
[docs] def bezier(self, t, p1, c1, c2, p2):
b = self.B(t)
pos = {
"x": p2["x"] * b[0] + c2["x"] * b[1] + c1["x"] * b[2] + p1["x"] * b[3],
"y": p2["y"] * b[0] + c2["y"] * b[1] + c1["y"] * b[2] + p1["y"] * b[3],
"z": p2["z"] * b[0] + c2["z"] * b[1] + c1["z"] * b[2] + p1["z"] * b[3],
}
return pos
[docs] def B(self, t):
t2 = t * t
t3 = t2 * t
return [
t3,
(3 * t2 * (1 - t)),
(3 * t * (1 - t) * (1 - t)),
((1 - t) * (1 - t) * (1 - t)),
] # noqa: E501