124 lines
3.2 KiB
Rust
124 lines
3.2 KiB
Rust
use crate::point::{Color, Point};
|
|
use crate::transformer::Transformers;
|
|
use crate::worldstate::WorldState;
|
|
|
|
use serde::{Deserialize, Serialize};
|
|
use std::f64::consts::PI;
|
|
//use std::cmp::min;
|
|
|
|
/// Angle Optimisation
|
|
|
|
#[derive(Serialize, Deserialize, Debug, Clone, Copy)]
|
|
pub struct AngleOptimisation {
|
|
coef: f64,
|
|
//pps: u16,
|
|
}
|
|
|
|
fn color_not_zero(p1: &Point, p2: &Point, p3: &Point) -> bool {
|
|
let c0 = Color { r: 0, g: 0, b: 0 };
|
|
if p1.color == c0 || p2.color == c0 || p3.color == c0 {
|
|
false
|
|
} else {
|
|
true
|
|
}
|
|
}
|
|
|
|
fn pos_different(p1: &Point, p2: &Point) -> bool {
|
|
if p1.x == p2.x && p1.y == p2.y {
|
|
false
|
|
} else {
|
|
true
|
|
}
|
|
}
|
|
|
|
/*
|
|
* Le temps a attendre a un angle est toujours le meme, peu importe les kpps.
|
|
* Donc le nombre de point a rajouter est proportionelle a la vitesse du laser.
|
|
*
|
|
* kpps /
|
|
* */
|
|
|
|
fn color_same(p1: &Point, p2: &Point) -> bool {
|
|
if p1.color.r == p2.color.r && p1.color.g == p2.color.g && p1.color.b == p2.color.b {
|
|
true
|
|
} else {
|
|
false
|
|
}
|
|
}
|
|
|
|
// quand on rajoute un point, on le rejoute ou et avec quelle couleur
|
|
// -> j'ai l'impression qu'on le rajoute surtout a l'arriver
|
|
|
|
fn get_prev(pl: &[Point], id: usize) -> (Option<&Point>, usize) {
|
|
for i in (0..id).rev() {
|
|
if pos_different(&pl[id], &pl[i]) {
|
|
return (Some(&pl[i]), id - i);
|
|
}
|
|
}
|
|
(None, id - 0)
|
|
}
|
|
|
|
fn get_next(pl: &[Point], id: usize) -> (Option<&Point>, usize) {
|
|
for i in id..(pl.len() - 1) {
|
|
if pos_different(&pl[id], &pl[i]) {
|
|
return (Some(&pl[i]), i - id);
|
|
}
|
|
}
|
|
(None, pl.len() - id - 1)
|
|
}
|
|
|
|
fn max(v1: f64, v2: f64) -> f64 {
|
|
if v1 > v2 {
|
|
v1
|
|
} else {
|
|
v2
|
|
}
|
|
}
|
|
|
|
impl Transformers for AngleOptimisation {
|
|
fn apply(&self, pl: &[Point], _ws: &WorldState) -> Vec<Point> {
|
|
let c0 = Color { r: 0, g: 0, b: 0 };
|
|
let mut v = vec![];
|
|
|
|
let to_add_max = _ws.kpps as f64 / self.coef; // for 180 deg
|
|
let to_add_min = max(to_add_max / 1.5, 2.); // for 180 deg
|
|
|
|
v.push(pl[0]); // push first
|
|
for i in 1..(pl.len() - 1) {
|
|
let node = &pl[i];
|
|
if let ((Some(prev), dist_prev), (Some(next), dist_next)) =
|
|
(get_prev(pl, i), get_next(pl, i))
|
|
{
|
|
// on push plusisieur fois le point que l'on regarde
|
|
// - on calcul l'angle
|
|
// - on en deduis le ratio de nombre a appliquer
|
|
// - on regarde si il y a suffisement de point avant
|
|
// - on rajoute les point manquant
|
|
let d1 = Point::diff(node, prev).normalize();
|
|
let d2 = Point::diff(next, node).normalize();
|
|
let angle = (d1.cross(&d2) as f64).acos();
|
|
let to_add = ((to_add_max - to_add_min) * (angle / PI) + to_add_min) as usize;
|
|
println!("\nangle: {}", (angle / PI * 180.));
|
|
dbg!(to_add);
|
|
dbg!(to_add_max);
|
|
for _ in 0..to_add {
|
|
v.push(*node);
|
|
}
|
|
} else {
|
|
v.push(*node);
|
|
};
|
|
//v.push(*node); // push node
|
|
}
|
|
v.push(pl[pl.len() - 1]); // push last
|
|
|
|
v
|
|
}
|
|
}
|
|
|
|
/*
|
|
* |\
|
|
* | \
|
|
* | \
|
|
* | |
|
|
* */
|