2933 lines
91 KiB
C
2933 lines
91 KiB
C
#define TINYSPLINE_EXPORT
|
|
#include "tinyspline.h"
|
|
#include "parson.h" /* serialization */
|
|
|
|
#include <math.h> /* fabs, sqrt, acos */
|
|
#include <stdarg.h> /* varargs */
|
|
#include <stdio.h> /* FILE, fopen */
|
|
#include <stdlib.h> /* malloc, free */
|
|
#include <string.h> /* memcpy, memmove */
|
|
|
|
/* Suppress some useless MSVC warnings. */
|
|
#ifdef _MSC_VER
|
|
#pragma warning(push)
|
|
/* address of dllimport */
|
|
#pragma warning(disable : 4232)
|
|
/* function not inlined */
|
|
#pragma warning(disable : 4710)
|
|
/* byte padding */
|
|
#pragma warning(disable : 4820)
|
|
/* meaningless deprecation */
|
|
#pragma warning(disable : 4996)
|
|
/* Spectre mitigation */
|
|
#pragma warning(disable : 5045)
|
|
#endif
|
|
|
|
#define INIT_OUT_BSPLINE(in, out) \
|
|
if ((in) != (out)) \
|
|
ts_int_bspline_init(out);
|
|
|
|
/*! @name Internal Structs and Functions
|
|
*
|
|
* Internal functions are prefixed with \e ts_int (int for internal).
|
|
*
|
|
* @{
|
|
*/
|
|
/**
|
|
* Stores the private data of ::tsBSpline.
|
|
*/
|
|
struct tsBSplineImpl {
|
|
size_t deg; /**< Degree of B-Spline basis function. */
|
|
size_t dim; /**< Dimensionality of the control points (2D => x, y). */
|
|
size_t n_ctrlp; /**< Number of control points. */
|
|
size_t n_knots; /**< Number of knots (n_ctrlp + deg + 1). */
|
|
};
|
|
|
|
/**
|
|
* Stores the private data of ::tsDeBoorNet.
|
|
*/
|
|
struct tsDeBoorNetImpl {
|
|
tsReal u; /**< The evaluated knot. */
|
|
size_t k; /**< The index [u_k, u_k+1) */
|
|
size_t s; /**< Multiplicity of u_k. */
|
|
size_t h; /**< Number of insertions required to obtain result. */
|
|
size_t dim; /**< Dimensionality of the points (2D => x, y). */
|
|
size_t n_points; /** Number of points in `points'. */
|
|
};
|
|
|
|
void ts_int_bspline_init(tsBSpline *spline) {
|
|
spline->pImpl = NULL;
|
|
}
|
|
|
|
size_t
|
|
ts_int_bspline_sof_state(const tsBSpline *spline) {
|
|
return sizeof(struct tsBSplineImpl) +
|
|
ts_bspline_sof_control_points(spline) +
|
|
ts_bspline_sof_knots(spline);
|
|
}
|
|
|
|
tsReal *
|
|
ts_int_bspline_access_ctrlp(const tsBSpline *spline) {
|
|
return (tsReal *)(&spline->pImpl[1]);
|
|
}
|
|
|
|
tsReal *
|
|
ts_int_bspline_access_knots(const tsBSpline *spline) {
|
|
return ts_int_bspline_access_ctrlp(spline) +
|
|
ts_bspline_len_control_points(spline);
|
|
}
|
|
|
|
tsError
|
|
ts_int_bspline_access_ctrlp_at(const tsBSpline *spline,
|
|
size_t index,
|
|
tsReal **ctrlp,
|
|
tsStatus *status) {
|
|
const size_t num = ts_bspline_num_control_points(spline);
|
|
if (index >= num) {
|
|
TS_RETURN_2(status, TS_INDEX_ERROR,
|
|
"index (%lu) >= num(control_points) (%lu)",
|
|
(unsigned long)index,
|
|
(unsigned long)num)
|
|
}
|
|
*ctrlp = ts_int_bspline_access_ctrlp(spline) +
|
|
index * ts_bspline_dimension(spline);
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_int_bspline_access_knot_at(const tsBSpline *spline,
|
|
size_t index,
|
|
tsReal *knot,
|
|
tsStatus *status) {
|
|
const size_t num = ts_bspline_num_knots(spline);
|
|
if (index >= num) {
|
|
TS_RETURN_2(status, TS_INDEX_ERROR,
|
|
"index (%lu) >= num(knots) (%lu)",
|
|
(unsigned long)index,
|
|
(unsigned long)num)
|
|
}
|
|
*knot = ts_int_bspline_access_knots(spline)[index];
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
void ts_int_deboornet_init(tsDeBoorNet *net) {
|
|
net->pImpl = NULL;
|
|
}
|
|
|
|
size_t
|
|
ts_int_deboornet_sof_state(const tsDeBoorNet *net) {
|
|
return sizeof(struct tsDeBoorNetImpl) +
|
|
ts_deboornet_sof_points(net) +
|
|
ts_deboornet_sof_result(net);
|
|
}
|
|
|
|
tsReal *
|
|
ts_int_deboornet_access_points(const tsDeBoorNet *net) {
|
|
return (tsReal *)(&net->pImpl[1]);
|
|
}
|
|
|
|
tsReal *
|
|
ts_int_deboornet_access_result(const tsDeBoorNet *net) {
|
|
if (ts_deboornet_num_result(net) == 2) {
|
|
return ts_int_deboornet_access_points(net);
|
|
} else {
|
|
return ts_int_deboornet_access_points(net) +
|
|
/* Last point in `points`. */
|
|
(ts_deboornet_len_points(net) -
|
|
ts_deboornet_dimension(net));
|
|
}
|
|
}
|
|
/*! @} */
|
|
|
|
/*! @name B-Spline Data
|
|
*
|
|
* @{
|
|
*/
|
|
size_t
|
|
ts_bspline_degree(const tsBSpline *spline) {
|
|
return spline->pImpl->deg;
|
|
}
|
|
|
|
size_t
|
|
ts_bspline_order(const tsBSpline *spline) {
|
|
return ts_bspline_degree(spline) + 1;
|
|
}
|
|
|
|
size_t
|
|
ts_bspline_dimension(const tsBSpline *spline) {
|
|
return spline->pImpl->dim;
|
|
}
|
|
|
|
size_t
|
|
ts_bspline_len_control_points(const tsBSpline *spline) {
|
|
return ts_bspline_num_control_points(spline) *
|
|
ts_bspline_dimension(spline);
|
|
}
|
|
|
|
size_t
|
|
ts_bspline_num_control_points(const tsBSpline *spline) {
|
|
return spline->pImpl->n_ctrlp;
|
|
}
|
|
|
|
size_t
|
|
ts_bspline_sof_control_points(const tsBSpline *spline) {
|
|
return ts_bspline_len_control_points(spline) * sizeof(tsReal);
|
|
}
|
|
|
|
const tsReal *
|
|
ts_bspline_control_points_ptr(const tsBSpline *spline) {
|
|
return ts_int_bspline_access_ctrlp(spline);
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_control_points(const tsBSpline *spline,
|
|
tsReal **ctrlp,
|
|
tsStatus *status) {
|
|
const size_t size = ts_bspline_sof_control_points(spline);
|
|
*ctrlp = (tsReal *)malloc(size);
|
|
if (!*ctrlp)
|
|
TS_RETURN_0(status, TS_MALLOC, "out of memory")
|
|
memcpy(*ctrlp, ts_int_bspline_access_ctrlp(spline), size);
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_control_point_at_ptr(const tsBSpline *spline,
|
|
size_t index,
|
|
const tsReal **ctrlp,
|
|
tsStatus *status) {
|
|
tsReal *vals;
|
|
tsError err;
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_int_bspline_access_ctrlp_at(spline, index, &vals, status))
|
|
*ctrlp = vals;
|
|
TS_CATCH(err)
|
|
*ctrlp = NULL;
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_set_control_points(tsBSpline *spline,
|
|
const tsReal *ctrlp,
|
|
tsStatus *status) {
|
|
const size_t size = ts_bspline_sof_control_points(spline);
|
|
memmove(ts_int_bspline_access_ctrlp(spline), ctrlp, size);
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_set_control_point_at(tsBSpline *spline,
|
|
size_t index,
|
|
const tsReal *ctrlp,
|
|
tsStatus *status) {
|
|
tsReal *to;
|
|
size_t size;
|
|
tsError err;
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_int_bspline_access_ctrlp_at(spline, index, &to, status))
|
|
size = ts_bspline_dimension(spline) * sizeof(tsReal);
|
|
memcpy(to, ctrlp, size);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
size_t
|
|
ts_bspline_num_knots(const tsBSpline *spline) {
|
|
return spline->pImpl->n_knots;
|
|
}
|
|
|
|
size_t
|
|
ts_bspline_sof_knots(const tsBSpline *spline) {
|
|
return ts_bspline_num_knots(spline) * sizeof(tsReal);
|
|
}
|
|
|
|
const tsReal *
|
|
ts_bspline_knots_ptr(const tsBSpline *spline) {
|
|
return ts_int_bspline_access_knots(spline);
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_knots(const tsBSpline *spline,
|
|
tsReal **knots,
|
|
tsStatus *status) {
|
|
const size_t size = ts_bspline_sof_knots(spline);
|
|
*knots = (tsReal *)malloc(size);
|
|
if (!*knots)
|
|
TS_RETURN_0(status, TS_MALLOC, "out of memory")
|
|
memcpy(*knots, ts_int_bspline_access_knots(spline), size);
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_knot_at(const tsBSpline *spline,
|
|
size_t index,
|
|
tsReal *knot,
|
|
tsStatus *status) {
|
|
return ts_int_bspline_access_knot_at(spline, index, knot, status);
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_set_knots(tsBSpline *spline,
|
|
const tsReal *knots,
|
|
tsStatus *status) {
|
|
const size_t size = ts_bspline_sof_knots(spline);
|
|
const size_t num_knots = ts_bspline_num_knots(spline);
|
|
const size_t order = ts_bspline_order(spline);
|
|
size_t idx, mult;
|
|
tsReal lst_knot, knot;
|
|
lst_knot = knots[0];
|
|
mult = 1;
|
|
for (idx = 1; idx < num_knots; idx++) {
|
|
knot = knots[idx];
|
|
if (ts_knots_equal(lst_knot, knot)) {
|
|
mult++;
|
|
} else if (lst_knot > knot) {
|
|
TS_RETURN_1(status, TS_KNOTS_DECR,
|
|
"decreasing knot vector at index: %lu",
|
|
(unsigned long)idx)
|
|
} else {
|
|
mult = 0;
|
|
}
|
|
if (mult > order) {
|
|
TS_RETURN_3(status, TS_MULTIPLICITY,
|
|
"mult(%f) (%lu) > order (%lu)",
|
|
knot, (unsigned long)mult,
|
|
(unsigned long)order)
|
|
}
|
|
lst_knot = knot;
|
|
}
|
|
memmove(ts_int_bspline_access_knots(spline), knots, size);
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_set_knots_varargs(tsBSpline *spline,
|
|
tsStatus *status,
|
|
tsReal knot0,
|
|
double knot1,
|
|
...) {
|
|
tsReal *values = NULL;
|
|
va_list argp;
|
|
size_t idx;
|
|
tsError err;
|
|
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_bspline_knots(spline, &values, status))
|
|
|
|
values[0] = knot0;
|
|
values[1] = (tsReal)knot1;
|
|
va_start(argp, knot1);
|
|
for (idx = 2; idx < ts_bspline_num_knots(spline); idx++)
|
|
values[idx] = (tsReal)va_arg(argp, double);
|
|
va_end(argp);
|
|
|
|
TS_CALL(try, err, ts_bspline_set_knots(spline, values, status))
|
|
TS_FINALLY
|
|
if (values)
|
|
free(values);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_set_knot_at(tsBSpline *spline,
|
|
size_t index,
|
|
tsReal knot,
|
|
tsStatus *status) {
|
|
tsError err;
|
|
tsReal *knots = NULL;
|
|
/* This is only for initialization. */
|
|
tsReal oldKnot = ts_int_bspline_access_knots(spline)[0];
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_int_bspline_access_knot_at(spline, index, &oldKnot, status))
|
|
/* knots must be set after reading oldKnot because the catch
|
|
* block assumes that oldKnot contains the correct value if
|
|
* knots is not NULL. */
|
|
knots = ts_int_bspline_access_knots(spline);
|
|
knots[index] = knot;
|
|
TS_CALL(try, err, ts_bspline_set_knots(spline, knots, status))
|
|
TS_CATCH(err)
|
|
/* If knots is not NULL, oldKnot contains the correct value. */
|
|
if (knots)
|
|
knots[index] = oldKnot;
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
/*! @} */
|
|
|
|
/*! @name B-Spline Initialization
|
|
*
|
|
* @{
|
|
*/
|
|
tsBSpline
|
|
ts_bspline_init(void) {
|
|
tsBSpline spline;
|
|
ts_int_bspline_init(&spline);
|
|
return spline;
|
|
}
|
|
|
|
tsError
|
|
ts_int_bspline_generate_knots(const tsBSpline *spline,
|
|
tsBSplineType type,
|
|
tsStatus *status) {
|
|
const size_t n_knots = ts_bspline_num_knots(spline);
|
|
const size_t deg = ts_bspline_degree(spline);
|
|
const size_t order = ts_bspline_order(spline);
|
|
tsReal fac; /**< Factor used to calculate the knot values. */
|
|
size_t i; /**< Used in for loops. */
|
|
tsReal *knots; /**< Pointer to the knots of \p _result_. */
|
|
|
|
/* order >= 1 implies 2*order >= 2 implies n_knots >= 2 */
|
|
if (type == TS_BEZIERS && n_knots % order != 0) {
|
|
TS_RETURN_2(status, TS_NUM_KNOTS,
|
|
"num(knots) (%lu) %% order (%lu) != 0",
|
|
(unsigned long)n_knots, (unsigned long)order)
|
|
}
|
|
|
|
knots = ts_int_bspline_access_knots(spline);
|
|
|
|
if (type == TS_OPENED) {
|
|
knots[0] = TS_DOMAIN_DEFAULT_MIN; /* n_knots >= 2 */
|
|
fac = (TS_DOMAIN_DEFAULT_MAX - TS_DOMAIN_DEFAULT_MIN) / (n_knots - 1); /* n_knots >= 2 */
|
|
for (i = 1; i < n_knots - 1; i++)
|
|
knots[i] = TS_DOMAIN_DEFAULT_MIN + i * fac;
|
|
knots[i] = TS_DOMAIN_DEFAULT_MAX; /* n_knots >= 2 */
|
|
} else if (type == TS_CLAMPED) {
|
|
/* n_knots >= 2*order == 2*(deg+1) == 2*deg + 2 > 2*deg - 1 */
|
|
fac = (TS_DOMAIN_DEFAULT_MAX - TS_DOMAIN_DEFAULT_MIN) / (n_knots - 2 * deg - 1);
|
|
ts_arr_fill(knots, order, TS_DOMAIN_DEFAULT_MIN);
|
|
for (i = order; i < n_knots - order; i++)
|
|
knots[i] = TS_DOMAIN_DEFAULT_MIN + (i - deg) * fac;
|
|
ts_arr_fill(knots + i, order, TS_DOMAIN_DEFAULT_MAX);
|
|
} else if (type == TS_BEZIERS) {
|
|
/* n_knots >= 2*order implies n_knots/order >= 2 */
|
|
fac = (TS_DOMAIN_DEFAULT_MAX - TS_DOMAIN_DEFAULT_MIN) / (n_knots / order - 1);
|
|
ts_arr_fill(knots, order, TS_DOMAIN_DEFAULT_MIN);
|
|
for (i = order; i < n_knots - order; i += order)
|
|
ts_arr_fill(knots + i,
|
|
order,
|
|
TS_DOMAIN_DEFAULT_MIN + (i / order) * fac);
|
|
ts_arr_fill(knots + i, order, TS_DOMAIN_DEFAULT_MAX);
|
|
}
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_new(size_t num_control_points,
|
|
size_t dimension,
|
|
size_t degree,
|
|
tsBSplineType type,
|
|
tsBSpline *spline,
|
|
tsStatus *status) {
|
|
const size_t order = degree + 1;
|
|
const size_t num_knots = num_control_points + order;
|
|
const size_t len_ctrlp = num_control_points * dimension;
|
|
|
|
const size_t sof_real = sizeof(tsReal);
|
|
const size_t sof_impl = sizeof(struct tsBSplineImpl);
|
|
const size_t sof_ctrlp_vec = len_ctrlp * sof_real;
|
|
const size_t sof_knots_vec = num_knots * sof_real;
|
|
const size_t sof_spline = sof_impl + sof_ctrlp_vec + sof_knots_vec;
|
|
tsError err;
|
|
|
|
ts_int_bspline_init(spline);
|
|
|
|
if (dimension < 1) {
|
|
TS_RETURN_0(status, TS_DIM_ZERO, "unsupported dimension: 0")
|
|
}
|
|
if (num_knots > TS_MAX_NUM_KNOTS) {
|
|
TS_RETURN_2(status, TS_NUM_KNOTS,
|
|
"unsupported number of knots: %lu > %i",
|
|
(unsigned long)num_knots, TS_MAX_NUM_KNOTS)
|
|
}
|
|
if (degree >= num_control_points) {
|
|
TS_RETURN_2(status, TS_DEG_GE_NCTRLP,
|
|
"degree (%lu) >= num(control_points) (%lu)",
|
|
(unsigned long)degree,
|
|
(unsigned long)num_control_points)
|
|
}
|
|
|
|
spline->pImpl = (struct tsBSplineImpl *)malloc(sof_spline);
|
|
if (!spline->pImpl)
|
|
TS_RETURN_0(status, TS_MALLOC, "out of memory")
|
|
|
|
spline->pImpl->deg = degree;
|
|
spline->pImpl->dim = dimension;
|
|
spline->pImpl->n_ctrlp = num_control_points;
|
|
spline->pImpl->n_knots = num_knots;
|
|
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_int_bspline_generate_knots(spline, type, status))
|
|
TS_CATCH(err)
|
|
ts_bspline_free(spline);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_new_with_control_points(size_t num_control_points,
|
|
size_t dimension,
|
|
size_t degree,
|
|
tsBSplineType type,
|
|
tsBSpline *spline,
|
|
tsStatus *status,
|
|
double first,
|
|
...) {
|
|
tsReal *ctrlp = NULL;
|
|
va_list argp;
|
|
size_t i;
|
|
tsError err;
|
|
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_bspline_new(num_control_points, dimension, degree, type, spline, status))
|
|
TS_CATCH(err)
|
|
ts_bspline_free(spline);
|
|
TS_END_TRY_ROE(err)
|
|
ctrlp = ts_int_bspline_access_ctrlp(spline);
|
|
|
|
ctrlp[0] = (tsReal)first;
|
|
va_start(argp, first);
|
|
for (i = 1; i < ts_bspline_len_control_points(spline); i++)
|
|
ctrlp[i] = (tsReal)va_arg(argp, double);
|
|
va_end(argp);
|
|
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_copy(const tsBSpline *src,
|
|
tsBSpline *dest,
|
|
tsStatus *status) {
|
|
size_t size;
|
|
if (src == dest)
|
|
TS_RETURN_SUCCESS(status)
|
|
ts_int_bspline_init(dest);
|
|
size = ts_int_bspline_sof_state(src);
|
|
dest->pImpl = (struct tsBSplineImpl *)malloc(size);
|
|
if (!dest->pImpl)
|
|
TS_RETURN_0(status, TS_MALLOC, "out of memory")
|
|
memcpy(dest->pImpl, src->pImpl, size);
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
void ts_bspline_move(tsBSpline *src,
|
|
tsBSpline *dest) {
|
|
if (src == dest)
|
|
return;
|
|
dest->pImpl = src->pImpl;
|
|
ts_int_bspline_init(src);
|
|
}
|
|
|
|
void ts_bspline_free(tsBSpline *spline) {
|
|
if (spline->pImpl)
|
|
free(spline->pImpl);
|
|
ts_int_bspline_init(spline);
|
|
}
|
|
/*! @} */
|
|
|
|
/*! @name De Boor Net Data
|
|
*
|
|
* @{
|
|
*/
|
|
tsReal
|
|
ts_deboornet_knot(const tsDeBoorNet *net) {
|
|
return net->pImpl->u;
|
|
}
|
|
|
|
size_t
|
|
ts_deboornet_index(const tsDeBoorNet *net) {
|
|
return net->pImpl->k;
|
|
}
|
|
|
|
size_t
|
|
ts_deboornet_multiplicity(const tsDeBoorNet *net) {
|
|
return net->pImpl->s;
|
|
}
|
|
|
|
size_t
|
|
ts_deboornet_num_insertions(const tsDeBoorNet *net) {
|
|
return net->pImpl->h;
|
|
}
|
|
|
|
size_t
|
|
ts_deboornet_dimension(const tsDeBoorNet *net) {
|
|
return net->pImpl->dim;
|
|
}
|
|
|
|
size_t
|
|
ts_deboornet_len_points(const tsDeBoorNet *net) {
|
|
return ts_deboornet_num_points(net) *
|
|
ts_deboornet_dimension(net);
|
|
}
|
|
|
|
size_t
|
|
ts_deboornet_num_points(const tsDeBoorNet *net) {
|
|
return net->pImpl->n_points;
|
|
}
|
|
|
|
size_t
|
|
ts_deboornet_sof_points(const tsDeBoorNet *net) {
|
|
return ts_deboornet_len_points(net) * sizeof(tsReal);
|
|
}
|
|
|
|
const tsReal *
|
|
ts_deboornet_points_ptr(const tsDeBoorNet *net) {
|
|
return ts_int_deboornet_access_points(net);
|
|
}
|
|
|
|
tsError
|
|
ts_deboornet_points(const tsDeBoorNet *net,
|
|
tsReal **points,
|
|
tsStatus *status) {
|
|
const size_t size = ts_deboornet_sof_points(net);
|
|
*points = (tsReal *)malloc(size);
|
|
if (!*points)
|
|
TS_RETURN_0(status, TS_MALLOC, "out of memory")
|
|
memcpy(*points, ts_int_deboornet_access_points(net), size);
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
size_t
|
|
ts_deboornet_len_result(const tsDeBoorNet *net) {
|
|
return ts_deboornet_num_result(net) *
|
|
ts_deboornet_dimension(net);
|
|
}
|
|
|
|
size_t
|
|
ts_deboornet_num_result(const tsDeBoorNet *net) {
|
|
return ts_deboornet_num_points(net) == 2 ? 2 : 1;
|
|
}
|
|
|
|
size_t
|
|
ts_deboornet_sof_result(const tsDeBoorNet *net) {
|
|
return ts_deboornet_len_result(net) * sizeof(tsReal);
|
|
}
|
|
|
|
const tsReal *
|
|
ts_deboornet_result_ptr(const tsDeBoorNet *net) {
|
|
return ts_int_deboornet_access_result(net);
|
|
}
|
|
|
|
tsError
|
|
ts_deboornet_result(const tsDeBoorNet *net,
|
|
tsReal **result,
|
|
tsStatus *status) {
|
|
const size_t size = ts_deboornet_sof_result(net);
|
|
*result = (tsReal *)malloc(size);
|
|
if (!*result)
|
|
TS_RETURN_0(status, TS_MALLOC, "out of memory")
|
|
memcpy(*result, ts_int_deboornet_access_result(net), size);
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
/*! @} */
|
|
|
|
/*! @name De Boor Net Initialization
|
|
*
|
|
* @{
|
|
*/
|
|
tsDeBoorNet
|
|
ts_deboornet_init(void) {
|
|
tsDeBoorNet net;
|
|
ts_int_deboornet_init(&net);
|
|
return net;
|
|
}
|
|
|
|
tsError
|
|
ts_int_deboornet_new(const tsBSpline *spline,
|
|
tsDeBoorNet *net,
|
|
tsStatus *status) {
|
|
const size_t dim = ts_bspline_dimension(spline);
|
|
const size_t deg = ts_bspline_degree(spline);
|
|
const size_t order = ts_bspline_order(spline);
|
|
const size_t num_points = (size_t)(order * (order + 1) * 0.5f);
|
|
/* Handle `order == 1' which generates too few points. */
|
|
const size_t fixed_num_points = num_points < 2 ? 2 : num_points;
|
|
|
|
const size_t sof_real = sizeof(tsReal);
|
|
const size_t sof_impl = sizeof(struct tsDeBoorNetImpl);
|
|
const size_t sof_points_vec = fixed_num_points * dim * sof_real;
|
|
const size_t sof_net = sof_impl * sof_points_vec;
|
|
|
|
net->pImpl = (struct tsDeBoorNetImpl *)malloc(sof_net);
|
|
if (!net->pImpl)
|
|
TS_RETURN_0(status, TS_MALLOC, "out of memory")
|
|
|
|
net->pImpl->u = 0.f;
|
|
net->pImpl->k = 0;
|
|
net->pImpl->s = 0;
|
|
net->pImpl->h = deg;
|
|
net->pImpl->dim = dim;
|
|
net->pImpl->n_points = fixed_num_points;
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
void ts_deboornet_free(tsDeBoorNet *net) {
|
|
if (net->pImpl)
|
|
free(net->pImpl);
|
|
ts_int_deboornet_init(net);
|
|
}
|
|
|
|
tsError
|
|
ts_deboornet_copy(const tsDeBoorNet *src,
|
|
tsDeBoorNet *dest,
|
|
tsStatus *status) {
|
|
size_t size;
|
|
if (src == dest)
|
|
TS_RETURN_SUCCESS(status)
|
|
ts_int_deboornet_init(dest);
|
|
size = ts_int_deboornet_sof_state(src);
|
|
dest->pImpl = (struct tsDeBoorNetImpl *)malloc(size);
|
|
if (!dest->pImpl)
|
|
TS_RETURN_0(status, TS_MALLOC, "out of memory")
|
|
memcpy(dest->pImpl, src->pImpl, size);
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
void ts_deboornet_move(tsDeBoorNet *src,
|
|
tsDeBoorNet *dest) {
|
|
if (src == dest)
|
|
return;
|
|
dest->pImpl = src->pImpl;
|
|
ts_int_deboornet_init(src);
|
|
}
|
|
/*! @} */
|
|
|
|
/*! @name Interpolation and Approximation Functions
|
|
*
|
|
* @{
|
|
*/
|
|
tsError
|
|
ts_int_cubic_point(const tsReal *point,
|
|
size_t dim,
|
|
tsBSpline *spline,
|
|
tsStatus *status) {
|
|
const size_t size = dim * sizeof(tsReal);
|
|
tsReal *ctrlp = NULL;
|
|
size_t i;
|
|
tsError err;
|
|
TS_CALL_ROE(err, ts_bspline_new(
|
|
4, dim, 3,
|
|
TS_CLAMPED, spline, status))
|
|
ctrlp = ts_int_bspline_access_ctrlp(spline);
|
|
for (i = 0; i < 4; i++) {
|
|
memcpy(ctrlp + i * dim,
|
|
point,
|
|
size);
|
|
}
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_int_thomas_algorithm(const tsReal *a,
|
|
const tsReal *b,
|
|
const tsReal *c,
|
|
size_t num,
|
|
size_t dim,
|
|
tsReal *d,
|
|
tsStatus *status) {
|
|
size_t i, j, k, l;
|
|
tsReal m, *cc = NULL;
|
|
tsError err;
|
|
|
|
if (dim == 0) {
|
|
TS_RETURN_0(status, TS_DIM_ZERO,
|
|
"unsupported dimension: 0")
|
|
}
|
|
if (num <= 1) {
|
|
TS_RETURN_1(status, TS_NUM_POINTS,
|
|
"num(points) (%lu) <= 1",
|
|
(unsigned long)num)
|
|
}
|
|
cc = (tsReal *)malloc(num * sizeof(tsReal));
|
|
if (!cc) TS_RETURN_0(status, TS_MALLOC, "out of memory")
|
|
|
|
TS_TRY(try, err, status)
|
|
/* Forward sweep. */
|
|
if (fabs(b[0]) <= fabs(c[0])) {
|
|
TS_THROW_2(try, err, status, TS_NO_RESULT,
|
|
"error: |%f| <= |%f|", b[0], c[0])
|
|
}
|
|
/* |b[i]| > |c[i]| implies that |b[i]| > 0. Thus, the following
|
|
* statements cannot evaluate to division by zero.*/
|
|
cc[0] = c[0] / b[0];
|
|
for (i = 0; i < dim; i++)
|
|
d[i] = d[i] / b[0];
|
|
for (i = 1; i < num; i++) {
|
|
if (fabs(b[i]) <= fabs(a[i]) + fabs(c[i])) {
|
|
TS_THROW_3(try, err, status, TS_NO_RESULT,
|
|
"error: |%f| <= |%f| + |%f|",
|
|
b[i], a[i], c[i])
|
|
}
|
|
/* |a[i]| < |b[i]| and cc[i - 1] < 1. Therefore, the
|
|
* following statement cannot evaluate to division by
|
|
* zero. */
|
|
m = 1.f / (b[i] - a[i] * cc[i - 1]);
|
|
/* |b[i]| > |a[i]| + |c[i]| implies that there must be
|
|
* an eps > 0 such that |b[i]| = |a[i]| + |c[i]| + eps.
|
|
* Even if |a[i]| is 0 (by which the result of the
|
|
* following statement becomes maximum), |c[i]| is less
|
|
* than |b[i]| by an amount of eps. By substituting the
|
|
* previous and the following statements (under the
|
|
* assumption that |a[i]| is 0), we obtain c[i] / b[i],
|
|
* which must be less than 1. */
|
|
cc[i] = c[i] * m;
|
|
for (j = 0; j < dim; j++) {
|
|
k = i * dim + j;
|
|
l = (i - 1) * dim + j;
|
|
d[k] = (d[k] - a[i] * d[l]) * m;
|
|
}
|
|
}
|
|
|
|
/* Back substitution. */
|
|
for (i = num - 1; i > 0; i--) {
|
|
for (j = 0; j < dim; j++) {
|
|
k = (i - 1) * dim + j;
|
|
l = i * dim + j;
|
|
d[k] -= cc[i - 1] * d[l];
|
|
}
|
|
}
|
|
TS_FINALLY
|
|
free(cc);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_int_relaxed_uniform_cubic_bspline(const tsReal *points,
|
|
size_t n,
|
|
size_t dim,
|
|
tsBSpline *spline,
|
|
tsStatus *status) {
|
|
const size_t order = 4; /**< Order of 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_ctrlp; /**< Size of a single control point. */
|
|
const tsReal *b = points; /**< Array of the b values. */
|
|
tsReal *s; /**< Array of the s values. */
|
|
size_t i, d; /**< Used in for loops */
|
|
size_t j, k, l; /**< Used as temporary indices. */
|
|
tsReal *ctrlp; /**< Pointer to the control points of \p _spline_. */
|
|
tsError err;
|
|
|
|
/* input validation */
|
|
if (dim == 0)
|
|
TS_RETURN_0(status, TS_DIM_ZERO, "unsupported dimension: 0")
|
|
if (n <= 1) {
|
|
TS_RETURN_1(status, TS_NUM_POINTS,
|
|
"num(points) (%lu) <= 1",
|
|
(unsigned long)n)
|
|
}
|
|
/* in the following n >= 2 applies */
|
|
|
|
sof_ctrlp = dim * sizeof(tsReal); /* dim > 0 implies sof_ctrlp > 0 */
|
|
|
|
s = NULL;
|
|
TS_TRY(try, err, status)
|
|
/* n >= 2 implies n-1 >= 1 implies (n-1)*4 >= 4 */
|
|
TS_CALL(try, err, ts_bspline_new((n - 1) * 4, dim, order - 1, TS_BEZIERS, spline, status))
|
|
ctrlp = ts_int_bspline_access_ctrlp(spline);
|
|
|
|
s = (tsReal *)malloc(n * sof_ctrlp);
|
|
if (!s) {
|
|
TS_THROW_0(try, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
|
|
/* set s_0 to b_0 and s_n = b_n */
|
|
memcpy(s, b, sof_ctrlp);
|
|
memcpy(s + (n - 1) * dim, b + (n - 1) * dim, sof_ctrlp);
|
|
|
|
/* 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;
|
|
ctrlp[k] = s[j];
|
|
ctrlp[k + dim] = tt * b[j] + at * b[l];
|
|
ctrlp[k + 2 * dim] = at * b[j] + tt * b[l];
|
|
ctrlp[k + 3 * dim] = s[l];
|
|
}
|
|
}
|
|
TS_CATCH(err)
|
|
ts_bspline_free(spline);
|
|
TS_FINALLY
|
|
if (s)
|
|
free(s);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_interpolate_cubic_natural(const tsReal *points,
|
|
size_t num_points,
|
|
size_t dimension,
|
|
tsBSpline *spline,
|
|
tsStatus *status) {
|
|
const size_t sof_ctrlp = dimension * sizeof(tsReal);
|
|
const size_t len_points = num_points * dimension;
|
|
const size_t num_int_points = num_points - 2;
|
|
const size_t len_int_points = num_int_points * dimension;
|
|
tsReal *thomas, *a, *b, *c, *d;
|
|
size_t i, j, k, l;
|
|
tsError err;
|
|
|
|
ts_int_bspline_init(spline);
|
|
if (num_points == 0)
|
|
TS_RETURN_0(status, TS_NUM_POINTS, "num(points) == 0")
|
|
if (num_points == 1) {
|
|
TS_CALL_ROE(err, ts_int_cubic_point(
|
|
points, dimension, spline, status))
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
if (num_points == 2) {
|
|
return ts_int_relaxed_uniform_cubic_bspline(
|
|
points, num_points, dimension, spline, status);
|
|
}
|
|
/* `num_points` >= 3 */
|
|
thomas = NULL;
|
|
TS_TRY(try, err, status)
|
|
thomas = (tsReal *)malloc(
|
|
/* `a', `b', `c' (note that `c' is equal to `a') */
|
|
2 * num_int_points * sizeof(tsReal) +
|
|
/* `d' and "result of the thomas algorithm" (which
|
|
contains `num_points' points) */
|
|
num_points * dimension * sizeof(tsReal));
|
|
if (!thomas) {
|
|
TS_THROW_0(try, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
/* The system of linear equations is taken from:
|
|
* http://www.bakoma-tex.com/doc/generic/pst-bspline/
|
|
* pst-bspline-doc.pdf */
|
|
a = c = thomas;
|
|
ts_arr_fill(a, num_int_points, 1);
|
|
b = a + num_int_points;
|
|
ts_arr_fill(b, num_int_points, 4);
|
|
d = b + num_int_points;
|
|
/* 6 * S_{i+1} */
|
|
for (i = 0; i < num_int_points; i++) {
|
|
for (j = 0; j < dimension; j++) {
|
|
k = i * dimension + j;
|
|
l = (i + 1) * dimension + j;
|
|
d[k] = 6 * points[l];
|
|
}
|
|
}
|
|
for (i = 0; i < dimension; i++) {
|
|
/* 6 * S_{1} - S_{0} */
|
|
d[i] -= points[i];
|
|
/* 6 * S_{n-1} - S_{n} */
|
|
k = len_int_points - (i + 1);
|
|
l = len_points - (i + 1);
|
|
d[k] -= points[l];
|
|
}
|
|
/* The Thomas algorithm requires at least two points. Hence,
|
|
* `num_int_points` == 1 must be handled separately (let's call
|
|
* it "Mini Thomas"). */
|
|
if (num_int_points == 1) {
|
|
for (i = 0; i < dimension; i++)
|
|
d[i] *= (tsReal)0.25f;
|
|
} else {
|
|
TS_CALL(try, err, ts_int_thomas_algorithm(a, b, c, num_int_points, dimension, d, status))
|
|
}
|
|
memcpy(thomas, points, sof_ctrlp);
|
|
memmove(thomas + dimension, d, num_int_points * sof_ctrlp);
|
|
memcpy(thomas + (num_int_points + 1) * dimension,
|
|
points + (num_points - 1) * dimension, sof_ctrlp);
|
|
TS_CALL(try, err, ts_int_relaxed_uniform_cubic_bspline(thomas, num_points, dimension, spline, status))
|
|
TS_CATCH(err)
|
|
ts_bspline_free(spline);
|
|
TS_FINALLY
|
|
if (thomas)
|
|
free(thomas);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_interpolate_catmull_rom(const tsReal *points,
|
|
size_t num_points,
|
|
size_t dimension,
|
|
tsReal alpha,
|
|
const tsReal *first,
|
|
const tsReal *last,
|
|
tsReal epsilon,
|
|
tsBSpline *spline,
|
|
tsStatus *status) {
|
|
const size_t sof_real = sizeof(tsReal);
|
|
const size_t sof_ctrlp = dimension * sof_real;
|
|
const tsReal eps = (tsReal)fabs(epsilon);
|
|
tsReal *bs_ctrlp; /* Points to the control points of `spline`. */
|
|
tsReal *cr_ctrlp; /**< The points to interpolate based on `points`. */
|
|
size_t i, d; /**< Used in for loops. */
|
|
tsError err; /**< Local error handling. */
|
|
/* [https://en.wikipedia.org/wiki/
|
|
* Centripetal_Catmull%E2%80%93Rom_spline] */
|
|
tsReal t0, t1, t2, t3; /**< Catmull-Rom knots. */
|
|
/* [https://stackoverflow.com/questions/30748316/
|
|
* catmull-rom-interpolation-on-svg-paths/30826434#30826434] */
|
|
tsReal c1, c2, d1, d2, m1, m2; /**< Used to calculate derivatives. */
|
|
tsReal *p0, *p1, *p2, *p3; /**< Processed Catmull-Rom points. */
|
|
|
|
ts_int_bspline_init(spline);
|
|
if (dimension == 0)
|
|
TS_RETURN_0(status, TS_DIM_ZERO, "unsupported dimension: 0")
|
|
if (num_points == 0)
|
|
TS_RETURN_0(status, TS_NUM_POINTS, "num(points) == 0")
|
|
if (alpha < (tsReal)0.0) alpha = (tsReal)0.0;
|
|
if (alpha > (tsReal)1.0) alpha = (tsReal)1.0;
|
|
|
|
/* Copy `points` to `cr_ctrlp`. Add space for `first` and `last`. */
|
|
cr_ctrlp = (tsReal *)malloc((num_points + 2) * sof_ctrlp);
|
|
if (!cr_ctrlp)
|
|
TS_RETURN_0(status, TS_MALLOC, "out of memory")
|
|
memcpy(cr_ctrlp + dimension, points, num_points * sof_ctrlp);
|
|
|
|
/* Remove redundant points from `cr_ctrlp`. Update `num_points`. */
|
|
for (i = 1 /* 0 (`first`) is not assigned yet */;
|
|
i < num_points /* skip last point (inclusive end) */;
|
|
i++) {
|
|
p0 = cr_ctrlp + (i * dimension);
|
|
p1 = p0 + dimension;
|
|
if (ts_distance(p0, p1, dimension) <= eps) {
|
|
/* Are there any other points (after the one that is
|
|
* to be removed) that need to be shifted? */
|
|
if (i < num_points - 1) {
|
|
memmove(p1, p1 + dimension,
|
|
(num_points - (i + 1)) * sof_ctrlp);
|
|
}
|
|
num_points--;
|
|
i--;
|
|
}
|
|
}
|
|
|
|
/* Check if there are still enough points for interpolation. */
|
|
if (num_points == 1) { /* `num_points` can't be 0 */
|
|
free(cr_ctrlp); /* The point is copied from `points`. */
|
|
TS_CALL_ROE(err, ts_int_cubic_point(
|
|
points, dimension, spline, status))
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
/* Add or generate `first` and `last`. Update `num_points`. */
|
|
p0 = cr_ctrlp + dimension;
|
|
if (first && ts_distance(first, p0, dimension) > eps) {
|
|
memcpy(cr_ctrlp, first, sof_ctrlp);
|
|
} else {
|
|
p1 = p0 + dimension;
|
|
for (d = 0; d < dimension; d++)
|
|
cr_ctrlp[d] = p0[d] + (p0[d] - p1[d]);
|
|
}
|
|
p1 = cr_ctrlp + (num_points * dimension);
|
|
if (last && ts_distance(p1, last, dimension) > eps) {
|
|
memcpy(cr_ctrlp + ((num_points + 1) * dimension),
|
|
last, sof_ctrlp);
|
|
} else {
|
|
p0 = p1 - dimension;
|
|
for (d = 0; d < dimension; d++) {
|
|
cr_ctrlp[((num_points + 1) * dimension) + d] =
|
|
p1[d] + (p1[d] - p0[d]);
|
|
}
|
|
}
|
|
num_points = num_points + 2;
|
|
|
|
/* Transform the sequence of Catmull-Rom splines. */
|
|
bs_ctrlp = NULL;
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_bspline_new((num_points - 3) * 4, dimension, 3, TS_BEZIERS, spline, status))
|
|
bs_ctrlp = ts_int_bspline_access_ctrlp(spline);
|
|
TS_CATCH(err)
|
|
free(cr_ctrlp);
|
|
TS_END_TRY_ROE(err)
|
|
for (i = 0; i < ts_bspline_num_control_points(spline) / 4; i++) {
|
|
p0 = cr_ctrlp + ((i + 0) * dimension);
|
|
p1 = cr_ctrlp + ((i + 1) * dimension);
|
|
p2 = cr_ctrlp + ((i + 2) * dimension);
|
|
p3 = cr_ctrlp + ((i + 3) * dimension);
|
|
|
|
t0 = (tsReal)0.f;
|
|
t1 = t0 + (tsReal)pow(ts_distance(p0, p1, dimension), alpha);
|
|
t2 = t1 + (tsReal)pow(ts_distance(p1, p2, dimension), alpha);
|
|
t3 = t2 + (tsReal)pow(ts_distance(p2, p3, dimension), alpha);
|
|
|
|
c1 = (t2 - t1) / (t2 - t0);
|
|
c2 = (t1 - t0) / (t2 - t0);
|
|
d1 = (t3 - t2) / (t3 - t1);
|
|
d2 = (t2 - t1) / (t3 - t1);
|
|
|
|
for (d = 0; d < dimension; d++) {
|
|
m1 = (t2 - t1) * (c1 * (p1[d] - p0[d]) / (t1 - t0) + c2 * (p2[d] - p1[d]) / (t2 - t1));
|
|
m2 = (t2 - t1) * (d1 * (p2[d] - p1[d]) / (t2 - t1) + d2 * (p3[d] - p2[d]) / (t3 - t2));
|
|
bs_ctrlp[((i * 4 + 0) * dimension) + d] = p1[d];
|
|
bs_ctrlp[((i * 4 + 1) * dimension) + d] = p1[d] + m1 / 3;
|
|
bs_ctrlp[((i * 4 + 2) * dimension) + d] = p2[d] - m2 / 3;
|
|
bs_ctrlp[((i * 4 + 3) * dimension) + d] = p2[d];
|
|
}
|
|
}
|
|
free(cr_ctrlp);
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
/*! @} */
|
|
|
|
/*! @name Query Functions
|
|
*
|
|
* @{
|
|
*/
|
|
tsError
|
|
ts_int_bspline_find_knot(const tsBSpline *spline,
|
|
tsReal knot,
|
|
size_t *index,
|
|
size_t *multiplicity,
|
|
tsStatus *status) {
|
|
const size_t deg = ts_bspline_degree(spline);
|
|
const size_t num_knots = ts_bspline_num_knots(spline);
|
|
const tsReal *knots = ts_int_bspline_access_knots(spline);
|
|
tsReal min, max;
|
|
size_t low, high;
|
|
|
|
ts_bspline_domain(spline, &min, &max);
|
|
if (knot < min && !ts_knots_equal(knot, min)) {
|
|
TS_RETURN_2(status, TS_U_UNDEFINED,
|
|
"knot (%f) < min(domain) (%f)",
|
|
knot, min)
|
|
}
|
|
if (knot > max && !ts_knots_equal(knot, max)) {
|
|
TS_RETURN_2(status, TS_U_UNDEFINED,
|
|
"knot (%f) > max(domain) (%f)",
|
|
knot, max)
|
|
}
|
|
|
|
/* Based on 'The NURBS Book' (Les Piegl and Wayne Tiller). */
|
|
if (ts_knots_equal(knot, knots[num_knots - 1])) {
|
|
*index = num_knots - 1;
|
|
} else {
|
|
low = 0;
|
|
high = num_knots - 1;
|
|
*index = (low + high) / 2;
|
|
while (knot < knots[*index] || knot >= knots[*index + 1]) {
|
|
if (knot < knots[*index])
|
|
high = *index;
|
|
else
|
|
low = *index;
|
|
*index = (low + high) / 2;
|
|
}
|
|
}
|
|
|
|
/* Handle floating point errors. */
|
|
while (*index < num_knots - 1 && /* there is a next knot */
|
|
ts_knots_equal(knot, knots[*index + 1])) {
|
|
(*index)++;
|
|
}
|
|
|
|
/* Calculate knot's multiplicity. */
|
|
for (*multiplicity = deg + 1; *multiplicity > 0; (*multiplicity)--) {
|
|
if (ts_knots_equal(knot, knots[*index - (*multiplicity - 1)]))
|
|
break;
|
|
}
|
|
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_int_bspline_eval_woa(const tsBSpline *spline,
|
|
tsReal u,
|
|
tsDeBoorNet *net,
|
|
tsStatus *status) {
|
|
const size_t deg = ts_bspline_degree(spline);
|
|
const size_t order = ts_bspline_order(spline);
|
|
const size_t dim = ts_bspline_dimension(spline);
|
|
const size_t num_knots = ts_bspline_num_knots(spline);
|
|
const size_t sof_ctrlp = dim * sizeof(tsReal);
|
|
|
|
const tsReal *ctrlp = ts_int_bspline_access_ctrlp(spline);
|
|
const tsReal *knots = ts_int_bspline_access_knots(spline);
|
|
tsReal *points = NULL; /**< Pointer to the points of \p net. */
|
|
|
|
size_t k; /**< Index of \p u. */
|
|
size_t s; /**< Multiplicity of \p u. */
|
|
|
|
tsReal uk; /**< The actual used u. */
|
|
size_t from; /**< Offset used to copy values. */
|
|
size_t fst; /**< First affected control point, inclusive. */
|
|
size_t lst; /**< Last affected control point, inclusive. */
|
|
size_t N; /**< Number of affected control points. */
|
|
|
|
/* The following indices are used to create the DeBoor net. */
|
|
size_t lidx; /**< Current left index. */
|
|
size_t ridx; /**< Current right index. */
|
|
size_t tidx; /**< Current to index. */
|
|
size_t r, i, d; /**< Used in for loop. */
|
|
tsReal ui; /**< Knot value at index i. */
|
|
tsReal a, a_hat; /**< Weighting factors of control points. */
|
|
|
|
tsError err;
|
|
|
|
points = ts_int_deboornet_access_points(net);
|
|
|
|
/* 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. */
|
|
k = s = 0;
|
|
TS_CALL_ROE(err, ts_int_bspline_find_knot(
|
|
spline, u, &k, &s, status))
|
|
|
|
/* 2. */
|
|
uk = knots[k]; /* Ensures that with any precision of */
|
|
net->pImpl->u = /* tsReal the knot vector stays valid. */
|
|
ts_knots_equal(u, uk) ? uk : u;
|
|
net->pImpl->k = k;
|
|
net->pImpl->s = s;
|
|
net->pImpl->h = deg < s ? 0 : deg - s; /* prevent underflow */
|
|
|
|
/* 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 */
|
|
k == num_knots - 1) { /* only the last */
|
|
from = k == deg ? 0 : (k - s) * dim;
|
|
net->pImpl->n_points = 1;
|
|
memcpy(points, ctrlp + from, sof_ctrlp);
|
|
} else {
|
|
from = (k - s) * dim;
|
|
net->pImpl->n_points = 2;
|
|
memcpy(points, ctrlp + from, 2 * sof_ctrlp);
|
|
}
|
|
} 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 */
|
|
|
|
net->pImpl->n_points = (size_t)(N * (N + 1) * 0.5f);
|
|
|
|
/* copy initial values to output */
|
|
memcpy(points, ctrlp + fst * dim, N * sof_ctrlp);
|
|
|
|
lidx = 0;
|
|
ridx = dim;
|
|
tidx = N * dim; /* N >= 1 implies tidx > 0 */
|
|
r = 1;
|
|
for (; r <= ts_deboornet_num_insertions(net); r++) {
|
|
i = fst + r;
|
|
for (; i <= lst; i++) {
|
|
ui = knots[i];
|
|
a = (ts_deboornet_knot(net) - ui) /
|
|
(knots[i + deg - r + 1] - ui);
|
|
a_hat = 1.f - a;
|
|
|
|
for (d = 0; d < dim; d++) {
|
|
points[tidx++] =
|
|
a_hat * points[lidx++] +
|
|
a * points[ridx++];
|
|
}
|
|
}
|
|
lidx += dim;
|
|
ridx += dim;
|
|
}
|
|
}
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_eval(const tsBSpline *spline,
|
|
tsReal knot,
|
|
tsDeBoorNet *net,
|
|
tsStatus *status) {
|
|
tsError err;
|
|
ts_int_deboornet_init(net);
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_int_deboornet_new(spline, net, status))
|
|
TS_CALL(try, err, ts_int_bspline_eval_woa(spline, knot, net, status))
|
|
TS_CATCH(err)
|
|
ts_deboornet_free(net);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_eval_all(const tsBSpline *spline,
|
|
const tsReal *knots,
|
|
size_t num,
|
|
tsReal **points,
|
|
tsStatus *status) {
|
|
const size_t dim = ts_bspline_dimension(spline);
|
|
const size_t sof_point = dim * sizeof(tsReal);
|
|
const size_t sof_points = num * sof_point;
|
|
tsDeBoorNet net = ts_deboornet_init();
|
|
tsReal *result;
|
|
size_t i;
|
|
tsError err;
|
|
TS_TRY(try, err, status)
|
|
*points = (tsReal *)malloc(sof_points);
|
|
if (!*points) {
|
|
TS_THROW_0(try, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
TS_CALL(try, err, ts_int_deboornet_new(spline, &net, status))
|
|
for (i = 0; i < num; i++) {
|
|
TS_CALL(try, err, ts_int_bspline_eval_woa(spline, knots[i], &net, status))
|
|
result = ts_int_deboornet_access_result(&net);
|
|
memcpy((*points) + i * dim, result, sof_point);
|
|
}
|
|
TS_CATCH(err)
|
|
if (*points)
|
|
free(*points);
|
|
*points = NULL;
|
|
TS_FINALLY
|
|
ts_deboornet_free(&net);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_sample(const tsBSpline *spline,
|
|
size_t num,
|
|
tsReal **points,
|
|
size_t *actual_num,
|
|
tsStatus *status) {
|
|
tsError err;
|
|
tsReal *knots;
|
|
|
|
num = num == 0 ? 100 : num;
|
|
*actual_num = num;
|
|
knots = (tsReal *)malloc(num * sizeof(tsReal));
|
|
if (!knots) {
|
|
*points = NULL;
|
|
TS_RETURN_0(status, TS_MALLOC, "out of memory")
|
|
}
|
|
ts_bspline_uniform_knot_seq(spline, num, knots);
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_bspline_eval_all(spline, knots, num, points, status))
|
|
TS_FINALLY
|
|
free(knots);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_bisect(const tsBSpline *spline,
|
|
tsReal value,
|
|
tsReal epsilon,
|
|
int persnickety,
|
|
size_t index,
|
|
int ascending,
|
|
size_t max_iter,
|
|
tsDeBoorNet *net,
|
|
tsStatus *status) {
|
|
tsError err;
|
|
const size_t dim = ts_bspline_dimension(spline);
|
|
const tsReal eps = (tsReal)fabs(epsilon);
|
|
size_t i = 0;
|
|
tsReal dist = 0;
|
|
tsReal min, max, mid;
|
|
tsReal *P;
|
|
|
|
ts_int_deboornet_init(net);
|
|
|
|
if (dim < index) {
|
|
TS_RETURN_2(status, TS_INDEX_ERROR,
|
|
"dimension (%lu) <= index (%lu)",
|
|
(unsigned long)dim,
|
|
(unsigned long)index)
|
|
}
|
|
if (max_iter == 0)
|
|
TS_RETURN_0(status, TS_NO_RESULT, "0 iterations")
|
|
|
|
ts_bspline_domain(spline, &min, &max);
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_int_deboornet_new(spline, net, status))
|
|
do {
|
|
mid = (tsReal)((min + max) / 2.0);
|
|
TS_CALL(try, err, ts_int_bspline_eval_woa(spline, mid, net, status))
|
|
P = ts_int_deboornet_access_result(net);
|
|
dist = ts_distance(&P[index], &value, 1);
|
|
if (dist <= eps)
|
|
TS_RETURN_SUCCESS(status)
|
|
if (ascending) {
|
|
if (P[index] < value)
|
|
min = mid;
|
|
else
|
|
max = mid;
|
|
} else {
|
|
if (P[index] < value)
|
|
max = mid;
|
|
else
|
|
min = mid;
|
|
}
|
|
} while (i++ < max_iter);
|
|
if (persnickety) {
|
|
TS_THROW_1(try, err, status, TS_NO_RESULT,
|
|
"maximum iterations (%lu) exceeded",
|
|
(unsigned long)max_iter)
|
|
}
|
|
TS_CATCH(err)
|
|
ts_deboornet_free(net);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
void ts_bspline_domain(const tsBSpline *spline,
|
|
tsReal *min,
|
|
tsReal *max) {
|
|
*min = ts_int_bspline_access_knots(spline)
|
|
[ts_bspline_degree(spline)];
|
|
*max = ts_int_bspline_access_knots(spline)
|
|
[ts_bspline_num_knots(spline) - ts_bspline_order(spline)];
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_is_closed(const tsBSpline *spline,
|
|
tsReal epsilon,
|
|
int *closed,
|
|
tsStatus *status) {
|
|
const size_t deg = ts_bspline_degree(spline);
|
|
const size_t dim = ts_bspline_dimension(spline);
|
|
tsBSpline derivative;
|
|
tsReal min, max;
|
|
tsDeBoorNet first, last;
|
|
size_t i;
|
|
tsError err;
|
|
|
|
ts_int_bspline_init(&derivative);
|
|
ts_int_deboornet_init(&first);
|
|
ts_int_deboornet_init(&last);
|
|
|
|
TS_TRY(try, err, status)
|
|
for (i = 0; i < deg; i++) {
|
|
TS_CALL(try, err, ts_bspline_derive(spline, i, -1.f, &derivative, status))
|
|
ts_bspline_domain(&derivative, &min, &max);
|
|
TS_CALL(try, err, ts_bspline_eval(&derivative, min, &first, status))
|
|
TS_CALL(try, err, ts_bspline_eval(&derivative, max, &last, status))
|
|
*closed = ts_distance(
|
|
ts_int_deboornet_access_result(&first),
|
|
ts_int_deboornet_access_result(&last),
|
|
dim) <= epsilon
|
|
? 1
|
|
: 0;
|
|
ts_bspline_free(&derivative);
|
|
ts_deboornet_free(&first);
|
|
ts_deboornet_free(&last);
|
|
if (!*closed)
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
TS_FINALLY
|
|
ts_bspline_free(&derivative);
|
|
ts_deboornet_free(&first);
|
|
ts_deboornet_free(&last);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_compute_rmf(const tsBSpline *spline,
|
|
const tsReal *knots,
|
|
size_t num,
|
|
int has_first_normal,
|
|
tsFrame *frames,
|
|
tsStatus *status) {
|
|
tsError err;
|
|
size_t i;
|
|
tsReal fx, fy, fz, fmin;
|
|
tsReal xc[3], xn[3], v1[3], c1, v2[3], c2, rL[3], tL[3];
|
|
tsBSpline deriv = ts_bspline_init();
|
|
tsDeBoorNet curr = ts_deboornet_init();
|
|
tsDeBoorNet next = ts_deboornet_init();
|
|
|
|
if (num < 1)
|
|
TS_RETURN_SUCCESS(status);
|
|
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_int_deboornet_new(spline, &curr, status))
|
|
TS_CALL(try, err, ts_int_deboornet_new(spline, &next, status))
|
|
TS_CALL(try, err, ts_bspline_derive(spline, 1, (tsReal)-1.0, &deriv, status))
|
|
|
|
/* Set position. */
|
|
TS_CALL(try, err, ts_int_bspline_eval_woa(spline, knots[0], &curr, status))
|
|
ts_vec3_set(frames[0].position,
|
|
ts_int_deboornet_access_result(&curr),
|
|
ts_bspline_dimension(spline));
|
|
/* Set tangent. */
|
|
TS_CALL(try, err, ts_int_bspline_eval_woa(&deriv, knots[0], &curr, status))
|
|
ts_vec3_set(frames[0].tangent,
|
|
ts_int_deboornet_access_result(&curr),
|
|
ts_bspline_dimension(&deriv));
|
|
ts_vec_norm(frames[0].tangent, 3, frames[0].tangent);
|
|
/* Set normal. */
|
|
if (!has_first_normal) {
|
|
fx = (tsReal)fabs(frames[0].tangent[0]);
|
|
fy = (tsReal)fabs(frames[0].tangent[1]);
|
|
fz = (tsReal)fabs(frames[0].tangent[2]);
|
|
fmin = fx; /* x is min => 1, 0, 0 */
|
|
ts_vec3_init(frames[0].normal,
|
|
(tsReal)1.0,
|
|
(tsReal)0.0,
|
|
(tsReal)0.0);
|
|
if (fy < fmin) { /* y is min => 0, 1, 0 */
|
|
fmin = fy;
|
|
ts_vec3_init(frames[0].normal,
|
|
(tsReal)0.0,
|
|
(tsReal)1.0,
|
|
(tsReal)0.0);
|
|
}
|
|
if (fz < fmin) { /* z is min => 0, 0, 1 */
|
|
ts_vec3_init(frames[0].normal,
|
|
(tsReal)0.0,
|
|
(tsReal)0.0,
|
|
(tsReal)1.0);
|
|
}
|
|
ts_vec3_cross(frames[0].tangent,
|
|
frames[0].normal,
|
|
frames[0].normal);
|
|
ts_vec_norm(frames[0].normal, 3, frames[0].normal);
|
|
ts_vec3_cross(frames[0].tangent,
|
|
frames[0].normal,
|
|
frames[0].normal);
|
|
} else {
|
|
/* Never trust user input! */
|
|
ts_vec_norm(frames[0].normal, 3, frames[0].normal);
|
|
}
|
|
/* Set binormal. */
|
|
ts_vec3_cross(frames[0].tangent,
|
|
frames[0].normal,
|
|
frames[0].binormal);
|
|
|
|
for (i = 0; i < num - 1; i++) {
|
|
/* Eval current and next point. */
|
|
TS_CALL(try, err, ts_int_bspline_eval_woa(spline, knots[i], &curr, status))
|
|
TS_CALL(try, err, ts_int_bspline_eval_woa(spline, knots[i + 1], &next, status))
|
|
ts_vec3_set(xc, /* xc is now the current point */
|
|
ts_int_deboornet_access_result(&curr),
|
|
ts_bspline_dimension(spline));
|
|
ts_vec3_set(xn, /* xn is now the next point */
|
|
ts_int_deboornet_access_result(&next),
|
|
ts_bspline_dimension(spline));
|
|
|
|
/* Set position of U_{i+1}. */
|
|
ts_vec3_set(frames[i + 1].position, xn, 3);
|
|
|
|
/* Compute reflection vector of R_{1}. */
|
|
ts_vec_sub(xn, xc, 3, v1);
|
|
c1 = ts_vec_dot(v1, v1, 3);
|
|
|
|
/* Compute r_{i}^{L} = R_{1} * r_{i}. */
|
|
rL[0] = (tsReal)2.0 / c1;
|
|
rL[1] = ts_vec_dot(v1, frames[i].normal, 3);
|
|
rL[2] = rL[0] * rL[1];
|
|
ts_vec_mul(v1, 3, rL[2], rL);
|
|
ts_vec_sub(frames[i].normal, rL, 3, rL);
|
|
|
|
/* Compute t_{i}^{L} = R_{1} * t_{i}. */
|
|
tL[0] = (tsReal)2.0 / c1;
|
|
tL[1] = ts_vec_dot(v1, frames[i].tangent, 3);
|
|
tL[2] = tL[0] * tL[1];
|
|
ts_vec_mul(v1, 3, tL[2], tL);
|
|
ts_vec_sub(frames[i].tangent, tL, 3, tL);
|
|
|
|
/* Compute reflection vector of R_{2}. */
|
|
TS_CALL(try, err, ts_int_bspline_eval_woa(&deriv, knots[i + 1], &next, status))
|
|
ts_vec3_set(xn, /* xn is now the next tangent */
|
|
ts_int_deboornet_access_result(&next),
|
|
ts_bspline_dimension(&deriv));
|
|
ts_vec_norm(xn, 3, xn);
|
|
ts_vec_sub(xn, tL, 3, v2);
|
|
c2 = ts_vec_dot(v2, v2, 3);
|
|
|
|
/* Compute r_{i+1} = R_{2} * r_{i}^{L}. */
|
|
ts_vec3_set(xc, /* xc is now the next normal */
|
|
frames[i + 1].normal, 3);
|
|
xc[0] = (tsReal)2.0 / c2;
|
|
xc[1] = ts_vec_dot(v2, rL, 3);
|
|
xc[2] = xc[0] * xc[1];
|
|
ts_vec_mul(v2, 3, xc[2], xc);
|
|
ts_vec_sub(rL, xc, 3, xc);
|
|
ts_vec_norm(xc, 3, xc);
|
|
|
|
/* Compute vector s_{i+1} of U_{i+1}. */
|
|
ts_vec3_cross(xn, xc, frames[i + 1].binormal);
|
|
|
|
/* Set vectors t_{i+1} and r_{i+1} of U_{i+1}. */
|
|
ts_vec3_set(frames[i + 1].tangent, xn, 3);
|
|
ts_vec3_set(frames[i + 1].normal, xc, 3);
|
|
}
|
|
TS_FINALLY
|
|
ts_bspline_free(&deriv);
|
|
ts_deboornet_free(&curr);
|
|
ts_deboornet_free(&next);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
void ts_bspline_uniform_knot_seq(const tsBSpline *spline,
|
|
size_t num,
|
|
tsReal *knots) {
|
|
size_t i;
|
|
tsReal min, max;
|
|
ts_bspline_domain(spline, &min, &max);
|
|
for (i = 0; i < num; i++) {
|
|
knots[i] = max - min;
|
|
knots[i] *= (tsReal)i / (num - 1);
|
|
knots[i] += min;
|
|
}
|
|
/* Set knots[0] after knots[num - 1] to ensure that
|
|
* knots[0] = min if num is 1. */
|
|
knots[num - 1] = max;
|
|
knots[0] = min;
|
|
}
|
|
/*! @} */
|
|
|
|
/*! @name Transformation Functions
|
|
*
|
|
* @{
|
|
*/
|
|
tsError
|
|
ts_int_bspline_resize(const tsBSpline *spline,
|
|
int n,
|
|
int back,
|
|
tsBSpline *resized,
|
|
tsStatus *status) {
|
|
const size_t deg = ts_bspline_degree(spline);
|
|
const size_t dim = ts_bspline_dimension(spline);
|
|
const size_t sof_real = sizeof(tsReal);
|
|
|
|
const size_t num_ctrlp = ts_bspline_num_control_points(spline);
|
|
const size_t num_knots = ts_bspline_num_knots(spline);
|
|
const size_t nnum_ctrlp = num_ctrlp + n; /**< New length of ctrlp. */
|
|
const size_t nnum_knots = num_knots + n; /**< New length of knots. */
|
|
const size_t min_num_ctrlp_vec = n < 0 ? nnum_ctrlp : num_ctrlp;
|
|
const size_t min_num_knots_vec = n < 0 ? nnum_knots : num_knots;
|
|
|
|
const size_t sof_min_num_ctrlp = min_num_ctrlp_vec * dim * sof_real;
|
|
const size_t sof_min_num_knots = min_num_knots_vec * sof_real;
|
|
|
|
tsBSpline tmp; /**< Temporarily stores the result. */
|
|
const tsReal *from_ctrlp = ts_int_bspline_access_ctrlp(spline);
|
|
const tsReal *from_knots = ts_int_bspline_access_knots(spline);
|
|
tsReal *to_ctrlp = NULL; /**< Pointer to the control points of tmp. */
|
|
tsReal *to_knots = NULL; /**< Pointer to the knots of tmp. */
|
|
|
|
tsError err;
|
|
|
|
if (n == 0)
|
|
return ts_bspline_copy(spline, resized, status);
|
|
|
|
INIT_OUT_BSPLINE(spline, resized)
|
|
TS_CALL_ROE(err, ts_bspline_new(
|
|
nnum_ctrlp, dim, deg, TS_OPENED,
|
|
&tmp, status))
|
|
to_ctrlp = ts_int_bspline_access_ctrlp(&tmp);
|
|
to_knots = ts_int_bspline_access_knots(&tmp);
|
|
|
|
/* Copy control points and knots. */
|
|
if (!back && n < 0) {
|
|
memcpy(to_ctrlp, from_ctrlp - n * dim, sof_min_num_ctrlp);
|
|
memcpy(to_knots, from_knots - n, sof_min_num_knots);
|
|
} else if (!back && n > 0) {
|
|
memcpy(to_ctrlp + n * dim, from_ctrlp, sof_min_num_ctrlp);
|
|
memcpy(to_knots + n, from_knots, sof_min_num_knots);
|
|
} else {
|
|
/* n != 0 implies back == true */
|
|
memcpy(to_ctrlp, from_ctrlp, sof_min_num_ctrlp);
|
|
memcpy(to_knots, from_knots, sof_min_num_knots);
|
|
}
|
|
|
|
if (spline == resized)
|
|
ts_bspline_free(resized);
|
|
ts_bspline_move(&tmp, resized);
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_derive(const tsBSpline *spline,
|
|
size_t n,
|
|
tsReal epsilon,
|
|
tsBSpline *deriv,
|
|
tsStatus *status) {
|
|
const size_t sof_real = sizeof(tsReal);
|
|
const size_t dim = ts_bspline_dimension(spline);
|
|
const size_t sof_ctrlp = dim * sof_real;
|
|
size_t deg = ts_bspline_degree(spline);
|
|
size_t num_ctrlp = ts_bspline_num_control_points(spline);
|
|
size_t num_knots = ts_bspline_num_knots(spline);
|
|
|
|
tsBSpline worker; /**< Stores the intermediate result. */
|
|
tsReal *ctrlp; /**< Pointer to the control points of worker. */
|
|
tsReal *knots; /**< Pointer to the knots of worker. */
|
|
|
|
size_t m, i, j, k, l; /**< Used in for loops. */
|
|
tsReal *fst, *snd; /**< Pointer to first and second control point. */
|
|
tsReal dist; /**< Distance between fst and snd. */
|
|
tsReal kid1, ki1; /**< Knots at i+deg+1 and i+1. */
|
|
tsReal span; /**< Distance between kid1 and ki1. */
|
|
|
|
tsBSpline swap; /**< Used to swap worker and derivative. */
|
|
tsError err;
|
|
|
|
INIT_OUT_BSPLINE(spline, deriv)
|
|
TS_CALL_ROE(err, ts_bspline_copy(spline, &worker, status))
|
|
ctrlp = ts_int_bspline_access_ctrlp(&worker);
|
|
knots = ts_int_bspline_access_knots(&worker);
|
|
|
|
TS_TRY(try, err, status)
|
|
for (m = 1; m <= n; m++) { /* from 1st to n'th derivative */
|
|
if (deg == 0) {
|
|
ts_arr_fill(ctrlp, dim, 0.f);
|
|
ts_bspline_domain(spline,
|
|
&knots[0],
|
|
&knots[1]);
|
|
num_ctrlp = 1;
|
|
num_knots = 2;
|
|
break;
|
|
}
|
|
/* Check and, if possible, fix discontinuity. */
|
|
for (i = 2 * deg + 1; i < num_knots - (deg + 1); i++) {
|
|
if (!ts_knots_equal(knots[i], knots[i - deg]))
|
|
continue;
|
|
fst = ctrlp + (i - (deg + 1)) * dim;
|
|
snd = fst + dim;
|
|
dist = ts_distance(fst, snd, dim);
|
|
if (epsilon >= 0.f && dist > epsilon) {
|
|
TS_THROW_1(try, err, status,
|
|
TS_UNDERIVABLE,
|
|
"discontinuity at knot: %f",
|
|
knots[i])
|
|
}
|
|
memmove(snd,
|
|
snd + dim,
|
|
(num_ctrlp - (i + 1 - deg)) * sof_ctrlp);
|
|
memmove(&knots[i],
|
|
&knots[i + 1],
|
|
(num_knots - (i + 1)) * sof_real);
|
|
num_ctrlp--;
|
|
num_knots--;
|
|
i += deg - 1;
|
|
}
|
|
/* Derive continuous worker. */
|
|
for (i = 0; i < num_ctrlp - 1; i++) {
|
|
for (j = 0; j < dim; j++) {
|
|
k = i * dim + j;
|
|
l = (i + 1) * dim + j;
|
|
ctrlp[k] = ctrlp[l] - ctrlp[k];
|
|
kid1 = knots[i + deg + 1];
|
|
ki1 = knots[i + 1];
|
|
span = kid1 - ki1;
|
|
if (span < TS_KNOT_EPSILON)
|
|
span = TS_KNOT_EPSILON;
|
|
ctrlp[k] *= deg;
|
|
ctrlp[k] /= span;
|
|
}
|
|
}
|
|
deg -= 1;
|
|
num_ctrlp -= 1;
|
|
num_knots -= 2;
|
|
knots += 1;
|
|
}
|
|
TS_CALL(try, err, ts_bspline_new(num_ctrlp, dim, deg, TS_OPENED, &swap, status))
|
|
memcpy(ts_int_bspline_access_ctrlp(&swap),
|
|
ctrlp,
|
|
num_ctrlp * sof_ctrlp);
|
|
memcpy(ts_int_bspline_access_knots(&swap),
|
|
knots,
|
|
num_knots * sof_real);
|
|
if (spline == deriv)
|
|
ts_bspline_free(deriv);
|
|
ts_bspline_move(&swap, deriv);
|
|
TS_FINALLY
|
|
ts_bspline_free(&worker);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_int_bspline_insert_knot(const tsBSpline *spline,
|
|
const tsDeBoorNet *net,
|
|
size_t n,
|
|
tsBSpline *result,
|
|
tsStatus *status) {
|
|
const size_t deg = ts_bspline_degree(spline);
|
|
const size_t order = ts_bspline_order(spline);
|
|
const size_t dim = ts_bspline_dimension(spline);
|
|
const tsReal knot = ts_deboornet_knot(net);
|
|
const size_t k = ts_deboornet_index(net);
|
|
const size_t mult = ts_deboornet_multiplicity(net);
|
|
const size_t sof_real = sizeof(tsReal);
|
|
const size_t sof_ctrlp = dim * sof_real;
|
|
|
|
size_t N; /**< Number of affected control points. */
|
|
tsReal *from; /**< Pointer to copy the values from. */
|
|
tsReal *to; /**< Pointer to copy the values to. */
|
|
int stride; /**< Stride of the next pointer to copy. */
|
|
size_t i; /**< Used in for loops. */
|
|
|
|
tsReal *ctrlp_spline, *ctrlp_result;
|
|
tsReal *knots_spline, *knots_result;
|
|
size_t num_ctrlp_result;
|
|
size_t num_knots_result;
|
|
|
|
tsError err;
|
|
|
|
INIT_OUT_BSPLINE(spline, result)
|
|
if (n == 0)
|
|
return ts_bspline_copy(spline, result, status);
|
|
if (mult + n > order) {
|
|
TS_RETURN_4(status, TS_MULTIPLICITY,
|
|
"multiplicity(%f) (%lu) + %lu > order (%lu)",
|
|
knot, (unsigned long)mult, (unsigned long)n,
|
|
(unsigned long)order)
|
|
}
|
|
|
|
TS_CALL_ROE(err, ts_int_bspline_resize(
|
|
spline, (int)n, 1, result, status))
|
|
ctrlp_spline = ts_int_bspline_access_ctrlp(spline);
|
|
knots_spline = ts_int_bspline_access_knots(spline);
|
|
ctrlp_result = ts_int_bspline_access_ctrlp(result);
|
|
knots_result = ts_int_bspline_access_knots(result);
|
|
num_ctrlp_result = ts_bspline_num_control_points(result);
|
|
num_knots_result = ts_bspline_num_knots(result);
|
|
|
|
/* mult + n <= deg + 1 (order) with n >= 1
|
|
* => mult <= deg
|
|
* => regular evaluation
|
|
* => N = h + 1 is valid */
|
|
N = ts_deboornet_num_insertions(net) + 1;
|
|
|
|
/* 1. Copy the relevant control points and knots from `spline'.
|
|
* 2. Copy the relevant control points and knots from `net'. */
|
|
|
|
/* 1.
|
|
*
|
|
* a) Copy left hand side control points from `spline'.
|
|
* b) Copy right hand side control points from `spline'.
|
|
* c) Copy left hand side knots from `spline'.
|
|
* d) Copy right hand side knots form `spline'. */
|
|
/*** Copy Control Points ***/
|
|
memmove(ctrlp_result, ctrlp_spline, (k - deg) * sof_ctrlp); /* a) */
|
|
from = (tsReal *)ctrlp_spline + dim * (k - deg + N);
|
|
/* n >= 0 implies to >= from */
|
|
to = ctrlp_result + dim * (k - deg + N + n);
|
|
memmove(to, from, (num_ctrlp_result - n - (k - deg + N)) * sof_ctrlp); /* b) */
|
|
/*** Copy Knots ***/
|
|
memmove(knots_result, knots_spline, (k + 1) * sof_real); /* c) */
|
|
from = (tsReal *)knots_spline + k + 1;
|
|
/* n >= 0 implies to >= from */
|
|
to = knots_result + k + 1 + n;
|
|
memmove(to, from, (num_knots_result - n - (k + 1)) * sof_real); /* d) */
|
|
|
|
/* 2.
|
|
*
|
|
* a) Copy left hand side control points from `net'.
|
|
* b) Copy middle part control points from `net'.
|
|
* c) Copy right hand side control points from `net'.
|
|
* d) Copy knot from `net' (`knot'). */
|
|
from = ts_int_deboornet_access_points(net);
|
|
to = ctrlp_result + (k - deg) * dim;
|
|
stride = (int)(N * dim);
|
|
|
|
/*** Copy Control Points ***/
|
|
for (i = 0; i < n; i++) { /* a) */
|
|
memcpy(to, from, sof_ctrlp);
|
|
from += stride;
|
|
to += dim;
|
|
stride -= (int)dim;
|
|
}
|
|
memcpy(to, from, (N - n) * sof_ctrlp); /* b) */
|
|
|
|
from -= dim;
|
|
to += (N - n) * dim;
|
|
/* N = h+1 with h = deg-mult (ts_int_bspline_eval)
|
|
* => N = deg-mult+1 = order-mult.
|
|
*
|
|
* n <= order-mult
|
|
* => N-n+1 >= order-mult - order-mult + 1 = 1
|
|
* => -(int)(N-n+1) <= -1. */
|
|
stride = -(int)(N - n + 1) * (int)dim;
|
|
|
|
for (i = 0; i < n; i++) { /* c) */
|
|
memcpy(to, from, sof_ctrlp);
|
|
from += stride;
|
|
stride -= (int)dim;
|
|
to += dim;
|
|
}
|
|
/*** Copy Knot ***/
|
|
to = knots_result + k + 1;
|
|
for (i = 0; i < n; i++) { /* d) */
|
|
*to = knot;
|
|
to++;
|
|
}
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_insert_knot(const tsBSpline *spline,
|
|
tsReal knot,
|
|
size_t num,
|
|
tsBSpline *result,
|
|
size_t *k,
|
|
tsStatus *status) {
|
|
tsDeBoorNet net;
|
|
tsError err;
|
|
INIT_OUT_BSPLINE(spline, result)
|
|
ts_int_deboornet_init(&net);
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_bspline_eval(spline, knot, &net, status))
|
|
TS_CALL(try, err, ts_int_bspline_insert_knot(spline, &net, num, result, status))
|
|
ts_deboornet_free(&net);
|
|
TS_CALL(try, err, ts_bspline_eval(result, knot, &net, status))
|
|
*k = ts_deboornet_index(&net);
|
|
TS_CATCH(err)
|
|
*k = 0;
|
|
TS_FINALLY
|
|
ts_deboornet_free(&net);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_split(const tsBSpline *spline,
|
|
tsReal knot,
|
|
tsBSpline *split,
|
|
size_t *k,
|
|
tsStatus *status) {
|
|
tsDeBoorNet net;
|
|
tsError err;
|
|
INIT_OUT_BSPLINE(spline, split)
|
|
ts_int_deboornet_init(&net);
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_bspline_eval(spline, knot, &net, status))
|
|
if (ts_deboornet_multiplicity(&net) == ts_bspline_order(spline)) {
|
|
TS_CALL(try, err, ts_bspline_copy(spline, split, status))
|
|
*k = ts_deboornet_index(&net);
|
|
} else {
|
|
TS_CALL(try, err, ts_int_bspline_insert_knot(spline, &net, ts_deboornet_num_insertions(&net) + 1, split, status))
|
|
*k = ts_deboornet_index(&net) +
|
|
ts_deboornet_num_insertions(&net) + 1;
|
|
}
|
|
TS_CATCH(err)
|
|
*k = 0;
|
|
TS_FINALLY
|
|
ts_deboornet_free(&net);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_tension(const tsBSpline *spline,
|
|
tsReal beta,
|
|
tsBSpline *out,
|
|
tsStatus *status) {
|
|
const size_t dim = ts_bspline_dimension(spline);
|
|
const size_t N = ts_bspline_num_control_points(spline);
|
|
const tsReal *p0 = ts_int_bspline_access_ctrlp(spline);
|
|
const tsReal *pn_1 = p0 + (N - 1) * dim;
|
|
|
|
tsReal s; /**< The straightening factor. */
|
|
tsReal *ctrlp; /**< Pointer to the control points of `out'. */
|
|
size_t i, d; /**< Used in for loops. */
|
|
tsReal vec; /**< Straightening vector. */
|
|
tsError err;
|
|
|
|
TS_CALL_ROE(err, ts_bspline_copy(spline, out, status))
|
|
ctrlp = ts_int_bspline_access_ctrlp(out);
|
|
if (beta < (tsReal)0.0) beta = (tsReal)0.0;
|
|
if (beta > (tsReal)1.0) beta = (tsReal)1.0;
|
|
s = 1.f - beta;
|
|
|
|
for (i = 0; i < N; i++) {
|
|
for (d = 0; d < dim; d++) {
|
|
vec = ((tsReal)i / (N - 1)) * (pn_1[d] - p0[d]);
|
|
ctrlp[i * dim + d] = beta * ctrlp[i * dim + d] +
|
|
s * (p0[d] + vec);
|
|
}
|
|
}
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_to_beziers(const tsBSpline *spline,
|
|
tsBSpline *beziers,
|
|
tsStatus *status) {
|
|
const size_t deg = ts_bspline_degree(spline);
|
|
const size_t order = ts_bspline_order(spline);
|
|
|
|
int resize; /**< Number of control points to add/remove. */
|
|
size_t k; /**< Index of the split knot value. */
|
|
tsReal u_min; /**< Minimum of the knot values. */
|
|
tsReal u_max; /**< Maximum of the knot values. */
|
|
|
|
tsBSpline tmp; /**< Temporarily stores the result. */
|
|
tsReal *knots; /**< Pointer to the knots of tmp. */
|
|
size_t num_knots; /**< Number of knots in tmp. */
|
|
|
|
tsError err;
|
|
|
|
INIT_OUT_BSPLINE(spline, beziers)
|
|
TS_CALL_ROE(err, ts_bspline_copy(spline, &tmp, status))
|
|
knots = ts_int_bspline_access_knots(&tmp);
|
|
num_knots = ts_bspline_num_knots(&tmp);
|
|
|
|
TS_TRY(try, err, status)
|
|
/* DO NOT FORGET TO UPDATE knots AND num_knots AFTER EACH
|
|
* TRANSFORMATION OF tmp! */
|
|
|
|
/* Fix first control point if necessary. */
|
|
u_min = knots[deg];
|
|
if (!ts_knots_equal(knots[0], u_min)) {
|
|
TS_CALL(try, err, ts_bspline_split(&tmp, u_min, &tmp, &k, status))
|
|
resize = (int)(-1 * deg + (deg * 2 - k));
|
|
TS_CALL(try, err, ts_int_bspline_resize(&tmp, resize, 0, &tmp, status))
|
|
knots = ts_int_bspline_access_knots(&tmp);
|
|
num_knots = ts_bspline_num_knots(&tmp);
|
|
}
|
|
|
|
/* Fix last control point if necessary. */
|
|
u_max = knots[num_knots - order];
|
|
if (!ts_knots_equal(knots[num_knots - 1], u_max)) {
|
|
TS_CALL(try, err, ts_bspline_split(&tmp, u_max, &tmp, &k, status))
|
|
num_knots = ts_bspline_num_knots(&tmp);
|
|
resize = (int)(-1 * deg + (k - (num_knots - order)));
|
|
TS_CALL(try, err, ts_int_bspline_resize(&tmp, resize, 1, &tmp, status))
|
|
knots = ts_int_bspline_access_knots(&tmp);
|
|
num_knots = ts_bspline_num_knots(&tmp);
|
|
}
|
|
|
|
/* Split internal knots. */
|
|
k = order;
|
|
while (k < num_knots - order) {
|
|
TS_CALL(try, err, ts_bspline_split(&tmp, knots[k], &tmp, &k, status))
|
|
knots = ts_int_bspline_access_knots(&tmp);
|
|
num_knots = ts_bspline_num_knots(&tmp);
|
|
k++;
|
|
}
|
|
|
|
if (spline == beziers)
|
|
ts_bspline_free(beziers);
|
|
ts_bspline_move(&tmp, beziers);
|
|
TS_FINALLY
|
|
ts_bspline_free(&tmp);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_elevate_degree(const tsBSpline *spline,
|
|
size_t amount,
|
|
tsReal epsilon,
|
|
tsBSpline *elevated,
|
|
tsStatus *status) {
|
|
tsBSpline worker;
|
|
size_t dim, order;
|
|
tsReal *ctrlp, *knots;
|
|
size_t num_beziers, i, a, c, d, offset, idx;
|
|
tsReal f, f_hat, *first, *last;
|
|
tsError err;
|
|
|
|
/* Trivial case. */
|
|
if (amount == 0)
|
|
return ts_bspline_copy(spline, elevated, status);
|
|
|
|
/* An overview of this algorithm can be found at:
|
|
* https://pages.mtu.edu/~shene/COURSES/cs3621/LAB/curve/elevation.html */
|
|
INIT_OUT_BSPLINE(spline, elevated);
|
|
worker = ts_bspline_init();
|
|
TS_TRY(try, err, status)
|
|
/* Decompose `spline' into a sequence of bezier curves and make
|
|
* space for the additional control points and knots that are
|
|
* to be inserted. Results are stored in `worker'. */
|
|
TS_CALL(try, err, ts_bspline_to_beziers(spline, &worker, status));
|
|
num_beziers = ts_bspline_num_control_points(&worker) /
|
|
ts_bspline_order(&worker);
|
|
TS_CALL(try, err, ts_int_bspline_resize(
|
|
/* Resize by the number of knots to be inserted. Note
|
|
* that this creates too many control points (due to
|
|
* increasing the degree), which are removed at the end
|
|
* of this function. */
|
|
&worker, (int)((num_beziers + 1) * amount), 1, &worker, status));
|
|
dim = ts_bspline_dimension(&worker);
|
|
order = ts_bspline_order(&worker);
|
|
ctrlp = ts_int_bspline_access_ctrlp(&worker);
|
|
knots = ts_int_bspline_access_knots(&worker);
|
|
|
|
/* Move all but the first bezier curve to their new location in
|
|
* the control point array so that the additional control
|
|
* points can be inserted without overwriting the others. Note
|
|
* that iteration must run backwards. Otherwise, the moved
|
|
* values overwrite each other. */
|
|
for (i = num_beziers - 1; i > 0; i--) {
|
|
/* `i' can be interpreted as the number of bezier
|
|
* curves before the current bezier curve. */
|
|
|
|
/* Location of current bezier curve. */
|
|
offset = i * order * dim;
|
|
/* Each elevation inserts an additional control point
|
|
* into every bezier curve. `i * amount' is the total
|
|
* number of control points to be inserted before the
|
|
* current bezier curve. */
|
|
memmove(ctrlp + offset + (i * amount * dim),
|
|
ctrlp + offset,
|
|
dim * order * sizeof(tsReal));
|
|
}
|
|
|
|
/* Move all but the first group of knots to their new location
|
|
* in the knot vector so that the additional knots can be
|
|
* inserted without overwriting the others. Note that iteration
|
|
* must run backwards. Otherwise, the moved values overwrite
|
|
* each other. */
|
|
for (i = num_beziers; i > 0; i--) {
|
|
/* Note that the number of knot groups is one more than
|
|
* the number of bezier curves. `i' can be interpreted
|
|
* as the number of knot groups before the current
|
|
* group. */
|
|
|
|
/* Location of current knot group. */
|
|
offset = i * order;
|
|
/* Each elevation inserts an additional knot into every
|
|
* group of knots. `i * amount' is the total number of
|
|
* knots to be inserted before the current knot
|
|
* group. */
|
|
memmove(knots + offset + (i * amount),
|
|
knots + offset,
|
|
order * sizeof(tsReal));
|
|
}
|
|
|
|
/* `worker' is now fully set up.
|
|
* The following formulas are based on:
|
|
* https://pages.mtu.edu/~shene/COURSES/cs3621/NOTES/spline/Bezier/bezier-elev.html */
|
|
for (a = 0; a < amount; a++) {
|
|
/* For each bezier curve... */
|
|
for (i = 0; i < num_beziers; i++) {
|
|
/* ... 1) Insert and update control points. */
|
|
|
|
/* Location of current bezier curve. Each
|
|
* elevation (`a') inserts an additional
|
|
* control point into every bezier curve and
|
|
* increases the degree (`order') by one. The
|
|
* location is thus made up of two parts:
|
|
*
|
|
* i) `i * order', which is the location taking
|
|
* into account the increasing order but
|
|
* neglecting the control points that are to be
|
|
* inserted before the current bezier curve. It
|
|
* can be seen as some sort of base location:
|
|
* Where would the bezier curve be (with
|
|
* respect to the current value of `order') if
|
|
* no additional control points had to be
|
|
* inserted?
|
|
*
|
|
* ii) `i * (amount - a)', which is the total
|
|
* number of control points to be inserted
|
|
* before the current bezier curve
|
|
* (`i * amount') taking into account the
|
|
* increasing order (`order' and `a' are
|
|
* increased equally, thus, `a' compensates for
|
|
* the increasing value of `order'). This part
|
|
* adds the necessary offset to the base
|
|
* location (`i * order'). */
|
|
offset = (i * order + i * (amount - a)) * dim;
|
|
/* Duplicate last control point to the new end
|
|
* position (next control point). */
|
|
memmove(ctrlp + offset + ((order)*dim),
|
|
ctrlp + offset + ((order - 1) * dim),
|
|
dim * sizeof(tsReal));
|
|
/* All but the outer control points must be
|
|
* recalculated (domain: [1, order - 1]). By
|
|
* traversing backwards, control points can be
|
|
* modified in-place. */
|
|
for (c = order - 1; c > 0; c--) {
|
|
/* Location of current control point
|
|
* within current bezier curve. */
|
|
idx = offset + c * dim;
|
|
f = (tsReal)c / (tsReal)(order);
|
|
f_hat = 1 - f;
|
|
for (d = 0; d < dim; d++) {
|
|
/* For the sake of space, we
|
|
* increment idx by d and
|
|
* decrement it at the end of
|
|
* this loop. */
|
|
idx += d;
|
|
ctrlp[idx] =
|
|
f * ctrlp[idx - dim] +
|
|
f_hat * ctrlp[idx];
|
|
/* Reset idx. */
|
|
idx -= d;
|
|
}
|
|
}
|
|
|
|
/* ...2) Increase the multiplicity of the
|
|
* second knot group (maximum of the domain of
|
|
* the current bezier curve) by one. Note that
|
|
* this loop misses the last knot group (the
|
|
* group of the last bezier curve) as there is
|
|
* one more knot group than bezier curves to
|
|
* process. Thus, the last group must be
|
|
* increased separately after this loop. */
|
|
|
|
/* Location of current knot group. Each
|
|
* elevation (`a') inserts an additional
|
|
* knot into the knot vector of every bezier
|
|
* curve and increases the degree (`order') by
|
|
* one. The location is thus made up of two
|
|
* parts:
|
|
*
|
|
* i) `i * order', which is the location taking
|
|
* into account the increasing order but
|
|
* neglecting the knots that are to be inserted
|
|
* before the current knot group. It can be
|
|
* seen as some sort of base location: Where
|
|
* would the knot group be (with respect to the
|
|
* current value of `order') if no additional
|
|
* knots had to be inserted?
|
|
*
|
|
* ii) `i * (amount - a)', which is the total
|
|
* number of knots to be inserted before the
|
|
* current knot group (`i * amount') taking
|
|
* into account the increasing order (`order'
|
|
* and `a' are increased equally, thus, `a'
|
|
* compensates for the increasing value of
|
|
* `order'). This part adds the necessary
|
|
* offset to the base location
|
|
* (`i * order'). */
|
|
offset = i * order + i * (amount - a);
|
|
/* Duplicate knot. */
|
|
knots[offset + order] = knots[offset];
|
|
}
|
|
|
|
/* Increase the multiplicity of the very last knot
|
|
* group (the second group of the last bezier curve)
|
|
* by one. For more details, see knot duplication in
|
|
* previous loop. */
|
|
offset = num_beziers * order +
|
|
num_beziers * (amount - a);
|
|
knots[offset + order] = knots[offset];
|
|
|
|
/* Elevated by one. */
|
|
order++;
|
|
}
|
|
|
|
/* Combine bezier curves. */
|
|
d = 0; /* Number of removed knots/control points. */
|
|
for (i = 0; i < num_beziers - 1; i++) {
|
|
/* Is the last control point of bezier curve `i' equal
|
|
* to the first control point of bezier curve `i+1'? */
|
|
last = ctrlp + (i * order /* base location of `i' */
|
|
- d /* minus the number of removed values */
|
|
+ (order - 1) /* jump to last control point */
|
|
) * dim;
|
|
first = last + dim; /* next control point */
|
|
if (ts_distance(last, first, dim) <= epsilon) {
|
|
/* Move control points. */
|
|
memmove(last, first, (num_beziers - 1 - i) * order * dim * sizeof(tsReal));
|
|
|
|
/* Move knots. `last' is the last knot of the
|
|
* second knot group of bezier curve `i'.
|
|
* `first' is the first knot of the first knot
|
|
* group of bezier curve `i+1'. The
|
|
* calculations are quite similar to those for
|
|
* the control points `last' and `first' (see
|
|
* above). */
|
|
last = knots + i * order - d + (2 * order - 1);
|
|
first = last + 1;
|
|
memmove(last, first, (num_beziers - 1 - i) * order * sizeof(tsReal));
|
|
|
|
/* Removed one knot/control point. */
|
|
d++;
|
|
}
|
|
}
|
|
|
|
/* Repair internal state. */
|
|
worker.pImpl->deg = order - 1;
|
|
worker.pImpl->n_knots -= d;
|
|
worker.pImpl->n_ctrlp = ts_bspline_num_knots(&worker) - order;
|
|
memmove(ts_int_bspline_access_knots(&worker),
|
|
knots, ts_bspline_sof_knots(&worker));
|
|
worker.pImpl = realloc(worker.pImpl,
|
|
ts_int_bspline_sof_state(&worker));
|
|
if (worker.pImpl == NULL) {
|
|
TS_THROW_0(try, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
|
|
/* Move `worker' to output parameter. */
|
|
if (spline == elevated)
|
|
ts_bspline_free(elevated);
|
|
ts_bspline_move(&worker, elevated);
|
|
TS_FINALLY
|
|
ts_bspline_free(&worker);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_align(const tsBSpline *s1,
|
|
const tsBSpline *s2,
|
|
tsReal epsilon,
|
|
tsBSpline *s1_out,
|
|
tsBSpline *s2_out,
|
|
tsStatus *status) {
|
|
tsBSpline s1_worker, s2_worker, *smaller, *larger;
|
|
tsDeBoorNet net; /* the net of `smaller'. */
|
|
size_t i, missing, remaining;
|
|
tsReal min, max, shift, nextKnot;
|
|
tsError err;
|
|
|
|
INIT_OUT_BSPLINE(s1, s1_out)
|
|
INIT_OUT_BSPLINE(s2, s2_out)
|
|
s1_worker = ts_bspline_init();
|
|
s2_worker = ts_bspline_init();
|
|
smaller = larger = NULL;
|
|
TS_TRY(try, err, status)
|
|
/* Set up `s1_worker' and `s2_worker'. After this
|
|
* if-elseif-else-block, `s1_worker' and `s2_worker' have same
|
|
* degree. */
|
|
if (ts_bspline_degree(s1) < ts_bspline_degree(s2)) {
|
|
TS_CALL(try, err, ts_bspline_elevate_degree(s1, ts_bspline_degree(s2) - ts_bspline_degree(s1), epsilon, &s1_worker, status))
|
|
TS_CALL(try, err, ts_bspline_copy(s2, &s2_worker, status))
|
|
} else if (ts_bspline_degree(s2) < ts_bspline_degree(s1)) {
|
|
TS_CALL(try, err, ts_bspline_elevate_degree(s2, ts_bspline_degree(s1) - ts_bspline_degree(s2), epsilon, &s2_worker, status))
|
|
TS_CALL(try, err, ts_bspline_copy(s1, &s1_worker, status))
|
|
} else {
|
|
TS_CALL(try, err, ts_bspline_copy(s1, &s1_worker, status))
|
|
TS_CALL(try, err, ts_bspline_copy(s2, &s2_worker, status))
|
|
}
|
|
|
|
/* Set up `smaller', `larger', and `net'. */
|
|
if (ts_bspline_num_knots(&s1_worker) <
|
|
ts_bspline_num_knots(&s2_worker)) {
|
|
smaller = &s1_worker;
|
|
larger = &s2_worker;
|
|
} else {
|
|
smaller = &s2_worker;
|
|
larger = &s1_worker;
|
|
}
|
|
TS_CALL(try, err, ts_int_deboornet_new(smaller, &net, status))
|
|
|
|
/* Insert knots into `smaller' until it has the same number of
|
|
* knots (and therefore the same number of control points) as
|
|
* `larger'. */
|
|
ts_bspline_domain(smaller, &min, &max);
|
|
missing = remaining = ts_bspline_num_knots(larger) -
|
|
ts_bspline_num_knots(smaller);
|
|
shift = (tsReal)0.0;
|
|
if (missing > 0)
|
|
shift = ((tsReal)1.0 / missing) * (tsReal)0.5;
|
|
for (i = 0; remaining > 0; i++, remaining--) {
|
|
nextKnot = (max - min) * ((tsReal)i / missing) + min;
|
|
nextKnot += shift;
|
|
TS_CALL(try, err, ts_int_bspline_eval_woa(smaller, nextKnot, &net, status))
|
|
while (!ts_deboornet_num_insertions(&net)) {
|
|
/* Linear exploration for next knot. */
|
|
nextKnot += 5 * TS_KNOT_EPSILON;
|
|
if (nextKnot > max) {
|
|
TS_THROW_0(try, err, status,
|
|
TS_NO_RESULT,
|
|
"no more knots for insertion")
|
|
}
|
|
TS_CALL(try, err, ts_int_bspline_eval_woa(smaller, nextKnot, &net, status))
|
|
}
|
|
TS_CALL(try, err, ts_int_bspline_insert_knot(smaller, &net, 1, smaller, status))
|
|
}
|
|
|
|
if (s1 == s1_out)
|
|
ts_bspline_free(s1_out);
|
|
if (s2 == s2_out)
|
|
ts_bspline_free(s2_out);
|
|
ts_bspline_move(&s1_worker, s1_out);
|
|
/* if `s1_out' == `s2_out', `s2_worker' must not be moved
|
|
* because otherwise the memory of `s1_worker' is leaked
|
|
* (`s2_worker' overrides `s1_worker'). */
|
|
if (s1_out != s2_out)
|
|
ts_bspline_move(&s2_worker, s2_out);
|
|
TS_FINALLY
|
|
ts_bspline_free(&s1_worker);
|
|
ts_bspline_free(&s2_worker);
|
|
ts_deboornet_free(&net);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_morph(const tsBSpline *origin,
|
|
const tsBSpline *target,
|
|
tsReal t,
|
|
tsReal epsilon,
|
|
tsBSpline *out,
|
|
tsStatus *status) {
|
|
tsBSpline origin_al, target_al; /* aligned origin and target */
|
|
tsReal *origin_al_c, *origin_al_k; /* control points and knots */
|
|
tsReal *target_al_c, *target_al_k; /* control points and knots */
|
|
|
|
/* Properties of `out'. */
|
|
size_t deg, dim, num_ctrlp, num_knots;
|
|
tsReal *ctrlp, *knots;
|
|
tsBSpline tmp; /* temporary buffer if `out' must be resized */
|
|
|
|
tsReal t_hat;
|
|
size_t i, offset, d;
|
|
tsError err;
|
|
|
|
origin_al = ts_bspline_init();
|
|
target_al = ts_bspline_init();
|
|
TS_TRY(try, err, status)
|
|
/* Clamp `t' to domain [0, 1] and set up `t_hat'. */
|
|
if (t < (tsReal)0.0) t = (tsReal)0.0;
|
|
if (t > (tsReal)1.0) t = (tsReal)1.0;
|
|
t_hat = (tsReal)1.0 - t;
|
|
|
|
/* Set up `origin_al' and `target_al'. */
|
|
/* Degree must be elevated... */
|
|
if (ts_bspline_degree(origin) != ts_bspline_degree(target) ||
|
|
/* .. or knots (and thus control points) must be inserted. */
|
|
ts_bspline_num_knots(origin) != ts_bspline_num_knots(target)) {
|
|
TS_CALL(try, err, ts_bspline_align(origin, target, epsilon, &origin_al, &target_al, status));
|
|
} else {
|
|
/* Flat copy. */
|
|
origin_al = *origin;
|
|
target_al = *target;
|
|
}
|
|
origin_al_c = ts_int_bspline_access_ctrlp(&origin_al);
|
|
origin_al_k = ts_int_bspline_access_knots(&origin_al);
|
|
target_al_c = ts_int_bspline_access_ctrlp(&target_al);
|
|
target_al_k = ts_int_bspline_access_knots(&target_al);
|
|
|
|
/* Set up `out'. */
|
|
deg = ts_bspline_degree(&origin_al);
|
|
num_ctrlp = ts_bspline_num_control_points(&origin_al);
|
|
dim = ts_bspline_dimension(&origin_al);
|
|
if (ts_bspline_dimension(&target_al) < dim)
|
|
dim = ts_bspline_dimension(&target_al);
|
|
if (out->pImpl == NULL) {
|
|
TS_CALL(try, err, ts_bspline_new(num_ctrlp, dim, deg, TS_OPENED /* doesn't matter */, out, status))
|
|
} else if (ts_bspline_degree(out) != deg ||
|
|
ts_bspline_num_control_points(out) != num_ctrlp ||
|
|
ts_bspline_dimension(out) != dim) {
|
|
TS_CALL(try, err, ts_bspline_new(num_ctrlp, dim, deg, TS_OPENED /* doesn't matter */, &tmp, status))
|
|
ts_bspline_free(out);
|
|
ts_bspline_move(&tmp, out);
|
|
}
|
|
num_knots = ts_bspline_num_knots(out);
|
|
ctrlp = ts_int_bspline_access_ctrlp(out);
|
|
knots = ts_int_bspline_access_knots(out);
|
|
|
|
/* Interpolate control points. */
|
|
for (i = 0; i < num_ctrlp; i++) {
|
|
for (d = 0; d < dim; d++) {
|
|
offset = i * dim + d;
|
|
ctrlp[offset] = t * target_al_c[offset] +
|
|
t_hat * origin_al_c[offset];
|
|
}
|
|
}
|
|
|
|
/* Interpolate knots. */
|
|
for (i = 0; i < num_knots; i++) {
|
|
knots[i] = t * target_al_k[i] +
|
|
t_hat * origin_al_k[i];
|
|
}
|
|
TS_FINALLY
|
|
if (origin->pImpl != origin_al.pImpl)
|
|
ts_bspline_free(&origin_al);
|
|
if (target->pImpl != target_al.pImpl)
|
|
ts_bspline_free(&target_al);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
/*! @} */
|
|
|
|
/*! @name Serialization and Persistence
|
|
*
|
|
* @{
|
|
*/
|
|
tsError
|
|
ts_int_bspline_to_json(const tsBSpline *spline,
|
|
JSON_Value **value,
|
|
tsStatus *status) {
|
|
const size_t deg = ts_bspline_degree(spline);
|
|
const size_t dim = ts_bspline_dimension(spline);
|
|
const size_t len_ctrlp = ts_bspline_len_control_points(spline);
|
|
const size_t len_knots = ts_bspline_num_knots(spline);
|
|
const tsReal *ctrlp = ts_int_bspline_access_ctrlp(spline);
|
|
const tsReal *knots = ts_int_bspline_access_knots(spline);
|
|
|
|
size_t i; /**< Used in loops */
|
|
tsError err;
|
|
|
|
JSON_Value *ctrlp_value;
|
|
JSON_Value *knots_value;
|
|
JSON_Object *spline_object;
|
|
JSON_Array *ctrlp_array;
|
|
JSON_Array *knots_array;
|
|
|
|
*value = ctrlp_value = knots_value = NULL;
|
|
TS_TRY(values, err, status)
|
|
/* Init memory. */
|
|
*value = json_value_init_object();
|
|
if (!*value) {
|
|
TS_THROW_0(values, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
ctrlp_value = json_value_init_array();
|
|
if (!ctrlp_value) {
|
|
TS_THROW_0(values, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
knots_value = json_value_init_array();
|
|
if (!knots_value) {
|
|
TS_THROW_0(values, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
|
|
/* Although the following functions cannot fail, that is, they
|
|
* won't return NULL or JSONFailure, we nevertheless handle
|
|
* unexpected return values. */
|
|
|
|
/* Init output. */
|
|
spline_object = json_value_get_object(*value);
|
|
if (!spline_object) {
|
|
TS_THROW_0(values, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
|
|
/* Set degree and dimension. */
|
|
if (JSONSuccess != json_object_set_number(spline_object,
|
|
"degree",
|
|
(double)deg)) {
|
|
TS_THROW_0(values, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
if (JSONSuccess != json_object_set_number(spline_object,
|
|
"dimension",
|
|
(double)dim)) {
|
|
TS_THROW_0(values, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
|
|
/* Set control points. */
|
|
if (JSONSuccess != json_object_set_value(spline_object,
|
|
"control_points",
|
|
ctrlp_value)) {
|
|
TS_THROW_0(values, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
ctrlp_array = json_array(ctrlp_value);
|
|
if (!ctrlp_array) {
|
|
TS_THROW_0(values, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
for (i = 0; i < len_ctrlp; i++) {
|
|
if (JSONSuccess != json_array_append_number(
|
|
ctrlp_array, (double)ctrlp[i])) {
|
|
TS_THROW_0(values, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
}
|
|
|
|
/* Set knots. */
|
|
if (JSONSuccess != json_object_set_value(spline_object,
|
|
"knots",
|
|
knots_value)) {
|
|
TS_THROW_0(values, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
knots_array = json_array(knots_value);
|
|
if (!knots_array) {
|
|
TS_THROW_0(values, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
for (i = 0; i < len_knots; i++) {
|
|
if (JSONSuccess != json_array_append_number(
|
|
knots_array, (double)knots[i])) {
|
|
TS_THROW_0(values, err, status, TS_MALLOC,
|
|
"out of memory")
|
|
}
|
|
}
|
|
TS_CATCH(err)
|
|
if (*value)
|
|
json_value_free(*value);
|
|
if (ctrlp_value && !json_value_get_parent(ctrlp_value))
|
|
json_value_free(ctrlp_value);
|
|
if (knots_value && !json_value_get_parent(knots_value))
|
|
json_value_free(knots_value);
|
|
*value = NULL;
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_int_bspline_parse_json(const JSON_Value *spline_value,
|
|
tsBSpline *spline,
|
|
tsStatus *status) {
|
|
size_t deg, dim, len_ctrlp, num_knots;
|
|
tsReal *ctrlp, *knots;
|
|
|
|
JSON_Object *spline_object;
|
|
JSON_Value *deg_value;
|
|
JSON_Value *dim_value;
|
|
JSON_Value *ctrlp_value;
|
|
JSON_Array *ctrlp_array;
|
|
JSON_Value *knots_value;
|
|
JSON_Array *knots_array;
|
|
JSON_Value *real_value;
|
|
size_t i;
|
|
tsError err;
|
|
|
|
ts_int_bspline_init(spline);
|
|
|
|
/* Read spline object. */
|
|
if (json_value_get_type(spline_value) != JSONObject)
|
|
TS_RETURN_0(status, TS_PARSE_ERROR, "invalid json input")
|
|
spline_object = json_value_get_object(spline_value);
|
|
if (!spline_object)
|
|
TS_RETURN_0(status, TS_PARSE_ERROR, "invalid json input")
|
|
|
|
/* Read degree. */
|
|
deg_value = json_object_get_value(spline_object, "degree");
|
|
if (json_value_get_type(deg_value) != JSONNumber)
|
|
TS_RETURN_0(status, TS_PARSE_ERROR, "degree is not a number")
|
|
if (json_value_get_number(deg_value) < -0.01f) {
|
|
TS_RETURN_1(status, TS_PARSE_ERROR,
|
|
"degree (%f) < 0",
|
|
json_value_get_number(deg_value))
|
|
}
|
|
deg = (size_t)json_value_get_number(deg_value);
|
|
|
|
/* Read dimension. */
|
|
dim_value = json_object_get_value(spline_object, "dimension");
|
|
if (json_value_get_type(dim_value) != JSONNumber) {
|
|
TS_RETURN_0(status, TS_PARSE_ERROR,
|
|
"dimension is not a number")
|
|
}
|
|
if (json_value_get_number(dim_value) < 0.99f) {
|
|
TS_RETURN_1(status, TS_PARSE_ERROR,
|
|
"dimension (%f) < 1",
|
|
json_value_get_number(deg_value))
|
|
}
|
|
dim = (size_t)json_value_get_number(dim_value);
|
|
|
|
/* Read length of control point vector. */
|
|
ctrlp_value = json_object_get_value(spline_object, "control_points");
|
|
if (json_value_get_type(ctrlp_value) != JSONArray) {
|
|
TS_RETURN_0(status, TS_PARSE_ERROR,
|
|
"control_points is not an array")
|
|
}
|
|
ctrlp_array = json_value_get_array(ctrlp_value);
|
|
len_ctrlp = json_array_get_count(ctrlp_array);
|
|
if (len_ctrlp % dim != 0) {
|
|
TS_RETURN_2(status, TS_PARSE_ERROR,
|
|
"len(control_points) (%lu) %% dimension (%lu) != 0",
|
|
(unsigned long)len_ctrlp, (unsigned long)dim)
|
|
}
|
|
|
|
/* Read number of knots. */
|
|
knots_value = json_object_get_value(spline_object, "knots");
|
|
if (json_value_get_type(knots_value) != JSONArray) {
|
|
TS_RETURN_0(status, TS_PARSE_ERROR,
|
|
"knots is not an array")
|
|
}
|
|
knots_array = json_value_get_array(knots_value);
|
|
num_knots = json_array_get_count(knots_array);
|
|
|
|
/* Create spline. */
|
|
TS_TRY(try, err, status)
|
|
TS_CALL(try, err, ts_bspline_new(len_ctrlp / dim, dim, deg, TS_CLAMPED, spline, status))
|
|
if (num_knots != ts_bspline_num_knots(spline))
|
|
TS_THROW_2(try, err, status, TS_NUM_KNOTS,
|
|
"unexpected num(knots): (%lu) != (%lu)",
|
|
(unsigned long)num_knots,
|
|
(unsigned long)ts_bspline_num_knots(spline))
|
|
|
|
/* Set control points. */
|
|
ctrlp = ts_int_bspline_access_ctrlp(spline);
|
|
for (i = 0; i < len_ctrlp; i++) {
|
|
real_value = json_array_get_value(ctrlp_array, i);
|
|
if (json_value_get_type(real_value) != JSONNumber)
|
|
TS_THROW_1(try, err, status, TS_PARSE_ERROR,
|
|
"control_points: value at index %lu is not a number",
|
|
(unsigned long)i)
|
|
ctrlp[i] = (tsReal)json_value_get_number(real_value);
|
|
}
|
|
TS_CALL(try, err, ts_bspline_set_control_points(spline, ctrlp, status))
|
|
|
|
/* Set knots. */
|
|
knots = ts_int_bspline_access_knots(spline);
|
|
for (i = 0; i < num_knots; i++) {
|
|
real_value = json_array_get_value(knots_array, i);
|
|
if (json_value_get_type(real_value) != JSONNumber)
|
|
TS_THROW_1(try, err, status, TS_PARSE_ERROR,
|
|
"knots: value at index %lu is not a number",
|
|
(unsigned long)i)
|
|
knots[i] = (tsReal)json_value_get_number(real_value);
|
|
}
|
|
TS_CALL(try, err, ts_bspline_set_knots(spline, knots, status))
|
|
TS_CATCH(err)
|
|
ts_bspline_free(spline);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_to_json(const tsBSpline *spline,
|
|
char **json,
|
|
tsStatus *status) {
|
|
tsError err;
|
|
JSON_Value *value = NULL;
|
|
*json = NULL;
|
|
TS_CALL_ROE(err, ts_int_bspline_to_json(spline, &value, status))
|
|
*json = json_serialize_to_string_pretty(value);
|
|
json_value_free(value);
|
|
if (!*json)
|
|
TS_RETURN_0(status, TS_MALLOC, "out of memory")
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_parse_json(const char *json,
|
|
tsBSpline *spline,
|
|
tsStatus *status) {
|
|
tsError err;
|
|
JSON_Value *value = NULL;
|
|
ts_int_bspline_init(spline);
|
|
TS_TRY(try, err, status)
|
|
value = json_parse_string(json);
|
|
if (!value) {
|
|
TS_RETURN_0(status, TS_PARSE_ERROR,
|
|
"invalid json input")
|
|
}
|
|
TS_CALL(try, err, ts_int_bspline_parse_json(value, spline, status))
|
|
TS_FINALLY
|
|
if (value)
|
|
json_value_free(value);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_save(const tsBSpline *spline,
|
|
const char *path,
|
|
tsStatus *status) {
|
|
tsError err;
|
|
JSON_Status json_status;
|
|
JSON_Value *value = NULL;
|
|
TS_CALL_ROE(err, ts_int_bspline_to_json(spline, &value, status))
|
|
json_status = json_serialize_to_file_pretty(value, path);
|
|
json_value_free(value);
|
|
if (json_status != JSONSuccess)
|
|
TS_RETURN_0(status, TS_IO_ERROR, "unexpected io error")
|
|
TS_RETURN_SUCCESS(status)
|
|
}
|
|
|
|
tsError
|
|
ts_bspline_load(const char *path,
|
|
tsBSpline *spline,
|
|
tsStatus *status) {
|
|
tsError err;
|
|
FILE *file = NULL;
|
|
JSON_Value *value = NULL;
|
|
ts_int_bspline_init(spline);
|
|
TS_TRY(try, err, status)
|
|
file = fopen(path, "r");
|
|
if (!file) {
|
|
TS_THROW_0(try, err, status, TS_IO_ERROR,
|
|
"unable to open file")
|
|
}
|
|
value = json_parse_file(path);
|
|
if (!value) {
|
|
TS_RETURN_0(status, TS_PARSE_ERROR,
|
|
"invalid json input")
|
|
}
|
|
TS_CALL(try, err, ts_int_bspline_parse_json(value, spline, status))
|
|
TS_FINALLY
|
|
if (file)
|
|
fclose(file);
|
|
if (value)
|
|
json_value_free(value);
|
|
TS_CATCH(err)
|
|
ts_bspline_free(spline);
|
|
TS_END_TRY_RETURN(err)
|
|
}
|
|
/*! @} */
|
|
|
|
/*! @name Vector Math
|
|
* @{
|
|
*/
|
|
void ts_vec2_init(tsReal *out,
|
|
tsReal x,
|
|
tsReal y) {
|
|
out[0] = x;
|
|
out[1] = y;
|
|
}
|
|
|
|
void ts_vec3_init(tsReal *out,
|
|
tsReal x,
|
|
tsReal y,
|
|
tsReal z) {
|
|
out[0] = x;
|
|
out[1] = y;
|
|
out[2] = z;
|
|
}
|
|
|
|
void ts_vec4_init(tsReal *out,
|
|
tsReal x,
|
|
tsReal y,
|
|
tsReal z,
|
|
tsReal w) {
|
|
out[0] = x;
|
|
out[1] = y;
|
|
out[2] = z;
|
|
out[3] = w;
|
|
}
|
|
|
|
void ts_vec2_set(tsReal *out,
|
|
const tsReal *x,
|
|
size_t dim) {
|
|
const size_t n = dim > 2 ? 2 : dim;
|
|
memmove(out, x, n * sizeof(tsReal));
|
|
if (dim < 2)
|
|
ts_arr_fill(out + dim, 2 - dim, (tsReal)0.0);
|
|
}
|
|
|
|
void ts_vec3_set(tsReal *out,
|
|
const tsReal *x,
|
|
size_t dim) {
|
|
const size_t n = dim > 3 ? 3 : dim;
|
|
memmove(out, x, n * sizeof(tsReal));
|
|
if (dim < 3)
|
|
ts_arr_fill(out + dim, 3 - dim, (tsReal)0.0);
|
|
}
|
|
|
|
void ts_vec4_set(tsReal *out,
|
|
const tsReal *x,
|
|
size_t dim) {
|
|
const size_t n = dim > 4 ? 4 : dim;
|
|
memmove(out, x, n * sizeof(tsReal));
|
|
if (dim < 4)
|
|
ts_arr_fill(out + dim, 4 - dim, (tsReal)0.0);
|
|
}
|
|
|
|
void ts_vec_add(const tsReal *x,
|
|
const tsReal *y,
|
|
size_t dim,
|
|
tsReal *out) {
|
|
size_t i;
|
|
for (i = 0; i < dim; i++)
|
|
out[i] = x[i] + y[i];
|
|
}
|
|
|
|
void ts_vec_sub(const tsReal *x,
|
|
const tsReal *y,
|
|
size_t dim,
|
|
tsReal *out) {
|
|
size_t i;
|
|
if (x == y) {
|
|
/* More stable version. */
|
|
ts_arr_fill(out, dim, (tsReal)0.0);
|
|
return;
|
|
}
|
|
for (i = 0; i < dim; i++)
|
|
out[i] = x[i] - y[i];
|
|
}
|
|
|
|
tsReal
|
|
ts_vec_dot(const tsReal *x,
|
|
const tsReal *y,
|
|
size_t dim) {
|
|
size_t i;
|
|
tsReal dot = 0;
|
|
for (i = 0; i < dim; i++)
|
|
dot += x[i] * y[i];
|
|
return dot;
|
|
}
|
|
|
|
tsReal
|
|
ts_vec_angle(const tsReal *x,
|
|
const tsReal *y,
|
|
tsReal *buf,
|
|
size_t dim) {
|
|
const tsReal *x_norm, *y_norm;
|
|
if (buf) {
|
|
ts_vec_norm(x, dim, buf);
|
|
ts_vec_norm(y, dim, buf + dim);
|
|
x_norm = buf;
|
|
y_norm = buf + dim;
|
|
} else {
|
|
x_norm = x;
|
|
y_norm = y;
|
|
}
|
|
return (tsReal)(
|
|
/* Use doubles as long as possible. */
|
|
acos(ts_vec_dot(x_norm,
|
|
y_norm,
|
|
dim)) *
|
|
(180.0 / TS_PI) /* radiant to degree */
|
|
);
|
|
}
|
|
|
|
void ts_vec3_cross(const tsReal *x,
|
|
const tsReal *y,
|
|
tsReal *out) {
|
|
tsReal a, b, c;
|
|
a = x[1] * y[2] - x[2] * y[1];
|
|
b = x[2] * y[0] - x[0] * y[2];
|
|
c = x[0] * y[1] - x[1] * y[0];
|
|
out[0] = a;
|
|
out[1] = b;
|
|
out[2] = c;
|
|
}
|
|
|
|
void ts_vec_norm(const tsReal *x,
|
|
size_t dim,
|
|
tsReal *out) {
|
|
size_t i;
|
|
const tsReal m = ts_vec_mag(x, dim);
|
|
if (m < TS_LENGTH_ZERO) {
|
|
ts_arr_fill(out, dim, (tsReal)0.0);
|
|
return;
|
|
}
|
|
for (i = 0; i < dim; i++)
|
|
out[i] = x[i] / m;
|
|
}
|
|
|
|
tsReal
|
|
ts_vec_mag(const tsReal *x,
|
|
size_t dim) {
|
|
size_t i;
|
|
tsReal sum = 0;
|
|
for (i = 0; i < dim; i++)
|
|
sum += (x[i] * x[i]);
|
|
return (tsReal)sqrt(sum);
|
|
}
|
|
|
|
void ts_vec_mul(const tsReal *x,
|
|
size_t dim,
|
|
tsReal val,
|
|
tsReal *out) {
|
|
size_t i;
|
|
for (i = 0; i < dim; i++)
|
|
out[i] = x[i] * val;
|
|
}
|
|
/*! @} */
|
|
|
|
/*! @name Utility Functions
|
|
*
|
|
* @{
|
|
*/
|
|
int ts_knots_equal(tsReal x,
|
|
tsReal y) {
|
|
return fabs(x - y) < TS_KNOT_EPSILON ? 1 : 0;
|
|
}
|
|
|
|
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_distance(const tsReal *x,
|
|
const tsReal *y,
|
|
size_t dim) {
|
|
size_t i;
|
|
tsReal sum = 0;
|
|
for (i = 0; i < dim; i++)
|
|
sum += (x[i] - y[i]) * (x[i] - y[i]);
|
|
return (tsReal)sqrt(sum);
|
|
}
|
|
/*! @} */
|
|
|
|
#ifdef _MSC_VER
|
|
#pragma warning(pop)
|
|
#endif
|