Curve Fitting: expose function for fitting a single curve

3 files changed, 238 insertions, 49 deletions

diff --git a/extern/curve_fit_nd/curve_fit_nd.h b/extern/curve_fit_nd/curve_fit_nd.h
index d20921c186a..98e6779fd6f 100644
--- a/extern/curve_fit_nd/curve_fit_nd.h
+++ b/extern/curve_fit_nd/curve_fit_nd.h
@@ -79,6 +79,43 @@ int curve_fit_cubic_to_points_fl(
         unsigned int **r_cubic_orig_index,
         unsigned int **r_corners_index_array, unsigned int *r_corners_index_len);
 
+/**
+ * Takes a flat array of points and evalues that to calculate handle lengths.
+ *
+ * \param points, points_len: The array of points to calculate a cubics from.
+ * \param dims: The number of dimensions for for each element in \a points.
+ * \param error_threshold: the error threshold to allow for,
+ * \param tan_l, tan_r: Normalized tangents the handles will be aligned to.
+ * Note that tangents must both point along the direction of the \a points,
+ * so \a tan_l points in the same direction of the resulting handle,
+ * where \a tan_r will point the opposite direction of its handle.
+ *
+ * \param r_handle_l, r_handle_r: Resulting calculated handles.
+ * \param r_error_sq: The maximum distance  (squared) this curve diverges from \a points.
+ */
+int curve_fit_cubic_to_points_single_db(
+        const double *points,
+        const uint    points_len,
+        const uint    dims,
+        const double  error_threshold,
+        const double tan_l[],
+        const double tan_r[],
+
+        double  r_handle_l[],
+        double  r_handle_r[],
+        double *r_error_sq);
+
+int curve_fit_cubic_to_points_single_fl(
+        const float  *points,
+        const uint    points_len,
+        const uint    dims,
+        const float   error_threshold,
+        const float   tan_l[],
+        const float   tan_r[],
+
+        float   r_handle_l[],
+        float   r_handle_r[],
+        float  *r_error_sq);
 
 /* curve_fit_corners_detect.c */
 
diff --git a/extern/curve_fit_nd/intern/curve_fit_cubic.c b/extern/curve_fit_nd/intern/curve_fit_cubic.c
index 6aee04f20b1..a4b51d97c27 100644
--- a/extern/curve_fit_nd/intern/curve_fit_cubic.c
+++ b/extern/curve_fit_nd/intern/curve_fit_cubic.c
@@ -109,9 +109,14 @@ typedef struct Cubic {
 	*_p3 = _p2 + (dims); ((void)0)
 
 
+static size_t cubic_alloc_size(const uint dims)
+{
+	return sizeof(Cubic) + (sizeof(double) * 4 * dims);
+}
+
 static Cubic *cubic_alloc(const uint dims)
 {
-	return malloc(sizeof(Cubic) + (sizeof(double) * 4 * dims));
+	return malloc(cubic_alloc_size(dims));
 }
 
 static void cubic_init(
@@ -286,20 +291,19 @@ static void cubic_calc_acceleration(
 }
 
 /**
- * Returns a 'measure' of the maximal discrepancy of the points specified
+ * Returns a 'measure' of the maximum distance (squared) of the points specified
  * by points_offset from the corresponding cubic(u[]) points.
  */
-static void cubic_calc_error(
+static double cubic_calc_error(
         const Cubic *cubic,
         const double *points_offset,
         const uint points_offset_len,
         const double *u,
         const uint dims,
 
-        double *r_error_sq_max,
         uint *r_error_index)
 {
-	double error_sq_max = 0.0;
+	double error_max_sq = 0.0;
 	uint   error_index = 0;
 
 	const double *pt_real = points_offset + dims;
@@ -313,14 +317,14 @@ static void cubic_calc_error(
 		cubic_evaluate(cubic, u[i], dims, pt_eval);
 
 		const double err_sq = len_squared_vnvn(pt_real, pt_eval, dims);
-		if (err_sq >= error_sq_max) {
-			error_sq_max = err_sq;
+		if (err_sq >= error_max_sq) {
+			error_max_sq = err_sq;
 			error_index = i;
 		}
 	}
 
-	*r_error_sq_max   = error_sq_max;
 	*r_error_index = error_index;
+	return error_max_sq;
 }
 
 /**
@@ -695,7 +699,7 @@ static bool cubic_reparameterize(
 }
 
 
-static void fit_cubic_to_points(
+static bool fit_cubic_to_points(
         const double *points_offset,
         const uint    points_offset_len,
 #ifdef USE_LENGTH_CACHE
@@ -703,19 +707,15 @@ static void fit_cubic_to_points(
 #endif
         const double  tan_l[],
         const double  tan_r[],
-        const double  error_threshold,
+        const double  error_threshold_sq,
         const uint    dims,
-        /* fill in the list */
-        CubicList *clist)
+
+        Cubic *r_cubic, double *r_error_max_sq, uint *r_split_index)
 {
 	const uint iteration_max = 4;
-	const double error_sq = sq(error_threshold);
-
-	Cubic *cubic;
 
 	if (points_offset_len == 2) {
-		cubic = cubic_alloc(dims);
-		CUBIC_VARS(cubic, dims, p0, p1, p2, p3);
+		CUBIC_VARS(r_cubic, dims, p0, p1, p2, p3);
 
 		copy_vnvn(p0, &points_offset[0 * dims], dims);
 		copy_vnvn(p3, &points_offset[1 * dims], dims);
@@ -725,11 +725,9 @@ static void fit_cubic_to_points(
 		madd_vn_vnvn_fl(p2, p3, tan_r, dist, dims);
 
 #ifdef USE_ORIG_INDEX_DATA
-		cubic->orig_span = 1;
+		r_cubic->orig_span = 1;
 #endif
-
-		cubic_list_prepend(clist, cubic);
-		return;
+		return true;
 	}
 
 	double *u = malloc(sizeof(double) * points_offset_len);
@@ -740,55 +738,97 @@ static void fit_cubic_to_points(
 #endif
 	        u);
 
-	cubic = cubic_alloc(dims);
-
-	double error_sq_max;
+	double error_max_sq;
 	uint split_index;
 
 	/* Parameterize points, and attempt to fit curve */
 	cubic_from_points(
-	        points_offset, points_offset_len, u, tan_l, tan_r, dims, cubic);
+	        points_offset, points_offset_len, u, tan_l, tan_r, dims, r_cubic);
 
 	/* Find max deviation of points to fitted curve */
-	cubic_calc_error(
-	        cubic, points_offset, points_offset_len, u, dims,
-	        &error_sq_max, &split_index);
+	error_max_sq = cubic_calc_error(
+	        r_cubic, points_offset, points_offset_len, u, dims,
+	        &split_index);
 
-	if (error_sq_max < error_sq) {
+	*r_error_max_sq = error_max_sq;
+	*r_split_index  = split_index;
+
+	if (error_max_sq < error_threshold_sq) {
 		free(u);
-		cubic_list_prepend(clist, cubic);
-		return;
+		return true;
 	}
 	else {
+		Cubic *cubic_test = alloca(cubic_alloc_size(dims));
+		*cubic_test = *r_cubic;
+
 		/* If error not too large, try some reparameterization and iteration */
 		double *u_prime = malloc(sizeof(double) * points_offset_len);
 		for (uint iter = 0; iter < iteration_max; iter++) {
 			if (!cubic_reparameterize(
-			        cubic, points_offset, points_offset_len, u, dims, u_prime))
+			        cubic_test, points_offset, points_offset_len, u, dims, u_prime))
 			{
 				break;
 			}
 
 			cubic_from_points(
 			        points_offset, points_offset_len, u_prime,
-			        tan_l, tan_r, dims, cubic);
-			cubic_calc_error(
-			        cubic, points_offset, points_offset_len, u_prime, dims,
-			        &error_sq_max, &split_index);
+			        tan_l, tan_r, dims, cubic_test);
+			error_max_sq = cubic_calc_error(
+			        cubic_test, points_offset, points_offset_len, u_prime, dims,
+			        &split_index);
 
-			if (error_sq_max < error_sq) {
+			if (error_max_sq < error_threshold_sq) {
 				free(u_prime);
 				free(u);
-				cubic_list_prepend(clist, cubic);
-				return;
+
+				*r_cubic = *cubic_test;
+				*r_error_max_sq = error_max_sq;
+				*r_split_index  = split_index;
+				return true;
+			}
+			else if (error_max_sq < *r_error_max_sq) {
+				*r_cubic = *cubic_test;
+				*r_error_max_sq = error_max_sq;
+				*r_split_index = split_index;
 			}
 
 			SWAP(double *, u, u_prime);
 		}
 		free(u_prime);
+		free(u);
+
+		return false;
 	}
+}
+
+static void fit_cubic_to_points_recursive(
+        const double *points_offset,
+        const uint    points_offset_len,
+#ifdef USE_LENGTH_CACHE
+        const double *points_length_cache,
+#endif
+        const double  tan_l[],
+        const double  tan_r[],
+        const double  error_threshold_sq,
+        const uint    dims,
+        /* fill in the list */
+        CubicList *clist)
+{
+	Cubic *cubic = cubic_alloc(dims);
+	uint split_index;
+	double error_max_sq;
 
-	free(u);
+	if (fit_cubic_to_points(
+	        points_offset, points_offset_len,
+#ifdef USE_LENGTH_CACHE
+	        points_length_cache,
+#endif
+	        tan_l, tan_r, error_threshold_sq, dims,
+	        cubic, &error_max_sq, &split_index))
+	{
+		cubic_list_prepend(clist, cubic);
+		return;
+	}
 	cubic_free(cubic);
 
 
@@ -831,18 +871,18 @@ static void fit_cubic_to_points(
 		normalize_vn(tan_center, dims);
 	}
 
-	fit_cubic_to_points(
+	fit_cubic_to_points_recursive(
 	        points_offset, split_index + 1,
 #ifdef USE_LENGTH_CACHE
 	        points_length_cache,
 #endif
-	        tan_l, tan_center, error_threshold, dims, clist);
-	fit_cubic_to_points(
+	        tan_l, tan_center, error_threshold_sq, dims, clist);
+	fit_cubic_to_points_recursive(
 	        &points_offset[split_index * dims], points_offset_len - split_index,
 #ifdef USE_LENGTH_CACHE
 	        points_length_cache + split_index,
 #endif
-	        tan_center, tan_r, error_threshold, dims, clist);
+	        tan_center, tan_r, error_threshold_sq, dims, clist);
 
 }
 
@@ -904,6 +944,8 @@ int curve_fit_cubic_to_points_db(
 		corner_index_array[corner_index++] = corners[0];
 	}
 
+	const double error_threshold_sq = sq(error_threshold);
+
 	for (uint i = 1; i < corners_len; i++) {
 		const uint points_offset_len = corners[i] - corners[i - 1] + 1;
 		const uint first_point = corners[i - 1];
@@ -933,12 +975,12 @@ int curve_fit_cubic_to_points_db(
 			        points_length_cache);
 #endif
 
-			fit_cubic_to_points(
+			fit_cubic_to_points_recursive(
 			        &points[first_point * dims], points_offset_len,
 #ifdef USE_LENGTH_CACHE
 			        points_length_cache,
 #endif
-			        tan_l, tan_r, error_threshold, dims, &clist);
+			        tan_l, tan_r, error_threshold_sq, dims, &clist);
 		}
 		else if (points_len == 1) {
 			assert(points_offset_len == 1);
@@ -1015,9 +1057,7 @@ int curve_fit_cubic_to_points_fl(
 	const uint points_flat_len = points_len * dims;
 	double *points_db = malloc(sizeof(double) * points_flat_len);
 
-	for (uint i = 0; i < points_flat_len; i++) {
-		points_db[i] = (double)points[i];
-	}
+	copy_vndb_vnfl(points_db, points, points_flat_len);
 
 	double *cubic_array_db = NULL;
 	float  *cubic_array_fl = NULL;
@@ -1045,4 +1085,100 @@ int curve_fit_cubic_to_points_fl(
 	return result;
 }
 
+/**
+ * Fit a single cubic to points.
+ */
+int curve_fit_cubic_to_points_single_db(
+        const double *points,
+        const uint    points_len,
+        const uint    dims,
+        const double  error_threshold,
+        const double tan_l[],
+        const double tan_r[],
+
+        double  r_handle_l[],
+        double  r_handle_r[],
+        double  *r_error_max_sq)
+{
+	Cubic *cubic = alloca(cubic_alloc_size(dims));
+
+	uint split_index;
+
+	/* in this instance theres no advantage in using length cache,
+	 * since we're not recursively calculating values. */
+#ifdef USE_LENGTH_CACHE
+	double *points_length_cache = malloc(sizeof(double) * points_len);
+	points_calc_coord_length_cache(
+	        points, points_len, dims,
+	        points_length_cache);
+#endif
+
+	fit_cubic_to_points(
+	        points, points_len,
+#ifdef USE_LENGTH_CACHE
+	        points_length_cache,
+#endif
+	        tan_l, tan_r, error_threshold, dims,
+
+	        cubic, r_error_max_sq, &split_index);
+
+#ifdef USE_LENGTH_CACHE
+	free(points_length_cache);
+#endif
+
+	copy_vnvn(r_handle_l, CUBIC_PT(cubic, 1, dims), dims);
+	copy_vnvn(r_handle_r, CUBIC_PT(cubic, 2, dims), dims);
+
+	return 0;
+}
+
+int curve_fit_cubic_to_points_single_fl(
+        const float  *points,
+        const uint    points_len,
+        const uint    dims,
+        const float   error_threshold,
+        const float   tan_l[],
+        const float   tan_r[],
+
+        float   r_handle_l[],
+        float   r_handle_r[],
+        float  *r_error_sq)
+{
+	const uint points_flat_len = points_len * dims;
+	double *points_db = malloc(sizeof(double) * points_flat_len);
+
+	copy_vndb_vnfl(points_db, points, points_flat_len);
+
+#ifdef USE_VLA
+	double tan_l_db[dims];
+	double tan_r_db[dims];
+	double r_handle_l_db[dims];
+	double r_handle_r_db[dims];
+#else
+	double *tan_l_db = alloca(sizeof(double) * dims);
+	double *tan_r_db = alloca(sizeof(double) * dims);
+	double *r_handle_l_db = alloca(sizeof(double) * dims);
+	double *r_handle_r_db = alloca(sizeof(double) * dims);
+#endif
+	double r_error_sq_db;
+
+	copy_vndb_vnfl(tan_l_db, tan_l, dims);
+	copy_vndb_vnfl(tan_r_db, tan_r, dims);
+
+	int result = curve_fit_cubic_to_points_single_db(
+	        points_db, points_len, dims,
+	        (double)error_threshold,
+	        tan_l_db, tan_r_db,
+	        r_handle_l_db, r_handle_r_db,
+	        &r_error_sq_db);
+
+	free(points_db);
+
+	copy_vnfl_vndb(r_handle_l, r_handle_l_db, dims);
+	copy_vnfl_vndb(r_handle_r, r_handle_r_db, dims);
+	*r_error_sq = (float)r_error_sq_db;
+
+	return result;
+}
+
 /** \} */
diff --git a/extern/curve_fit_nd/intern/curve_fit_inline.h b/extern/curve_fit_nd/intern/curve_fit_inline.h
index 1b47cbd5eb5..085148cc119 100644
--- a/extern/curve_fit_nd/intern/curve_fit_inline.h
+++ b/extern/curve_fit_nd/intern/curve_fit_inline.h
@@ -80,6 +80,22 @@ MINLINE void copy_vnvn(
 	}
 }
 
+MINLINE void copy_vnfl_vndb(
+        float v0[], const double v1[], const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v0[j] = (float)v1[j];
+	}
+}
+
+MINLINE void copy_vndb_vnfl(
+        double v0[], const float v1[], const uint dims)
+{
+	for (uint j = 0; j < dims; j++) {
+		v0[j] = (double)v1[j];
+	}
+}
+
 MINLINE double dot_vnvn(
         const double v0[], const double v1[], const uint dims)
 {