diff options
Diffstat (limited to 'src/devices/math_device.rs')
| -rw-r--r-- | src/devices/math_device.rs | 211 |
1 files changed, 128 insertions, 83 deletions
diff --git a/src/devices/math_device.rs b/src/devices/math_device.rs index 015545e..7944b48 100644 --- a/src/devices/math_device.rs +++ b/src/devices/math_device.rs @@ -1,59 +1,155 @@ -use bedrock_core::*; +use crate::*; +const ANGLE_SCALE: f64 = 10430.378350470453; // 65536 / 2π -pub struct MathDevice { - pub op1: u16, - pub op2: u16, - pub sqrt: Option<u16>, - pub atan: Option<u16>, - pub prod: Option<(u16, u16)>, // (low, high) +pub struct MathDevice { + pub x: u16, + pub y: u16, + pub r: u16, + pub t: u16, + pub x_read: Option<u16>, + pub y_read: Option<u16>, + pub r_read: Option<u16>, + pub t_read: Option<u16>, + /// (low, high) + pub prod: Option<(u16, u16)>, pub quot: Option<u16>, pub rem: Option<u16>, } + +impl Device for MathDevice { + fn read(&mut self, port: u8) -> u8 { + match port { + 0x0 => read_h!(self.x()), + 0x1 => read_l!(self.x()), + 0x2 => read_h!(self.y()), + 0x3 => read_l!(self.y()), + 0x4 => read_h!(self.r()), + 0x5 => read_l!(self.r()), + 0x6 => read_h!(self.t()), + 0x7 => read_l!(self.t()), + 0x8 => read_h!(self.prod().1), + 0x9 => read_l!(self.prod().1), + 0xa => read_h!(self.prod().0), + 0xb => read_l!(self.prod().0), + 0xc => read_h!(self.quot()), + 0xd => read_l!(self.quot()), + 0xe => read_h!(self.rem()), + 0xf => read_l!(self.rem()), + _ => unreachable!(), + } + } + + fn write(&mut self, port: u8, value: u8) -> Option<Signal> { + match port { + 0x0 => { write_h!(self.x, value); self.clear_polar(); }, + 0x1 => { write_l!(self.x, value); self.clear_polar(); }, + 0x2 => { write_h!(self.y, value); self.clear_polar(); }, + 0x3 => { write_l!(self.y, value); self.clear_polar(); }, + 0x4 => { write_h!(self.r, value); self.clear_cartesian(); }, + 0x5 => { write_l!(self.r, value); self.clear_cartesian(); }, + 0x6 => { write_h!(self.t, value); self.clear_cartesian(); }, + 0x7 => { write_l!(self.t, value); self.clear_cartesian(); }, + 0x8 => (), + 0x9 => (), + 0xa => (), + 0xb => (), + 0xc => (), + 0xd => (), + 0xe => (), + 0xf => (), + _ => unreachable!(), + }; + return None; + } + + fn wake(&mut self) -> bool { + false + } +} + + impl MathDevice { pub fn new() -> Self { Self { - op1: 0, - op2: 0, + x: 0, + y: 0, + r: 0, + t: 0, + x_read: None, + y_read: None, + r_read: None, + t_read: None, - sqrt: None, - atan: None, prod: None, quot: None, rem: None, } } - pub fn clear(&mut self) { - self.sqrt = None; - self.atan = None; + pub fn clear_cartesian(&mut self) { + self.x_read = None; + self.y_read = None; + } + + pub fn clear_polar(&mut self) { + self.r_read = None; + self.t_read = None; self.prod = None; self.quot = None; - self.rem = None; + self.rem = None; + } + + pub fn x(&mut self) -> u16 { + match self.x_read { + Some(x) => x, + None => { + let r = self.r as f64; + let t = self.t as f64; + let angle = t / ANGLE_SCALE; + let x = angle.cos() * r; + self.x_read = Some(x as i16 as u16); + self.x_read.unwrap() + } + } } - pub fn atan(&mut self) -> u16 { - match self.atan { - Some(atan) => atan, + pub fn y(&mut self) -> u16 { + match self.y_read { + Some(y) => y, None => { - let x = self.op1 as i16 as f64; - let y = self.op2 as i16 as f64; - const SCALE: f64 = 10430.378350470453; // PI * 32768 - self.atan = Some((f64::atan2(x, y) * SCALE) as i16 as u16); - self.atan.unwrap() + let r = self.r as f64; + let t = self.t as f64; + let angle = t / ANGLE_SCALE; + let y = angle.sin() * r; + self.y_read = Some(y as i16 as u16); + self.y_read.unwrap() } } } - pub fn sqrt(&mut self) -> u16 { - match self.sqrt { - Some(sqrt) => sqrt, + pub fn r(&mut self) -> u16 { + match self.r_read { + Some(r) => r, None => { - let input = ((self.op1 as u32) << 16) | (self.op2 as u32); - self.sqrt = Some((input as f64).sqrt() as u16); - self.sqrt.unwrap() + let sum = (self.x as f64).powi(2) + (self.y as f64).powi(2); + self.r_read = Some(sum.sqrt() as u16); + self.r_read.unwrap() + } + } + } + + pub fn t(&mut self) -> u16 { + match self.t_read { + Some(t) => t, + None => { + let x = self.x as i16 as f64; + let y = self.x as i16 as f64; + let angle = f64::atan2(y, x) * ANGLE_SCALE; + self.t_read = Some(angle as i16 as u16); + self.t_read.unwrap() } } } @@ -62,7 +158,7 @@ impl MathDevice { match self.prod { Some(prod) => prod, None => { - self.prod = Some(self.op1.widening_mul(self.op2)); + self.prod = Some(self.x.widening_mul(self.y)); self.prod.unwrap() } } @@ -72,7 +168,7 @@ impl MathDevice { match self.quot { Some(quot) => quot, None => { - self.quot = Some(self.op1.checked_div(self.op2).unwrap_or(0)); + self.quot = Some(self.x.checked_div(self.y).unwrap_or(0)); self.quot.unwrap() } } @@ -82,60 +178,9 @@ impl MathDevice { match self.rem { Some(rem) => rem, None => { - self.rem = Some(self.op1.checked_rem(self.op2).unwrap_or(0)); + self.rem = Some(self.x.checked_rem(self.y).unwrap_or(0)); self.rem.unwrap() } } } } - -impl Device for MathDevice { - fn read(&mut self, port: u8) -> u8 { - match port { - 0x0 => read_h!(self.op1), - 0x1 => read_l!(self.op1), - 0x2 => read_h!(self.op2), - 0x3 => read_l!(self.op2), - 0x4 => read_h!(self.sqrt()), - 0x5 => read_l!(self.sqrt()), - 0x6 => read_h!(self.atan()), - 0x7 => read_l!(self.atan()), - 0x8 => read_h!(self.prod().1), - 0x9 => read_l!(self.prod().1), - 0xa => read_h!(self.prod().0), - 0xb => read_l!(self.prod().0), - 0xc => read_h!(self.quot()), - 0xd => read_l!(self.quot()), - 0xe => read_h!(self.rem()), - 0xf => read_l!(self.rem()), - _ => unreachable!(), - } - } - - fn write(&mut self, port: u8, value: u8) -> Option<Signal> { - match port { - 0x0 => { write_h!(self.op1, value); self.clear(); }, - 0x1 => { write_l!(self.op1, value); self.clear(); }, - 0x2 => { write_h!(self.op2, value); self.clear(); }, - 0x3 => { write_l!(self.op2, value); self.clear(); }, - 0x4 => (), - 0x5 => (), - 0x6 => (), - 0x7 => (), - 0x8 => (), - 0x9 => (), - 0xa => (), - 0xb => (), - 0xc => (), - 0xd => (), - 0xe => (), - 0xf => (), - _ => unreachable!(), - }; - return None; - } - - fn wake(&mut self) -> bool { - false - } -} |