Source

geoalg / geoalg / nurbs / cspline.py

Full commit
#!/usr/bin/env python
#coding:utf-8
# Purpose: Spline through breakpoints
# Created: 26.03.2010
# License: GPLv3
# Source: http://www-lehre.informatik.uni-osnabrueck.de/~cg/2000/skript/7_2_Splines.html

from __future__ import division
from __future__ import print_function
from __future__ import unicode_literals
from __future__ import absolute_import

__author__ = "mozman <mozman@gmx.at>"
__all__ = ['CubicSpline']

import math

def _coords(points, index=0):
    return [float(point[index]) for point in points]

class CubicSpline(object):
    """ Creates a Spline 'through' the breakpoints!

    Special behavior: 2D in -> 2D out and 3D in -> 3D out!
    """
    class Params:
        def __init__(self, axis_vector, a, b, c):
            self.axis_vector = axis_vector
            self.a = a
            self.b = b
            self.c = c

    def __init__(self, points, target_length=None):
        self.breakpoints = points
        self.count = len(points)
        self.t = self._get_t_array(points, target_length)
        self.spline_params = []
        self._set_params()

    @property
    def axis(self):
        return range(len(self.breakpoints[0]))

    @property
    def max_t(self):
        return self.t[-1]

    def _create_array(self):
        return [0.0] * self.count

    def _get_t_array(self, points, target_length):
        def distance(p1, p2):
            return sum( ((p1[axis]-p2[axis])**2 for axis in self.axis) ) ** 0.5

        t = [0.0]
        for p1, p2 in zip(points[:-1], points[1:]):
            t.append(t[-1] + distance(p1, p2))

        if target_length is not None:
            # stretch distances to get spline 'length'
            # this works only, if the spline is very 'flat', the distance
            # between curve secants and spline should be very small.
            stretch = float(target_length) / t[-1]
            for i in range(len(t)):
                t[i] *= stretch
        return t

    def approximate(self, segments):
        step = self.max_t / float(segments)
        for i in range(segments+1):
            yield self.get_point(i * step)

    def approximated_length(self, segments=1000):
        def distance(p1, p2):
            return sum( ((p1[axis]-p2[axis])**2 for axis in self.axis) ) ** 0.5

        points = list(self.approximate(segments))
        return sum( distance(p1, p2) for p1, p2 in zip(points[:-1], points[1:]))

    def tangents(self, segments):
        step = self.max_t / float(segments)
        for i in range(segments+1):
            yield self.get_derivative1(i * step)

    def get_point(self, t):
        return self._evaluate(t, point)

    def get_derivative1(self, t):
        return self._evaluate(t, derivative1)

    def get_derivative2(self, t):
        return self._evaluate(t, derivative2)

    def _evaluate(self, t, func):
        j = 0
        while (j < (self.count-1)) and (self.t[j+1] < t):
            j += 1
        h = t - self.t[j]
        return tuple(func(h, j, self.spline_params[axis]) for axis in self.axis)

    def _set_params(self):
        for axis in self.axis:
            self.spline_params.append(self._get_spline_params(_coords(self.breakpoints, axis)))

    def _get_spline_params(self, axis_vector):
        def get_delta_t_D():
            delta_t = self._create_array()
            D = self._create_array()
            for i in nrange:
                delta_t[i] = t[i] - t[i-1]
                D[i] = (axis_vector[i] - axis_vector[i-1])/ delta_t[i]
            delta_t[0] = t[2] - t[0]
            return delta_t, D

        def solve_k_m(D, delta_t):
            m = self._create_array()
            k = self._create_array()
            h = delta_t[1]
            m[0] = delta_t[2]
            k[0] = ((h + 2 * delta_t[0]) * D[1] * delta_t[2] + h * h * D[2]) / delta_t[0]
            for i in nrange[1:-1]:
                h = -delta_t[i+1] / m[i-1]
                k[i] = h * k[i-1] + 3 * (delta_t[i] * D[i+1] + delta_t[i+1] * D[i])
                m[i] = h * delta_t[i-1] + 2 * (delta_t[i] + delta_t[i+1])

            h = t[n-1] - t[n-3]
            dh = delta_t[n-1]
            k[n-1] = ((dh + h + h) * D[n-1] * delta_t[n-2] + dh * dh * D[n-2]) / h
            h = -h / m[n-2]
            m[n-1] = (h + 1.) * delta_t[n-2]
            return k, m

        def solve_a(k, m, delta_t):
            a = self._create_array()
            h = (t[n-1] - t[n-3]) / -m[n-2]
            a[n-1] = (h * k[n-2] + k[n-1]) / m[n-1]
            for i in reversed(nrange[:-1]):
                a[i] = (k[i] - delta_t[i] * a[i+1]) / m[i]
            return a

        def solve_b_c(a, D, delta_t):
            b = self._create_array()
            c = self._create_array()
            for i in nrange[1:]:
                dh = D[i]
                bh = delta_t[i]
                e = a[i-1] + a[i] - 2. * dh
                b[i-1] = 2. * (dh - a[i-1] - e) / bh
                c[i-1] = 6. * e / (bh * bh)
            return b, c

        n = self.count
        t = self.t
        nrange = list(range(n))

        delta_t, D = get_delta_t_D()
        k, m = solve_k_m(D, delta_t)
        a = solve_a(k, m, delta_t)
        b, c = solve_b_c(a, D, delta_t)
        return CubicSpline.Params(axis_vector, a, b, c)

def point(h, j, params):
    return params.axis_vector[j] + h*(params.a[j] + h*(params.b[j] + h*params.c[j]/3.0)/2.0)

def derivative1(h, j, params):
    return params.a[j] + h*(2.0*params.b[j] + h*params.c[j])/2.0

def derivative2(h, j, params):
    return params.b[j] + h*params.c[j]