From 7459bca95529fc9a89f0d628bc19393c66499aba Mon Sep 17 00:00:00 2001
From: fcolin
Date: Thu, 1 Feb 2007 12:51:27 +0000
Subject:  Utilisateur : Fcolin       Date : 25/10/01   Heure : 11:44 Créé (vss
 1)

---
 Bus/Horloge/Curvefit.cpp | 235 +++++++++++++++++++++++++++++++++++++++++++++++
 1 file changed, 235 insertions(+)
 create mode 100644 Bus/Horloge/Curvefit.cpp

(limited to 'Bus/Horloge')

diff --git a/Bus/Horloge/Curvefit.cpp b/Bus/Horloge/Curvefit.cpp
new file mode 100644
index 0000000..a1854d0
--- /dev/null
+++ b/Bus/Horloge/Curvefit.cpp
@@ -0,0 +1,235 @@
+// 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;
+}
\ No newline at end of file
-- 
cgit v1.1