traffic_sim/math/
util.rs

1use super::{Point2d, Vector2d};
2use cgmath::prelude::*;
3
4/// Projects a point onto a local coordinate system.
5///
6/// # Parameters
7/// * `point` - The point to project
8/// * `origin` - The origin of the coordinate system
9/// * `x_axis` - The basis vector pointing in the positive x-axis.
10/// * `y_axis` - The basis vector pointing in the positive y-axis.
11pub fn project_local(
12    point: Point2d,
13    origin: Point2d,
14    x_axis: Vector2d,
15    y_axis: Vector2d,
16) -> Point2d {
17    let point = point - origin;
18    Point2d::new(point.dot(x_axis), point.dot(y_axis))
19}
20
21/// Rotates a vector 90 degrees clockwise.
22pub fn rot90(vec: Vector2d) -> Vector2d {
23    Vector2d::new(-vec.y, vec.x)
24}
25
26/// Normalises a vector and computes the derivative of the normalised vector.
27///
28/// # Parameters
29/// * `v` - The vector to normalize
30/// * `dv` - The derivative of `v`
31///
32/// # Returns
33/// A tuple containing the the normalization of `v`, and its derivative.
34#[inline(always)]
35pub fn normalize_with_derivative(v: Vector2d, dv: Vector2d) -> (Vector2d, Vector2d) {
36    let mag = v.magnitude();
37    let s = v / mag;
38    let ds = (dv - s * s.dot(dv)) / mag;
39    (s, ds)
40}
traffic_sim/math/util.rs

traffic_sim/math/
util.rs
