Add second order pole dragging

src/lib.rs

@@ -161,21 +161,7 @@ mod pole_position_app {
 
             // Handle dragging
             if dragged {
-                let (re_off, im_off) = match self.pole_drag_offset {
-                    Some((x, y)) => (x, y),
-                    None => {
-                        // We have just started to drag, find offset to closest pole so we don't jump
-                        //
-                        // For now, just assume we want to place the pole where we click
-                        self.pole_drag_offset = Some((0.0, 0.0));
-                        (0.0, 0.0)
-                    }
-                };
-
-                if let Some((re,im)) = pointer_coordinate {
-                    // This should never fail
-                    let (re, im) = (re + re_off, im + im_off);
-
+                if let Some((re,im)) = pointer_coordinate { // This should never fail
                     match self.order {
                         Order::First => self.fo.adjust_poles_to(re, im),
                         Order::Second => self.so.adjust_poles_to(re, im),

src/transfer_functions.rs

@@ -127,5 +127,38 @@ impl TransferFunction for SecondOrderSystem {
         }
     }
 
-    fn adjust_poles_to(&mut self, _re: f64, _im: f64) {}
+    fn adjust_poles_to(&mut self, re: f64, im: f64) {
+        if re >= 0.0 {
+            return
+        }
+
+        let mut d_new = self.d;
+        let w_new;
+
+        if self.d < 1.0 {
+            // two complex poles
+            let (re2, im2) = (re.powi(2), im.powi(2));
+            d_new = (re2/(re2+im2)).sqrt();
+            w_new = -re/d_new;
+        } else if self.d == 1.0 {
+            w_new = -re;
+            // real double pole
+        } else {
+            // two real poles
+            let d2 = self.d.powi(2);
+            let fast = -self.d*self.w + self.w*(d2 - 1.0).sqrt();
+            let slow = -self.d*self.w - self.w*(d2 - 1.0).sqrt();
+
+            if (re - fast).abs() < (re - slow).abs() {
+                w_new = re/( -self.d + (d2-1.0).sqrt() );
+            } else {
+                w_new = re/( -self.d - (d2-1.0).sqrt() );
+            }
+        }
+
+        if self.d_lower <= d_new && self.d_upper >= d_new && self.w_lower <= w_new && self.w_upper >= w_new {
+            self.d = d_new;
+            self.w = w_new;
+        }
+    }
 }