hooked up the line drawing algorithm to the compass.
This entailed fixing a few bugs in it as well.
This commit is contained in:
parent
3e29d8bc6a
commit
4e4b314ccd
5 changed files with 240 additions and 190 deletions
|
|
@ -1,53 +1,20 @@
|
|||
#![allow(unused)]
|
||||
use core::f32::consts::PI;
|
||||
use libm::{cosf, roundf, sinf};
|
||||
|
||||
use crate::line_drawing::{draw_line, FourQuadrantMatrix, Line, Point, UPoint};
|
||||
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
struct Sector {
|
||||
total_sectors: usize,
|
||||
sector: usize,
|
||||
}
|
||||
|
||||
//heading starts at north, with the positive direction being clockwise.
|
||||
//Heading ranges from -pi to pi.
|
||||
//
|
||||
//sectors have 0 at north an proceed clockwise, always being positive.
|
||||
fn heading_to_sector(sectors: usize, heading: f32) -> Sector {
|
||||
let half_sector = PI / sectors as f32;
|
||||
let sector_size = 2.0 * half_sector;
|
||||
Sector {
|
||||
total_sectors: sectors,
|
||||
sector: (modulo(heading + half_sector, 2.0 * PI) / (sector_size)) as usize,
|
||||
}
|
||||
}
|
||||
|
||||
fn modulo(a: f32, b: f32) -> f32 {
|
||||
((a % b) + b) % b
|
||||
}
|
||||
use crate::line_drawing::{draw_line, FourQuadrantMatrix, Line, Point};
|
||||
|
||||
fn heading_to_line(heading: f32, square_size: usize) -> Line {
|
||||
todo!()
|
||||
Line(
|
||||
Point { x: 0, y: 0 },
|
||||
Point {
|
||||
x: roundf((square_size as f32) * sinf(heading)) as isize,
|
||||
y: roundf((square_size as f32) * cosf(heading)) as isize,
|
||||
},
|
||||
)
|
||||
}
|
||||
|
||||
pub fn draw_heading<const X: usize, const Y: usize>(
|
||||
heading: f32,
|
||||
) -> FourQuadrantMatrix<{ X }, { Y }, u8> {
|
||||
let mut ret = FourQuadrantMatrix::new(UPoint { x: X / 2, y: Y / 2 });
|
||||
draw_line::<X, Y>(&heading_to_line(heading, X.min(Y)), &mut ret);
|
||||
ret
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn sectors() {
|
||||
assert_eq!(heading_to_sector(4,0.0).sector, 0);
|
||||
assert_eq!(heading_to_sector(4,PI/2.0).sector, 1);
|
||||
assert_eq!(heading_to_sector(4,-PI/2.0).sector, 3);
|
||||
assert_eq!(heading_to_sector(4,PI).sector, 2);
|
||||
assert_eq!(heading_to_sector(4,-PI).sector, 2);
|
||||
}
|
||||
matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>,
|
||||
) {
|
||||
draw_line::<X, Y>(&heading_to_line(heading, X.min(Y)), matrix);
|
||||
}
|
||||
|
|
|
|||
|
|
@ -1,133 +0,0 @@
|
|||
use core::f32::consts::PI;
|
||||
use crate::tilt_compensation::Heading;
|
||||
|
||||
#[derive(Debug)]
|
||||
pub enum Direction {
|
||||
North,
|
||||
NorthEast,
|
||||
East,
|
||||
SouthEast,
|
||||
South,
|
||||
SouthWest,
|
||||
West,
|
||||
NorthWest,
|
||||
}
|
||||
|
||||
//forward is towards usb port
|
||||
const NORTH: [[u8; 5]; 5] = [
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 1, 1, 1, 0],
|
||||
[1, 0, 1, 0, 1],
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
];
|
||||
|
||||
const NORTH_EAST: [[u8; 5]; 5] = [
|
||||
[1, 1, 1, 0, 0],
|
||||
[1, 1, 0, 0, 0],
|
||||
[1, 0, 1, 0, 0],
|
||||
[0, 0, 0, 1, 0],
|
||||
[0, 0, 0, 0, 1],
|
||||
];
|
||||
|
||||
const EAST: [[u8; 5]; 5] = [
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 1, 0, 0, 0],
|
||||
[1, 1, 1, 1, 1],
|
||||
[0, 1, 0, 0, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
];
|
||||
|
||||
const SOUTH_EAST: [[u8; 5]; 5] = [
|
||||
[0, 0, 0, 0, 1],
|
||||
[0, 0, 0, 1, 0],
|
||||
[1, 0, 1, 0, 0],
|
||||
[1, 1, 0, 0, 0],
|
||||
[1, 1, 1, 0, 0],
|
||||
];
|
||||
|
||||
const SOUTH: [[u8; 5]; 5] = [
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
[1, 0, 1, 0, 1],
|
||||
[0, 1, 1, 1, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
];
|
||||
|
||||
const SOUTH_WEST: [[u8; 5]; 5] = [
|
||||
[1, 0, 0, 0, 0],
|
||||
[0, 1, 0, 0, 0],
|
||||
[0, 0, 1, 0, 1],
|
||||
[0, 0, 0, 1, 1],
|
||||
[0, 0, 1, 1, 1],
|
||||
];
|
||||
|
||||
const WEST: [[u8; 5]; 5] = [
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 0, 0, 1, 0],
|
||||
[1, 1, 1, 1, 1],
|
||||
[0, 0, 0, 1, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
];
|
||||
|
||||
const NORTH_WEST: [[u8; 5]; 5] = [
|
||||
[0, 0, 1, 1, 1],
|
||||
[0, 0, 0, 1, 1],
|
||||
[0, 0, 1, 0, 1],
|
||||
[0, 1, 0, 0, 0],
|
||||
[1, 0, 0, 0, 0],
|
||||
];
|
||||
|
||||
pub fn direction_to_led(direction: Direction) -> [[u8; 5]; 5] {
|
||||
match direction {
|
||||
Direction::North => NORTH,
|
||||
Direction::NorthEast => NORTH_EAST,
|
||||
Direction::East => EAST,
|
||||
Direction::SouthEast => SOUTH_EAST,
|
||||
Direction::South => SOUTH,
|
||||
Direction::SouthWest => SOUTH_WEST,
|
||||
Direction::West => WEST,
|
||||
Direction::NorthWest => NORTH_WEST,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn theta_to_direction(heading: Heading) -> Direction {
|
||||
// if heading.0 < (-7. * PI / 8.) {
|
||||
// Direction::North
|
||||
// } else if heading.0 < (-5. * PI / 8.) {
|
||||
// Direction::NorthWest
|
||||
// } else if heading.0 < (-3. * PI / 8.) {
|
||||
// Direction::West
|
||||
// } else if heading.0 < (-PI / 8.) {
|
||||
// Direction::SouthWest
|
||||
// } else if heading.0 < (PI / 8.) {
|
||||
// Direction::South
|
||||
// } else if heading.0 < (3. * PI / 8.) {
|
||||
// Direction::SouthEast
|
||||
// } else if heading.0 < (5. * PI / 8.) {
|
||||
// Direction::East
|
||||
// } else if heading.0 < (7. * PI / 8.) {
|
||||
// Direction::NorthEast
|
||||
// } else {
|
||||
// Direction::North
|
||||
// }
|
||||
if heading.0 < (-7. * PI / 8.) {
|
||||
Direction::South
|
||||
} else if heading.0 < (-5. * PI / 8.) {
|
||||
Direction::SouthEast
|
||||
} else if heading.0 < (-3. * PI / 8.) {
|
||||
Direction::East
|
||||
} else if heading.0 < (-PI / 8.) {
|
||||
Direction::NorthEast
|
||||
} else if heading.0 < (PI / 8.) {
|
||||
Direction::North
|
||||
} else if heading.0 < (3. * PI / 8.) {
|
||||
Direction::NorthWest
|
||||
} else if heading.0 < (5. * PI / 8.) {
|
||||
Direction::West
|
||||
} else if heading.0 < (7. * PI / 8.) {
|
||||
Direction::SouthWest
|
||||
} else {
|
||||
Direction::South
|
||||
}
|
||||
}
|
||||
|
|
@ -1,5 +1,5 @@
|
|||
#![no_std]
|
||||
//to help debug failed tests wiht dbg!()
|
||||
#![cfg_attr(not(test), no_std)]
|
||||
pub mod heading_drawing;
|
||||
pub mod line_drawing;
|
||||
pub mod tilt_compensation;
|
||||
pub mod led;
|
||||
|
|
|
|||
|
|
@ -2,6 +2,8 @@ use core::{
|
|||
mem::swap,
|
||||
ops::{Index, IndexMut},
|
||||
};
|
||||
#[cfg(test)]
|
||||
use std::dbg;
|
||||
|
||||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct Point {
|
||||
|
|
@ -44,7 +46,9 @@ impl UPoint {
|
|||
#[derive(Debug, Clone, Copy, PartialEq, Eq)]
|
||||
pub struct FourQuadrantMatrix<const X: usize, const Y: usize, T> {
|
||||
matrix: [[T; X]; Y],
|
||||
pub zero_coord: UPoint,
|
||||
max_point: Point,
|
||||
min_point: Point,
|
||||
zero_coord: UPoint,
|
||||
}
|
||||
|
||||
impl<const X: usize, const Y: usize, T> FourQuadrantMatrix<{ X }, { Y }, T>
|
||||
|
|
@ -55,9 +59,51 @@ where
|
|||
pub fn new(zero_coord: UPoint) -> FourQuadrantMatrix<{ X }, { Y }, T> {
|
||||
FourQuadrantMatrix {
|
||||
matrix: [[T::default(); X]; Y],
|
||||
max_point: UPoint { x: X - 1, y: 0 }.to_point(&zero_coord),
|
||||
min_point: UPoint { x: 0, y: Y - 1 }.to_point(&zero_coord),
|
||||
zero_coord,
|
||||
}
|
||||
}
|
||||
|
||||
pub fn zero_coord(&self) -> UPoint {
|
||||
self.zero_coord
|
||||
}
|
||||
|
||||
pub fn min_point(&self) -> Point {
|
||||
self.min_point
|
||||
}
|
||||
|
||||
pub fn max_point(&self) -> Point {
|
||||
self.max_point
|
||||
}
|
||||
|
||||
pub fn bound_point(&self, point: &mut Point) {
|
||||
if point.x > self.max_point.x {
|
||||
point.x = self.max_point.x
|
||||
}
|
||||
|
||||
if point.y > self.max_point.y {
|
||||
point.y = self.max_point.y
|
||||
}
|
||||
|
||||
if point.x < self.min_point.x {
|
||||
point.x = self.min_point.x
|
||||
}
|
||||
|
||||
if point.y < self.min_point.y {
|
||||
point.y = self.min_point.y
|
||||
}
|
||||
}
|
||||
|
||||
pub fn is_in_bounds(&self, point: &Point) -> bool {
|
||||
point.x <= self.max_point.x
|
||||
&& point.y <= self.max_point.y
|
||||
&& point.x >= self.min_point.x
|
||||
&& point.y >= self.min_point.y
|
||||
}
|
||||
pub fn reset_matrix(&mut self) {
|
||||
self.matrix = [[T::default(); X]; Y];
|
||||
}
|
||||
}
|
||||
|
||||
impl<T, const X: usize, const Y: usize> IndexMut<Point> for FourQuadrantMatrix<{ X }, { Y }, T> {
|
||||
|
|
@ -94,48 +140,90 @@ pub struct Line(pub Point, pub Point);
|
|||
pub struct ULine(pub UPoint, pub UPoint);
|
||||
|
||||
/// Renders a line into a matrix of pixels.
|
||||
/// Will not attempt to mutate outside bounds of the matrix, so it is safe to draw lines that
|
||||
/// extend past its edges.
|
||||
pub fn draw_line<const X: usize, const Y: usize>(
|
||||
line: &Line,
|
||||
matrix: &mut FourQuadrantMatrix<{ X }, { Y }, u8>,
|
||||
) {
|
||||
let mut line = *line;
|
||||
let steep = (line.0.x - line.1.x).abs() < (line.0.y - line.1.x).abs();
|
||||
#[cfg(test)]
|
||||
dbg!(line);
|
||||
|
||||
// Is it steeper than 45°? If so, we transpose the line. This essentially guarantees we are
|
||||
// drawing a line less steep than 45°.
|
||||
#[cfg(test)]
|
||||
dbg!((line.0.x - line.1.x).abs() < (line.0.y - line.1.y).abs());
|
||||
let steep = (line.0.x - line.1.x).abs() < (line.0.y - line.1.y).abs();
|
||||
if steep {
|
||||
swap(&mut line.0.x, &mut line.0.y);
|
||||
swap(&mut line.1.x, &mut line.1.y);
|
||||
}
|
||||
|
||||
// If our line is running right-to-left, flip the points
|
||||
// so we start on the left.
|
||||
if line.0.x > line.1.x {
|
||||
swap(&mut line.0.x, &mut line.1.x);
|
||||
swap(&mut line.0.y, &mut line.1.y)
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
dbg!((line, steep));
|
||||
let dx = line.1.x - line.0.x;
|
||||
let dy = line.1.y - line.0.y;
|
||||
let derror2 = dy.abs() * 2;
|
||||
let mut error2 = 0;
|
||||
let mut y = line.0.y;
|
||||
let mut draw_point: Point;
|
||||
|
||||
//if the first point is out of bounds, we want to wait for us to get in bounds before arming
|
||||
//the early return.
|
||||
let mut prev_out_of_bounds = !matrix.is_in_bounds(&line.0);
|
||||
|
||||
// For each X coordinate (which is actually a Y coordinate if the line is steep), we calculate
|
||||
// the Y coordinate the same way as before
|
||||
for x in line.0.x..=line.1.x {
|
||||
#[cfg(test)]
|
||||
dbg!((dx, dy, derror2, error2, y, steep, prev_out_of_bounds));
|
||||
if steep {
|
||||
matrix[Point { x: y, y: x }] = 1;
|
||||
// Remember the transpose? This is where we undo it, by swapping our y and x
|
||||
// coordinates again
|
||||
draw_point = Point { x: y, y: x };
|
||||
} else {
|
||||
matrix[Point { x, y }] = 1;
|
||||
draw_point = Point { x, y };
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
dbg!(draw_point);
|
||||
|
||||
if matrix.is_in_bounds(&draw_point) {
|
||||
matrix[draw_point] = 1;
|
||||
prev_out_of_bounds = false;
|
||||
} else {
|
||||
if !prev_out_of_bounds {
|
||||
break;
|
||||
}
|
||||
prev_out_of_bounds = true;
|
||||
}
|
||||
|
||||
error2 += derror2;
|
||||
|
||||
#[cfg(test)]
|
||||
dbg!((dx, dy, derror2, error2, y, steep, prev_out_of_bounds));
|
||||
|
||||
if error2 > dx {
|
||||
y += if line.1.y > line.0.y { 1 } else { -1 };
|
||||
error2 -= dx * 2
|
||||
}
|
||||
#[cfg(test)]
|
||||
dbg!((dx, dy, derror2, error2, y, steep, prev_out_of_bounds));
|
||||
}
|
||||
}
|
||||
|
||||
#[cfg(test)]
|
||||
mod tests {
|
||||
use super::*;
|
||||
|
||||
#[test]
|
||||
fn point_upoint_conv() {
|
||||
let zero_coord = UPoint { x: 2, y: 2 };
|
||||
|
|
@ -223,6 +311,126 @@ mod tests {
|
|||
)
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn diagonal_signed_both_oob_line() {
|
||||
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
|
||||
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
|
||||
draw_line(
|
||||
&Line(Point { x: -10, y: -10 }, Point { x: 10, y: 10 }),
|
||||
&mut canvas,
|
||||
);
|
||||
assert_eq!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
[
|
||||
[0, 0, 0, 0, 1],
|
||||
[0, 0, 0, 1, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 1, 0, 0, 0],
|
||||
[1, 0, 0, 0, 0],
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn diagonal_signed_first_oob_line() {
|
||||
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
|
||||
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
|
||||
draw_line(
|
||||
&Line(Point { x: -10, y: -10 }, Point { x: 2, y: 2 }),
|
||||
&mut canvas,
|
||||
);
|
||||
assert_eq!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
[
|
||||
[0, 0, 0, 0, 1],
|
||||
[0, 0, 0, 1, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 1, 0, 0, 0],
|
||||
[1, 0, 0, 0, 0],
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn diagonal_signed_second_oob_line() {
|
||||
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
|
||||
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
|
||||
draw_line(
|
||||
&Line(Point { x: -2, y: -2 }, Point { x: 10, y: 10 }),
|
||||
&mut canvas,
|
||||
);
|
||||
assert_eq!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
[
|
||||
[0, 0, 0, 0, 1],
|
||||
[0, 0, 0, 1, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 1, 0, 0, 0],
|
||||
[1, 0, 0, 0, 0],
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn vertical_signed_both_oob_line() {
|
||||
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
|
||||
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
|
||||
draw_line(
|
||||
&Line(Point { x: 0, y: -10 }, Point { x: 0, y: 10 }),
|
||||
&mut canvas,
|
||||
);
|
||||
assert_eq!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
[
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn vertical_signed_first_oob_line() {
|
||||
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
|
||||
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
|
||||
draw_line(
|
||||
&Line(Point { x: 0, y: -10 }, Point { x: 0, y: 0 }),
|
||||
&mut canvas,
|
||||
);
|
||||
assert_eq!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
[
|
||||
[0, 0, 0, 0, 0],
|
||||
[0, 0, 0, 0, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn vertical_signed_second_oob_line() {
|
||||
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
|
||||
FourQuadrantMatrix::new(UPoint { x: 2, y: 2 });
|
||||
draw_line(
|
||||
&Line(Point { x: 0, y: 0 }, Point { x: 0, y: 10 }),
|
||||
&mut canvas,
|
||||
);
|
||||
assert_eq!(
|
||||
<FourQuadrantMatrix<5, 5, u8> as Into<[[u8; 5]; 5]>>::into(canvas),
|
||||
[
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 0, 1, 0, 0],
|
||||
[0, 0, 0, 0, 0],
|
||||
[0, 0, 0, 0, 0],
|
||||
]
|
||||
);
|
||||
}
|
||||
|
||||
#[test]
|
||||
fn cross_signed_line() {
|
||||
let mut canvas: FourQuadrantMatrix<5, 5, u8> =
|
||||
|
|
|
|||
Loading…
Add table
Add a link
Reference in a new issue