%% options copyright owner = Dirk Krause copyright year = 2018-xxxx SPDX-License-Identifier: BSD-3-Clause %% header /** @file dk4xsp.h X-spline segments. @section secxspoverview Overview X-splines were introduced by C. Blanc and C. Schlick on the SIGGRAPH'95, they are described in detail in the paper: "X-Splines: A Spline Model Designed for the End-User". Unfortunately the X-spline implementation in XFig contained a bug for many years, for interpolated splines a wrong coefficient calculation (q=-s instead of q=-0.5s) was used. Probably the resulting unexpected strong curvature and sometimes unwanted oscillations kept people from using interpolated X-splines in XFig. Fixing the bug and introducing the correct calculation would break appearance of existing *.fig files containing interpolated X-splines. So the decision was made to keep the calculation as is. For new interpolated X-splines a factor s=-0.5 is used instead of -1, so the q value does not exceed 0.5. This module contains functions for X-spline calculations, both in correct mode and in XFig compatibility mode. An X-spline is a continuous curve represented by a set of discrete control points. Each control point consists of coordinates (x, y, optionally z) and a shape factor s in range -1 to 1 (positive for approximation points, 0 for corner points, negative for interpolation points). An X-spline segment is the curve range corresponding to two consecutive control points. It is controlled by up to 4 control points: - Control point corresponding to start of segment (index 1), - Control point corresponding to end of segment (index 2), - Neighbour control point "left" from segment start control point (index 0), - Neighbour control point "right" from segment end control point (index 3). For open X-splines there is no left neighbour for the first segment and no right neighbour for the final segment. For closed X-splines the final point is the left neighbour of the first segment and the first point is the right neighbour of the final segment. The t parameter is relative to the segment, it is in the range 0.0<=t<=1.0. @section secxsphighlevel High level API A high level API is available for 2D X-splines using the dk4_xsp_2d_t data type. Before doing any calculation, use dk4xsp2d_reset() to reset the entire structure. Optionally use dk4xsp2d_set_xfig() to set up XFig compatibility mode. For each segment use dk4xsp2d_reset_points() to reset the data in the dk4_xsp_2d_t structure. Now use dk4xsp2d_set_point() with index 0, 1, 2, 3 to set coordinates and shape factors for left neighbour control point of segment, start of segment, end of segment and right neighbour. For the first segment of open X-splines skip index 0 as there is no "left" neighbour. For the final segment of open X-splines skip index 3 as there is no "right" neighbour. To draw an X-spline using polyline/polygon approximation you need to calculate point coordinates, use the dk4xsp2d_calculate_value() function. The t paramater is in the range 0<=t<=1. A pointer to an array of at least 2 elements is required as rp argument, x and y value are stored here. I suggest to use at least 32 subsegments for each X-spline segment. To draw an X-spline using a sequence of Bezier curves you need to calculate point coordinates (x, y) and the first derivatives (dx/dt, dy/dt), use the dk4xsp2d_calculate_value_derivative function. The function stores x, dx/dt, y, dy/dt in the array rp points to. A pointer to an array of at least 4 elements is required as rp argument. I suggest to use at least 4 subsegments for each X-spline segment, 8 subsegments are recommended. @section secxsplowlevel Low level API The low level API calculates values for just one coordinate. Before doing any calculation, use dk4xsp_reset() to reset the entire structure, optionally use dk4xsp_set_xfig() to set up XFig compatibility mode. For each segment use dk4xsp_reset_data() to reset all data components, use dk4xsp_set_point() multiple times with i=(0),1,2,(3) to set shape factors for the control points. Omitted calls with i=0 or i=3 indicate that the neighbour control points are not present. Each point within a segment is represented by a t value in range 0 to 1. For each point use dk4xsp_prepare() first to set up internal coefficients (blending functions values and optionally blending functions derivative values) corresponding to the t value. Next use dk4xsp_calculate() to calculate the coordinate value and optionally the first derivative. After using dk4xsp_prepare() once to set up the segment you can use dk4xsp_calculate() multiple times for different coordinates. The dk4xsp_prepare() function calculates the blending function values and optionally the derivatives value. The dk4xsp_calculate() function applies the blending function values and optionally derivatives to a set of x, y or z coordinates of control points. The same d value must be used in both calls, you can not calculate a derivative value if the structure was not prepared to do so. */ #ifndef DK4ERROR_H_INCLUDED #if DK4_BUILDING_DKTOOLS4 #include "dk4error.h" #else #include #endif #endif /** X-spline coefficients without coordinates. The dk4xsp_set_point() function sets h, s, p, q, dudt. The dk4xsp_prepare() function sets u, f, dfdt. */ typedef struct { double s[4]; /**< Shape factors. */ double p[4]; /**< Coefficients p for blending functions. */ double q[4]; /**< Coefficients q for blending functions. */ double u[4]; /**< Parameter u for blending functions. */ double dudt[4]; /**< Derivative du/dt. */ double f[4]; /**< Blending function results. */ double dfdt[4]; /**< Derivatives df/dt of the blending functions. */ uint8_t h[4]; /**< Availability of coordinates and shape factors. */ uint8_t xfig; /**< Flag: XFig compatibility. */ } dk4_xsp_t; /** X-spline segment in two dimensions. */ typedef struct { double x[4]; /**< X coordinates. */ double y[4]; /**< Y coordinates. */ dk4_xsp_t xsp; /**< Shape factors and coefficients. */ } dk4_xsp_2d_t; #ifdef __cplusplus extern "C" { #endif /* LOW LEVEL API ------------- */ /** Reset X-spline structure including the Xfig compatibility flag. @param xsp Spline structure to reset. */ void dk4xsp_reset(dk4_xsp_t *xsp); /** Reset data components in X-spline structure, leave Xfig compatibility flag unchanged. @param xsp Spline structure to reset. */ void dk4xsp_reset_data(dk4_xsp_t *xsp); /** Set or reset xfig compatibility flag. @param xsp Spline structure to modify. @param val New Xfig compatibility flag value. */ void dk4xsp_set_xfig(dk4_xsp_t *xsp, uint8_t val); /** Set shape factor for one control point, calculate values derived from shape factor. Used 2 to 4 times to set up a segment. @param xsp Spline structure to modify. @param s Shape factor @param i Index of control point: 0=left neighbour, 1=segment start, 2=segment end, 3=right neighbour. */ void dk4xsp_set_point( dk4_xsp_t *xsp, double s, size_t i ); /** Prepare calculations for one t value. @param xsp Spline structure to prepare. @param t Parameter t in range 0 to 1. @param d Prepare to calculate derivative. @param erp Error report, may be NULL. @return 1 on success, 0 on error. Error codes: - DK4_E_INVALID_ARGUMENTS
if invalid arguments were passed to the function. */ int dk4xsp_prepare( dk4_xsp_t *xsp, double t, int d, dk4_er_t *erp ); /** Calculations for one t value. For d=0 the function just calculates the position, rp is a pointer to one result variable. For other d values the function also calculates the first derivative, rp is a pointer to an array of two elements: position and derivative. @param rp Result variable or array address. @param xsp Spline structure to use. @param x Coordinates values: 4 elements for values at left neighbour, left border, right border and right neighbour. @param d Flag: Calculate derivative too. @param erp Error report for diagnostics. @return 1 on success, 0 on error. Error codes: - DK4_E_INVALID_ARGUMENTS
if invalid arguments were passed to the function, - DK4_E_MATH_OVERFLOW
if a mathematical overflow occured in the calculation. */ int dk4xsp_calculate( double *rp, dk4_xsp_t const *xsp, double const *x, int d, dk4_er_t *erp ); /* HIGH LEVEL API FOR 2D X-SPLINES ------------------------------- */ /** Reset spline structure completely. @param sp Spline structure to reset. */ void dk4xsp2d_reset( dk4_xsp_2d_t *sp ); /** Set or reset XFig compatibility flag. @param sp Spline structure to set up. @param val New flag value. */ void dk4xsp2d_set_xfig( dk4_xsp_2d_t *sp, uint8_t val ); /** Reset spline structure to "all points unset". @param sp Spline structure to reset. */ void dk4xsp2d_reset_points( dk4_xsp_2d_t *sp ); /** Set one point in spline structure. @param sp Spline structure to set up. @param x X coordinate. @param y Y coordinate. @param s Shape factor. @param i Index (0=A, 1=B, 2=C, 3=D). */ void dk4xsp2d_set_point( dk4_xsp_2d_t *sp, double x, double y, double s, size_t i ); /** Calculate just the value at a given point. @param rp Address of results array, 2 elements: rp[0]=x, rp[1]=y. @param sp Spline structure to use. @param t Value t in range 0 to 1. @param erp Error report, may be NULL. @return 1 on success, 0 on error. Error codes: - DK4_E_INVALID_ARGUMENTS
if invalid arguments were passed to the function, - DK4_E_MATH_OVERFLOW
if a numeric overflow occured in calculation. */ int dk4xsp2d_calculate_value( double *rp, dk4_xsp_2d_t *sp, double t, dk4_er_t *erp ); /** Calculate values and first derivative. @param rp Address of results array, 4 elements: rp[0]=x, rp[1]=dx/dt, rp[2]=y, rp[3]=dy/dt. @param sp Spline structure to use. @param t Value t in range 0 to 1. @param erp Error report, may be NULL. @return 1 on success, 0 on error. Error codes: - DK4_E_INVALID_ARGUMENTS
if invalid arguments were passed to the function, - DK4_E_MATH_OVERFLOW
if a numeric overflow occured in calculation. */ int dk4xsp2d_calculate_value_derivative( double *rp, dk4_xsp_2d_t *sp, double t, dk4_er_t *erp ); /** Check whether the segment is a line (Shape factor s is 0 for points 1 and 2). This check can be applied after setting the control points for the segment. @param sp Spline segment to check. @return 1 for lines, 0 for curved spline segment. */ int dk4xsp2d_is_line(dk4_xsp_2d_t const *sp); /** Calculate X-spline segment length. @param rp Address of result variable. @param sp Spline segment to calculate length for. @param eps Tolerance to stop iteration. @param erp Error report, may be NULL. @return 1 on success, 0 on error. Error codes: - DK4_E_INVALID_ARGUMENTS
if invalid arguments were passed to the function, - DK4_E_MATH_OVERFLOW
if a numeric overflow occured in calculation, - DK4_E_NOT_FOUND
if the iteration did not succeed. */ int dk4xsp2d_calculate_length( double *rp, dk4_xsp_2d_t *sp, double eps, dk4_er_t *erp ); /** Calculate partial X-spline segment length from 0 to t_end. @param rp Address of result variable. @param sp Spline segment to calculate length for. @param tend End value for parameter t. @param eps Tolerance to stop iteration. @param erp Error report, may be NULL. @return 1 on success, 0 on error. Error codes: - DK4_E_INVALID_ARGUMENTS
if invalid arguments were passed to the function, - DK4_E_MATH_OVERFLOW
if a numeric overflow occured in calculation, - DK4_E_NOT_FOUND
if the iteration did not succeed. */ int dk4xsp2d_calculate_partial_length( double *rp, dk4_xsp_2d_t *sp, double tend, double eps, dk4_er_t *erp ); #ifdef __cplusplus } #endif /* vim: set ai sw=4 ts=4 : */ %% module #include "dk4xsp.h" #ifndef DK4MEM_H_INCLUDED #include "dk4mem.h" #endif #ifndef DK4MATH_H_INCLUDED #include "dk4math.h" #endif #if DK4_HAVE_ASSERT_H #ifndef ASSERT_H_INCLUDED #include #define ASSERT_H_INCLUDED 1 #endif #endif $!trace-include /** Approximation function. */ #define FCT_F(u,p) \ u*u*u*(u*(u*(6.0-p)+2.0*p-15.0)+10.0-p) /** Interpolation function for 0<=u<=1. */ #define FCT_G(u,p,q) \ u*(u*(u*(u*(u*(6.0-5.0*q-p)+2.0*p+14.0*q-15.0)+10.0-12.0*q-p)+2.0*q)+q) /** Interpolation function for -1<=u<0. */ #define FCT_H(u,q) \ u*(u*(u*u*(-2.0*q-q*u)+2.0*q)+q) /** First derivative of approximation function. */ #define FCT_DFDU(u,p) \ u*u*(u*(u*(30.0-5.0*p)+8.0*p-60.0)+30.0-3.0*p) /** First derivative of interpolation function for 0<=u<=1. */ #define FCT_DGDU(u,p,q) \ u*(u*(u*(u*(30.0-25.0*q-5.0*p)+8.0*p+56.0*q-60.0)+30.0-36.0*q-3.0*p)+4.0*q)+q /** First derivative of interpolation function for -1<=u<0. */ #define FCT_DHDU(u,q) \ u*(4.0*q-u*u*(8.0*q+5.0*q*u))+q #if TRACE_DEBUG /** Show one array contents. @param ptr Array start address. */ static void dk4xsp_show_array4(double const *ptr) { size_t i; for (i = 0; i < 4; i++) { $? ". [%u] = %lg", (unsigned)i, ptr[i] } } #endif /** Calculate approximation blending function value. @param u Parameter u. @param p Constant value p derived from shape factor. @return Calculation result. */ static double fct_f(double u, double p) { #if TRACE_DEBUG double back = 0.0; back = FCT_F(u,p); $? "= fct_f(u=%lg,p=%lg) = %lg", u, p, back return back; #else return ( FCT_F(u,p) ); #endif } /** Calculate interpolation blending function value for positive u arguments. @param u Parameter u. @param p Constant value p derived from shape factor. @param q Constant value q derived from shape factor. @return Calculation result. */ static double fct_g(double u, double p, double q) { #if TRACE_DEBUG double back = 0.0; back = FCT_G(u,p,q); $? "= fct_g(u=%lg,p=%lg,q=%lg) = %lg", u, p, q, back return back; #else return ( FCT_G(u,p,q) ); #endif } /** Calculate interpolation blending function value for negative u arguments. @param u Parameter u. @param q Constant value q derived from shape factor. @return Calculation result. */ static double fct_h(double u, double q) { #if TRACE_DEBUG double back = 0.0; back = FCT_H(u,q); $? "= fct_h(u=%lg,q=%lg) = %lg", u, q, back return back; #else return ( FCT_H(u,q) ); #endif } /** Calculate approximation blending function derivative value. @param u Parameter u. @param p Constant value p derived from shape factor. @return Calculation result. */ static double fct_dfdu(double u, double p) { #if TRACE_DEBUG double back; back = FCT_DFDU(u,p); $? "= fct_dfdu(u=%lg,p=%lg) = %lg", u, p, back return back; #else return ( FCT_DFDU(u,p) ); #endif } /** Calculate interpolation blending function derivative value for positive u arguments. @param u Parameter u. @param p Constant value p derived from shape factor. @param q Constant value q derived from shape factor. @return Calculation result. */ static double fct_dgdu(double u, double p, double q) { #if TRACE_DEBUG double back; back = FCT_DGDU(u,p,q); $? "= fct_dgdu(u=%lg,p=%lg,q=%lg) = %lg", u, p, q, back return back; #else return ( FCT_DGDU(u,p,q) ); #endif } /** Calculate interpolation blending function derivative value for negative u arguments. @param u Parameter u. @param q Constant value q derived from shape factor. @return Calculation result. */ static double fct_dhdu(double u, double q) { #if TRACE_DEBUG double back; back = FCT_DHDU(u,q); $? "= fct_dhdu(u=%lg,q=%lg) = %lg", u, q, back return back; #else return ( FCT_DHDU(u,q) ); #endif } /** Prepare a coordinates calculation, calculate blending functions values and optionally blending functions derivatives values. @param xsp Spline structure to use. @param t Parameter t in range 0 to 1. @param d Flag: Derivatives required. */ static void dk4xsp_i_prepare( dk4_xsp_t *xsp, double t, int d ) { size_t i; $? "+ dk4xsp_i_prepare t=%lg", t #if DK4_USE_ASSERT assert(NULL != xsp); #endif for (i = 0; i < 4; i++) { xsp->f[i] = xsp->u[i] = 0.0; } if (0.0 > xsp->s[1]) { $? ". point 1 s < 0" xsp->u[0] = 0.0 - t; xsp->u[2] = t; if ((uint8_t)0U != xsp->h[0]) { xsp->f[0] = fct_h(xsp->u[0], xsp->q[0]); } xsp->f[2] = fct_g(xsp->u[2], xsp->p[2], xsp->q[2]); } else { $? ". point 1 s >= 0" xsp->u[0] = (xsp->s[1] - t) / (1.0 + xsp->s[1]); xsp->u[2] = (xsp->s[1] + t) / (1.0 + xsp->s[1]); if (t < xsp->s[1]) { $? ". t allows f[0]" if ((uint8_t)0U != xsp->h[0]) { $? ". have 0" xsp->f[0] = fct_f(xsp->u[0], xsp->p[0]); } #if TRACE_DEBUG else { $? ". do not have 0" } #endif } #if TRACE_DEBUG else { $? ". t does not allow f[0]" } #endif xsp->f[2] = fct_f(xsp->u[2], xsp->p[2]); } if (0.0 > xsp->s[2]) { $? ". point 2 s < 0" xsp->u[1] = 1.0 - t; xsp->u[3] = t - 1.0; xsp->f[1] = fct_g(xsp->u[1], xsp->p[1], xsp->q[1]); if ((uint8_t)0U != xsp->h[3]) { xsp->f[3] = fct_h(xsp->u[3], xsp->q[3]); } } else { $? ". point 2 s >= 0" xsp->u[1] = (1.0 + xsp->s[2] - t) / (1.0 + xsp->s[2]); xsp->u[3] = (t + xsp->s[2] - 1.0) / (1.0 + xsp->s[2]); xsp->f[1] = fct_f(xsp->u[1], xsp->p[1]); if (t > (1.0 - xsp->s[2])) { $? ". t allows f[3]" if ((uint8_t)0U != xsp->h[3]) { $? ". have f[3]" xsp->f[3] = fct_f(xsp->u[3], xsp->p[3]); } else { $? ". no f[3]" } } else { $? ". t does not allow f[3]" } } if (0 == d) { goto finished; } for (i = 0; i < 4; i++) { xsp->dfdt[i] = 0.0; } if (0.0 > xsp->s[1]) { if ((uint8_t)0U != xsp->h[0]) { xsp->dfdt[0] = fct_dhdu(xsp->u[0], xsp->q[0]); xsp->dfdt[0] *= xsp->dudt[0]; } xsp->dfdt[2] = fct_dgdu(xsp->u[2], xsp->p[2], xsp->q[2]); xsp->dfdt[2] *= xsp->dudt[2]; } else { if (t < xsp->s[1]) { $? ". t allows f[0]" if ((uint8_t)0U != xsp->h[0]) { $? ". have f[0]" xsp->dfdt[0] = fct_dfdu(xsp->u[0], xsp->p[0]); xsp->dfdt[0] *= xsp->dudt[0]; } #if TRACE_DEBUG else { $? ". no f[0]" } #endif } #if TRACE_DEBUG else { $? ". t does not allow f[0]" } #endif xsp->dfdt[2] = fct_dfdu(xsp->u[2], xsp->p[2]); xsp->dfdt[2] *= xsp->dudt[2]; } if (0.0 > xsp->s[2]) { xsp->dfdt[1] = fct_dgdu(xsp->u[1], xsp->p[1], xsp->q[1]); xsp->dfdt[1] *= xsp->dudt[1]; if ((uint8_t)0U != xsp->h[3]) { xsp->dfdt[3] = fct_dhdu(xsp->u[3], xsp->q[3]); xsp->dfdt[3] *= xsp->dudt[3]; } } else { xsp->dfdt[1] = fct_dfdu(xsp->u[1], xsp->p[1]); xsp->dfdt[1] *= xsp->dudt[1]; if (t > (1.0 - xsp->s[2])) { $? ". t allows f[3]" if ((uint8_t)0U != xsp->h[3]) { $? ". have f[3]" xsp->dfdt[3] = fct_dfdu(xsp->u[3], xsp->p[3]); xsp->dfdt[3] *= xsp->dudt[3]; } #if TRACE_DEBUG else { $? ". no f[3]" } #endif } #if TRACE_DEBUG else { $? ". t does not allow f[3]" } #endif } finished: $? ". s" $!trace-code dk4xsp_show_array4(&(xsp->s[0])); $? ". p" $!trace-code dk4xsp_show_array4(&(xsp->p[0])); $? ". q" $!trace-code dk4xsp_show_array4(&(xsp->q[0])); $? ". u" $!trace-code dk4xsp_show_array4(&(xsp->u[0])); $? ". dudt" $!trace-code dk4xsp_show_array4(&(xsp->dudt[0])); $? ". f" $!trace-code dk4xsp_show_array4(&(xsp->f[0])); $? ". dfdt" $!trace-code dk4xsp_show_array4(&(xsp->dfdt[0])); $? "- dk4xsp_i_prepare" return; } /** Calculate coordinates for one dimension. @param rp Address of result variable or result array. When calculating the derivative, rp must be a 2 elements array. @param xsp Spline structure to use. @param x Array of 4 coordinates. @param d Flag: Calculate derivative. @param erp Error report, may be NULL. @return 1 on success, 0 on error. */ static int dk4xsp_i_calculate( double *rp, dk4_xsp_t const *xsp, double const *x, int d, dk4_er_t *erp ) { double z; double n; double dzdt; double dndt; int back = 1; $? "+ dk4xsp_i_calculate" #if DK4_USE_ASSERT assert(NULL != rp); assert(NULL != xsp); #endif /* Calculate coordinates */ z = x[1] * xsp->f[1] + x[2] * xsp->f[2]; n = xsp->f[1] + xsp->f[2]; if (DK4_UINT8_0 != xsp->h[0]) { z += x[0] * xsp->f[0]; n += xsp->f[0]; } if (DK4_UINT8_0 != xsp->h[3]) { z += x[3] * xsp->f[3]; n += xsp->f[3]; } *rp = z / n; if (!(dk4ma_is_finite(*rp))) { $? "! math problem" back = 0; dk4error_set_simple_error_code(erp, DK4_E_MATH_OVERFLOW); } /* Abort on errors or if no derivative needed */ if ((0 == d) || (0 == back)) { goto finished; } /* Calculate derivative */ dzdt = x[1] * xsp->dfdt[1] + x[2] * xsp->dfdt[2]; dndt = xsp->dfdt[1] + xsp->dfdt[2]; if (DK4_UINT8_0 != xsp->h[0]) { dzdt += x[0] * xsp->dfdt[0]; dndt += xsp->dfdt[0]; } if (DK4_UINT8_0 != xsp->h[3]) { dzdt += x[3] * xsp->dfdt[3]; dndt += xsp->dfdt[3]; } rp[1] = (n * dzdt - z * dndt) / (n * n); if (!(dk4ma_is_finite(rp[1]))) { $? "! math problem" back = 0; dk4error_set_simple_error_code(erp, DK4_E_MATH_OVERFLOW); } finished: $? ". rp x = %lg", rp[0] $!trace-code if (0 != d) { $? ". rp dx/dt = %lg", rp[1] $!trace-code } $? "- dk4xsp_i_calculate %d", back return back; } void dk4xsp_reset_data(dk4_xsp_t *xsp) { size_t i; /* Traverse arrays */ $? "+ dk4xsp_reset_data" #if DK4_USE_ASSERT assert(NULL != xsp); #endif if (NULL != xsp) { for (i = 0; i < 4; i++) { xsp->s[i] = xsp->q[i] = 0.0; xsp->p[i] = 2.0; xsp->h[i] = ((uint8_t)0U); } xsp->dudt[0] = xsp->dudt[1] = -1.0; xsp->dudt[2] = xsp->dudt[3] = 1.0; } $? ". f" $!trace-code dk4xsp_show_array4(&(xsp->f[0])); $? "- dk4xsp_reset_data" } void dk4xsp_reset(dk4_xsp_t *xsp) { $? "+ dk4xsp_reset" #if DK4_USE_ASSERT assert(NULL != xsp); #endif if (NULL != xsp) { DK4_MEMRES(xsp,sizeof(dk4_xsp_t)); dk4xsp_reset_data(xsp); xsp->xfig = ((uint8_t)0U); } $? "- dk4xsp_reset" } void dk4xsp_set_xfig(dk4_xsp_t *xsp, uint8_t val) { $? "+ dk4xsp_set_xfig %u", (unsigned)val #if DK4_USE_ASSERT assert(NULL != xsp); #endif if (NULL != xsp) { xsp->xfig = val; } $? "- dk4xsp_set_xfig" } void dk4xsp_set_point( dk4_xsp_t *xsp, double s, size_t i ) { $? "+ dk4xsp_set_point i=%u s=%lg", (unsigned)i, s #if DK4_USE_ASSERT assert(NULL != xsp); #endif if ((NULL != xsp) && (4 > i)) { xsp->s[i] = s; xsp->h[i] = ((uint8_t)1U); switch ((int)i) { case 1: { /* Start of segment */ if (0.0 > s) { /* Interpolation */ if (((uint8_t)0U) != xsp->xfig) { xsp->q[0] = xsp->q[2] = 0.0 - s; } else { xsp->q[0] = xsp->q[2] = -0.5 * s; } } else { /* Approximation */ xsp->p[0] = xsp->p[2] = 2.0 * (1.0 + s) * (1.0 + s); xsp->dudt[2] = 1.0 / (1.0 + s); xsp->dudt[0] = 0.0 - xsp->dudt[2]; } } break; case 2: { /* End of segment */ if (0.0 > s) { /* Interpolation */ if (((uint8_t)0U) != xsp->xfig) { xsp->q[1] = xsp->q[3] = 0.0 - s; } else { xsp->q[1] = xsp->q[3] = -0.5 * s; } } else { /* Approximation */ xsp->p[1] = xsp->p[3] = 2.0 * (1.0 + s) * (1.0 + s); xsp->dudt[3] = 1.0 / (1.0 + s); xsp->dudt[1] = 0.0 - xsp->dudt[3]; } } break; } } $? "- dk4xsp_set_point" } int dk4xsp_prepare( dk4_xsp_t *xsp, double t, int d, dk4_er_t *erp ) { int back = 0; $? "+ dk4xsp_prepare" #if DK4_USE_ASSERT assert(NULL != xsp); assert(0.0 <= t); assert(1.0 >= t); #endif if ( (NULL != xsp) && (0.0 <= t) && (1.0 >= t)) { if (((uint8_t)0U != xsp->h[1]) && ((uint8_t)0U != xsp->h[2])) { back = 1; dk4xsp_i_prepare(xsp, t, d); } else { dk4error_set_simple_error_code(erp, DK4_E_INVALID_ARGUMENTS); } } else { dk4error_set_simple_error_code(erp, DK4_E_INVALID_ARGUMENTS); } $? "- dk4xsp_prepare %d", back return back; } int dk4xsp_calculate( double *rp, dk4_xsp_t const *xsp, double const *x, int d, dk4_er_t *erp ) { int back = 0; $? "+ dk4xsp_calculate" #if DK4_USE_ASSERT assert(NULL != xsp); assert(NULL != rp); #endif if ((NULL != rp) && (NULL != xsp) && (NULL != x)) { if (((uint8_t)0U != xsp->h[1]) && ((uint8_t)0U != xsp->h[2])) { back = dk4xsp_i_calculate(rp, xsp, x, d, erp); } else { dk4error_set_simple_error_code(erp, DK4_E_INVALID_ARGUMENTS); } } else { dk4error_set_simple_error_code(erp, DK4_E_INVALID_ARGUMENTS); } $? "- dk4xsp_calculate %d", back return back; } void dk4xsp2d_reset( dk4_xsp_2d_t *sp ) { size_t i; $? "+ dk4xsp2d_reset" #if DK4_USE_ASSERT assert(NULL != sp); #endif if (NULL != sp) { DK4_MEMRES(sp,sizeof(dk4_xsp_2d_t)); dk4xsp_reset(&(sp->xsp)); for (i = 0; i < 4; i++) { sp->x[i] = sp->y[i] = 0.0; } } $? "- dk4xsp2d_reset" } void dk4xsp2d_set_xfig( dk4_xsp_2d_t *sp, uint8_t val ) { $? "+ dk4xsp2d_set_xfig %u", (unsigned)val #if DK4_USE_ASSERT assert(NULL != sp); #endif if (NULL != sp) { dk4xsp_set_xfig(&(sp->xsp), val); } $? "- dk4xsp2d_set_xfig" } void dk4xsp2d_reset_points( dk4_xsp_2d_t *sp ) { size_t i; $? "+ dk4xsp2d_reset_points" #if DK4_USE_ASSERT assert(NULL != sp); #endif if (NULL != sp) { dk4xsp_reset_data(&(sp->xsp)); for (i = 0; i < 4; i++) { sp->x[i] = sp->y[i] = 0.0; } } $? "- dk4xsp2d_reset_points" } void dk4xsp2d_set_point( dk4_xsp_2d_t *sp, double x, double y, double s, size_t i ) { $? "+ dk4xsp2d_set_point i=%u x=%lg y=%lg s=%lg", (unsigned)i, x, y, s #if DK4_USE_ASSERT assert(NULL != sp); #endif if ((NULL != sp) && (4 > i)) { sp->x[i] = x; sp->y[i] = y; dk4xsp_set_point(&(sp->xsp), s, i); } $? "- dk4xsp2d_set_point" } int dk4xsp2d_calculate_value( double *rp, dk4_xsp_2d_t *sp, double t, dk4_er_t *erp ) { int res; int back = 0; $? "+ dk4xsp2d_calculate_value" #if DK4_USE_ASSERT assert(NULL != sp); assert(NULL != rp); assert(0.0 <= t); assert(1.0 >= t); #endif if ((NULL != rp) && (NULL != sp) && (0.0 <= t) && (1.0 >= t)) { if (((uint8_t)0U != sp->xsp.h[1]) && ((uint8_t)0U != sp->xsp.h[2])) { back = 1; dk4xsp_i_prepare(&(sp->xsp), t, 0); res = dk4xsp_i_calculate(&(rp[0]), &(sp->xsp), &(sp->x[0]), 0, erp); if (0 == res) { $? "! x calculation" back = 0; } res = dk4xsp_i_calculate(&(rp[1]), &(sp->xsp), &(sp->y[0]), 0, erp); if (0 == res) { $? "! y calculation" back = 0; } $? ". x = %lg", rp[0] $? ". y = %lg", rp[1] } else { $? "! args" dk4error_set_simple_error_code(erp, DK4_E_INVALID_ARGUMENTS); } } else { $? "! args" dk4error_set_simple_error_code(erp, DK4_E_INVALID_ARGUMENTS); } $? "- dk4xsp2d_calculate_value" return back; } int dk4xsp2d_calculate_value_derivative( double *rp, dk4_xsp_2d_t *sp, double t, dk4_er_t *erp ) { int res; int back = 0; $? "+ dk4xsp2d_calculate_value_derivative" #if DK4_USE_ASSERT assert(NULL != sp); assert(NULL != rp); assert(0.0 <= t); assert(1.0 >= t); #endif if ((NULL != rp) && (NULL != sp) && (0.0 <= t) && (1.0 >= t)) { if (((uint8_t)0U != sp->xsp.h[1]) && ((uint8_t)0U != sp->xsp.h[2])) { back = 1; dk4xsp_i_prepare(&(sp->xsp), t, 1); res = dk4xsp_i_calculate(&(rp[0]), &(sp->xsp), &(sp->x[0]), 1, erp); if (0 == res) { $? "! x calculation" back = 0; } res = dk4xsp_i_calculate(&(rp[2]), &(sp->xsp), &(sp->y[0]), 1, erp); if (0 == res) { $? "! y calculation" back = 0; } $? ". x = %lg", rp[0] $? ". dx/dt = %lg", rp[1] $? ". y = %lg", rp[2] $? ". dy/dt = %lg", rp[3] } else { $? "! args" dk4error_set_simple_error_code(erp, DK4_E_INVALID_ARGUMENTS); } } else { $? "! args" dk4error_set_simple_error_code(erp, DK4_E_INVALID_ARGUMENTS); } $? "- dk4xsp2d_calculate_value_derivative" return back; } int dk4xsp2d_is_line(dk4_xsp_2d_t const *sp) { int back = 0; $? "+ dk4xsp2d_is_line" #if DK4_USE_ASSERT assert(NULL != sp); #endif if (NULL != sp) { if (1.0e-8 > fabs(sp->xsp.s[1])) { if (1.0e-8 > fabs(sp->xsp.s[2])) { back = 1; } } } #if TRACE_DEBUG else { $? "! args" } #endif $? "- dk4xsp2d_is_line %d", back return back; } int dk4xsp2d_calculate_length( double *rp, dk4_xsp_2d_t *sp, double eps, dk4_er_t *erp ) { double v0[2]; /* Previous point */ double v1[2]; /* Current point */ double oldres; /* Length result from previous pass */ double res; /* Length result from current pass */ double dx; /* X difference */ double dy; /* Y difference */ double t; /* t value at end of current sub segment */ size_t nsegs; /* Number of sub segments in current pass */ size_t i; /* Current sub segment */ unsigned passno; /* Pass number */ int back = 0; /* Function result */ $? "+ dk4xsp2d_calculate_length" #if DK4_USE_ASSERT assert(NULL != sp); assert(NULL != rp); assert(0.0 < eps); #endif if ((NULL != rp) && (NULL != sp) && (0.0 < eps)) { if (0 != dk4xsp2d_is_line(sp)) { $? ". straight line" dx = sp->x[2] - sp->x[1]; dy = sp->y[2] - sp->y[1]; res = sqrt(dx * dx + dy * dy); if (0 != dk4ma_is_finite(res)) { *rp = res; back = 1; } else { $? "! math overflow" dk4error_set_simple_error_code(erp, DK4_E_MATH_OVERFLOW); } } else { $? ". curved segment" nsegs = (size_t)256U; oldres = -1.0; for (passno = 0U; ((0 == back) && (passno < 16U)); passno++) { res = 0.0; if (0 != dk4xsp2d_calculate_value(v0, sp, 0.0, erp)) { for (i = (size_t)0U; i < nsegs; i++) { t = ((double)(i + (size_t)1U)) / ((double)nsegs); if (0 != dk4xsp2d_calculate_value(v1, sp, t, erp)) { dx = v1[0] - v0[0]; dy = v1[1] - v0[1]; res += sqrt(dx * dx + dy * dy); } else { $? "! calculation failed %g", t back = -1; dk4error_set_simple_error_code( erp, DK4_E_MATH_OVERFLOW ); } DK4_MEMCPY(v0,v1,sizeof(v0)); } $? ". pass %u result %g", passno, res if (0 != dk4ma_is_finite(res)) { #if TRACE_DEBUG if (0U != passno) { $? ". pass %u delta = %g",passno,fabs(res - oldres) } #endif if ((0U != passno) && (eps > fabs(res - oldres))) { back = 1; $? ". success pass %u", passno *rp = res; } else { oldres = res; nsegs *= (size_t)2U; } } else { $? "! math overflow" back = -1; dk4error_set_simple_error_code(erp,DK4_E_MATH_OVERFLOW); } } else { $? "! calculation failed" back = -1; dk4error_set_simple_error_code(erp, DK4_E_MATH_OVERFLOW); } } if (0 == back) { $? "! number of passes exceeded" dk4error_set_simple_error_code(erp, DK4_E_NOT_FOUND); } } } else { $? "! args" dk4error_set_simple_error_code(erp, DK4_E_INVALID_ARGUMENTS); } if (-1 == back) { back = 0; } $? "- dk4xsp2d_calculate_length %d", back return back; } int dk4xsp2d_calculate_partial_length( double *rp, dk4_xsp_2d_t *sp, double tend, double eps, dk4_er_t *erp ) { double v0[2]; /* Previous point */ double v1[2]; /* Current point */ double oldres; /* Length result from previous pass */ double res; /* Length result from current pass */ double dx; /* X difference */ double dy; /* Y difference */ double t; /* t value at end of current sub segment */ size_t nsegs; /* Number of sub segments in current pass */ size_t i; /* Current sub segment */ unsigned passno; /* Pass number */ int back = 0; /* Function result */ $? "+ dk4xsp2d_calculate_partial_length" #if DK4_USE_ASSERT assert(NULL != sp); assert(NULL != rp); assert(0.0 < eps); assert(0.0 <= tend); assert(1.0 >= tend); #endif if ( (NULL != rp) && (NULL != sp) && (0.0 < eps) && (0.0 <= tend) && (1.0 >= tend) ) { if (0.0 >= tend) { *rp = 0.0; back = 1; } else { if (1.0 <= tend) { back = dk4xsp2d_calculate_length(rp, sp, eps, erp); } else { if (0 != dk4xsp2d_is_line(sp)) { $? ". straight line" if (0 != dk4xsp2d_calculate_value(v0, sp, tend, erp)) { dx = v0[0] - sp->x[1]; dy = v0[1] - sp->y[1]; res = sqrt(dx * dx + dy * dy); if (0 != dk4ma_is_finite(res)) { *rp = res; back = 1; } else { dk4error_set_simple_error_code( erp, DK4_E_MATH_OVERFLOW ); } } else { back = -1; dk4error_set_simple_error_code( erp, DK4_E_MATH_OVERFLOW ); } } else { $? ". curved segment" nsegs = (size_t)256U; oldres = -1.0; for (passno = 0U;((0 == back) && (passno < 16U));passno++) { res = 0.0; if (0 != dk4xsp2d_calculate_value(v0, sp, 0.0, erp)) { for (i = (size_t)0U; i < nsegs; i++) { t = (tend * ((double)(i + (size_t)1U))) / ((double)nsegs); if ( 0 != dk4xsp2d_calculate_value(v1,sp,t,erp) ) { dx = v1[0] - v0[0]; dy = v1[1] - v0[1]; res += sqrt(dx * dx + dy * dy); } else { $? "! calculation failed %g", t back = -1; dk4error_set_simple_error_code( erp, DK4_E_MATH_OVERFLOW ); } DK4_MEMCPY(v0,v1,sizeof(v0)); } $? ". pass %u result %g", passno, res if (0 != dk4ma_is_finite(res)) { #if TRACE_DEBUG if (0U != passno) { $? ". pass %u delta = %g",passno,fabs(res - oldres) } #endif if ( (0U != passno) && (eps > fabs(res - oldres)) ) { back = 1; $? ". success pass %u", passno *rp = res; } else { oldres = res; nsegs *= (size_t)2U; } } else { $? "! math overflow" back = -1; dk4error_set_simple_error_code( erp, DK4_E_MATH_OVERFLOW ); } } else { $? "! calculation failed" back = -1; dk4error_set_simple_error_code( erp, DK4_E_MATH_OVERFLOW ); } } if (0 == back) { $? "! number of passes exceeded" dk4error_set_simple_error_code(erp, DK4_E_NOT_FOUND); } } } } } else { $? "! args" dk4error_set_simple_error_code(erp, DK4_E_INVALID_ARGUMENTS); } if (-1 == back) { back = 0; } $? "- dk4xsp2d_calculate_partial_length %d", back return back; } /* vim: set ai sw=4 ts=4 : */