nurbs_frame.py

"""
in curve_in C
in ts_in s
out matrix_out m
"""

import numpy as np
from sverchok.data_structure import zip_long_repeat, ensure_nesting_level, repeat_last_for_length
from sverchok.utils.curve.core import SvCurve, UnsupportedCurveTypeException
from sverchok.utils.curve.nurbs import SvNurbsCurve
from sverchok.utils.geom import CubicSpline

from mathutils import Matrix, Quaternion, Vector

curve_in = ensure_nesting_level(curve_in, 1, data_types=(SvCurve,))
ts_in = ensure_nesting_level(ts_in, 2)

def calc_node_quats(node_vectors):
    node_normals = np.cross(node_vectors[:-1], node_vectors[1:])
    node_vectors = [Vector(v) for v in node_vectors]
    result = []
    for dv1, dv2, normal in zip(node_vectors[:-1], node_vectors[1:], node_normals):
        angle = dv1.angle(dv2)
        quat = Quaternion(Vector(normal).normalized(), angle)
        result.append(quat)
    return result

def process(curve, ts):
    node_ts = curve.calc_greville_ts()
    node_vectors = curve.tangent_array(node_ts)
    quats = calc_node_quats(node_vectors)
    
    pts = curve.evaluate_array(ts)
    tangents = curve.tangent_array(ts)
    
    tilt_pairs = curve.get_tilt_pairs()
    if not tilt_pairs:
        tilt_quats = [Quaternion() for t in ts]
    else:
        tilt_pairs = np.array(tilt_pairs)
        #tilt_spline = CubicSpline(tilt_verts, metric='X', is_cyclic=False)
        tilt_values = CubicSpline.resample(tilt_pairs[:,0].copy(), tilt_pairs[:,1].copy(), ts)
        #print(tilt_values)
        tilt_quats = [Quaternion(Vector(tangent), tilt) for tangent, tilt in zip(tangents, tilt_values)]
    
    start_quat = Vector(tangents[0]).to_track_quat("Z", "X")
    
    quats = [start_quat] + quats
        
    base_indexes = node_ts.searchsorted(ts, side='left')-1
    base_indexes[base_indexes < 0] = 0
    t1s = node_ts[base_indexes]
    t2s = node_ts[base_indexes+1]
    dts = (ts - t1s) / (t2s - t1s)
    
    prev_q = quats[0]
    cumm_quats = [prev_q]
    for q1 in quats[1:]:
        q = q1 @ prev_q
        cumm_quats.append(q)
        prev_q = q
    quats = cumm_quats
    
    matrices = []
    
    for pt, tangent, tilt_quat, dt, base_index in zip(pts, tangents, tilt_quats, dts, base_indexes):
        q1 = quats[base_index]
        q2 = quats[base_index+1]
        if dt <= 0:
            q = q1
        elif dt >= 1.0:
            q = q2
        else:
            q = q1.slerp(q2, dt)
        
        qz = q @ Vector((0,0,1))
        dq = qz.rotation_difference(Vector(tangent))
        
        matrix = (tilt_quat @ dq @ q).to_matrix().to_4x4()
        matrix.translation = Vector(pt)
        matrices.append(matrix)
    return matrices
    

matrix_out = []
for curve, ts in zip_long_repeat(curve_in, ts_in):
    curve = SvNurbsCurve.to_nurbs(curve)
    if curve is None:
        raise UnsupportedCurveTypeException("Curve is not NURBS")
    matrix = process(curve, np.array(ts))
    matrix_out.append(matrix)