Skip to content

Commit

Permalink
fix: indexing bug
Browse files Browse the repository at this point in the history
  • Loading branch information
@alexDickhans
alexDickhans committed Aug 21, 2024
1 parent 3bdf867 commit f87c23d
Show file tree
Hide file tree
Showing 7 changed files with 381 additions and 167 deletions.
2 changes: 1 addition & 1 deletion .pre-commit-config.yaml
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,6 +8,6 @@ repos:
hooks:
- id: cargo-test
name: Run Cargo Test
entry: cargo test --all-targets
entry: cargo test --all-targets --
language: system
types: [ rust ]
188 changes: 138 additions & 50 deletions src/bezier.rs
View file
Original file line number Diff line number Diff line change
@@ -1,20 +1,21 @@
use crate::bezier::BezierConstructionError::{TooFewPoints, TooManyPoints};
use crate::path::PathSegment;
use core::cmp::Ordering;
use interp::interp;
use nalgebra::{Matrix2x4, Matrix3x4, Matrix4, RowVector2, RowVector3, RowVector4, Vector2, Vector3, Vector4};
use crate::bezier::BezierConstructionError::{TooFewPoints, TooManyPoints};
use crate::path::{PathSegment};
use nalgebra::{
Matrix2x4, Matrix3x4, Matrix4, RowVector2, RowVector3, RowVector4, Vector2, Vector3, Vector4,
};

const BEZIER_MATRIX: Matrix4<f64> = Matrix4::new(-1.0, 3.0, -3.0, 1.0,
3.0, -6.0, 3.0, 0.0,
-3.0, 3.0, 0.0, 0.0,
1.0, 0.0, 0.0, 0.0);
const BEZIER_MATRIX: Matrix4<f64> = Matrix4::new(
-1.0, 3.0, -3.0, 1.0, 3.0, -6.0, 3.0, 0.0, -3.0, 3.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0,
);

const BEZIER_D_MATRIX: Matrix3x4<f64> = Matrix3x4::new(-3.0, 9.0, -9.0, 3.0,
6.0, -12.0, 6.0, 0.0,
-3.0, 3.0, 0.0, 0.0);
const BEZIER_D_MATRIX: Matrix3x4<f64> = Matrix3x4::new(
-3.0, 9.0, -9.0, 3.0, 6.0, -12.0, 6.0, 0.0, -3.0, 3.0, 0.0, 0.0,
);

const BEZIER_DD_MATRIX: Matrix2x4<f64> = Matrix2x4::new(-6.0, 18.0, -18.0, 6.0,
6.0, -12.0, 6.0, 0.0);
const BEZIER_DD_MATRIX: Matrix2x4<f64> =
Matrix2x4::new(-6.0, 18.0, -18.0, 6.0, 6.0, -12.0, 6.0, 0.0);

#[derive(Debug, Copy, Clone)]
pub enum BezierConstructionError {
Expand All @@ -30,12 +31,16 @@ pub struct Bezier<const D: usize> {
}

impl<const D: usize> Bezier<D> {
pub fn new(points: (Vector2<f64>, Vector2<f64>, Vector2<f64>, Vector2<f64>), vel: f64, accel: f64) -> Self {
pub fn new(
points: (Vector2<f64>, Vector2<f64>, Vector2<f64>, Vector2<f64>),
vel: f64,
accel: f64,
) -> Self {
let mut res = Self {
points,
dist_to_t: ([0.0; D], [0.0; D]),
vel,
accel
accel,
};

res.compute_dist_to_t();
Expand All @@ -47,7 +52,7 @@ impl<const D: usize> Bezier<D> {
let mut mag_sum = 0.0;

for i in 0..D {
let t = i as f64 / (D-1) as f64;
let t = i as f64 / (D - 1) as f64;
mag_sum += self.get_d(t).magnitude() / D as f64;
self.dist_to_t.0[i] = t;
self.dist_to_t.1[i] = mag_sum;
Expand All @@ -60,8 +65,22 @@ impl<const D: usize> Bezier<D> {

pub fn get(&self, t: f64) -> Vector3<f64> {
let t_vector = RowVector4::new(t.powi(3), t.powi(2), t, 1.0);
let x = t_vector * BEZIER_MATRIX * Vector4::new(self.points.0.x, self.points.1.x, self.points.2.x, self.points.3.x);
let y = t_vector * BEZIER_MATRIX * Vector4::new(self.points.0.y, self.points.1.y, self.points.2.y, self.points.3.y);
let x = t_vector
* BEZIER_MATRIX
* Vector4::new(
self.points.0.x,
self.points.1.x,
self.points.2.x,
self.points.3.x,
);
let y = t_vector
* BEZIER_MATRIX
* Vector4::new(
self.points.0.y,
self.points.1.y,
self.points.2.y,
self.points.3.y,
);

let d = self.get_d(t);

Expand All @@ -70,16 +89,44 @@ impl<const D: usize> Bezier<D> {

pub fn get_d(&self, t: f64) -> Vector2<f64> {
let t_vector = RowVector3::new(t.powi(2), t, 1.0);
let x = t_vector * BEZIER_D_MATRIX * Vector4::new(self.points.0.x, self.points.1.x, self.points.2.x, self.points.3.x);
let y = t_vector * BEZIER_D_MATRIX * Vector4::new(self.points.0.y, self.points.1.y, self.points.2.y, self.points.3.y);
let x = t_vector
* BEZIER_D_MATRIX
* Vector4::new(
self.points.0.x,
self.points.1.x,
self.points.2.x,
self.points.3.x,
);
let y = t_vector
* BEZIER_D_MATRIX
* Vector4::new(
self.points.0.y,
self.points.1.y,
self.points.2.y,
self.points.3.y,
);

Vector2::new(x.sum(), y.sum())
}

pub fn get_dd(&self, t: f64) -> Vector2<f64> {
let t_vector = RowVector2::new(t, 1.0);
let x = t_vector * BEZIER_DD_MATRIX * Vector4::new(self.points.0.x, self.points.1.x, self.points.2.x, self.points.3.x);
let y = t_vector * BEZIER_DD_MATRIX * Vector4::new(self.points.0.y, self.points.1.y, self.points.2.y, self.points.3.y);
let x = t_vector
* BEZIER_DD_MATRIX
* Vector4::new(
self.points.0.x,
self.points.1.x,
self.points.2.x,
self.points.3.x,
);
let y = t_vector
* BEZIER_DD_MATRIX
* Vector4::new(
self.points.0.y,
self.points.1.y,
self.points.2.y,
self.points.3.y,
);

Vector2::new(x.sum(), y.sum())
}
Expand All @@ -102,9 +149,18 @@ impl<const D: usize> TryFrom<PathSegment> for Bezier<D> {

fn try_from(path: PathSegment) -> Result<Self, Self::Error> {
match path.path.len().cmp(&4usize) {
Ordering::Less => {Err(TooFewPoints)}
Ordering::Equal => { Ok(Self::new((path.path[0].into(), path.path[1].into(), path.path[2].into(), path.path[3].into()), path.constraints.velocity, path.constraints.accel))}
Ordering::Greater => { Err(TooManyPoints)}
Ordering::Less => Err(TooFewPoints),
Ordering::Equal => Ok(Self::new(
(
path.path[0].into(),
path.path[1].into(),
path.path[2].into(),
path.path[3].into(),
),
path.constraints.velocity,
path.constraints.accel,
)),
Ordering::Greater => Err(TooManyPoints),
}
}
}
Expand All @@ -115,26 +171,32 @@ fn cross_2d(a: Vector2<f64>, b: Vector2<f64>) -> f64 {

#[cfg(test)]
mod tests {
use alloc::{vec};
use super::*;
use nalgebra::Vector2;
use crate::path::{Constraints, Point};
use alloc::vec;
use nalgebra::Vector2;

#[test]
fn test_bezier_new() {
let points = (
Vector2::new(0.0, 0.0),
Vector2::new(1.0, 2.0),
Vector2::new(2.0, 3.0),
Vector2::new(3.0, 0.0)
Vector2::new(3.0, 0.0),
);
let bezier = Bezier::<50>::new(points, 1.0, 0.5);

assert_eq!(bezier.points.0, Vector2::new(0.0, 0.0));
assert_eq!(bezier.vel, 1.0);
assert_eq!(bezier.accel, 0.5);
assert!(!bezier.dist_to_t.0.is_empty(), "dist_to_t.0 should not be empty after initialization");
assert!(!bezier.dist_to_t.1.is_empty(), "dist_to_t.1 should not be empty after initialization");
assert!(
!bezier.dist_to_t.0.is_empty(),
"dist_to_t.0 should not be empty after initialization"
);
assert!(
!bezier.dist_to_t.1.is_empty(),
"dist_to_t.1 should not be empty after initialization"
);
}

#[test]
Expand All @@ -143,7 +205,7 @@ mod tests {
Vector2::new(0.0, 0.0),
Vector2::new(0.25, 0.0),
Vector2::new(0.75, 0.0),
Vector2::new(1.0, 0.0)
Vector2::new(1.0, 0.0),
);
let bezier = Bezier::<50>::new(points, 1.0, 0.5);

Expand All @@ -165,7 +227,7 @@ mod tests {
Vector2::new(0.0, 0.0),
Vector2::new(1.0, 2.0),
Vector2::new(2.0, 3.0),
Vector2::new(3.0, 0.0)
Vector2::new(3.0, 0.0),
);
let bezier = Bezier::<50>::new(points, 1.0, 0.5);

Expand All @@ -181,14 +243,20 @@ mod tests {
Vector2::new(0.0, 0.0),
Vector2::new(1.0, 2.0),
Vector2::new(2.0, 3.0),
Vector2::new(3.0, 0.0)
Vector2::new(3.0, 0.0),
);
let bezier = Bezier::<50>::new(points, 1.0, 0.5);

let dd_vector = bezier.get_dd(0.5);

assert!(dd_vector.x.is_finite(), "X second derivative should be finite");
assert!(dd_vector.y.is_finite(), "Y second derivative should be finite");
assert!(
dd_vector.x.is_finite(),
"X second derivative should be finite"
);
assert!(
dd_vector.y.is_finite(),
"Y second derivative should be finite"
);
}

#[test]
Expand All @@ -197,7 +265,7 @@ mod tests {
Vector2::new(0.0, 0.0),
Vector2::new(1.0, 2.0),
Vector2::new(2.0, 3.0),
Vector2::new(3.0, 0.0)
Vector2::new(3.0, 0.0),
);
let bezier = Bezier::<50>::new(points, 1.0, 0.5);

Expand All @@ -212,7 +280,7 @@ mod tests {
Vector2::new(0.0, 0.0),
Vector2::new(0.25, 0.0),
Vector2::new(0.75, 0.0),
Vector2::new(1.0, 0.0)
Vector2::new(1.0, 0.0),
);
let bezier = Bezier::<50>::new(points, 1.0, 0.5);

Expand All @@ -229,7 +297,7 @@ mod tests {
Vector2::new(0.0, 0.0),
Vector2::new(0.25, 0.0),
Vector2::new(0.75, 0.0),
Vector2::new(1.0, 0.0)
Vector2::new(1.0, 0.0),
);
let bezier = Bezier::<50>::new(points, 1.0, 0.5);

Expand All @@ -238,7 +306,12 @@ mod tests {

assert!(distance.is_finite(), "Distance should be finite");
assert!(distance >= 0.0, "Distance should be non-negative");
assert!((distance - correct_distance).abs() < correct_distance * 0.05, "calculated: {}, correct: {}", distance, correct_distance);
assert!(
(distance - correct_distance).abs() < correct_distance * 0.05,
"calculated: {}, correct: {}",
distance,
correct_distance
);
}

#[test]
Expand All @@ -247,7 +320,7 @@ mod tests {
Vector2::new(0.0, 0.0),
Vector2::new(1.0, 2.0),
Vector2::new(2.0, 3.0),
Vector2::new(3.0, 0.0)
Vector2::new(3.0, 0.0),
);
let bezier = Bezier::<50>::new(points, 1.0, 0.5);

Expand All @@ -266,31 +339,40 @@ mod tests {
Point { x: 2.0, y: 3.0 },
Point { x: 3.0, y: 0.0 },
],
constraints: Constraints{ velocity: 1.0, accel: 1.0 },
constraints: Constraints {
velocity: 1.0,
accel: 1.0,
},
inverted: false,
stop_end: false,
};

let bezier_result = Bezier::<50>::try_from(path_segment);

assert!(bezier_result.is_ok(), "Should successfully create a Bezier from PathSegment");
assert!(
bezier_result.is_ok(),
"Should successfully create a Bezier from PathSegment"
);
}

#[test]
fn test_bezier_try_from_too_few_points() {
let path_segment = PathSegment {
path: vec![
Point { x: 0.0, y: 0.0 },
Point { x: 1.0, y: 2.0 },
],
constraints: Constraints{ velocity: 1.0, accel: 1.0 },
path: vec![Point { x: 0.0, y: 0.0 }, Point { x: 1.0, y: 2.0 }],
constraints: Constraints {
velocity: 1.0,
accel: 1.0,
},
inverted: false,
stop_end: false,
};

let bezier_result = Bezier::<50>::try_from(path_segment);

assert!(matches!(bezier_result, Err(BezierConstructionError::TooFewPoints)));
assert!(matches!(
bezier_result,
Err(BezierConstructionError::TooFewPoints)
));
}

#[test]
Expand All @@ -301,15 +383,21 @@ mod tests {
Point { x: 1.0, y: 2.0 },
Point { x: 2.0, y: 3.0 },
Point { x: 3.0, y: 0.0 },
Point { x: 4.0, y: 1.0 }
Point { x: 4.0, y: 1.0 },
],
constraints: Constraints{ velocity: 1.0, accel: 1.0 },
constraints: Constraints {
velocity: 1.0,
accel: 1.0,
},
inverted: false,
stop_end: false,
};

let bezier_result = Bezier::<50>::try_from(path_segment);

assert!(matches!(bezier_result, Err(BezierConstructionError::TooManyPoints)));
assert!(matches!(
bezier_result,
Err(BezierConstructionError::TooManyPoints)
));
}
}
Loading

0 comments on commit f87c23d

Please sign in to comment.