# Source code for klampt.math.spline

```"""Spline utilities."""
from . import vectorops

[docs]def hermite_eval(x1,v1,x2,v2,u):
"""Returns the position a hermite curve with control points
x1, v1, x2, v2 at the parameter u in [0,1]."""
assert len(x1)==len(v1)
assert len(x1)==len(x2)
assert len(x1)==len(v2)
u2 = u*u
u3 = u*u*u
cx1 = 2.0*u3-3.0*u2+1.0
cx2 = -2.0*u3+3.0*u2
cv1 = (u3-2.0*u2+u)
cv2 = (u3-u2)
x = [0]*len(x1)
for i in range(len(x1)):
x[i] = cx1*x1[i] + cx2*x2[i] + cv1*v1[i] + cv2*v2[i]
return x

[docs]def hermite_deriv(x1,v1,x2,v2,u,order=1):
"""Returns the derivative of a hermite curve with control points
x1, v1, x2, v2 at the parameter u in [0,1].  If order > 1, higher
order derivatives are returned."""
assert len(x1)==len(v1)
assert len(x1)==len(x2)
assert len(x1)==len(v2)
if order == 1:
u2 = u*u
dcx1 = (6.0*u2-6.0*u)
dcx2 = (-6.0*u2+6.0*u)
dcv1 = 3.0*u2-4.0*u+1.0
dcv2 = 3.0*u2-2.0*u
dx = [0]*len(x1)
for i in range(len(x1)):
dx[i] = dcx1*x1[i] + dcx2*x2[i] + dcv1*v1[i] + dcv2*v2[i];
return dx
elif order == 2:
ddcx1 = 12*u-6.0
ddcx2 = -12.0*u+6.0
ddcv1 = 6.0*u-4.0
ddcv2 = 6.0*u-2.0
ddx = [0]*len(x1)
for i in range(len(x1)):
ddx[i] = ddcx1*x1[i] + ddcx2*x2[i] + ddcv1*v1[i] + ddcv2*v2[i]
return ddx
elif order == 3:
cx1 = 12
cx2 = -12.0
cv1 = 6.0
cv2 = 6.0
dddx = [0]*len(x1)
for i in range(len(x1)):
dddx[i] = cx1*x1[i] + cx2*x2[i] + cv1*v1[i] + cv2*v2[i]
return dddx
elif order == 0:
return hermite_eval(x1,v1,x2,v2,u)
else:
return [0]*len(x1)

[docs]def hermite_subdivide(x1,v1,x2,v2,u=0.5):
"""Subdivides a hermite curve into two hermite curves."""
xm = hermite_eval(x1,v1,x2,v2,u)
vm = hermite_deriv(x1,v1,x2,v2,u)
return [(x1,vectorops.mul(v1,u),xm,vectorops.mul(vm,u)),(xm,vectorops.mul(vm,1.0-u),x2,vectorops.mul(v2,1.0-u))]

[docs]def hermite_length_bound(x1,v1,x2,v2):
"""Returns an upper bound on the arc length of the hermite curve"""
return bezier_length_bound(*hermite_to_bezier(x1,v1,x2,v2))

[docs]def hermite_to_bezier(x1,v1,x2,v2):
"""Returns the cubic bezier representation of a hermite curve"""
return x1,c1,c2,x2

[docs]def bezier_subdivide(x1,x2,x3,x4,u=0.5):
"""Subdivides a Bezier curve at the parameter u"""
p1 = vectorops.interpolate(x1,x2,u)
p2 = vectorops.interpolate(x2,x3,u)
p3 = vectorops.interpolate(x3,x4,u)
q1 = vectorops.interpolate(p1,p2,u)
q2 = vectorops.interpolate(p2,p3,u)
r1 = vectorops.interpolate(q1,q2,u)
return [(x1,p1,q1,r1),(r1,q2,p3,x4)]

[docs]def bezier_length_bound(x1,x2,x3,x4):
"""Returns an upper bound on the arc length of the bezier curve"""
return vectorops.distance(x1,x2)+ vectorops.distance(x2,x3) + vectorops.distance(x3,x4)

[docs]def bezier_discretize(x1,x2,x3,x4,res,return_params = False):
"""Discretizes a bezier curve into a list of points.  If return_params
is True, a pair (points,params) is returned where params are the exact
parameter values for which each point was generated.  Otherwise, just
the points are returned."""
stack = [(x1,x2,x3,x4)]
path = [x1]
if return_params:
params = [0.0]
param_stack = [(0.0,1.0)]
while len(stack) > 0:
c = stack[-1]
stack.pop(-1)
if return_params:
a,b = param_stack[-1]
param_stack.pop(-1)
if bezier_length_bound(*c) > res:
half1,half2 = bezier_subdivide(*c)
stack.append(half2)
stack.append(half1)
m=(a+b)*0.5
param_stack.append((m,b))
param_stack.append((a,m))
else:
assert path[-1] == c[0]
assert params[-1] == a
path += c[1:]
params.append(b)
else:
if bezier_length_bound(*c) > res:
half1,half2 = bezier_subdivide(*c)
stack.append(half2)
stack.append(half1)
else:
assert path[-1] == c[0]
path += c[1:]
if return_params:
return path,params
return path

[docs]def bezier_to_hermite(x1,x2,x3,x4):
"""Returns the cubic bezier representation of a hermite curve"""
v1 = vectorops.mul(vectorops.sub(x2,x1),3.0)
v2 = vectorops.mul(vectorops.sub(x4,x3),3.0)
return x1,v1,x4,v2
```