/** * 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; }