From 38e60dc71ba5b9032b1c3b61a565fd7f5cd128c2 Mon Sep 17 00:00:00 2001
From: Martin Morin <martin.morin@control.lth.se>
Date: Tue, 6 Dec 2022 01:09:09 +0100
Subject: [PATCH] Add second order pole dragging
---
src/lib.rs | 16 +---------------
src/transfer_functions.rs | 35 ++++++++++++++++++++++++++++++++++-
2 files changed, 35 insertions(+), 16 deletions(-)
diff --git a/src/lib.rs b/src/lib.rs
index 2a585d2..8878e8c 100644
--- a/src/lib.rs
+++ b/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),
diff --git a/src/transfer_functions.rs b/src/transfer_functions.rs
index e1772af..4cf45f9 100644
--- a/src/transfer_functions.rs
+++ b/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;
+ }
+ }
}
--
GitLab