kicad/thirdparty/tinyspline_lib/tinyspline.c

1390 lines
38 KiB
C

/*
* The MIT License (MIT)
*
* Copyright (c) 2016 Marcel Steinbeck
*
* Permission is hereby granted, free of charge, to any person obtaining a copy
* of this software and associated documentation files (the "Software"), to deal
* in the Software without restriction, including without limitation the rights
* to use, copy, modify, merge, publish, distribute, sublicense, and/or sell
* copies of the Software, and to permit persons to whom the Software is
* furnished to do so, subject to the following conditions:
*
* The above copyright notice and this permission notice shall be included in all
* copies or substantial portions of the Software.
*
* THE SOFTWARE IS PROVIDED "AS IS", WITHOUT WARRANTY OF ANY KIND, EXPRESS OR
* IMPLIED, INCLUDING BUT NOT LIMITED TO THE WARRANTIES OF MERCHANTABILITY,
* FITNESS FOR A PARTICULAR PURPOSE AND NONINFRINGEMENT. IN NO EVENT SHALL THE
* AUTHORS OR COPYRIGHT HOLDERS BE LIABLE FOR ANY CLAIM, DAMAGES OR OTHER
* LIABILITY, WHETHER IN AN ACTION OF CONTRACT, TORT OR OTHERWISE, ARISING FROM,
* OUT OF OR IN CONNECTION WITH THE SOFTWARE OR THE USE OR OTHER DEALINGS IN THE
* SOFTWARE.
*/
#include "tinyspline.h"
#include <stdlib.h> /* malloc, free */
#include <math.h> /* fabs, sqrt */
#include <string.h> /* memcpy, memmove, strcmp */
#include <setjmp.h> /* setjmp, longjmp */
/********************************************************
* *
* Error handling *
* *
********************************************************/
#define TRY( x, y ) y = (tsError) setjmp( x ); if( y == 0 ) {
#define CATCH } \
else {
#define ETRY }
/********************************************************
* *
* Internal functions *
* *
********************************************************/
void ts_internal_deboornet_copy( const tsDeBoorNet* original,
tsDeBoorNet* copy, jmp_buf buf
)
{
const size_t dim = original->dim;
const size_t n_points = original->n_points;
const size_t sof_f = sizeof(tsReal);
const size_t sof_p = n_points * dim * sof_f;
if( original == copy )
return;
copy->u = original->u;
copy->k = original->k;
copy->s = original->s;
copy->h = original->h;
copy->dim = dim;
copy->n_points = n_points;
copy->points = (tsReal*) malloc( sof_p );
if( copy->points == NULL )
longjmp( buf, TS_MALLOC );
memcpy( copy->points, original->points, sof_p );
copy->result = copy->points + (n_points - 1) * dim;
}
void ts_internal_bspline_find_u( const tsBSpline* bspline, const tsReal u,
size_t* k, size_t* s, jmp_buf buf
)
{
const size_t deg = bspline->deg;
const size_t order = bspline->order;
const size_t n_knots = bspline->n_knots;
*k = *s = 0;
for( ; *k < n_knots; (*k)++ )
{
const tsReal uk = bspline->knots[*k];
if( ts_fequals( u, uk ) )
{
(*s)++;
}
else if( u < uk )
{
break;
}
}
/* keep in mind that currently k is k+1 */
if( *s > order )
longjmp( buf, TS_MULTIPLICITY );
if( *k <= deg ) /* u < u_min */
longjmp( buf, TS_U_UNDEFINED );
if( *k == n_knots && *s == 0 ) /* u > u_last */
longjmp( buf, TS_U_UNDEFINED );
if( *k > n_knots - deg + *s - 1 ) /* u > u_max */
longjmp( buf, TS_U_UNDEFINED );
(*k)--; /* k+1 - 1 will never underflow */
}
void ts_internal_bspline_copy( const tsBSpline* original,
tsBSpline* copy, jmp_buf buf
)
{
const size_t dim = original->dim;
const size_t sof_f = sizeof(tsReal);
const size_t n_ctrlp = original->n_ctrlp;
const size_t n_knots = original->n_knots;
const size_t sof_ck = (n_ctrlp * dim + n_knots) * sof_f;
/* Nothing to do here. */
if( original == copy )
return;
copy->deg = original->deg;
copy->order = original->order;
copy->dim = original->dim;
copy->n_ctrlp = original->n_ctrlp;
copy->n_knots = original->n_knots;
copy->ctrlp = (tsReal*) malloc( sof_ck );
if( copy->ctrlp == NULL )
longjmp( buf, TS_MALLOC );
memcpy( copy->ctrlp, original->ctrlp, sof_ck );
copy->knots = copy->ctrlp + n_ctrlp * dim;
}
void ts_internal_bspline_fill_knots( const tsBSpline* original, const tsBSplineType type,
const tsReal min, const tsReal max,
tsBSpline* result, jmp_buf buf
)
{
const size_t n_knots = original->n_knots;
const size_t deg = original->deg;
const size_t order = deg + 1; /* Using deg+1 instead of original->order
* ensures order >= 1. */
tsReal fac; /* The factor used to calculate the knot values. */
size_t i; /* Used in for loops. */
/* order >= 1 implies 2*order >= 2 implies n_knots >= 2 */
if( n_knots < 2 * order )
longjmp( buf, TS_DEG_GE_NCTRLP );
if( type == TS_BEZIERS && n_knots % order != 0 )
longjmp( buf, TS_NUM_KNOTS );
if( min > max || ts_fequals( min, max ) )
longjmp( buf, TS_KNOTS_DECR );
/* copy spline even if type is TS_NONE */
ts_internal_bspline_copy( original, result, buf );
if( type == TS_OPENED )
{
/* ensures that the first knot value is exactly \min */
result->knots[0] = min; /* n_knots >= 2 */
fac = (max - min) / (n_knots - 1); /* n_knots >= 2 */
for( i = 1; i < n_knots - 1; i++ )
result->knots[i] = min + i * fac;
/* ensure that the last knot value is exactly \max */
result->knots[i] = max;
}
else if( type == TS_CLAMPED )
{
/* n_knots >= 2*order == 2*(deg+1) == 2*deg + 2 > 2*deg - 1 */
fac = (max - min) / (n_knots - 2 * deg - 1);
ts_arr_fill( result->knots, order, min );
for( i = order; i < n_knots - order; i++ )
result->knots[i] = min + (i - deg) * fac;
ts_arr_fill( result->knots + i, order, max );
}
else if( type == TS_BEZIERS )
{
/* n_knots >= 2*order implies n_knots/order >= 2 */
fac = (max - min) / (n_knots / order - 1);
ts_arr_fill( result->knots, order, min );
for( i = order; i < n_knots - order; i += order )
ts_arr_fill( result->knots + i, order, min + (i / order) * fac );
ts_arr_fill( result->knots + i, order, max );
}
}
void ts_internal_bspline_new( const size_t n_ctrlp, const size_t dim, const size_t deg,
const tsBSplineType type, tsBSpline* bspline, jmp_buf buf
)
{
const size_t order = deg + 1;
const size_t n_knots = n_ctrlp + order;
const size_t sof_f = sizeof(tsReal);
const size_t sof_ck = (n_ctrlp * dim + n_knots) * sof_f;
tsError e;
jmp_buf b;
if( dim < 1 )
longjmp( buf, TS_DIM_ZERO );
if( deg >= n_ctrlp )
longjmp( buf, TS_DEG_GE_NCTRLP );
bspline->deg = deg;
bspline->order = order;
bspline->dim = dim;
bspline->n_ctrlp = n_ctrlp;
bspline->n_knots = n_knots;
bspline->ctrlp = (tsReal*) malloc( sof_ck );
if( bspline->ctrlp == NULL )
longjmp( buf, TS_MALLOC );
bspline->knots = bspline->ctrlp + n_ctrlp * dim;
TRY( b, e )
ts_internal_bspline_fill_knots( bspline, type, 0.f, 1.f, bspline, b );
CATCH free( bspline->ctrlp );
longjmp( buf, e );
ETRY
}
void ts_internal_bspline_resize( const tsBSpline* bspline, const int n, const int back,
tsBSpline* resized, jmp_buf buf
)
{
const size_t deg = bspline->deg;
const size_t dim = bspline->dim;
const size_t sof_f = sizeof(tsReal);
const size_t sof_c = dim * sof_f;
const size_t n_ctrlp = bspline->n_ctrlp;
const size_t n_knots = bspline->n_knots;
const size_t nn_ctrlp = n_ctrlp + n; /* The new length of ctrlp. */
const size_t nn_knots = n_knots + n; /* The new length of knots. */
const size_t sof_ncnk = (nn_ctrlp * dim + nn_knots) * sof_f;
const size_t min_n_ctrlp = n < 0 ? nn_ctrlp : n_ctrlp; /* The minimum of
* the control points old and new size. */
const size_t min_n_knots = n < 0 ? nn_knots : n_knots; /* the minimum of
* the knots old and new size. */
tsReal* from_ctrlp = bspline->ctrlp;
tsReal* from_knots = bspline->knots;
tsReal* to_ctrlp = NULL;
tsReal* to_knots = NULL;
/* If n is 0 the spline must not be resized. */
if( n == 0 )
{
ts_internal_bspline_copy( bspline, resized, buf );
return;
}
if( bspline != resized )
{
ts_internal_bspline_new( nn_ctrlp, dim, deg, TS_NONE, resized, buf );
to_ctrlp = resized->ctrlp;
to_knots = resized->knots;
}
else
{
if( nn_ctrlp <= deg )
longjmp( buf, TS_DEG_GE_NCTRLP );
to_ctrlp = (tsReal*) malloc( sof_ncnk );
if( to_ctrlp == NULL )
longjmp( buf, TS_MALLOC );
to_knots = to_ctrlp + nn_ctrlp * dim;
}
/* Copy control points and knots. */
if( !back && n < 0 )
{
memcpy( to_ctrlp, from_ctrlp - n * dim, min_n_ctrlp * sof_c );
memcpy( to_knots, from_knots - n, min_n_knots * sof_f );
}
else if( !back && n > 0 )
{
memcpy( to_ctrlp + n * dim, from_ctrlp, min_n_ctrlp * sof_c );
memcpy( to_knots + n, from_knots, min_n_knots * sof_f );
}
else
{
/* n != 0 implies back == true */
memcpy( to_ctrlp, from_ctrlp, min_n_ctrlp * sof_c );
memcpy( to_knots, from_knots, min_n_knots * sof_f );
}
/* Cleanup if necessary. */
if( bspline == resized )
{
/* free old memory */
free( from_ctrlp );
/* assign new values */
resized->ctrlp = to_ctrlp;
resized->knots = to_knots;
resized->n_ctrlp = nn_ctrlp;
resized->n_knots = nn_knots;
}
}
void ts_internal_bspline_insert_knot( const tsBSpline* bspline,
const tsDeBoorNet* deBoorNet,
const size_t n,
tsBSpline* result,
jmp_buf buf
)
{
const size_t deg = bspline->deg;
const size_t dim = bspline->dim;
const size_t k = deBoorNet->k;
const size_t sof_f = sizeof(tsReal);
const size_t sof_c = dim * sof_f;
size_t N; /* The number of affected control points. */
tsReal* from; /* The pointer to copy the values from. */
tsReal* to; /* The pointer to copy the values to. */
int stride; /* The stride of the next pointer to copy. Will be negative
* later on, thus use int. */
size_t i; /* Used in for loops. */
if( deBoorNet->s + n > bspline->order )
{
longjmp( buf, TS_MULTIPLICITY );
}
/* Use ::ts_bspline_resize even if \n is 0 to copy
* the spline if necessary. */
ts_internal_bspline_resize( bspline, (int) n, 1, result, buf );
if( n == 0 ) /* Nothing to insert. */
return;
N = deBoorNet->h + 1; /* n > 0 implies s <= deg implies a regular evaluation
* implies h+1 is valid. */
/* 1. Copy all necessary control points and knots from
* the original B-Spline.
* 2. Copy all necessary control points and knots from
* the de Boor net. */
/* 1.
*
* a) Copy left hand side control points from original b-spline.
* b) Copy right hand side control points from original b-spline.
* c) Copy left hand side knots from original b-spline.
* d) Copy right hand side knots form original b-spline. */
/* copy control points */
memmove( result->ctrlp, bspline->ctrlp, (k - deg) * sof_c ); /* a) */
from = bspline->ctrlp + dim * (k - deg + N);
to = result->ctrlp + dim * (k - deg + N + n); /* n >= 0 implies to >= from */
memmove( to, from, ( result->n_ctrlp - n - (k - deg + N) ) * sof_c ); /* b) */
/* copy knots */
memmove( result->knots, bspline->knots, (k + 1) * sof_f ); /* c) */
from = bspline->knots + k + 1;
to = result->knots + k + 1 + n; /* n >= 0 implies to >= from */
memmove( to, from, ( result->n_knots - n - (k + 1) ) * sof_f ); /* d) */
/* 2.
*
* a) Copy left hand side control points from de boor net.
* b) Copy middle part control points from de boor net.
* c) Copy right hand side control points from de boor net.
* d) Insert knots with u_k. */
from = deBoorNet->points;
to = result->ctrlp + (k - deg) * dim;
stride = (int) (N * dim);
/* copy control points */
for( i = 0; i < n; i++ ) /* a) */
{
memcpy( to, from, sof_c );
from += stride;
to += dim;
stride -= (int) dim;
}
memcpy( to, from, (N - n) * sof_c ); /* b) */
from -= dim;
to += (N - n) * dim;
stride = -(int) (N - n + 1) * (int) dim; /* N = h+1 with h = deg-s
* (ts_internal_bspline_evaluate) implies N = deg-s+1 = order-s.
* n <= order-s implies N-n+1 >= order-s - order-s + 1 = 1. Thus,
* -(int)(N-n+1) <= -1. */
for( i = 0; i < n; i++ ) /* c) */
{
memcpy( to, from, sof_c );
from += stride;
stride -= (int) dim;
to += dim;
}
/* copy knots */
to = result->knots + k + 1;
for( i = 0; i < n; i++ ) /* d) */
{
*to = deBoorNet->u;
to++;
}
}
void ts_internal_bspline_evaluate( const tsBSpline* bspline, const tsReal u,
tsDeBoorNet* deBoorNet, jmp_buf buf
)
{
const size_t deg = bspline->deg;
const size_t order = bspline->order;
const size_t dim = bspline->dim;
const size_t sof_c = dim * sizeof(tsReal); /* The size of a single
* control points.*/
size_t k;
size_t s;
tsReal uk; /* The actual used u. */
size_t from; /* An offset used to copy values. */
size_t fst; /* The first affected control point, inclusive. */
size_t lst; /* The last affected control point, inclusive. */
size_t N; /* The number of affected control points. */
/* the following indices are used to create the DeBoor net. */
size_t lidx; /* The current left index. */
size_t ridx; /* The current right index. */
size_t tidx; /* The current to index. */
size_t r, i, d; /* Used in for loop. */
tsReal ui; /* The knot value at index i. */
tsReal a, a_hat; /* The weighting factors of the control points. */
/* Setup the net with its default values. */
ts_deboornet_default( deBoorNet );
/* 1. Find index k such that u is in between [u_k, u_k+1).
* 2. Setup already known values.
* 3. Decide by multiplicity of u how to calculate point P(u). */
/* 1. */
ts_internal_bspline_find_u( bspline, u, &k, &s, buf );
deBoorNet->k = k;
deBoorNet->s = s;
/* 2. */
uk = bspline->knots[k]; /* Ensures that with any */
deBoorNet->u = ts_fequals( u, uk ) ? uk : u; /* tsReal precision the
* knot vector stays valid. */
deBoorNet->h = deg < s ? 0 : deg - s; /* prevent underflow */
deBoorNet->dim = dim;
/* 3. (by 1. s <= order)
*
* 3a) Check for s = order.
* Take the two points k-s and k-s + 1. If one of
* them doesn't exist, take only the other.
* 3b) Use de boor algorithm to find point P(u). */
if( s == order )
{
/* only one of the two control points exists */
if( k == deg /* only the first control point */
|| k == bspline->n_knots - 1 ) /* only the last control point */
{
deBoorNet->points = (tsReal*) malloc( sof_c );
if( deBoorNet->points == NULL )
longjmp( buf, TS_MALLOC );
deBoorNet->result = deBoorNet->points;
deBoorNet->n_points = 1;
from = k == deg ? 0 : (k - s) * dim;
memcpy( deBoorNet->points, bspline->ctrlp + from, sof_c );
}
else
{
deBoorNet->points = (tsReal*) malloc( 2 * sof_c );
if( deBoorNet->points == NULL )
longjmp( buf, TS_MALLOC );
deBoorNet->result = deBoorNet->points + dim;
deBoorNet->n_points = 2;
from = (k - s) * dim;
memcpy( deBoorNet->points, bspline->ctrlp + from, 2 * sof_c );
}
}
else /* by 3a) s <= deg (order = deg+1) */
{
fst = k - deg; /* by 1. k >= deg */
lst = k - s; /* s <= deg <= k */
N = lst - fst + 1; /* lst <= fst implies N >= 1 */
deBoorNet->n_points = (size_t) (N * (N + 1) * 0.5f); /* always fits */
deBoorNet->points = (tsReal*) malloc( deBoorNet->n_points * sof_c );
if( deBoorNet->points == NULL )
longjmp( buf, TS_MALLOC );
deBoorNet->result = deBoorNet->points + (deBoorNet->n_points - 1) * dim;
/* copy initial values to output */
memcpy( deBoorNet->points, bspline->ctrlp + fst * dim, N * sof_c );
lidx = 0;
ridx = dim;
tidx = N * dim; /* N >= 1 implies tidx > 0 */
r = 1;
for( ; r <= deBoorNet->h; r++ )
{
i = fst + r;
for( ; i <= lst; i++ )
{
ui = bspline->knots[i];
a = (deBoorNet->u - ui) / (bspline->knots[i + deg - r + 1] - ui);
a_hat = 1.f - a;
for( d = 0; d < dim; d++ )
{
deBoorNet->points[tidx++] =
a_hat * deBoorNet->points[lidx++] +
a * deBoorNet->points[ridx++];
}
}
lidx += dim;
ridx += dim;
}
}
}
void ts_internal_bspline_split( const tsBSpline* bspline, const tsReal u,
tsBSpline* split, size_t* k, jmp_buf buf
)
{
tsDeBoorNet net;
tsError e;
jmp_buf b;
TRY( b, e )
ts_internal_bspline_evaluate( bspline, u, &net, b );
if( net.s == bspline->order )
{
ts_internal_bspline_copy( bspline, split, b );
*k = net.k;
}
else
{
ts_internal_bspline_insert_knot(
bspline, &net, net.h + 1, split, b );
*k = net.k + net.h + 1;
}
CATCH
* k = 0;
ETRY ts_deboornet_free(& net );
if( e < 0 )
longjmp( buf, e );
}
void ts_internal_bspline_thomas_algorithm( const tsReal* points, const size_t n, const size_t dim,
tsReal* output, jmp_buf buf
)
{
const size_t sof_f = sizeof(tsReal); /* The size of a tsReal. */
const size_t sof_c = dim * sof_f; /* The size of a single control point. */
size_t len_m; /* The length m. */
tsReal* m; /* The array of weights. */
size_t lst; /* The index of the last control point in \points. */
size_t i, d; /* Used in for loops. */
size_t j, k, l; /* Used as temporary indices. */
/* input validation */
if( dim == 0 )
longjmp( buf, TS_DIM_ZERO );
if( n == 0 )
longjmp( buf, TS_DEG_GE_NCTRLP );
if( n <= 2 )
{
memcpy( output, points, n * sof_c );
return;
}
/* In the following n >= 3 applies... */
len_m = n - 2; /* ... len_m >= 1 */
lst = (n - 1) * dim; /* ... lst >= 2*dim */
/* m_0 = 1/4, m_{k+1} = 1/(4-m_k), for k = 0,...,n-2 */
m = (tsReal*) malloc( len_m * sof_f );
if( m == NULL )
longjmp( buf, TS_MALLOC );
m[0] = 0.25f;
for( i = 1; i < len_m; i++ )
m[i] = 1.f / (4 - m[i - 1]);
/* forward sweep */
ts_arr_fill( output, n * dim, 0.f );
memcpy( output, points, sof_c );
memcpy( output + lst, points + lst, sof_c );
for( d = 0; d < dim; d++ )
{
k = dim + d;
output[k] = 6 * points[k];
output[k] -= points[d];
}
for( i = 2; i <= n - 2; i++ )
{
for( d = 0; d < dim; d++ )
{
j = (i - 1) * dim + d;
k = i * dim + d;
l = (i + 1) * dim + d;
output[k] = 6 * points[k];
output[k] -= output[l];
output[k] -= m[i - 2] * output[j]; /* i >= 2 */
}
}
/* back substitution */
if( n > 3 )
ts_arr_fill( output + lst, dim, 0.f );
for( i = n - 2; i >= 1; i-- )
{
for( d = 0; d < dim; d++ )
{
k = i * dim + d;
l = (i + 1) * dim + d;
/* The following line is the reason why it's important to not fill
* output with 0 if n = 3. On the other hand, if n > 3 subtracting
* 0 is exactly what we want. */
output[k] -= output[l];
output[k] *= m[i - 1]; /* i >= 1 */
}
}
if( n > 3 )
memcpy( output + lst, points + lst, sof_c );
/* we are done */
free( m );
}
void ts_internal_relaxed_uniform_cubic_bspline( const tsReal* points,
const size_t n,
const size_t dim,
tsBSpline* bspline,
jmp_buf buf
)
{
const size_t order = 4; /* The order of the spline to interpolate. */
const tsReal as = 1.f / 6.f; /* The value 'a sixth'. */
const tsReal at = 1.f / 3.f; /* The value 'a third'. */
const tsReal tt = 2.f / 3.f; /* The value 'two third'. */
size_t sof_c; /* The size of a single control point. */
const tsReal* b = points; /* The array of the b values. */
tsReal* s; /* The array of the s values. */
size_t i, d; /* Used in for loops */
size_t j, k, l; /* Uses as temporary indices. */
tsError e_;
jmp_buf b_;
/* input validation */
if( dim == 0 )
longjmp( buf, TS_DIM_ZERO );
if( n <= 1 )
longjmp( buf, TS_DEG_GE_NCTRLP );
/* in the following n >= 2 applies */
sof_c = dim * sizeof(tsReal); /* dim > 0 implies sof_c > 0 */
/* n >= 2 implies n-1 >= 1 implies (n-1)*4 >= 4 */
ts_internal_bspline_new( (n - 1) * 4, dim, order - 1, TS_BEZIERS, bspline, buf );
TRY( b_, e_ )
s = (tsReal*) malloc( n * sof_c );
if( s == NULL )
longjmp( b_, TS_MALLOC );
CATCH ts_bspline_free( bspline );
longjmp( buf, e_ );
ETRY
/* set s_0 to b_0 and s_n = b_n */
memcpy( s, b, sof_c );
memcpy( s + (n - 1) * dim, b + (n - 1) * dim, sof_c );
/* set s_i = 1/6*b_i + 2/3*b_{i-1} + 1/6*b_{i+1}*/
for( i = 1; i < n - 1; i++ )
{
for( d = 0; d < dim; d++ )
{
j = (i - 1) * dim + d;
k = i * dim + d;
l = (i + 1) * dim + d;
s[k] = as * b[j];
s[k] += tt * b[k];
s[k] += as * b[l];
}
}
/* create beziers from b and s */
for( i = 0; i < n - 1; i++ )
{
for( d = 0; d < dim; d++ )
{
j = i * dim + d;
k = i * 4 * dim + d;
l = (i + 1) * dim + d;
bspline->ctrlp[k] = s[j];
bspline->ctrlp[k + dim] = tt * b[j] + at * b[l];
bspline->ctrlp[k + 2 * dim] = at * b[j] + tt * b[l];
bspline->ctrlp[k + 3 * dim] = s[l];
}
}
free( s );
}
void ts_internal_bspline_interpolate_cubic( const tsReal* points, const size_t n, const size_t dim,
tsBSpline* bspline, jmp_buf buf
)
{
tsError e;
jmp_buf b;
tsReal* thomas = (tsReal*) malloc( n * dim * sizeof(tsReal) );
if( thomas == NULL )
longjmp( buf, TS_MALLOC );
TRY( b, e )
ts_internal_bspline_thomas_algorithm( points, n, dim, thomas, b );
ts_internal_relaxed_uniform_cubic_bspline( thomas, n, dim, bspline, b );
ETRY free( thomas );
if( e < 0 )
longjmp( buf, e );
}
void ts_internal_bspline_derive( const tsBSpline* original,
tsBSpline* derivative, jmp_buf buf
)
{
const size_t sof_f = sizeof(tsReal);
const size_t dim = original->dim;
const size_t deg = original->deg;
const size_t nc = original->n_ctrlp;
const size_t nk = original->n_knots;
tsReal* from_ctrlp = original->ctrlp;
tsReal* from_knots = original->knots;
tsReal* to_ctrlp = NULL;
tsReal* to_knots = NULL;
size_t i, j, k;
if( deg < 1 || nc < 2 )
longjmp( buf, TS_UNDERIVABLE );
if( original != derivative )
{
ts_internal_bspline_new( nc - 1, dim, deg - 1, TS_NONE, derivative, buf );
to_ctrlp = derivative->ctrlp;
to_knots = derivative->knots;
}
else
{
to_ctrlp = (tsReal*) malloc( ( (nc - 1) * dim + (nk - 2) ) * sof_f );
if( to_ctrlp == NULL )
longjmp( buf, TS_MALLOC );
to_knots = to_ctrlp + (nc - 1) * dim;
}
for( i = 0; i < nc - 1; i++ )
{
for( j = 0; j < dim; j++ )
{
if( ts_fequals( from_knots[i + deg + 1], from_knots[i + 1] ) )
{
free( to_ctrlp );
longjmp( buf, TS_UNDERIVABLE );
}
else
{
k = i * dim + j;
to_ctrlp[k] = from_ctrlp[(i + 1) * dim + j] - from_ctrlp[k];
to_ctrlp[k] *= deg;
to_ctrlp[k] /= from_knots[i + deg + 1] - from_knots[i + 1];
}
}
}
memcpy( to_knots, from_knots + 1, (nk - 2) * sof_f );
if( original == derivative )
{
/* free old memory */
free( from_ctrlp );
/* assign new values */
derivative->deg = deg - 1;
derivative->order = deg;
derivative->n_ctrlp = nc - 1;
derivative->n_knots = nk - 2;
derivative->ctrlp = to_ctrlp;
derivative->knots = to_knots;
}
}
void ts_internal_bspline_buckle( const tsBSpline* bspline, const tsReal b,
tsBSpline* buckled, jmp_buf buf
)
{
const tsReal b_hat = 1.f - b; /* The straightening factor. */
const size_t dim = bspline->dim;
const size_t N = bspline->n_ctrlp;
const tsReal* p0 = bspline->ctrlp; /* Pointer to first ctrlp. */
const tsReal* pn_1 = p0 + (N - 1) * dim; /* Pointer to the last ctrlp. */
size_t i, d; /* Used in for loops. */
ts_internal_bspline_copy( bspline, buckled, buf );
for( i = 0; i < N; i++ )
{
for( d = 0; d < dim; d++ )
{
buckled->ctrlp[i * dim + d] =
b * buckled->ctrlp[i * dim + d] +
b_hat * ( p0[d] + ( (tsReal) i / (N - 1) ) * (pn_1[d] - p0[d]) );
}
}
}
void ts_internal_bspline_to_beziers( const tsBSpline* bspline,
tsBSpline* beziers, jmp_buf buf
)
{
tsError e;
jmp_buf b;
const size_t deg = bspline->deg;
const size_t order = bspline->order;
tsBSpline tmp;
int resize; /* The number of control points to add/remove. */
size_t k; /* The index of the splitted knot value. */
tsReal u_min; /* The minimum of the knot values. */
tsReal u_max; /* The maximum of the knot values. */
ts_internal_bspline_copy( bspline, &tmp, buf );
TRY( b, e )
/* fix first control point if necessary */
u_min = tmp.knots[deg];
if( !ts_fequals( tmp.knots[0], u_min ) )
{
ts_internal_bspline_split( &tmp, u_min, &tmp, &k, b );
resize = (int) ( -1 * deg + (deg * 2 - k) );
ts_internal_bspline_resize( &tmp, resize, 0, &tmp, b );
}
/* fix last control point if necessary */
u_max = tmp.knots[tmp.n_knots - order];
if( !ts_fequals( tmp.knots[tmp.n_knots - 1], u_max ) )
{
ts_internal_bspline_split( &tmp, u_max, &tmp, &k, b );
resize = (int) ( -1 * deg + ( k - (tmp.n_knots - order) ) );
ts_internal_bspline_resize( &tmp, resize, 1, &tmp, b );
}
k = order;
while( k < tmp.n_knots - order )
{
ts_internal_bspline_split( &tmp, tmp.knots[k], &tmp, &k, b );
k++;
}
if( bspline == beziers )
ts_bspline_free( beziers );
ts_bspline_move( &tmp, beziers );
ETRY ts_bspline_free(& tmp );
if( e < 0 )
longjmp( buf, e );
}
void ts_internal_bspline_set_ctrlp( const tsBSpline* bspline, const tsReal* ctrlp,
tsBSpline* result, jmp_buf buf
)
{
const size_t s = bspline->n_ctrlp * bspline->dim * sizeof(tsReal);
ts_internal_bspline_copy( bspline, result, buf );
memmove( result->ctrlp, ctrlp, s );
}
void ts_internal_bspline_set_knots( const tsBSpline* bspline, const tsReal* knots,
tsBSpline* result, jmp_buf buf
)
{
const size_t s = bspline->n_knots * sizeof(tsReal);
ts_internal_bspline_copy( bspline, result, buf );
memmove( result->knots, knots, s );
}
/********************************************************
* *
* Interface implementation *
* *
********************************************************/
void ts_deboornet_default( tsDeBoorNet* deBoorNet )
{
deBoorNet->u = 0.f;
deBoorNet->k = 0;
deBoorNet->s = 0;
deBoorNet->h = 0;
deBoorNet->dim = 0;
deBoorNet->n_points = 0;
deBoorNet->points = NULL;
deBoorNet->result = NULL;
}
void ts_deboornet_move( tsDeBoorNet* from, tsDeBoorNet* to )
{
if( from == to )
return;
to->u = from->u;
to->k = from->k;
to->s = from->s;
to->h = from->h;
to->dim = from->dim;
to->n_points = from->n_points;
to->points = from->points;
to->result = from->result;
ts_deboornet_default( from );
}
void ts_deboornet_free( tsDeBoorNet* deBoorNet )
{
if( deBoorNet->points != NULL )
free( deBoorNet->points ); /* automatically frees the field result */
ts_deboornet_default( deBoorNet );
}
void ts_bspline_default( tsBSpline* bspline )
{
bspline->deg = 0;
bspline->order = 0;
bspline->dim = 0;
bspline->n_ctrlp = 0;
bspline->n_knots = 0;
bspline->ctrlp = NULL;
bspline->knots = NULL;
}
void ts_bspline_free( tsBSpline* bspline )
{
if( bspline->ctrlp != NULL )
free( bspline->ctrlp );
ts_bspline_default( bspline );
}
void ts_bspline_move( tsBSpline* from, tsBSpline* to )
{
if( from == to )
return;
to->deg = from->deg;
to->order = from->order;
to->dim = from->dim;
to->n_ctrlp = from->n_ctrlp;
to->n_knots = from->n_knots;
to->ctrlp = from->ctrlp;
to->knots = from->knots;
ts_bspline_default( from );
}
tsError ts_bspline_new( size_t n_ctrlp, size_t dim, size_t deg, tsBSplineType type,
tsBSpline* bspline
)
{
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_bspline_new( n_ctrlp, dim, deg, type, bspline, buf );
CATCH ts_bspline_default( bspline );
ETRY
return err;
}
tsError ts_bspline_interpolate_cubic( const tsReal* points, size_t n, size_t dim,
tsBSpline* bspline
)
{
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_bspline_interpolate_cubic( points, n, dim, bspline, buf );
CATCH ts_bspline_default( bspline );
ETRY
return err;
}
tsError ts_bspline_derive( const tsBSpline* original,
tsBSpline* derivative
)
{
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_bspline_derive( original, derivative, buf );
CATCH
if( original != derivative )
ts_bspline_default( derivative );
ETRY
return err;
}
tsError ts_deboornet_copy( const tsDeBoorNet* original,
tsDeBoorNet* copy
)
{
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_deboornet_copy( original, copy, buf );
CATCH
if( original != copy )
ts_deboornet_default( copy );
ETRY
return err;
}
tsError ts_bspline_copy( const tsBSpline* original,
tsBSpline* copy
)
{
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_bspline_copy( original, copy, buf );
CATCH
if( original != copy )
ts_bspline_default( copy );
ETRY
return err;
}
tsError ts_bspline_set_ctrlp( const tsBSpline* bspline, const tsReal* ctrlp,
tsBSpline* result
)
{
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_bspline_set_ctrlp( bspline, ctrlp, result, buf );
CATCH
if( bspline != result )
ts_bspline_default( result );
ETRY
return err;
}
tsError ts_bspline_set_knots( const tsBSpline* bspline, const tsReal* knots,
tsBSpline* result
)
{
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_bspline_set_knots( bspline, knots, result, buf );
CATCH
if( bspline != result )
ts_bspline_default( result );
ETRY
return err;
}
tsError ts_bspline_fill_knots( const tsBSpline* original,
tsBSplineType type,
tsReal min,
tsReal max,
tsBSpline* result
)
{
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_bspline_fill_knots( original, type, min, max, result, buf );
CATCH
if( original != result )
ts_bspline_default( result );
ETRY
return err;
}
tsError ts_bspline_evaluate( const tsBSpline* bspline, tsReal u,
tsDeBoorNet* deBoorNet
)
{
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_bspline_evaluate( bspline, u, deBoorNet, buf );
CATCH ts_deboornet_default( deBoorNet );
ETRY
return err;
}
tsError ts_bspline_insert_knot( const tsBSpline* bspline, tsReal u, size_t n,
tsBSpline* result, size_t* k
)
{
tsDeBoorNet net;
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_bspline_evaluate( bspline, u, &net, buf );
ts_internal_bspline_insert_knot( bspline, &net, n, result, buf );
*k = net.k + n;
CATCH
if( bspline != result )
ts_bspline_default( result );
*k = 0;
ETRY ts_deboornet_free(& net );
return err;
}
tsError ts_bspline_resize( const tsBSpline* bspline, int n, int back,
tsBSpline* resized
)
{
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_bspline_resize( bspline, n, back, resized, buf );
CATCH
if( bspline != resized )
ts_bspline_default( resized );
ETRY
return err;
}
tsError ts_bspline_split( const tsBSpline* bspline, tsReal u,
tsBSpline* split, size_t* k
)
{
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_bspline_split( bspline, u, split, k, buf );
CATCH
if( bspline != split )
ts_bspline_default( split );
ETRY
return err;
}
tsError ts_bspline_buckle( const tsBSpline* bspline, tsReal b,
tsBSpline* buckled
)
{
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_bspline_buckle( bspline, b, buckled, buf );
CATCH
if( bspline != buckled )
ts_bspline_default( buckled );
ETRY
return err;
}
tsError ts_bspline_to_beziers( const tsBSpline* bspline,
tsBSpline* beziers
)
{
tsError err;
jmp_buf buf;
TRY( buf, err )
ts_internal_bspline_to_beziers( bspline, beziers, buf );
CATCH
if( bspline != beziers )
ts_bspline_default( beziers );
ETRY
return err;
}
int ts_fequals( tsReal x, tsReal y )
{
if( fabs( x - y ) <= FLT_MAX_ABS_ERROR )
{
return 1;
}
// KIDAD FIX for divide-by-zero
else if( y == 0.0 )
{
return x == 0.0;
}
else
{
const tsReal r = (tsReal) fabs( x ) > (tsReal) fabs( y ) ?
(tsReal) fabs( (x - y) / x ) : (tsReal) fabs( (x - y) / y );
return r <= FLT_MAX_REL_ERROR;
}
}
const char* ts_enum_str( tsError err )
{
if( err == TS_MALLOC )
return "malloc failed";
else if( err == TS_DIM_ZERO )
return "dim == 0";
else if( err == TS_DEG_GE_NCTRLP )
return "deg >= #ctrlp";
else if( err == TS_U_UNDEFINED )
return "spline is undefined at given u";
else if( err == TS_MULTIPLICITY )
return "s > order";
else if( err == TS_KNOTS_DECR )
return "decreasing knot vector";
else if( err == TS_NUM_KNOTS )
return "unexpected number of knots";
else if( err == TS_UNDERIVABLE )
return "spline is not derivable";
return "unknown error";
}
tsError ts_str_enum( const char* str )
{
if( !strcmp( str, ts_enum_str( TS_MALLOC ) ) )
return TS_MALLOC;
else if( !strcmp( str, ts_enum_str( TS_DIM_ZERO ) ) )
return TS_DIM_ZERO;
else if( !strcmp( str, ts_enum_str( TS_DEG_GE_NCTRLP ) ) )
return TS_DEG_GE_NCTRLP;
else if( !strcmp( str, ts_enum_str( TS_U_UNDEFINED ) ) )
return TS_U_UNDEFINED;
else if( !strcmp( str, ts_enum_str( TS_MULTIPLICITY ) ) )
return TS_MULTIPLICITY;
else if( !strcmp( str, ts_enum_str( TS_KNOTS_DECR ) ) )
return TS_KNOTS_DECR;
else if( !strcmp( str, ts_enum_str( TS_NUM_KNOTS ) ) )
return TS_NUM_KNOTS;
else if( !strcmp( str, ts_enum_str( TS_UNDERIVABLE ) ) )
return TS_UNDERIVABLE;
return TS_SUCCESS;
}
void ts_arr_fill( tsReal* arr, size_t num, tsReal val )
{
size_t i;
for( i = 0; i < num; i++ )
arr[i] = val;
}
tsReal ts_ctrlp_dist2( const tsReal* x, const tsReal* y, size_t dim
)
{
tsReal sum = 0;
size_t i;
for( i = 0; i < dim; i++ )
sum += (x[i] - y[i]) * (x[i] - y[i]);
return (tsReal) sqrt( sum );
}