robotics : Ss9rw
Arcs and Bézier Curves
Mathematics and code for evaluating arcs and bézier curves.
MarkdownIntroduction
This post will cover the mathematics and code for evaluating arcs and bézier curves. Arcs can prove to be useful for the obvious case when you want a pure arc trajectory. Bézier curves may prove useful tracing fonts which are usually described with bézier curves.
As always, the code presented in this post is ANSI C. Not every sub-routine and datatype will be covered in this post. For that, visit the repository at github.com/fullnitrous/arm, all the relevant code is found under /src/core/.
Arcs
An arc is considered to be part of, or a circle, it is defined to span from the vectors \(\vec{v_0}\) to \(\vec{v_1}\) and a center point \(\vec{c}\). Optionally, the arc can overwrite the angle between \(\vec{v_0}\) and \(\vec{v_1}\) to span by angle \(a_{\text{offset}}\).
Given \(\vec{v_0}\) \(\vec{v_1}\), \(\vec{c}\) and \(a_{\text{offset}}\), the vectors \(\vec{u}\) and \(\vec{v}\) are defined by the following equations.
\[ \begin{aligned} \vec{u} &= \vec{v_0} - \vec{c} \\ \vec{v} &= \vec{v_1} - \vec{c} \end{aligned} \]
The following conditions must be met for \(\vec{u}\) and \(\vec{v}\) in order for \(\vec{v_0}\), \(\vec{v_1}\) and \(\vec{c}\) to specify a valid arc.
- \(|\vec{u}| = |\vec{v}|\)
- The lengths of vectors \(\vec{u}\) and \(\vec{v}\) must be the same because points on the arc must have the same radius (distance from \(\vec{c}\)).
- \(|\vec{u} \times \vec{v}| > 0\)
- The magnitude of the cross product between \(\vec{u}\) and \(\vec{v}\) must be non-zero, otherwise the vectors do not specify a plane for the arc.
If the conditions are met, then the arc is defined by its normal vector \(\vec{n}\), center point \(\vec{c}\) and radius vector (initial point) \(\vec{r}\) and angle \(a\).
\[ \begin{aligned} \vec{n} &= \frac{\vec{u} \times \vec{v}}{|\vec{u} \times \vec{v}|} \\ \vec{r} &= \vec{u} \\ a &= \begin{cases} a_{\text{offset}} & \text{ if use $a_{\text{offset}}$} \\ acos \left ( \frac{\vec{u}\cdot\vec{v}}{|\vec{u}|\cdot|\vec{v}|} \right ) & \text{else} \\ \end{cases} \end{aligned} \]
The type arc_t stores all the aforementioned parameters.
Then using the intrpl_arc routine given the vectors v0, v1, c and optionally angle offset ao (set to 0.0 if not used). Will save the computed arc parameters to arc.
int intrpl_arc(vec3_t v0, vec3_t v1, vec3_t c, double ao, arc_t* arc) {
vec3_t u, v;
u = vecsub(v0, c);
v = vecsub(v1, c);
if(!APPROACHES_ZERO(vecmag(u) - vecmag(v))) { return 0; }
if(APPROACHES_ZERO(vecmag(veccross(u, v)))) { return 0; }
arc->n = vecnorm(veccross(u, v));
arc->c = c;
arc->r = u;
arc->a = ao > 0.0 ? ao : vecangle(u, v);
return 1;
}The arc interpolation function \(A(t), \space t \in [0,1]\) is defined as the following.
\[ A(t) = \vec{r}cos(ta) + (\vec{r}\cdot\vec{n})\vec{n}(1-cos(ta))+(\vec{n}\times\vec{r})sin(ta) \]
This function is derived from a formula I used in the post about Three Dimensional Rocket Simulation. Essentially how it works is by rotating a point around an axis which essentially creates an arc.
Which is programmed as the function eval_arc.
eval_t eval_arc(void* f_, double t) {
arc_t* f = (arc_t*)f_;
vec3_t r = f->r;
vec3_t n = f->n;
vec3_t c = f->c;
double a = t * f->a;
eval_t e;
double cosa = cos(a);
double one_minus_cosa = 1.0 - cosa;
double sina = sin(a);
double minus_sina = -sina;
double minus_cosa = -cosa;
double rdotn = r.x*n.x + r.y*n.y + r.z*n.z;
double ncrossr_x = n.y*r.z - n.z*r.y;
double ncrossr_y = n.z*r.x - n.x*r.z;
double ncrossr_z = n.x*r.y - n.y*r.x;
e.p.x = r.x*cosa + one_minus_cosa*rdotn*n.x + sina*ncrossr_x + c.x;
e.p.y = r.y*cosa + one_minus_cosa*rdotn*n.y + sina*ncrossr_y + c.y;
e.p.z = r.z*cosa + one_minus_cosa*rdotn*n.z + sina*ncrossr_z + c.z;
e.v.x = f->a*(r.x*minus_sina + sina*rdotn*n.x + cosa*ncrossr_x);
e.v.y = f->a*(r.y*minus_sina + sina*rdotn*n.y + cosa*ncrossr_y);
e.v.z = f->a*(r.z*minus_sina + sina*rdotn*n.z + cosa*ncrossr_z);
e.a.x = f->a*f->a*(r.x*minus_cosa + cosa*rdotn*n.x + minus_sina*ncrossr_x);
e.a.y = f->a*f->a*(r.y*minus_cosa + cosa*rdotn*n.y + minus_sina*ncrossr_y);
e.a.z = f->a*f->a*(r.z*minus_cosa + cosa*rdotn*n.z + minus_sina*ncrossr_z);
e.m = vecmag(e.v);
return e;
}
Bézier
A bézier curve \(B(t)\) is defined by its degree \(n\) and a set of \(n+1\) control points \(P_i\).
\[ \begin{aligned} B(t) &= \sum_{i=0}^{n}b_{i,n}P_i, \quad t \in [0,1] \\ b_{i,n}(t) &= \begin{pmatrix} n \\ i \end{pmatrix} t^i(1-t)^{n-i} \\ \begin{pmatrix}n\\i\end{pmatrix} &= \frac{n!}{i!(n-i)~} \end{aligned} \]
Its first and second derivatives are.
\[ \begin{aligned} B'(t) &= n\sum_{i=0}^{n-1}b_{i,{n-1}}(P_{i+1}-P_i) \\ B''(t) &= (n-1)n\sum_{i=0}^{n-2}b_{i,{n-2}}(P_{i+2}-2P_{i+1}+P_i) \\ \end{aligned} \]
Binomial Coefficient Lookup Table
Since bézier curves are a linear combination of Bernstein Basis Polynomials which are defined by binomial coefficients, a lookup table for the coefficients is preffered for faster evaluation of the curve.
A maximum degree for the bézier curve EVAL_BEZIER_MAX_DEG is defined and the size of the lookup table is defined by this.
The function eval_bezier_lut_init initializes the global lookup table.
void eval_bezier_lut_init(void) {
int n, i;
for(n = 0; n <= EVAL_BEZIER_MAX_DEG; n++) {
for(i = 0; i <= n; i++) {
eval_bico_lut[n][i] = __eval_bico(n, i);
}
}
return;
}Helper function __eval_fac for calculating the factorial of a number.
Helper function __eval_bico to calculate the binomial coefficient.
Then finally the function eval_bico for evaluating the binomial coefficient for n and i. If its possible to find the coefficient with the lookuptable then it simply does a lookup, otherwise it evaluates it using __eval_bico.
int eval_bico(int n, int i) {
if(n <= EVAL_BEZIER_MAX_DEG) { return eval_bico_lut[n][i]; }
return __eval_bico(n, i);
}The function eval_bernstein_basis for evaluating the Bernstein basis polynomial \(b_{i,n}(t)\) simply follows by the formula.
double eval_bernstein_basis(int i, int n, double t) {
return eval_bico(n, i) * pow(t, i) * pow(1.0 - t, n - i);
}Then finally the functions eval_bezier and __eval_bezier_fast are used in order to evaluate the curve. The first function calculates the derivatives where the second only calculates the position.
eval_t eval_bezier(void* f_, double t) {
bezier_t* f = (bezier_t*)f_;
vec3_t* p = f->p;
int n = f->n;
eval_t e;
int i;
double x, y, z, b;
for(i = 0, x = y = z = 0.0; i <= n; i++) {
b = eval_bernstein_basis(i, n, t);
x += b*p[i].x;
y += b*p[i].y;
z += b*p[i].z;
}
e.p.x = x;
e.p.y = y;
e.p.z = z;
for(i = 0, x = y = z = 0.0; i <= n - 1; i++) {
b = eval_bernstein_basis(i, n - 1, t);
x += b*(p[i+1].x - p[i].x);
y += b*(p[i+1].y - p[i].y);
z += b*(p[i+1].z - p[i].z);
}
e.v.x = n*x;
e.v.y = n*y;
e.v.z = n*z;
e.m = vecmag(e.v);
for(i = 0, x = y = z = 0.0; i <= n - 2; i++) {
b = eval_bernstein_basis(i, n - 2, t);
x += b*(p[i+2].x - 2*p[i+1].x + p[i].x);
y += b*(p[i+2].y - 2*p[i+1].y + p[i].y);
z += b*(p[i+2].z - 2*p[i+1].z + p[i].z);
}
b = (n-1)*n;
e.a.x = b*x;
e.a.y = b*y;
e.a.z = b*z;
return e;
}vec3_t __eval_bezier_fast(void* f_, int k, double t) {
bezier_t* f = (bezier_t*)f_;
vec3_t* p = f->p;
int n = f->n;
vec3_t e;
int i;
double b;
e.x = e.y = e.z = 0.0;
for(i = 0; i <= n; i++) {
b = eval_bernstein_basis(i, n, t);
e.x += b*p[i].x;
e.y += b*p[i].y;
e.z += b*p[i].z;
}
return e;
}