From 6a09782d30c14df6d8d96acf5fda63b5f7f17e37 Mon Sep 17 00:00:00 2001 From: jess Date: Thu, 2 Apr 2026 17:13:35 -0700 Subject: [PATCH] add linear/circle Nyquist fit resolver with cumulative turning detection --- cue/src/plot.rs | 144 ++++++++++++++++++++++++++++++++++++++---------- 1 file changed, 116 insertions(+), 28 deletions(-) diff --git a/cue/src/plot.rs b/cue/src/plot.rs index e835e1e..0023fb2 100644 --- a/cue/src/plot.rs +++ b/cue/src/plot.rs @@ -153,15 +153,60 @@ fn kasa_fit(pts: &[(f64, f64)]) -> Option<(f64, f64, f64)> { Some((cx, cy, r_sq.sqrt())) } -/// Fit the dominant Nyquist semicircle, trimming first-arc points from the -/// low-frequency end before falling back to outlier removal within each subset. -fn fit_nyquist_circle(points: &[EisPoint]) -> Option { +fn cumulative_turning(pts: &[(f64, f64)]) -> f64 { + if pts.len() < 3 { return 0.0; } + let mut total = 0.0; + for i in 1..pts.len() - 1 { + let (dx1, dy1) = (pts[i].0 - pts[i-1].0, pts[i].1 - pts[i-1].1); + let (dx2, dy2) = (pts[i+1].0 - pts[i].0, pts[i+1].1 - pts[i].1); + let cross = dx1 * dy2 - dy1 * dx2; + let dot = dx1 * dx2 + dy1 * dy2; + total += cross.atan2(dot).abs(); + } + total +} + +struct LinearFit { + slope: f32, + y_intercept: f32, + rs: f32, +} + +fn fit_linear(pts: &[(f64, f64)]) -> Option { + if pts.len() < 2 { return None; } + let n = pts.len() as f64; + let sx: f64 = pts.iter().map(|p| p.0).sum(); + let sy: f64 = pts.iter().map(|p| p.1).sum(); + let sx2: f64 = pts.iter().map(|p| p.0 * p.0).sum(); + let sxy: f64 = pts.iter().map(|p| p.0 * p.1).sum(); + let denom = n * sx2 - sx * sx; + if denom.abs() < 1e-20 { return None; } + let slope = (n * sxy - sx * sy) / denom; + let y_int = (sy - slope * sx) / n; + let rs = if slope.abs() > 1e-10 { -y_int / slope } else { sx / n }; + Some(LinearFit { + slope: slope as f32, + y_intercept: y_int as f32, + rs: rs as f32, + }) +} + +enum NyquistFit { + Circle(CircleFit), + Linear(LinearFit), +} + +fn fit_nyquist(points: &[EisPoint]) -> Option { let all: Vec<(f64, f64)> = points.iter() .filter(|p| p.z_real.is_finite() && p.z_imag.is_finite()) .map(|p| (p.z_real as f64, -p.z_imag as f64)) .collect(); if all.len() < 4 { return None; } + if cumulative_turning(&all) < 0.524 { + return fit_linear(&all).map(NyquistFit::Linear); + } + let min_pts = 4.max(all.len() / 3); let mut best: Option = None; let mut best_score = f64::MAX; @@ -210,7 +255,17 @@ fn fit_nyquist_circle(points: &[EisPoint]) -> Option { } } } - best + + if let Some(circle) = best { + if best_score > 0.15 { + if let Some(lin) = fit_linear(&all) { + return Some(NyquistFit::Linear(lin)); + } + } + Some(NyquistFit::Circle(circle)) + } else { + fit_linear(&all).map(NyquistFit::Linear) + } } /* ---- Bode ---- */ @@ -676,33 +731,66 @@ impl<'a> canvas::Program for NyquistPlot<'a> { draw_polyline(&mut frame, &pts, COL_NYQ, 2.0); draw_dots(&mut frame, &pts, COL_NYQ, 3.0); - if let Some(fit) = fit_nyquist_circle(self.points) { - let theta_r = (-fit.cy).atan2((fit.r * fit.r - fit.cy * fit.cy).sqrt()); - let mut theta_l = (-fit.cy).atan2(-(fit.r * fit.r - fit.cy * fit.cy).sqrt()); - if theta_l < theta_r { theta_l += std::f32::consts::TAU; } + match fit_nyquist(self.points) { + Some(NyquistFit::Circle(fit)) => { + let theta_r = (-fit.cy).atan2((fit.r * fit.r - fit.cy * fit.cy).sqrt()); + let mut theta_l = (-fit.cy).atan2(-(fit.r * fit.r - fit.cy * fit.cy).sqrt()); + if theta_l < theta_r { theta_l += std::f32::consts::TAU; } - let n_arc = 120; - let arc_pts: Vec = (0..=n_arc).map(|i| { - let t = theta_r + (theta_l - theta_r) * i as f32 / n_arc as f32; - let x = fit.cx + fit.r * t.cos(); - let y = fit.cy + fit.r * t.sin(); - Point::new( - lerp(x, xv.lo, xv.hi, xl, xr), - lerp(y, yv.hi, yv.lo, yt, yb), - ) - }).collect(); - draw_polyline(&mut frame, &arc_pts, COL_FIT, 1.5); + let n_arc = 120; + let arc_pts: Vec = (0..=n_arc).map(|i| { + let t = theta_r + (theta_l - theta_r) * i as f32 / n_arc as f32; + let x = fit.cx + fit.r * t.cos(); + let y = fit.cy + fit.r * t.sin(); + Point::new( + lerp(x, xv.lo, xv.hi, xl, xr), + lerp(y, yv.hi, yv.lo, yt, yb), + ) + }).collect(); + draw_polyline(&mut frame, &arc_pts, COL_FIT, 1.5); - let y0_scr = lerp(0.0, yv.hi, yv.lo, yt, yb); - let rs_scr = Point::new(lerp(fit.rs, xv.lo, xv.hi, xl, xr), y0_scr); - let rp_scr = Point::new(lerp(fit.rs + fit.rp, xv.lo, xv.hi, xl, xr), y0_scr); - frame.fill(&Path::circle(rs_scr, 5.0), COL_FIT_PT); - frame.fill(&Path::circle(rp_scr, 5.0), COL_FIT_PT); + let y0_scr = lerp(0.0, yv.hi, yv.lo, yt, yb); + let rs_scr = Point::new(lerp(fit.rs, xv.lo, xv.hi, xl, xr), y0_scr); + let rp_scr = Point::new(lerp(fit.rs + fit.rp, xv.lo, xv.hi, xl, xr), y0_scr); + frame.fill(&Path::circle(rs_scr, 5.0), COL_FIT_PT); + frame.fill(&Path::circle(rp_scr, 5.0), COL_FIT_PT); - dt(&mut frame, Point::new(rs_scr.x, rs_scr.y + 6.0), - &format!("Rs={:.0}", fit.rs), COL_FIT_PT, 10.0); - dt(&mut frame, Point::new(rp_scr.x - 30.0, rp_scr.y + 6.0), - &format!("Rp={:.0}", fit.rp), COL_FIT_PT, 10.0); + dt(&mut frame, Point::new(rs_scr.x, rs_scr.y + 6.0), + &format!("Rs={:.0}", fit.rs), COL_FIT_PT, 10.0); + dt(&mut frame, Point::new(rp_scr.x - 30.0, rp_scr.y + 6.0), + &format!("Rp={:.0}", fit.rp), COL_FIT_PT, 10.0); + } + Some(NyquistFit::Linear(fit)) => { + let x_min = self.points.iter() + .filter(|p| p.z_real.is_finite()) + .map(|p| p.z_real) + .fold(f32::INFINITY, f32::min); + let x_max = self.points.iter() + .filter(|p| p.z_real.is_finite()) + .map(|p| p.z_real) + .fold(f32::NEG_INFINITY, f32::max); + let pad = (x_max - x_min) * 0.1; + let x0 = x_min - pad; + let x1 = x_max + pad; + let y0 = fit.slope * x0 + fit.y_intercept; + let y1 = fit.slope * x1 + fit.y_intercept; + let p0 = Point::new( + lerp(x0, xv.lo, xv.hi, xl, xr), + lerp(y0, yv.hi, yv.lo, yt, yb), + ); + let p1 = Point::new( + lerp(x1, xv.lo, xv.hi, xl, xr), + lerp(y1, yv.hi, yv.lo, yt, yb), + ); + dl(&mut frame, p0, p1, COL_FIT, 1.5); + + let y0_scr = lerp(0.0, yv.hi, yv.lo, yt, yb); + let rs_scr = Point::new(lerp(fit.rs, xv.lo, xv.hi, xl, xr), y0_scr); + frame.fill(&Path::circle(rs_scr, 5.0), COL_FIT_PT); + dt(&mut frame, Point::new(rs_scr.x, rs_scr.y + 6.0), + &format!("Rs={:.0}", fit.rs), COL_FIT_PT, 10.0); + } + None => {} } if let Some(pos) = cursor.position_in(bounds) {