Write code

src/main.rs

@@ -0,0 +1,44 @@
+mod swerve_math;
+mod swervemodule;
+
+extern crate piston_window;
+
+use piston_window::*;
+use piston_window::rectangle::square;
+
+fn main() {
+    let mut window: PistonWindow = WindowSettings::new("Line Drawing", [512, 512])
+        .exit_on_esc(true)
+        .build()
+        .unwrap();
+
+
+
+    while let Some(event) = window.next() {
+        if let Some(Button::Keyboard(key)) = event.press_args() {
+            println!("Key pressed: {:?}", key);
+            if key.code() == 113 {break;}
+        }
+
+        if let Some(Button::Mouse(key)) = event.press_args() {
+            println!("Key pressed: {:?}", key);
+        }
+
+        window.draw_2d(&event, |c, g, _d| {
+            clear(color::BLACK, g);
+
+            // Ellipse::new_border(color::WHITE, 1.0).draw(
+            //     rectangle::centered_square(10.0, 10.0, 50.0),
+            //     &c.draw_state,
+            //     c.transform,
+            //     g,
+            // );
+
+            swervemodule::SwerveModule::draw(&c,);
+
+            Rectangle::new_border(color::WHITE, 1.0).draw(square(0.0, 0.0, 512.0), &c.draw_state, c.transform, g);
+
+
+        });
+    }
+}

src/swerve_math.rs

@@ -0,0 +1,151 @@
+use std::f32::consts::PI;
+
+//https://github.com/Pantherbotics/SwerveSim/blob/master/src/main/java/com/pantherbotics/swervesim/util/Vector2d.java
+#[derive(Clone, Copy)]
+pub struct Vector2d {
+    x: f32,
+    y: f32
+}
+impl Vector2d {
+    pub fn create(mut self, x:f32, y:f32) {
+        self.x = x;
+        self.y = y;
+    }
+    pub fn rotate(mut self, angle: f32) {
+        let cos_a:f32 = f32::cos(angle * (PI / 180.0));
+        let sin_a:f32 = f32::sin(angle * (PI / 180.0));
+        self.x = self.x * cos_a - self.y * sin_a;
+        self.y = self.x * sin_a + self.y * cos_a;
+    }
+
+    /**
+     * Returns dot product of this vector with argument.
+     *
+     * @param vec Vector with which to perform dot product.
+     * @return Dot product of this vector with argument.
+     */
+    pub fn dot(mut self, vec: Vector2d) -> f32 {
+        return self.x * vec.x + self.y * vec.y;
+    }
+
+    /**
+     * Returns magnitude of vector.
+     *
+     * @return Magnitude of vector.
+     */
+    pub fn magnitude(self) -> f32 {
+        return f32::sqrt(self.x * self.x + self.y * self.y);
+    }
+
+    /**
+     * Returns scalar projection of this vector onto argument.
+     *
+     * @param vec Vector onto which to project this vector.
+     * @return scalar projection of this vector onto argument.
+     */
+    pub fn scalar_project(&mut self, mut vec: Vector2d) -> f32 {
+        let mag = vec.magnitude();
+        return self.dot(vec) / mag;
+    }
+}
+
+pub fn degrees_to_radians(degrees: f32) -> f32{
+    return degrees * (PI / 180.0);
+}
+
+pub fn radians_to_degrees(radians: f32) -> f32{
+    return radians * (180.0 / PI);
+}
+
+
+
+
+/**
+ * I spent like half an hour figuring this out, don't try to figure it out just appreciate the results :)
+ * 0 Degrees is straight forward, 90 degrees is to the right, 180 degrees is backwards, 270 degrees is to the left
+ * Aka clockwise degrees and 0 is straight forward on the joystick :)
+ * @param x the X value of a coordinate
+ * @param y the Y value of a coordinate
+ */
+pub fn get_heading(x:f32, y:f32) -> f32 {
+    if x == 0.0 && y == 0.0 { return 0.0; }
+
+    let mut angle = (360. - ((f32::atan2(y, x)*180.0/PI) + 180.0)) - 90.0;
+    if angle < 0. {
+        angle = 270. + (90. - f32::abs(angle));
+    }
+
+    return angle;
+}
+
+
+/**
+ * Used to re-obtain the X value of the point on a unit circle from an angle
+ * The angles are in degrees from getHeading()
+ * @param angle The angle (from getHeading()) to get the X value for
+ */
+pub fn get_heading_x(mut angle:f32) -> f32 {
+    //Ensure values are [0, 360)
+    while angle > 360. { angle -= 360.; }
+    while angle < 0. { angle += 360.; }
+
+    if angle >= 0. && angle <= 90. {
+        return f32::cos(degrees_to_radians(90. - angle));
+    }else if angle >= 90. && angle <= 270. {
+        return f32::cos(-degrees_to_radians(angle - 90.));
+    }else if angle >= 270. && angle <= 360. {
+        return -f32::cos(degrees_to_radians(270. - angle));
+    }
+    return 0.;
+}
+
+/**
+ * Used to re-obtain the Y value of the point on a unit circle from an angle
+ * The angles are in degrees from getHeading()
+ * @param angle The angle (from getHeading()) to get the Y value for
+ */
+pub fn getHeadingY(mut angle:f32) -> f32 {
+    //Ensure values are [0, 360)
+    while angle > 360. { angle -= 360.; }
+    while angle < 0. { angle += 360.; }
+
+    if angle >= 0. && angle <= 90. {
+        return f32::sin(degrees_to_radians(90. - angle));
+    }else if angle >= 90. && angle <= 270. {
+        return f32::sin(-degrees_to_radians(angle - 90.));
+    }else if angle >= 270. && angle <= 360. {
+        return -f32::sin(degrees_to_radians(270. - angle));
+    }
+    return 0.;
+}
+
+/**
+ * Takes a value and shifts it towards 0 by a specified amount
+ * @param value the value to shift
+ * @param shift the amount to shift it
+ * @return the shifted value
+ */
+pub fn approach_zero(value:f32, shift:f32) -> f32 {
+    if value >= 0. {
+        return f32::max(0., value - shift);
+    }else if value < 0. {
+        return f32::min(0., value + shift);
+    }
+    return 0.;
+}
+
+
+/**
+ * Returns a speed value from [-1, 1] based on joystick X and Y inputs
+ * More critically it's snapped to the unit circle so X=1 Y=1 won't be sqrt(2)
+ * @param X the X of a coordinate [-1, 1]
+ * @param Y the Y of a coordinate [-1, 1]
+ */
+pub fn get_joystick_speed(Y:f32, X:f32) -> f32 {
+    let mut v: Vector2d = Vector2d {x:X, y:Y};
+
+    let angle = f32::atan2(v.x, v.y);
+    let max_magnitude = if f32::abs(v.x) > f32::abs(v.y) {1. / f32::sin(angle)} else {1. / f32::cos(angle)};
+    return f32::abs(Vector2d::magnitude(v) / max_magnitude);
+}
+

src/swervemodule.rs

@@ -0,0 +1,46 @@
+use piston_window::{color, Context, Ellipse, G2d, rectangle};
+use crate::swerve_math;
+
+pub struct SwerveModule {
+    id:i32,
+    w:i32,
+    h: i32,
+    swerve_rot: f32
+}
+impl SwerveModule {
+
+    /**
+     * Draws the wheel vector given wheel data and screen data.
+     * @param g the Graphics2D object to draw with
+     * @param angle the angle of the wheel, in degrees
+     * @param speed the speed of the wheel [-1, 1]
+     * @param font the font size currently in use
+     * @param startFactor the factor [0, 1] to start the vector at, when scaling towards the center
+     * @param width the width of the swerve box (in pixels)
+     * @param height the height of the swerve box (in pixels)
+     * @param vecScale the scalar for the wheel vector
+     */
+    pub fn draw(c: &Context, mut g: &mut G2d,
+        angle:f32, speed:f32, font:i32, startFactor:f32, width:f32, height:f32, vecScale:f32) {
+    // pub fn draw(c: &Context, mut g: &mut G2d) {
+        Ellipse::new_border(color::WHITE, 1.0).draw(
+            rectangle::centered_square(10.0, 10.0, 50.0),
+            &c.draw_state,
+            c.transform,
+            g,
+        );
+    }
+
+    fn get_swerve_corner_x(id: i8) -> f32 {
+        if (id == 1) {
+            return swerve_math::get_heading_x(-45 + swerve_rot);
+        }else if (id == 2) {
+            return swerve_math::get_heading_x(45 + swerve_rot);
+        }else if (id == 3) {
+            return swerve_math::get_heading_x(135 + swerve_rot);
+        }else if (id == 4) {
+            return swerve_math::get_heading_x(-135 + swerve_rot);
+        }
+        return 0;
+    }
+}