def pre_calc_torsion_integral(self, resolution):
t_min, t_max = self.get_u_bounds()
ts = np.linspace(t_min, t_max, resolution)
vectors = self.evaluate_array(ts)
dvs = vectors[1:] - vectors[:-1]
lengths = np.linalg.norm(dvs, axis=1)
xs = np.insert(np.cumsum(lengths), 0, 0)
ys = self.torsion_array(ts)
self._torsion_integral = TrapezoidIntegral(ts, xs, ys)
self._torsion_integral.calc()
class SvCurve(object):
def __repr__(self):
if hasattr(self, '__description__'):
description = self.__description__
else:
description = self.__class__.__name__
return "<{} curve>".format(description)
def evaluate(self, t):
raise Exception("not implemented!")
def evaluate_array(self, ts):
raise Exception("not implemented!")
def get_tangent_delta(self, tangent_delta=None):
if tangent_delta is None:
if hasattr(self, 'tangent_delta'):
h = self.tangent_delta
else:
h = DEFAULT_TANGENT_DELTA
else:
h = DEFAULT_TANGENT_DELTA
return h
def calc_length(self, t_min, t_max, resolution=50):
ts = np.linspace(t_min, t_max, num=resolution)
vectors = self.evaluate_array(ts)
dvs = vectors[1:] - vectors[:-1]
lengths = np.linalg.norm(dvs, axis=1)
return np.sum(lengths)
def tangent(self, t, tangent_delta=None):
v = self.evaluate(t)
h = self.get_tangent_delta(tangent_delta)
v_h = self.evaluate(t + h)
return (v_h - v) / h
def tangent_array(self, ts, tangent_delta=None):
vs = self.evaluate_array(ts)
h = self.get_tangent_delta(tangent_delta)
u_max = self.get_u_bounds()[1]
bad_idxs = (ts + h) > u_max
good_idxs = (ts + h) <= u_max
ts_h = ts + h
ts_h[bad_idxs] = (ts - h)[bad_idxs]
vs_h = self.evaluate_array(ts_h)
tangents_plus = (vs_h - vs) / h
tangents_minus = (vs - vs_h) / h
tangents_x = np.where(good_idxs, tangents_plus[:, 0],
tangents_minus[:, 0])
tangents_y = np.where(good_idxs, tangents_plus[:, 1],
tangents_minus[:, 1])
tangents_z = np.where(good_idxs, tangents_plus[:, 2],
tangents_minus[:, 2])
tangents = np.stack((tangents_x, tangents_y, tangents_z)).T
return tangents
def second_derivative(self, t, tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
v0 = self.evaluate(t - h)
v1 = self.evaluate(t)
v2 = self.evaluate(t + h)
return (v2 - 2 * v1 + v0) / (h * h)
def second_derivative_array(self, ts, tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
v0s = self.evaluate_array(ts - h)
v1s = self.evaluate_array(ts)
v2s = self.evaluate_array(ts + h)
return (v2s - 2 * v1s + v0s) / (h * h)
def third_derivative(self, t, tangent_delta=None):
return self.third_derivative_array(np.array([t]),
tangent_delta=tangent_delta)[0]
def third_derivative_array(self, ts, tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
v0s = self.evaluate_array(ts)
v1s = self.evaluate_array(ts + h)
v2s = self.evaluate_array(ts + 2 * h)
v3s = self.evaluate_array(ts + 3 * h)
return (-v0s + 3 * v1s - 3 * v2s + v3s) / (h * h * h)
def derivatives_array(self, n, ts, tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
result = []
N = len(ts)
t_min, t_max = self.get_u_bounds()
if n >= 1:
first = self.tangent_array(ts, tangent_delta=h)
result.append(first)
if n >= 2:
low = ts < t_min + h
high = ts > t_max - h
good = np.logical_and(np.logical_not(low), np.logical_not(high))
points = np.empty((N, 3))
minus_h = np.empty((N, 3))
plus_h = np.empty((N, 3))
if good.any():
minus_h[good] = self.evaluate_array(ts[good] - h)
points[good] = self.evaluate_array(ts[good])
plus_h[good] = self.evaluate_array(ts[good] + h)
if low.any():
minus_h[low] = self.evaluate_array(ts[low])
points[low] = self.evaluate_array(ts[low] + h)
plus_h[low] = self.evaluate_array(ts[low] + 2 * h)
if high.any():
minus_h[high] = self.evaluate_array(ts[high] - 2 * h)
points[high] = self.evaluate_array(ts[high] - h)
plus_h[high] = self.evaluate_array(ts[high])
second = (plus_h - 2 * points + minus_h) / (h * h)
result.append(second)
if n >= 3:
v0s = points
v1s = plus_h
v2s = self.evaluate_array(ts + 2 * h)
v3s = self.evaluate_array(ts + 3 * h)
third = (-v0s + 3 * v1s - 3 * v2s + v3s) / (h * h * h)
result.append(third)
return result
def main_normal(self, t, normalize=True, tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
tangent = self.tangent(t, tangent_delta=h)
binormal = self.binormal(t, normalize, tangent_delta=h)
v = np.cross(binormal, tangent)
if normalize:
v = v / np.linalg.norm(v)
return v
def binormal(self, t, normalize=True, tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
tangent = self.tangent(t, tangent_delta=h)
second = self.second_derivative(t, tangent_delta=h)
v = np.cross(tangent, second)
if normalize:
v = v / np.linalg.norm(v)
return v
def main_normal_array(self, ts, normalize=True, tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
tangents = self.tangent_array(ts, tangent_delta=h)
binormals = self.binormal_array(ts, normalize, tangent_delta=h)
v = np.cross(binormals, tangents)
if normalize:
norms = np.linalg.norm(v, axis=1, keepdims=True)
nonzero = (norms > 0)[:, 0]
v[nonzero] = v[nonzero] / norms[nonzero][:, 0][np.newaxis].T
return v
def binormal_array(self, ts, normalize=True, tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
tangents, seconds = self.derivatives_array(2, ts, tangent_delta=h)
v = np.cross(tangents, seconds)
if normalize:
norms = np.linalg.norm(v, axis=1, keepdims=True)
nonzero = (norms > 0)[:, 0]
v[nonzero] = v[nonzero] / norms[nonzero][:, 0][np.newaxis].T
return v
def tangent_normal_binormal_array(self,
ts,
normalize=True,
tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
tangents, seconds = self.derivatives_array(2, ts, tangent_delta=h)
binormals = np.cross(tangents, seconds)
if normalize:
norms = np.linalg.norm(binormals, axis=1, keepdims=True)
nonzero = (norms > 0)[:, 0]
binormals[nonzero] = binormals[nonzero] / norms[nonzero][:, 0][
np.newaxis].T
normals = np.cross(binormals, tangents)
if normalize:
norms = np.linalg.norm(normals, axis=1, keepdims=True)
nonzero = (norms > 0)[:, 0]
normals[nonzero] = normals[nonzero] / norms[nonzero][:, 0][
np.newaxis].T
return tangents, normals, binormals
def arbitrary_frame_array(self, ts, tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
normals = []
binormals = []
points = self.evaluate_array(ts)
tangents = self.tangent_array(ts, tangent_delta=h)
tangents /= np.linalg.norm(tangents, axis=1, keepdims=True)
for i, t in enumerate(ts):
tangent = tangents[i]
normal = np.array(Vector(tangent).orthogonal())
binormal = np.cross(tangent, normal)
binormal /= np.linalg.norm(binormal)
normals.append(normal)
binormals.append(binormal)
normals = np.array(normals)
binormals = np.array(binormals)
matrices_np = np.dstack((normals, binormals, tangents))
matrices_np = np.transpose(matrices_np, axes=(0, 2, 1))
matrices_np = np.linalg.inv(matrices_np)
return matrices_np, normals, binormals
def frame_by_plane_array(self, ts, plane_normal, tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
n = len(ts)
tangents = self.tangent_array(ts, tangent_delta=h)
tangents /= np.linalg.norm(tangents, axis=1, keepdims=True)
plane_normals = np.tile(plane_normal[np.newaxis].T, n).T
normals = np.cross(tangents, plane_normals)
normals /= np.linalg.norm(normals, axis=1, keepdims=True)
binormals = np.cross(tangents, normals)
matrices_np = np.dstack((normals, binormals, tangents))
matrices_np = np.transpose(matrices_np, axes=(0, 2, 1))
matrices_np = np.linalg.inv(matrices_np)
return matrices_np, normals, binormals
FAIL = 'fail'
ASIS = 'asis'
RETURN_NONE = 'none'
def frame_array(self, ts, on_zero_curvature=ASIS, tangent_delta=None):
"""
input:
* ts - np.array of shape (n,)
* on_zero_curvature - what to do if the curve has zero curvature at one of T values.
The supported options are:
* SvCurve.FAIL: raise ZeroCurvatureException
* SvCurve.RETURN_NONE: return None
* SvCurve.ASIS: do not perform special check for this case, the
algorithm will raise a general LinAlgError exception if it can't calculate the matrix.
output: tuple:
* matrices: np.array of shape (n, 3, 3)
* normals: np.array of shape (n, 3)
* binormals: np.array of shape (n, 3)
"""
h = self.get_tangent_delta(tangent_delta)
tangents, normals, binormals = self.tangent_normal_binormal_array(
ts, tangent_delta=h)
if on_zero_curvature != SvCurve.ASIS:
zero_normal = np.linalg.norm(normals, axis=1) < 1e-6
if zero_normal.any():
if on_zero_curvature == SvCurve.FAIL:
raise ZeroCurvatureException(np.unique(ts[zero_normal]),
zero_normal)
elif on_zero_curvature == SvCurve.RETURN_NONE:
return None
tangents = tangents / np.linalg.norm(tangents, axis=1)[np.newaxis].T
matrices_np = np.dstack((normals, binormals, tangents))
matrices_np = np.transpose(matrices_np, axes=(0, 2, 1))
try:
matrices_np = np.linalg.inv(matrices_np)
return matrices_np, normals, binormals
except np.linalg.LinAlgError as e:
error("Some of matrices are singular:")
for i, m in enumerate(matrices_np):
if abs(np.linalg.det(m) < 1e-5):
error("M[%s] (t = %s):\n%s", i, ts[i], m)
raise e
def zero_torsion_frame_array(self, ts, tangent_delta=None):
"""
input: ts - np.array of shape (n,)
output: tuple:
* cumulative torsion - np.array of shape (n,) (rotation angles in radians)
* matrices - np.array of shape (n, 3, 3)
"""
if not hasattr(self, '_torsion_integral'):
raise Exception(
"pre_calc_torsion_integral() has to be called first")
h = self.get_tangent_delta(tangent_delta)
vectors = self.evaluate_array(ts)
matrices_np, normals, binormals = self.frame_array(ts, tangent_delta=h)
integral = self.torsion_integral(ts)
new_matrices = []
for matrix_np, point, angle in zip(matrices_np, vectors, integral):
frenet_matrix = Matrix(matrix_np.tolist()).to_4x4()
rotation_matrix = Matrix.Rotation(-angle, 4, 'Z')
matrix = frenet_matrix @ rotation_matrix
matrix.translation = Vector(point)
new_matrices.append(matrix)
return integral, new_matrices
def curvature_array(self, ts, tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
tangents, seconds = self.derivatives_array(2, ts, tangent_delta=h)
numerator = np.linalg.norm(np.cross(tangents, seconds), axis=1)
tangents_norm = np.linalg.norm(tangents, axis=1)
denominator = tangents_norm * tangents_norm * tangents_norm
return numerator / denominator
def torsion_array(self, ts, tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
tangents, seconds, thirds = self.derivatives_array(3,
ts,
tangent_delta=h)
seconds_thirds = np.cross(seconds, thirds)
numerator = (tangents * seconds_thirds).sum(axis=1)
#numerator = np.apply_along_axis(lambda tangent: tangent.dot(seconds_thirds), 1, tangents)
first_second = np.cross(tangents, seconds)
denominator = np.linalg.norm(first_second, axis=1)
return numerator / (denominator * denominator)
def pre_calc_torsion_integral(self, resolution, tangent_delta=None):
h = self.get_tangent_delta(tangent_delta)
t_min, t_max = self.get_u_bounds()
ts = np.linspace(t_min, t_max, resolution)
vectors = self.evaluate_array(ts)
dvs = vectors[1:] - vectors[:-1]
lengths = np.linalg.norm(dvs, axis=1)
xs = np.insert(np.cumsum(lengths), 0, 0)
ys = self.torsion_array(ts, tangent_delta=h)
self._torsion_integral = TrapezoidIntegral(ts, xs, ys)
self._torsion_integral.calc()
def torsion_integral(self, ts):
return self._torsion_integral.evaluate_cubic(ts)
def get_u_bounds(self):
raise Exception("not implemented!")
def get_degree(self):
raise Exception(
"`Get Degree' method is not applicable to curve of type `{}'".
format(type(self)))
def get_control_points(self):
"""
Returns: np.array of shape (n, 3)
"""
return np.array([])
class SvCurve(object):
def __repr__(self):
if hasattr(self, '__description__'):
description = self.__description__
else:
description = self.__class__.__name__
return "<{} curve>".format(description)
def evaluate(self, t):
raise Exception("not implemented!")
def evaluate_array(self, ts):
raise Exception("not implemented!")
def calc_length(self, t_min, t_max, resolution=50):
ts = np.linspace(t_min, t_max, num=resolution)
vectors = self.evaluate_array(ts)
dvs = vectors[1:] - vectors[:-1]
lengths = np.linalg.norm(dvs, axis=1)
return np.sum(lengths)
def tangent(self, t):
v = self.evaluate(t)
h = self.tangent_delta
v_h = self.evaluate(t + h)
return (v_h - v) / h
def tangent_array(self, ts):
vs = self.evaluate_array(ts)
h = self.tangent_delta
u_max = self.get_u_bounds()[1]
bad_idxs = (ts + h) > u_max
good_idxs = (ts + h) <= u_max
ts_h = ts + h
ts_h[bad_idxs] = (ts - h)[bad_idxs]
vs_h = self.evaluate_array(ts_h)
tangents_plus = (vs_h - vs) / h
tangents_minus = (vs - vs_h) / h
tangents_x = np.where(good_idxs, tangents_plus[:, 0],
tangents_minus[:, 0])
tangents_y = np.where(good_idxs, tangents_plus[:, 1],
tangents_minus[:, 1])
tangents_z = np.where(good_idxs, tangents_plus[:, 2],
tangents_minus[:, 2])
tangents = np.stack((tangents_x, tangents_y, tangents_z)).T
return tangents
def second_derivative(self, t):
if hasattr(self, 'tangent_delta'):
h = self.tangent_delta
else:
h = 0.001
v0 = self.evaluate(t - h)
v1 = self.evaluate(t)
v2 = self.evaluate(t + h)
return (v2 - 2 * v1 + v0) / (h * h)
def second_derivative_array(self, ts):
h = 0.001
v0s = self.evaluate_array(ts - h)
v1s = self.evaluate_array(ts)
v2s = self.evaluate_array(ts + h)
return (v2s - 2 * v1s + v0s) / (h * h)
def third_derivative_array(self, ts):
h = 0.001
v0s = self.evaluate_array(ts)
v1s = self.evaluate_array(ts + h)
v2s = self.evaluate_array(ts + 2 * h)
v3s = self.evaluate_array(ts + 3 * h)
return (-v0s + 3 * v1s - 3 * v2s + v3s) / (h * h * h)
def derivatives_array(self, n, ts):
result = []
if n >= 1:
first = self.tangent_array(ts)
result.append(first)
h = 0.001
if n >= 2:
minus_h = self.evaluate_array(ts - h)
points = self.evaluate_array(ts)
plus_h = self.evaluate_array(ts + h)
second = (plus_h - 2 * points + minus_h) / (h * h)
result.append(second)
if n >= 3:
v0s = points
v1s = plus_h
v2s = self.evaluate_array(ts + 2 * h)
v3s = self.evaluate_array(ts + 3 * h)
third = (-v0s + 3 * v1s - 3 * v2s + v3s) / (h * h * h)
result.append(third)
return result
def main_normal(self, t, normalize=True):
tangent = self.tangent(t)
binormal = self.binormal(t, normalize)
v = np.cross(binormal, tangent)
if normalize:
v = v / np.linalg.norm(v)
return v
def binormal(self, t, normalize=True):
tangent = self.tangent(t)
second = self.second_derivative(t)
v = np.cross(tangent, second)
if normalize:
v = v / np.linalg.norm(v)
return v
def main_normal_array(self, ts, normalize=True):
tangents = self.tangent_array(ts)
binormals = self.binormal_array(ts, normalize)
v = np.cross(binormals, tangents)
if normalize:
norms = np.linalg.norm(v, axis=1, keepdims=True)
nonzero = (norms > 0)[:, 0]
v[nonzero] = v[nonzero] / norms[nonzero][:, 0][np.newaxis].T
return v
def binormal_array(self, ts, normalize=True):
tangents, seconds = self.derivatives_array(2, ts)
v = np.cross(tangents, seconds)
if normalize:
norms = np.linalg.norm(v, axis=1, keepdims=True)
nonzero = (norms > 0)[:, 0]
v[nonzero] = v[nonzero] / norms[nonzero][:, 0][np.newaxis].T
return v
def tangent_normal_binormal_array(self, ts, normalize=True):
tangents, seconds = self.derivatives_array(2, ts)
binormals = np.cross(tangents, seconds)
if normalize:
norms = np.linalg.norm(binormals, axis=1, keepdims=True)
nonzero = (norms > 0)[:, 0]
binormals[nonzero] = binormals[nonzero] / norms[nonzero][:, 0][
np.newaxis].T
normals = np.cross(binormals, tangents)
if normalize:
norms = np.linalg.norm(normals, axis=1, keepdims=True)
nonzero = (norms > 0)[:, 0]
normals[nonzero] = normals[nonzero] / norms[nonzero][:, 0][
np.newaxis].T
return tangents, normals, binormals
def frame_array(self, ts):
tangents, normals, binormals = self.tangent_normal_binormal_array(ts)
tangents = tangents / np.linalg.norm(tangents, axis=1)[np.newaxis].T
matrices_np = np.dstack((normals, binormals, tangents))
matrices_np = np.transpose(matrices_np, axes=(0, 2, 1))
try:
matrices_np = np.linalg.inv(matrices_np)
return matrices_np, normals, binormals
except np.linalg.LinAlgError as e:
error("Some of matrices are singular:")
for m in matrices_np:
error("M:\n%s", m)
raise e
def curvature_array(self, ts):
tangents, seconds = self.derivatives_array(2, ts)
numerator = np.linalg.norm(np.cross(tangents, seconds), axis=1)
tangents_norm = np.linalg.norm(tangents, axis=1)
denominator = tangents_norm * tangents_norm * tangents_norm
return numerator / denominator
def torsion_array(self, ts):
tangents, seconds, thirds = self.derivatives_array(3, ts)
seconds_thirds = np.cross(seconds, thirds)
numerator = (tangents * seconds_thirds).sum(axis=1)
#numerator = np.apply_along_axis(lambda tangent: tangent.dot(seconds_thirds), 1, tangents)
first_second = np.cross(tangents, seconds)
denominator = np.linalg.norm(first_second, axis=1)
return numerator / (denominator * denominator)
def pre_calc_torsion_integral(self, resolution):
t_min, t_max = self.get_u_bounds()
ts = np.linspace(t_min, t_max, resolution)
vectors = self.evaluate_array(ts)
dvs = vectors[1:] - vectors[:-1]
lengths = np.linalg.norm(dvs, axis=1)
xs = np.insert(np.cumsum(lengths), 0, 0)
ys = self.torsion_array(ts)
self._torsion_integral = TrapezoidIntegral(ts, xs, ys)
self._torsion_integral.calc()
def torsion_integral(self, ts):
return self._torsion_integral.evaluate_cubic(ts)
def get_u_bounds(self):
raise Exception("not implemented!")