Spline and curve interpolation for many variants. Based upon the works of Steve H. Noskowicz.

curve.nim

import sequtils
import vmath

export vmath

#[ 
Spline and curve interpolation for many variants.
Based upon the works of Steve H. Noskowicz.
http://web.archive.org/web/20151002232205/http://home.comcast.net/~k9dci/site/?/page/Piecewise_Polynomial_Interpolation/
Get the book PDF and the appendix.
]#

type
  CurveSegmentError* = object of Defect
  CurveOrderError* = object of Defect

  Curve*[T] = object
    stepSize*: int
    segments*: int
    segmentDim*: int
    current_seg*: int
    matrix*: seq[seq[float]]
    degree*: int
    cp*: seq[T]
    curmult*: seq[T]
    origin*: T

  ICurve*[T] = object
    stepSize*: int
    segmentDim*: int
    matrix*: seq[seq[float]]
    degree*: int
    cp*: iterator: T
    origin*: T


func nSegments(length: int, stepSize: int, segmentDim: int): int {.inline.} =
  let lsd = length - segmentDim
  if lsd < 0:
    raise newException(CurveSegmentError, "Too few points in curve array")
  if (lsd mod stepSize) == 0:
    result = 1 + (lsd div stepSize)
  else:
    raise newException(CurveSegmentError, "Incorrect number of points in curve array")


func divideMatrix(matrix: seq[seq[float]], divisor: float): seq[seq[float]] {.inline.} =
  matrix.mapIt(it.mapIt(it / divisor))


func initCurve[T](
  cp: seq[T] or iterator(): T, segmentDim: int, stepSize: int, degree: int, 
  matrix: seq[seq[float]]
): auto =

  when cp is iterator: T:
    var curve = ICurve[T]()
  else:
    var curve = Curve[T]()

  curve.cp = cp
  curve.segmentDim = segmentDim
  curve.stepSize = stepSize
  curve.degree = degree
  curve.matrix = matrix

  when curve is Curve[T]:
    curve.current_seg = -1
    curve.segments = nSegments(len(curve.cp), curve.stepSize, curve.segmentDim)

  return curve


func linear*[T](cp: seq[T] or iterator: T): auto =
  ## Linear "curve". 
  ## Two control points define a segment.
  ## The curve passes through all control points.
  ## Number of control points in a multi segment curve is 2 + (n * 1)
  initCurve(
    cp, 2, 1, 1, 
    @[
      @[-1.0, 1.0],
      @[ 1.0]
    ]
  )


func simple3*[T](cp: seq[T] or iterator: T): auto =
  ## Simple Cubic curve. Linear segments
  ## Two control points define a segment.
  ## The curve passes through all control points.
  ## Number of control points in a multi segment curve is 2 + (n * 1)
  initCurve(
    cp, 2, 1, 3, 
    @[
      @[ 2.0, -2.0],
      @[-3.0, 3.0],
      @[ 0.0, 0.0],
      @[ 1.0]
    ]
  )


func simple3ESC*[T](cp: seq[T] or iterator: T, esc: float): auto =
  ## Simple Cubic curve with Endpoint Slope Control. Linear segments
  ## Two control points define a segment.
  ## The curve passes through all control points.
  ## Number of control points in a multi segment curve is 2 + (n * 1)
  ## esc: controls the velocity, spacing of points. It goes to the value of esc
  ## at the control points.
  initCurve(
    cp, 2, 1, 3,
    @[
      @[ 2.0 - (2.0 * esc), (2.0 * esc) - 2.0],
      @[ (3.0 * esc) - 3.0, 3.0 - (3.0 * esc)],
      @[-esc, esc],
      @[ 1.0]
    ]
  )


func simple3MSC*[T](cp: seq[T] or iterator: T, msc: float): auto =
  ## Simple Cubic curve with Midpoint Slope Control
  ## Two control points define a segment.
  ## The curve passes through all control points.
  ## Number of control points in a multi segment curve is 2 + (n * 1)
  ## msc: controls the velocity, spacing of points. It goes to the value of msc
  ## at the mid point between the control points.
  initCurve(
    cp, 2, 1, 3,
    @[
      @[ (4.0 * msc) - 4.0, 4.0 - (4.0 * msc)],
      @[ 6.0 - (6.0 * msc), (6.0 * msc) - 6.0],
      @[ (2.0 * msc) - 3.0, 3.0 - (2.0 * msc)],
      @[ 1.0]
    ]
  )


func simple3IESC*[T](cp: seq[T] or iterator: T, esc0: float, esc1: float): auto =
  ## Simple Cubic with Independent Endpoint Slope Control
  ## Two control points define a segment.
  ## The curve passes through all control points.
  ## Number of control points in a multi segment curve is 2 + (n * 1)
  ## esc0, esc1: control the velocity, spacing of points. It goes to the value of esc
  ## at the control point.
  initCurve(
    cp, 2, 1, 3,
    @[
      @[ 2.0 - esc0 - esc1, esc0 + esc1 - 2.0],
      @[ (2.0 * esc0) + esc1 - 3.0, 3.0 - (2.0 * esc0) - esc1],
      @[-esc0, esc0],
      @[ 1.0]
    ]
  )


func bezier2*[T](cp: seq[T] or iterator: T): auto =
  ## Quadratic Bezier curve
  ## Three control points define a segment.
  ## The curve passes through the first and last control point of a segment.
  ## The curve is "attracted" to the middel control point.
  ## Number of control points in a multi segment curve is 3 + (n * 2)
  initCurve(
    cp, 3, 2, 2,
    @[
      @[ 1.0, -2.0, 1.0],
      @[-2.0, 2.0],
      @[ 1.0]
    ]
  )


func bezier2EVC*[T](cp: seq[T] or iterator: T, evc: float): auto =
  ## Quadratic Bezier curve with Endpoint Velocity Control 
  ## Three control points define a segment.
  ## The curve passes through the first and last control point of a segment.
  ## The curve is "attracted" to the middel control point.
  ## Number of control points in a multi segment curve is 3 + (n * 2)
  ## evc: Changes the length of the end tangent vectors. This pushes the centre of
  ## the curve towards the middle control point. At evc = 0 it is a quadratic
  ## Bezier curve. At vec = -2 it is a simple cubic curve and at vec = 2 the 
  ## curve goes through the middle control point.
  initCurve(
    cp, 3, 2, 2,
    @[
      @[-evc, 0.0, evc],
      @[ 1.0 + (2.0 * evc), -2.0 - evc, 1.0 - evc],
      @[-2.0 - evc, 2.0 + evc],
      @[ 1.0]
    ]
  )


func bezier3*[T](cp: seq[T] or iterator: T): auto =
  ## Cubic Bezier curve 
  ## Four control points define a segment.
  ## The curve passes through the first and last control point of a segment.
  ## The curve is "attracted" to the middel control points.
  ## Number of control points in a multi segment curve is 3 + (n * 3)
  initCurve(
    cp, 4, 3, 3, 
    @[
      @[-1.0, 3.0, -3.0, 1.0],
      @[ 3.0, -6.0, 3.0],
      @[-3.0, 3.0],
      @[ 1.0]
    ]
  )


func bezier3EVC*[T](cp: seq[T] or iterator: T, evc: float): auto =
  ## Cubic Bezier Curve with Endpoint Velocity Control    
  ## Four control points define a segment.
  ## The curve passes through the first and last control point of a segment.
  ## The curve is "attracted" to the middel control points.
  ## Number of control points in a multi segment curve is 3 + (n * 3)
  ## evc: Changes the length of the end tangent vectors. This pushes 
  ## the curve towards the middle control point.  
  initCurve(
    cp, 4, 3, 3,
    @[
      @[-1.0 - evc, 3.0 + evc, -3.0 - evc, 1.0 + evc],
      @[ 3.0 + (2 * evc), -6.0 - (2 * evc), 3.0 + evc, -evc],
      @[-3.0 - evc, 3.0 + evc],
      @[ 1.0]
    ]
  )


func bspline2*[T](cp: seq[T] or iterator: T): auto =
  ## Quadratic b-spline aka parabolic spline
  ## Three control points define a segment.
  ## The curve does not pass through any control point. It starts half way P0P1
  ## and ends half way P1P2.
  ## The curve is "attracted" to the middel control points.
  ## Number of control points in a multi segment curve is 3 + (n * 1)
  initCurve(
    cp, 3, 1, 2,
    divideMatrix(@[
      @[ 1.0, -2.0, 1.0],
      @[-2.0, 2.0],
      @[ 1.0, 1.0]
    ], 2.0) 
  )


func bspline3*[T](cp: seq[T] or iterator: T): auto =
  ## Cubic b-spline
  ## Four control points define a segment.
  ## The curve does not pass through any control point. It goes from near
  ## P1 to near P2
  ## The curve is "attracted" to the middel control points.
  ## Number of control points in a multi segment curve is 4 + (n * 1)
  initCurve(
    cp, 4, 1, 3, 
    divideMatrix(@[
      @[-1.0, 3.0, -3.0, 1.0],
      @[ 3.0, -6.0, 3.0],
      @[-3.0, 0.0, 3.0],
      @[ 1.0, 4.0, 1.0]
    ], 6.0)
  )


func cardinal*[T](cp: seq[T] or iterator: T, tension: float): auto =
  ## Cardinal spline
  ## Four control points define a segment.
  ## Number of control points in a multi segment curve is 4 + (n * 1)
  let t = (1.0 - tension) / 2.0
  initCurve(
    cp, 4, 1, 3,
    @[
      @[-t, 2.0 - t, t - 2.0, t],
      @[2.0 * t, t - 3.0, 3.0 - (2.0 * t), -t],
      @[-t, 0.0, t, 0.0],
      @[ 0.0, 1.0, 0.0, 0.0]
    ]
  )


func golden_curve*[T](cp: seq[T] or iterator: T): auto =
  ## Golden curve, quadratic three point 
  ## Three control points define a segment.
  ## The curve passes through all control point. 
  ## At t = 0.5 it passes through the middle control point.
  ## Number of control points in a multi segment curve is 3 + (n * 2)
  initCurve(
    cp, 3, 2, 2,
    @[
      @[ 2.0,-4.0, 2.0],
      @[-3.0, 4.0,-1.0],
      @[1.0]
    ]
  )


func four_point3_curve*[T](cp: seq[T] or iterator: T): auto =
  ## Cubic Four Point curve
  ## (identical to a quadratic Lagrange with t1 = 1/3 and t2 = 2/3)
  ## Four control points define a segment.
  ## The curve passes through all control point. 
  ## Number of control points in a multi segment curve is 4 + (n * 3)
  initCurve(
    cp, 4, 3,  3,
    divideMatrix(@[
      @[ -9.0, 27.0,-27.0, 9.0],
      @[ 18.0,-45.0, 36.0,-9.0],
      @[-11.0, 18.0, -9.0, 2.0],
      @[  2.0]
    ], 2.0) 
  )


func hermiteC*[T](cp: seq[T] or iterator: T): auto =
  ## Hermite curve in conventional form
  ## Four control points define a segment.
  ## The curve passes through the first and second control point. The 
  ## third and fourth control points are tangent vectors.
  ## Number of control points in a multi segment curve is 4 + (n * 3)
  ## Not realy suitable for piece wise interpolation.
  initCurve(
    cp, 4, 3, 3,
    @[
      @[ 2.0,-2.0, 1.0, 1.0],
      @[-3.0, 3.0,-2.0,-1.0],
      @[ 0.0, 0.0, 1.0],
      @[ 1.0]
    ]
  )


func hermite*[T](cp: seq[T] or iterator: T, tension: float, bias: float): auto =
  ## Hermite curve (in Bezier form)
  ## Four control points define a segment.
  ## The curve passes through the first and last control point. The two
  ## middle control points are tangent vectors.
  ## Number of control points in a multi segment curve is 4 + (n * 3)
  initCurve(
    cp, 4, 1, 3,
    @[
      @[2.0, -3.0, 0.0, 1.0],
      @[-2.0, 3.0, 0.0, 0.0],
      @[tension + bias, tension - bias, -tension - (2.0 * bias), bias - tension],
      @[bias - tension, tension - (2.0 * bias), bias, 0.0]
    ]
  )


func catmullrom*[T](cp: seq[T] or iterator: T): auto =
  ## Catmull Rom spline 
  ## Four control points define a segment.
  ## The curve passes through the middle two control point.
  ## Number of control points in a multi segment curve is 4 + (n * 1)
  initCurve(
    cp, 4, 1, 3,
    divideMatrix(@[
      @[-1.0, 3.0,-3.0, 1.0],
      @[ 2.0,-5.0, 4.0,-1.0],
      @[-1.0, 0.0, 1.0],
      @[ 0.0, 2.0]
    ], 2.0)
  )


func lagrange2*[T](cp: seq[T] or iterator: T, t2: float): auto =
  ## Quadratic Lagrange curve
  ## Tree control points define a segment.
  ## The curve passes through all three control points.
  ## Number of control points in a multi segment curve is 3 + (n * 2)
  ## t2: defines at what value for t the curve goes through P2
  let 
    n1 = 1.0 / t2
    n2 = 1.0 / (t2 * (t2 - 1.0))
    n3 = 1.0 / (1.0 - t2)
  initCurve(cp, 3, 2, 
    @[
      @[ n1, n2, n3],
      @[-n1 - 1.0, -n2, -n3 * t2],
      @[ 1.0],
    ]
  )


func beta_spline*[T](cp: seq[T] or iterator: T, tension: float, bias: float): auto =
  ## Beta spline, Cubic B-spline with Tension, Bias 
  ## Four control points define a segment.
  ## The curve passes through none of the control points. It goes from near P1 to near P2.
  ## Number of control points in a multi segment curve is 4 + (n * 1)
  ## T: from 0 to infinity, attracts to control point P1. From 0 to -6 pushes away from P1.
  ## B: from 0 to 1, pushes towards P2, from 1 to infinity pushes towards P1
  ## B = 1 & T = 0 --> B-spline
  initCurve(
    cp, 4, 1, 3,
    divideMatrix(
      @[
        @[-2.0 * pow(bias, 3.0),
           2.0 * (tension + pow(bias, 3.0) + pow(bias, 2.0) + bias), 
          -2.0 * (tension + pow(bias, 2.0) + bias + 1.0),         
           2.0
        ],
        @[ 6.0 * pow(bias, 3.0),                             
          -3.0 * (tension + (2.0 * pow(bias, 3.0) + (2.0 * pow(bias, 2.0)))),
           3.0 * (tension + (2.0 * pow(bias, 2.0)))                 
        ],
        @[-6.0 * pow(bias, 3.0),     
           6.0 * (pow(bias, 3.0) - bias),
           6.0 * bias             
        ],
        @[ 2.0 * pow(bias, 3.0),            
           tension + (4.0 * (pow(bias, 2.0) + bias)),
           2.0
        ]
      ], 
      (tension + (2.0 * pow(bias, 3.0)) + (4.0 * pow(bias,2.0)) + (4.0 * bias) + 2.0)
    ),
  )


func tcb_spline*[T](cp: seq[T] or iterator:T, tension:float, cont:float, bias:float): auto =
  ## Kochanek Bartels aka TCB-spline. Tension, Continuity, Bias.
  ## Four control points define a segment.
  ## The curve passes trough P1 and P2.
  ## Number of control points in a multi segment curve is 4 + (n * 1)
  ## Tension T = +1 --> Tight, T = -1 --> Round
  ## Bias B = 1 --> Post Shoot,  B = -1 --> Pre shoot
  ## Continuity C = +1 --> Inverted corners,  C = -1 --> Box corners
  ## T = B = C --> Catmull Rom
  ## T = 1 --> simple cubic
  ## T = B = 0 & C = -1 linear interpolation
  let 
    a = (1.0 - tension) * (1.0 + cont) * (1.0 + bias)
    b = (1.0 - tension) * (1.0 - cont) * (1.0 - bias)
    c = (1.0 - tension) * (1.0 - cont) * (1.0 + bias)
    d = (1.0 - tension) * (1.0 + cont) * (1.0 - bias) 
  initCurve(
    cp, 4, 1, 3,
    divideMatrix(@[
      @[-a,          
         4.0 + a - b - c,
        -4.0 + b + c - d,
        d                
      ],
      @[ 2.0 * a,
        -6.0 - (2.0 * a) + (2.0 * b) + c,
         6.0 - (2.0 * b) - c + d,
        -d
      ],
      @[-a,
         a - b,
         b
      ],
      @[0.0, 2.0]
    ], 2.0)
  )


func palmer*[T](cp: seq[T] or iterator:T, t:float, b:float): auto =
  ## Palmer with Tension, Bias 
  ## Three control points define a segment.
  ## The curve passes trough P1 and P3 and passes by P2.
  ## Number of control points in a multi segment curve is 3 + (n * 2)
  let 
    n0 = 1.0 / b
    n1 = 1.0 / (b * (1.0 - b))
    n2 = 1.0 / (1.0 - b)
  initCurve(
    cp, 3, 2, 3,
    @[
      @[-2.0 * t, 0.0, 2.0 * t],
      @[n0 + (3.0 * t), -n1, n2 - (3.0 * t)],
      @[-1.0 - n0 - t, n1, t - (n2 / n0)],
      @[ 1.0]
    ]
  )



#[ 
t goes from 0 - 1. From t figure out in what segment of the curve we are.
from the segment number and t figure out what the equivalent segment time tseg is.
segments are interpolated from 0-1, using tseg.
if in a new segment, precalculate the weighting matrix values for that segment
finaly run Horner 
]#
func getPoint*[T](curve: var Curve[T], t: float): auto =
  ## Gets a single point from the curve, with t in range[0,1].
  var
    segment: int
    tseg: float
  if t < 1.0:
    segment = int(floor(t * float(curve.segments)))
    tseg = (t * float(curve.segments)) - float(segment)
  else:
    segment = curve.segments - 1
    tseg = 1.0
  if segment != curve.current_seg:
    curve.current_seg = segment
    let current_pos = segment * curve.stepSize
    curve.curmult = newSeqWith[T](len(curve.matrix), curve.origin)
    for i in 0..<len(curve.matrix):
      for j in 0..<len(curve.matrix[i]):
        let jSegment = j + current_pos
        curve.curmult[i] = curve.curmult[i] + (curve.cp[jSegment] * curve.matrix[i][j])
  var F = curve.curmult[0]
  for i in 1..<len(curve.curmult):
    F = (F * tseg) + curve.curmult[i]
  return F 


func getIterVecs1[T](curmult:var seq[T], delta:seq[float]):seq[T]=
  # set up vectors for forward diffences: initial speed, initial accelleration and
  # accelleration change.
  var iterCurves: seq[T]
  #starting point of segment
  iterCurves.add(pop(curmult))
  ##initial velocity
  var d1: T
  for i in 0..<len(curmult):
    d1 += curmult[i] * delta[i+2]
  iterCurves.add(d1)
  ##initial acceleration
  var d2: T
  for i in 0..<len(curmult)-1:
    var n:float
    if i == 0:
      n = 6.0
    elif i == 1:
      n = 2.0
    d2 += curmult[i] * delta[i+2] * n
  iterCurves.add(d2)
  #acceleration change
  var d3: T
  for i in 0..<len(curmult)-2:
    d3 += curmult[i] * delta[i+2] * 6.0
  iterCurves.add(d3)
  return iterCurves


func getIterVecs2[T](curmult:var seq[T], delta:seq[float]):seq[T]=
  var iterCurves: seq[T]
  #starting point of segment
  iterCurves.add(pop(curmult))
  ##initial velocity
  var d1: T
  for i in 0..<len(curmult):
    d1 += curmult[i] * delta[i+1]
  iterCurves.add(d1)
  ##initial acceleration
  var d2: T
  for i in 0..<len(curmult)-1:
    var n:float
    if i == 0:
      n = 2.0
    elif i == 1:
      n = 6.0
    d2 += curmult[i] * delta[i+1] * n
  iterCurves.add(d2)
  #acceleration change
  var d3: T
  for i in 0..<len(curmult)-2:
    d3 += curmult[i] * delta[i+1] * 6.0
  iterCurves.add(d3)
  return iterCurves


func getIterVecs3[T](curmult:var seq[T], delta:seq[float]):seq[T]=
  var iterCurves: seq[T]
  #starting point of segment
  iterCurves.add(pop(curmult))
  ##initial velocity
  var d1: T
  for i in 0..<len(curmult):
    d1 += curmult[i] * delta[i]
  iterCurves.add(d1)
  ##initial acceleration
  var d2: T
  for i in 0..<len(curmult)-1:
    var n:float
    if i == 0:
      n = 6.0
    elif i == 1:
      n = 2.0
    d2 += curmult[i] * delta[i] * n
  iterCurves.add(d2)
  #acceleration change
  var d3: T
  for i in 0..<len(curmult)-2:
    d3 += curmult[i] * delta[i] * 6.0
  iterCurves.add(d3)
  return iterCurves


proc iterCurve*[T](curve: Curve[T] or ICurve[T], steps:int):auto=
  #create an "internal" closure, if needed, to get the same interface for 
  #sequences and closure iterators as input to iterCurves.
  when curve is Curve[T]: 
    func curvePoints (cps:seq[T]): auto=
      return iterator(): auto=
        for i in cps:
          yield i
    let cp = curvePoints(curve.cp)
  else:
    let cp = curve.cp
  let fsteps = steps.float
  let delta:seq[float] = @[1/(fsteps*fsteps*fsteps), 1/(fsteps*fsteps), 1/fsteps]
  var segment = newSeqWith[T](curve.segmentDim, cp())
  return iterator(): auto=
    while true:
      #multiply the matrix coefficients by the control points
      var curmult = newSeqWith[T](len(curve.matrix), curve.origin)
      for i in 0..<len(curve.matrix):
        curmult[i] = curve.origin
        for j in 0..<len(curve.matrix[i]):
          curmult[i] = curmult[i] + (segment[j] * curve.matrix[i][j])
      # set up vectors for forward diffences: initial speed, initial acelleration and
      # acelleration change.
      var iterCurves = case curve.degree:
        of 1: getIterVecs1(curmult, delta)
        of 2: getIterVecs2(curmult, delta)
        of 3: getIterVecs3(curmult, delta)
        else: raise newException(CurveOrderError, "curve not of degree 1, 2 or 3")
      # step interpolate through one segment, except for last point
      # as it is the same as the first of the next segment
      var i = 0
      while i < steps:
        yield iterCurves[0]
        iterCurves[0] += iterCurves[1]
        iterCurves[1] += iterCurves[2]
        iterCurves[2] += iterCurves[3]
        inc(i)
      #get the points for the next segment
      for i in 0..<curve.stepSize:
        segment.delete(0)
        let val:T = cp()
        if finished(cp): 
          break
        segment.add(val)
      if finished(cp):
        #for the last segment, also yield its last point.
        yield iterCurves[0] 
        break


when is_main_module:
  import pixie
  
  func vals(data:seq):auto=
    return iterator():auto=
      for i in data:
        yield i

  let 
    image = newImage(800, 800)
    ctx = newContext(image)
    v0 = @[
      vec2(200, 400), vec2(200, 200), vec2(400, 200), vec2(400, 400),
      vec2(400, 600), vec2(600, 600), vec2(600, 400)
    ]
    resolution = 100
    steps = 50

  var
    #return interpolated points based on t-value
    tlin = bezier3(v0)

  let
    ip = iterCurve(tlin, steps)
    itlin = bezier3(vals(v0))
    itp = iterCurve(itlin, steps)


  image.fill(rgba(255, 255, 255, 255))
  ctx.fillStyle = rgba(255, 0, 0, 255)
  for i in v0:
    ctx.fillCircle(circle(i, 8))

  
  ctx.fillStyle = rgba(0, 255, 0, 125)
  for i in 0..<resolution:
    var t = i / (resolution - 1)
    ctx.fillCircle(circle(getPoint(tlin, t), 9))


  ctx.fillStyle = rgba(255, 0, 0, 125)
  while true:
    let val = ip()
    if finished(ip):break
    ctx.fillCircle(circle(val, 6))


  ctx.fillStyle = rgba(0, 0, 255, 125)
  while true:
    let val = itp()
    if finished(itp):break
    ctx.fillCircle(circle(val, 3))


  image.writeFile("curve_img.png")