- d = atan2 (F->yf, F->xf) / (2 * M_PI);
-
- if (_inv) {
- d += 0.5f;
- }
-
- if (d > 0.5f) {
- d -= 1.0f;
- }
-
- f0 = _freq [0].f;
- m = 1;
- _err = 0.0;
-
- for (i = 0; i < 4; i++) {
- F++;
- p = atan2 (F->yf, F->xf) / (2 * M_PI) - d * F->f / f0;
- if (_inv) {
- p += 0.5f;
- }
- p -= floor (p);
- p *= 8;
- k = (int)(floor (p + 0.5));
- e = fabs (p - k);
- if (e > _err) {
- _err = e;
- }
- if (e > 0.4) {
- return 1;
- }
- d += m * (k & 7);
- m *= 8;
- }
+int MTDM::resolve (void)
+{
+ int i, k, m;
+ double d, e, f0, p;
+ Freq *F = _freq;
+
+ if (hypot (F->x2, F->y2) < 0.001) return -1;
+ d = atan2 (F->y2, F->x2) / (2 * M_PI);
+ if (_inv) d += 0.5;
+ if (d > 0.5) d -= 1.0;
+ f0 = _freq [0].f;
+ m = 1;
+ _err = 0.0;
+ for (i = 0; i < 12; i++)
+ {
+ F++;
+ p = atan2 (F->y2, F->x2) / (2 * M_PI) - d * F->f / f0;
+ if (_inv) p += 0.5;
+ p -= floor (p);
+ p *= 2;
+ k = (int)(floor (p + 0.5));
+ e = fabs (p - k);
+ if (e > _err) _err = e;
+ if (e > 0.4) return 1;
+ d += m * (k & 1);
+ m *= 2;
+ }
+ _del = 16 * d;
+
+ return 0;
+}