/**
* Find the most distant point between the 2 knots.
*/
static uint knot_find_split_point(
const struct PointData *pd,
const struct Knot *knot_l, const struct Knot *knot_r,
const uint knots_len,
const uint dims)
{
uint split_point = SPLIT_POINT_INVALID;
double split_point_dist_best = -DBL_MAX;
const double *offset = &pd->points[knot_l->index * dims];
#ifdef USE_VLA
double v_plane[dims];
double v_proj[dims];
double v_offset[dims];
#else
double *v_plane = alloca(sizeof(double) * dims);
double *v_proj = alloca(sizeof(double) * dims);
double *v_offset = alloca(sizeof(double) * dims);
#endif
sub_vn_vnvn(
v_plane,
&pd->points[knot_l->index * dims],
&pd->points[knot_r->index * dims],
dims);
normalize_vn(v_plane, dims);
const uint knots_end = knots_len - 1;
const struct Knot *k_step = knot_l;
do {
if (k_step->index != knots_end) {
k_step += 1;
}
else {
/* wrap around */
k_step = k_step - knots_end;
}
if (k_step != knot_r) {
sub_vn_vnvn(v_offset, &pd->points[k_step->index * dims], offset, dims);
project_plane_vn_vnvn_normalized(v_proj, v_offset, v_plane, dims);
double split_point_dist_test = len_squared_vn(v_proj, dims);
if (split_point_dist_test > split_point_dist_best) {
split_point_dist_best = split_point_dist_test;
split_point = k_step->index;
}
}
else {
break;
}
} while (true);
return split_point;
}
/* subtraction: obj - obj */
static PyObject *Color_sub(PyObject *v1, PyObject *v2)
{
ColorObject *color1 = NULL, *color2 = NULL;
float col[COLOR_SIZE];
if (!ColorObject_Check(v1) || !ColorObject_Check(v2)) {
PyErr_SetString(PyExc_TypeError,
"Color subtraction: "
"arguments not valid for this operation");
return NULL;
}
color1 = (ColorObject*)v1;
color2 = (ColorObject*)v2;
if(BaseMath_ReadCallback(color1) == -1 || BaseMath_ReadCallback(color2) == -1)
return NULL;
sub_vn_vnvn(col, color1->col, color2->col, COLOR_SIZE);
return newColorObject(col, Py_NEW, Py_TYPE(v1));
}
/* subtraction: obj - obj */
static PyObject *Color_sub(PyObject *v1, PyObject *v2)
{
ColorObject *color1 = NULL, *color2 = NULL;
float col[COLOR_SIZE];
if (!ColorObject_Check(v1) || !ColorObject_Check(v2)) {
PyErr_Format(PyExc_TypeError,
"Color subtraction: (%s - %s) "
"invalid type for this operation",
Py_TYPE(v1)->tp_name, Py_TYPE(v2)->tp_name);
return NULL;
}
color1 = (ColorObject*)v1;
color2 = (ColorObject*)v2;
if (BaseMath_ReadCallback(color1) == -1 || BaseMath_ReadCallback(color2) == -1)
return NULL;
sub_vn_vnvn(col, color1->col, color2->col, COLOR_SIZE);
return Color_CreatePyObject(col, Py_NEW, Py_TYPE(v1));
}
/**
* Attempt to collapse close knots into corners,
* as long as they fall below the error threshold.
*/
static uint curve_incremental_simplify_corners(
const struct PointData *pd,
struct Knot *knots, const uint knots_len, uint knots_len_remaining,
const double error_sq_max, const double error_sq_2x_max,
const double corner_angle,
const uint dims,
uint *r_corner_index_len)
{
#ifdef USE_TPOOL
struct ElemPool_KnotCornerState epool;
corner_pool_create(&epool, 0);
#endif
Heap *heap = HEAP_new(0);
struct KnotCorner_Params params = {
.pd = pd,
.heap = heap,
#ifdef USE_TPOOL
.epool = &epool,
#endif
};
#ifdef USE_VLA
double plane_no[dims];
double k_proj_ref[dims];
double k_proj_split[dims];
#else
double *plane_no = alloca(sizeof(double) * dims);
double *k_proj_ref = alloca(sizeof(double) * dims);
double *k_proj_split = alloca(sizeof(double) * dims);
#endif
const double corner_angle_cos = cos(corner_angle);
uint corner_index_len = 0;
for (uint i = 0; i < knots_len; i++) {
if ((knots[i].is_removed == false) &&
(knots[i].can_remove == true) &&
(knots[i].next && knots[i].next->can_remove))
{
struct Knot *k_prev = &knots[i];
struct Knot *k_next = k_prev->next;
/* Angle outside threshold */
if (dot_vnvn(k_prev->tan[0], k_next->tan[1], dims) < corner_angle_cos) {
/* Measure distance projected onto a plane,
* since the points may be offset along their own tangents. */
sub_vn_vnvn(plane_no, k_next->tan[0], k_prev->tan[1], dims);
/* Compare 2x so as to allow both to be changed by maximum of error_sq_max */
const uint split_index = knot_find_split_point_on_axis(
pd, k_prev, k_next,
knots_len,
plane_no,
dims);
if (split_index != SPLIT_POINT_INVALID) {
const double *co_prev = ¶ms.pd->points[k_prev->index * dims];
const double *co_next = ¶ms.pd->points[k_next->index * dims];
const double *co_split = ¶ms.pd->points[split_index * dims];
project_vn_vnvn_normalized(k_proj_ref, co_prev, k_prev->tan[1], dims);
project_vn_vnvn_normalized(k_proj_split, co_split, k_prev->tan[1], dims);
if (len_squared_vnvn(k_proj_ref, k_proj_split, dims) < error_sq_2x_max) {
project_vn_vnvn_normalized(k_proj_ref, co_next, k_next->tan[0], dims);
project_vn_vnvn_normalized(k_proj_split, co_split, k_next->tan[0], dims);
if (len_squared_vnvn(k_proj_ref, k_proj_split, dims) < error_sq_2x_max) {
struct Knot *k_split = &knots[split_index];
knot_corner_error_recalculate(
¶ms,
k_split, k_prev, k_next,
error_sq_max,
dims);
}
}
}
}
}
}
while (HEAP_is_empty(heap) == false) {
struct KnotCornerState *c = HEAP_popmin(heap);
struct Knot *k_split = &knots[c->index];
/* Remove while collapsing */
struct Knot *k_prev = &knots[c->index_adjacent[0]];
struct Knot *k_next = &knots[c->index_adjacent[1]];
/* Insert */
k_split->is_removed = false;
k_split->prev = k_prev;
k_split->next = k_next;
k_prev->next = k_split;
k_next->prev = k_split;
/* Update tangents */
k_split->tan[0] = k_prev->tan[1];
k_split->tan[1] = k_next->tan[0];
/* Own handles */
k_prev->handles[1] = c->handles_prev[0];
k_split->handles[0] = c->handles_prev[1];
k_split->handles[1] = c->handles_next[0];
k_next->handles[0] = c->handles_next[1];
k_prev->error_sq_next = c->error_sq[0];
k_split->error_sq_next = c->error_sq[1];
k_split->heap_node = NULL;
#ifdef USE_TPOOL
corner_pool_elem_free(&epool, c);
#else
free(c);
#endif
k_split->is_corner = true;
knots_len_remaining++;
corner_index_len++;
}
#ifdef USE_TPOOL
corner_pool_destroy(&epool);
#endif
HEAP_free(heap, free);
*r_corner_index_len = corner_index_len;
return knots_len_remaining;
}
int curve_fit_cubic_to_points_refit_db(
const double *points,
const uint points_len,
const uint dims,
const double error_threshold,
const uint calc_flag,
const uint *corners,
const uint corners_len,
const double corner_angle,
double **r_cubic_array, uint *r_cubic_array_len,
uint **r_cubic_orig_index,
uint **r_corner_index_array, uint *r_corner_index_len)
{
const uint knots_len = points_len;
struct Knot *knots = malloc(sizeof(Knot) * knots_len);
#ifndef USE_CORNER_DETECT
(void)r_corner_index_array;
(void)r_corner_index_len;
#endif
(void)corners;
(void)corners_len;
const bool is_cyclic = (calc_flag & CURVE_FIT_CALC_CYCLIC) != 0 && (points_len > 2);
#ifdef USE_CORNER_DETECT
const bool use_corner = (corner_angle < M_PI);
#else
(void)corner_angle;
#endif
/* Over alloc the list x2 for cyclic curves,
* so we can evaluate across the start/end */
double *points_alloc = NULL;
if (is_cyclic) {
points_alloc = malloc((sizeof(double) * points_len * dims) * 2);
memcpy(points_alloc, points, sizeof(double) * points_len * dims);
memcpy(points_alloc + (points_len * dims), points_alloc, sizeof(double) * points_len * dims);
points = points_alloc;
}
double *tangents = malloc(sizeof(double) * knots_len * 2 * dims);
{
double *t_step = tangents;
for (uint i = 0; i < knots_len; i++) {
knots[i].next = (knots + i) + 1;
knots[i].prev = (knots + i) - 1;
knots[i].heap_node = NULL;
knots[i].index = i;
knots[i].can_remove = true;
knots[i].is_removed = false;
knots[i].is_corner = false;
knots[i].error_sq_next = 0.0;
knots[i].tan[0] = t_step; t_step += dims;
knots[i].tan[1] = t_step; t_step += dims;
}
assert(t_step == &tangents[knots_len * 2 * dims]);
}
if (is_cyclic) {
knots[0].prev = &knots[knots_len - 1];
knots[knots_len - 1].next = &knots[0];
}
else {
knots[0].prev = NULL;
knots[knots_len - 1].next = NULL;
/* always keep end-points */
knots[0].can_remove = false;
knots[knots_len - 1].can_remove = false;
}
#ifdef USE_LENGTH_CACHE
double *points_length_cache = malloc(sizeof(double) * points_len * (is_cyclic ? 2 : 1));
#endif
/* Initialize tangents,
* also set the values for knot handles since some may not collapse. */
{
#ifdef USE_VLA
double tan_prev[dims];
double tan_next[dims];
#else
double *tan_prev = alloca(sizeof(double) * dims);
double *tan_next = alloca(sizeof(double) * dims);
#endif
double len_prev, len_next;
#if 0
/* 2x normalize calculations, but correct */
for (uint i = 0; i < knots_len; i++) {
Knot *k = &knots[i];
if (k->prev) {
sub_vn_vnvn(tan_prev, &points[k->prev->index * dims], &points[k->index * dims], dims);
#ifdef USE_LENGTH_CACHE
points_length_cache[i] =
#endif
len_prev = normalize_vn(tan_prev, dims);
}
else {
zero_vn(tan_prev, dims);
len_prev = 0.0;
}
if (k->next) {
sub_vn_vnvn(tan_next, &points[k->index * dims], &points[k->next->index * dims], dims);
len_next = normalize_vn(tan_next, dims);
}
else {
zero_vn(tan_next, dims);
len_next = 0.0;
}
add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims);
normalize_vn(k->tan[0], dims);
copy_vnvn(k->tan[1], k->tan[0], dims);
k->handles[0] = len_prev / 3;
k->handles[1] = len_next / -3;
}
#else
if (knots_len < 2) {
/* NOP, set dummy values */
for (uint i = 0; i < knots_len; i++) {
struct Knot *k = &knots[i];
zero_vn(k->tan[0], dims);
zero_vn(k->tan[1], dims);
k->handles[0] = 0.0;
k->handles[1] = 0.0;
#ifdef USE_LENGTH_CACHE
points_length_cache[i] = 0.0;
#endif
}
}
else if (is_cyclic) {
len_prev = normalize_vn_vnvn(
tan_prev, &points[(knots_len - 2) * dims], &points[(knots_len - 1) * dims], dims);
for (uint i_curr = knots_len - 1, i_next = 0; i_next < knots_len; i_curr = i_next++) {
struct Knot *k = &knots[i_curr];
#ifdef USE_LENGTH_CACHE
points_length_cache[i_next] =
#endif
len_next = normalize_vn_vnvn(tan_next, &points[i_curr * dims], &points[i_next * dims], dims);
add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims);
normalize_vn(k->tan[0], dims);
copy_vnvn(k->tan[1], k->tan[0], dims);
k->handles[0] = len_prev / 3;
k->handles[1] = len_next / -3;
copy_vnvn(tan_prev, tan_next, dims);
len_prev = len_next;
}
}
else {
#ifdef USE_LENGTH_CACHE
points_length_cache[0] = 0.0;
points_length_cache[1] =
#endif
len_prev = normalize_vn_vnvn(
tan_prev, &points[0 * dims], &points[1 * dims], dims);
copy_vnvn(knots[0].tan[0], tan_prev, dims);
copy_vnvn(knots[0].tan[1], tan_prev, dims);
knots[0].handles[0] = len_prev / 3;
knots[0].handles[1] = len_prev / -3;
for (uint i_curr = 1, i_next = 2; i_next < knots_len; i_curr = i_next++) {
struct Knot *k = &knots[i_curr];
#ifdef USE_LENGTH_CACHE
points_length_cache[i_next] =
#endif
len_next = normalize_vn_vnvn(tan_next, &points[i_curr * dims], &points[i_next * dims], dims);
add_vn_vnvn(k->tan[0], tan_prev, tan_next, dims);
normalize_vn(k->tan[0], dims);
copy_vnvn(k->tan[1], k->tan[0], dims);
k->handles[0] = len_prev / 3;
k->handles[1] = len_next / -3;
copy_vnvn(tan_prev, tan_next, dims);
len_prev = len_next;
}
copy_vnvn(knots[knots_len - 1].tan[0], tan_next, dims);
copy_vnvn(knots[knots_len - 1].tan[1], tan_next, dims);
knots[knots_len - 1].handles[0] = len_next / 3;
knots[knots_len - 1].handles[1] = len_next / -3;
}
#endif
}
#ifdef USE_LENGTH_CACHE
if (is_cyclic) {
memcpy(&points_length_cache[points_len], points_length_cache, sizeof(double) * points_len);
}
#endif
#if 0
for (uint i = 0; i < knots_len; i++) {
Knot *k = &knots[i];
printf("TAN %.8f %.8f %.8f %.8f\n", k->tan[0][0], k->tan[0][1], k->tan[1][0], k->tan[0][1]);
}
#endif
const struct PointData pd = {
.points = points,
.points_len = points_len,
#ifdef USE_LENGTH_CACHE
.points_length_cache = points_length_cache,
#endif
};
uint knots_len_remaining = knots_len;
/* 'curve_incremental_simplify_refit' can be called here, but its very slow
* just remove all within the threshold first. */
knots_len_remaining = curve_incremental_simplify(
&pd, knots, knots_len, knots_len_remaining,
SQUARE(error_threshold), dims);
#ifdef USE_CORNER_DETECT
if (use_corner) {
for (uint i = 0; i < knots_len; i++) {
assert(knots[i].heap_node == NULL);
}
knots_len_remaining = curve_incremental_simplify_corners(
&pd, knots, knots_len, knots_len_remaining,
SQUARE(error_threshold), SQUARE(error_threshold * 3),
corner_angle,
dims,
r_corner_index_len);
}
#endif /* USE_CORNER_DETECT */
#ifdef USE_KNOT_REFIT
knots_len_remaining = curve_incremental_simplify_refit(
&pd, knots, knots_len, knots_len_remaining,
SQUARE(error_threshold),
dims);
#endif /* USE_KNOT_REFIT */
#ifdef USE_CORNER_DETECT
if (use_corner) {
if (is_cyclic == false) {
*r_corner_index_len += 2;
}
uint *corner_index_array = malloc(sizeof(uint) * (*r_corner_index_len));
uint k_index = 0, c_index = 0;
uint i = 0;
if (is_cyclic == false) {
corner_index_array[c_index++] = k_index;
k_index++;
i++;
}
for (; i < knots_len; i++) {
if (knots[i].is_removed == false) {
if (knots[i].is_corner == true) {
corner_index_array[c_index++] = k_index;
}
k_index++;
}
}
if (is_cyclic == false) {
corner_index_array[c_index++] = k_index;
k_index++;
}
assert(c_index == *r_corner_index_len);
*r_corner_index_array = corner_index_array;
}
#endif /* USE_CORNER_DETECT */
#ifdef USE_LENGTH_CACHE
free(points_length_cache);
#endif
uint *cubic_orig_index = NULL;
if (r_cubic_orig_index) {
cubic_orig_index = malloc(sizeof(uint) * knots_len_remaining);
}
struct Knot *knots_first = NULL;
{
struct Knot *k;
for (uint i = 0; i < knots_len; i++) {
if (knots[i].is_removed == false) {
knots_first = &knots[i];
break;
}
}
if (cubic_orig_index) {
k = knots_first;
for (uint i = 0; i < knots_len_remaining; i++, k = k->next) {
cubic_orig_index[i] = k->index;
}
}
}
/* Correct unused handle endpoints - not essential, but nice behavior */
if (is_cyclic == false) {
struct Knot *knots_last = knots_first;
while (knots_last->next) {
knots_last = knots_last->next;
}
knots_first->handles[0] = -knots_first->handles[1];
knots_last->handles[1] = -knots_last->handles[0];
}
/* 3x for one knot and two handles */
double *cubic_array = malloc(sizeof(double) * knots_len_remaining * 3 * dims);
{
double *c_step = cubic_array;
struct Knot *k = knots_first;
for (uint i = 0; i < knots_len_remaining; i++, k = k->next) {
const double *p = &points[k->index * dims];
madd_vn_vnvn_fl(c_step, p, k->tan[0], k->handles[0], dims);
c_step += dims;
copy_vnvn(c_step, p, dims);
c_step += dims;
madd_vn_vnvn_fl(c_step, p, k->tan[1], k->handles[1], dims);
c_step += dims;
}
assert(c_step == &cubic_array[knots_len_remaining * 3 * dims]);
}
if (points_alloc) {
free(points_alloc);
points_alloc = NULL;
}
free(knots);
free(tangents);
if (r_cubic_orig_index) {
*r_cubic_orig_index = cubic_orig_index;
}
*r_cubic_array = cubic_array;
*r_cubic_array_len = knots_len_remaining;
return 0;
}
