diff options
Diffstat (limited to 'Bus/Horloge')
| -rw-r--r-- | Bus/Horloge/Curvefit.cpp | 235 |
1 files changed, 235 insertions, 0 deletions
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 |