At the bottom of De Boor's Algorithm, it is said that
De Boor's algorithm also works for NURBS curves. We just multiply every control point by its weight converting the NURBS curve to a 4D B-spline curve, perform de Boor's algorithm on this 4D B-spline curve, and then project the resulting curve back by dividing the first three components with the fourth and keeping the fourth component as its new weight.
Then modifying the code from B-Spline derivative using de Boor's algorithm, I came up with the following.
import numpy as np
import math as m
weights = [0.3, 1, 1, 2, 1, 1, 0.5, 1, 1, 3, 1]
def deBoor(k, x, t, c_, p):
c = []
for point, w in zip(c_, weights):
c.append([point[0]*w, point[1]*w, point[2]*w, w])
c = np.array(c)
d = [c[j + k - p] for j in range(0, p+1)]
for r in range(1, p+1):
for j in range(p, r-1, -1):
alpha = (x - t[j+k-p]) / (t[j+1+k-r] - t[j+k-p])
d[j] = (1.0 - alpha) * d[j-1] + alpha * d[j]
return np.array([
d[p][0] / d[p][3],
d[p][1] / d[p][3],
d[p][2] / d[p][3]
])
def deBoorDerivative(k, x, t, c_, p):
c = []
for point, w in zip(c_, weights):
c.append([point[0]*w, point[1]*w, point[2]*w, w])
c = np.array(c)
q = [p * (c[j+k-p+1] - c[j+k-p]) / (t[j+k+1] - t[j+k-p+1]) for j in range(0, p)]
for r in range(1, p):
for j in range(p-1, r-1, -1):
right = j+1+k-r
left = j+k-(p-1)
alpha = (x - t[left]) / (t[right] - t[left])
q[j] = (1.0 - alpha) * q[j-1] + alpha * q[j]
return np.array([
q[p-1][0] / q[p-1][3],
q[p-1][1] / q[p-1][3],
q[p-1][2] / q[p-1][3]
])
def finiteDifferenceDerivative(k, x, t, c, p):
f = lambda xx : deBoor(k, xx, t, c, p)
dx = 1e-7
return (- f(x + 2 * dx) \
+ 8 * f(x + dx) \
- 8 * f(x - dx) \
+ f(x - 2 * dx)) / ( 12 * dx )
points = np.array([[i, m.sin(i / 3.0), m.cos(i / 2)] for i in range(0, 11)])
knots = np.array([0, 0, 0, 0, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7, 0.8, 1.0, 1.0, 1.0, 1.0])
a = deBoorDerivative(7, 0.44, knots, points, 3)
b = finiteDifferenceDerivative(7, 0.44, knots, points, 3)
print(a)
print(b)
Although the derivative calculated from finite difference is not the same as the one when using deboors algorithm.
[ 9.125 1.02221755 -2.22839545]
[16.85238398 0.14138772 -5.90135073]
Solved it. This computes the derivative (velocity) and the position (point) at t at once using deboors algorithm, (written in C).
typedef struct vec3 { double x, y, z; } vec3_t;
typedef struct vec4 { double x, y, z, w; } vec4_t;
vec4_t vec4homo (vec3_t u, double w) { return (vec4_t){u.x * w, u.y * w, u.z * w, w }; }
vec4_t vec4add (vec4_t u, vec4_t v) { return (vec4_t){u.x + v.x, u.y + v.y, u.z + v.z, u.w + v.w}; }
vec4_t vec4sub (vec4_t u, vec4_t v) { return (vec4_t){u.x - v.x, u.y - v.y, u.z - v.z, u.w - v.w}; }
vec4_t vec4mul (vec4_t u, double s) { return (vec4_t){u.x * s, u.y * s, u.z * s, u.w * s }; }
vec4_t vec4div (vec4_t u, double s) { return (vec4_t){u.x / s, u.y / s, u.z / s, u.w / s }; }
vec3_t vec4trunc (vec4_t u) { return (vec3_t){u.x, u.y, u.z }; }
vec3_t vecadd (vec3_t u, vec3_t v) { return (vec3_t){u.x + v.x, u.y + v.y, u.z + v.z}; }
vec3_t vecsub (vec3_t u, vec3_t v) { return (vec3_t){u.x - v.x, u.y - v.y, u.z - v.z}; }
vec3_t vecmul (vec3_t u, double s) { return (vec3_t){u.x * s, u.y * s, u.z * s }; }
vec3_t vecdiv (vec3_t u, double s) { return (vec3_t){u.x / s, u.y / s, u.z / s }; }
typedef struct pv {
vec3_t position;
vec3_t velocity;
} pv_t;
typedef struct nurbs {
vec3_t P[100];
double w[100];
double U[100];
int p;
int m;
int n;
} nurbs_t;
int findspan(double* U, double t, int n, int p) {
if(t >= U[n]) { return n - 1; }
if(t <= U[p]) { return p; }
int low = p;
int high = n;
int mid = (low + high) / 2;
while(t < U[mid] || t >= U[mid+1]) {
if(t < U[mid]) { high = mid; }
else { low = mid; }
mid = (low + high) / 2;
}
return mid;
}
pv_t nurbs_deboor(double t, nurbs_t* func) {
vec3_t* P = func->P;
double* U = func->U;
double* w = func->w;
int p = func->p;
int m = func->m;
int n = func->n;
int k = findspan(U, t, n, p);
vec4_t d[30];
vec4_t q[30];
for(int i = 0; i < p + 1; i++) {
d[i] = vec4homo(P[i+k-p], w[i+k-p]);
if(!(i < p)) { continue; }
q[i] = vec4mul(vec4sub(vec4homo(P[i+k-p+1], w[i+k-p+1]), vec4homo(P[i+k-p], w[i+k-p])), p);
q[i] = vec4div(q[i], U[i+k+1] - U[i+k-p+1]);
}
for(int r = 1; r < p + 1; r++) {
for(int j = p; j > r - 1; j--) {
double alpha = (t - U[j+k-p]) / (U[j+1+k-r] - U[j+k-p]);
d[j] = vec4add(vec4mul(d[j-1], 1.0-alpha), vec4mul(d[j], alpha));
if(!(r < p && j < p)) { continue; }
alpha = (t - U[j+k-p+1]) / (U[j+1+k-r] - U[j+k-p+1]);
q[j] = vec4add(vec4mul(q[j-1], 1.0-alpha), vec4mul(q[j], alpha));
}
}
pv_t pv;
pv.position = vecdiv(vec4trunc(d[p]), d[p].w);
pv.velocity = vecdiv(vecsub(vec4trunc(q[p-1]), vecmul(pv.position, q[p-1].w)), d[p].w);
return pv;
}