Static LED control - CDigiStatic 1.3

// Curvefit.cpp: implementation of the CBezierfit class.
//
//////////////////////////////////////////////////////////////////////

#include "stdafx.h"
#include <MATH.H>
#include "Curvefit.h"

#ifdef _DEBUG
#undef THIS_FILE
static char THIS_FILE[]=__FILE__;
#define new DEBUG_NEW
#endif

static double DistanceError(const double *x, const double *y, const double *Rawdata, int n);
static void Bezier(double u, const double *a, const double *b, 
			double x4, double y4, double &z, double &s);

void CalcBezier(const double *Rawdata, int n, double *Control)
{
	double x[4];
	double y[4];
	double e1,e2, e3;
	int		Retry;
	double x1a,x2a,y1a,y2a;

	x[0] = Rawdata[0];
	y[0] = Rawdata[1];

	x[1] = Rawdata[2];
	y[1] = Rawdata[3];

	x[2] = Rawdata[n-4];
	y[2] = Rawdata[n-3];

	x[3] = Rawdata[n-2];
	y[3] = Rawdata[n-1];

	// seed with linear interpolation...
	x[1] += x[1] - x[0];
	y[1] += y[1] - y[0];
	x[2] += x[2] - x[3];
	y[2] += y[2] - y[3];

	e1 = DistanceError(x, y, Rawdata, n);
	for (Retry = 1; Retry <= 2; Retry++)
	{
//		TRACE("Retry %d\n", Retry);
//		TRACE("      x1       y2       x2       y2    error\n");
		e3 = 0.5;
		x1a = x[1];
		while (fabs(e3) >= 0.01)
		{
			x[1] += (x[1] - x[0])*e3;
			e2 = DistanceError(x, y, Rawdata, n);
			if (e2 == e1)
				break;
			if (e2 > e1)
			{
				x[1] = x1a;
				e3 /=-3;
			}
			else
			{
				e1 = e2;
				x1a = x[1];
			}
		}

		e3 = 0.5;
		y1a = y[1];
		while (fabs(e3) >= 0.01)
		{
			y[1] += (y[1] - y[0])*e3;
			e2 = DistanceError(x, y, Rawdata, n);
			if (e2 == e1)
				break;
			if (e2 > e1)
			{
				y[1] = y1a;
				e3 /=-3;
			}
			else
			{
				e1 = e2;
				y1a = y[1];
			}
		}

		e3 = 0.5;
		x2a = x[2];
		while (fabs(e3) >= 0.01)
		{
			x[2] += (x[2] - x[3])*e3;
			e2 = DistanceError(x, y, Rawdata, n);
			if (e2 == e1)
				break;
			if (e2 > e1)
			{
				x[2] = x2a;
				e3 /=-3;
			}
			else
			{
				e1 = e2;
				x2a = x[2];
			}
		}

		e3 = 0.5;
		y2a = y[2];
		while (fabs(e3) >= 0.01)
		{
			y[2] += (y[2] - y[3])*e3;
			e2 = DistanceError(x, y, Rawdata, n);
			if (e2 == e1)
				break;
			if (e2 > e1)
			{
				y[2] = y2a;
				e3 /=-3;
			}
			else
			{
				e1 = e2;
				y2a = y[2];
			}
		}
	} // for

	Control[0] = x[1];
	Control[1] = y[1];
	Control[2] = x[2];
	Control[3] = y[2];
}

double DistanceError(const double *x, const double *y, 
					 const double *Rawdata, int n)
{
	int i;
	double a[4];
	double b[4];
	double u, u1, u2;
	double z, z1, z2,s,s1;
	double temp;
	double totalerror;
	double stepsize;
	double x4, y4;

	totalerror = 0;
	a[3] = (x[3]-x[0]+3*(x[1]-x[2]))/8;
	b[3] = (y[3]-y[0]+3*(y[1]-y[2]))/8;
	a[2] = (x[3]+x[0]-x[1]-x[2])*3/8;
	b[2] = (y[3]+y[0]-y[1]-y[2])*3/8;
	a[1] = (x[3]-x[0])/2 -a[3];
	b[1] = (y[3]-y[0])/2 -b[3];
	a[0] = (x[3]+x[0])/2 -a[2];
	b[0] = (y[3]+y[0])/2 -b[2];

	stepsize = 2.0/(n);
	for (i = 2; i <= n-3; i+=2)
	{
		x4 = Rawdata[i];
		y4 = Rawdata[i+1];
		for (u = -1; u <= 1.01; u += stepsize)
		{
			Bezier(u, a, b, x4, y4, z, s);
			if (s == 0)
			{
				u1 = u;
				z1 = z;
				s1 = s;
				break;
			}
			if (u == -1)
			{
				u1 = u;
				z1 = z;
				s1 = s;
			} 
			if (s < s1)
			{
				u1 = u;
				z1 = z;
				s1 = s;
			}
		}
		if (s1 != 0)
		{
			u = u1 + stepsize;
			if (u > 1)
				u = 1 - stepsize;
			Bezier(u, a, b, x4, y4, z, s);
			while (s != 0 && z != 0)
			{
				u2 = u;
				z2 = z;
				temp = z2-z1;
				if (temp != 0)
					u = (z2 * u1-z1*u2)/temp;
				else
					u = (u1 + u2)/2;

				if (u > 1)
					u = 1;
				else if (u < -1)
					u = -1;
				if (fabs(u-u2) < 0.001)
					break;
				u1 = u2;
				z1 = z2;
				Bezier(u, a, b, x4, y4, z, s);
			}
		}
		totalerror += s;
	}
//	TRACE("%8.3lf %8.3lf %8.3lf %8.3lf %8.3lf\n", x[1], y[1], x[2], y[2], totalerror);
	return totalerror;
}

void Bezier(double u, const double *a, const double *b, 
			double x4, double y4, double &z, double &s)
{
	double x, y;
	double dx4,dy4;
	double dx,dy;

	x = a[0] + u*(a[1] + u*(a[2]+u*a[3]));
	y = b[0] + u*(b[1] + u*(b[2]+u*b[3]));
	dx4 = x-x4; dy4 = y-y4;
	dx = a[1] + u * (a[2] + a[2] + u*3*a[3]);
	dy = b[1] + u * (b[2] + b[2] + u*3*b[3]);
	z = dx * dx4 + dy*dy4;
	s = dx4*dx4 + dy4*dy4;
}