From 25b5b10ad02dac439b6abf500d21c2c5baf11998 Mon Sep 17 00:00:00 2001 From: Jens Date: Thu, 21 Mar 2024 10:02:04 +0100 Subject: [PATCH] feat(assets): junction_sdf back in action --- .gitignore | 7 +- gbpplanner-rs/assets/imgs/junction_sdf.png | Bin 0 -> 88865 bytes gbpplanner-rs/src/config/environment.rs | 8 +- gbpplanner-rs/src/config/formation.rs | 137 +-------------------- gbpplanner-rs/src/config/geometry.rs | 100 +++++++++++++++ gbpplanner-rs/src/config/mod.rs | 1 + gbpplanner-rs/src/planner/spawner.rs | 2 +- 7 files changed, 116 insertions(+), 139 deletions(-) create mode 100644 gbpplanner-rs/assets/imgs/junction_sdf.png create mode 100644 gbpplanner-rs/src/config/geometry.rs diff --git a/.gitignore b/.gitignore index 3e582a4d..c27d3eee 100644 --- a/.gitignore +++ b/.gitignore @@ -5,8 +5,11 @@ # pixi environments .pixi -*.png *.dot -screenshot_*.png +# factorgraphs.png +# screenshot_*.png +**/*.png +!**/assets/**/* + .bacon-locations diff --git a/gbpplanner-rs/assets/imgs/junction_sdf.png b/gbpplanner-rs/assets/imgs/junction_sdf.png new file mode 100644 index 0000000000000000000000000000000000000000..cbb5204857f8bad389867842d34f7069aa7a9ecd GIT binary patch literal 88865 zcmeHQ3s_TEwmv>;v05c+HCl(5Sfv@O7^$sMAr@nmNMVc}TcZ_g)T+4_G~@UPA!?Z+ z)F?hu9!`*3YoxY~RNIu=5#j?AEflmcp(8d?kw%Lckw+lUyY@*?r_;GJ-#2sb?VYak zp;2j}R(b9&|{6g7&97ryZF zrXIIXe`}t_4yP{eUpa@9-|A259J5ELVjU~)Xg@KfQX=WgF3+wRMlq$~d(1kz z768}gQOba#4Aiio8VA)lsK!Ax&d*hi(_R=e<tg$Lv16i~JI3Kv-q6kERxXyUHMH9-OnH^jYV#}>g|U~+9mmet#8(cN-d9V8QR+ED zuD*J-UMN(DQ9N__c9)I+hGDY#=3L)J$+(v4D*pGD6CF00f$w0x=dwvYG)}@yu({B> zaGc<@f)gbA6ZDK^+XVs%#9fFW5pZMZfRPr)j2L1gS%8oTsVF4Xkb^`v^`9r7wKNY(m4nB$6jCD+$2AiZ#=oNYN2c=&n+GZCqSmd36K$0=PDIW zqg5x@TvDVM7z|F&Q;vQV9BCtwmAZIlo!nq_t^AH0|F~(qAtU^HW7`VNG3!*vNf$eV zDHU-Yi$zC_e&wGebGdwnR;=M=TrI^+u({B>aGc<@f)gbA6ZDJ-BoGoIf<(ZLp~JsC z(mFo>-Lu_V01WLgNX3{Q*$E_bkd{Ig5BW%>Z;^;bA{vQkB%+asMj{%CXe6SMh(;nB ziD)FEk%<166VXUMBl(QvGm_6gJ@UCyph*3Arls#wk7>7a#^y1lcjmMPOwSJ+JTh+1 z(^q5I?H|-_yd*9d{f=1uzVSKptFi*Q%@ZD{Y1xp)`+fLHR~27o;Jql@sTOg$V@6)@ zL-qE2Tw3z^pDhwuRlHEDu_F8q6FDQK zZMH=+qg5=b;_KVW4r$j{>9rD>%;1VOk2=VANMx+-Jaa_9-cn7>$dO`^eh6ocVJFWj zap+vJ8R62j*OqD6y`8195glkB##eCSv?pA{>v(k(Q+oT_)!pJ>>ci*B);_TKhNeN%{_IW8 z0=ZQp)mTR`-VuvMQ_3Hw!a818q^$cS8=~-iYV*Ud8ZV9F>UDCi-g;TmRGQ6Guc|s> zy_^Y6W$AgEDING|rdG`T9Vk(y8l#o(C>_WyXC~(|rD3NPPTeQg5zi4yK#b?-W zQZqgtSg+Xe2jxe`?{vav%{NO+tZNK zGZj;&9MX!o!dl6b4-NwD{Gz=3o#|HQd&U=qdaZWQfOWm7?aFU#i>}<38YVlQI>>v` zS!!G$8?V2in=f+C4J|r+X5HeH_UG)EA2oKcp{b6|fc`&wnyr6X#6D=f%#<27RBd+7 z4C9&W#+aFLcWu)g9~sR|!(`4c^@p^n>L;o2+h;p$*H^M&CsQP2CvS_>V3f$?JCrpkVV^#6bXT)p@)hS6$f$_(-<1uIU?E1U4pF{Y~wjCtHK`iz-Z zsI5y`pSgxU?|Bto|DLsq7XOAcykC3viofLNiq{wnMw^HEmijxL%%CJq^$h9%)YLg# z94f)XUw>LdC2yd2N4FLLhYb!J^e~BB^f2gQf~W2v7(+0IU<~6Hj9V~n!4Mxqd<^k1 z#78y^*)U|okT^!-7>Q$CW5G2RTw}rINL-G@SS8GwBDC zC!fn{ml7pfi^t{shx77wISOf=Sev6zmGfhAp&3GuCMET`ui~n7ngV_cOO@v77@u*s z!IE9qXvt==Gg9njz7`7$zWn2VE41?n0^ZiTWR=g|KF;B7byT|=9o2<=N3tp1*It(l zUnc!P;FZ>OZx+a}_7VA&CB@|HXsj3p(x^N2B`NF-mLUD)l452=}4Zo02{w_yxvdQoFIN_f~v?#GV?||PK2-@6@ZE8sqRNV`=ksfQv7C`n9ah>b> zveTepvjzT6C>5z9^b+XdzU=j|gyyW2eX4)JjEuzyR}*%uM$M@9i%Me7an*T zzK0gr2tvQ6Z!(wO4jg-Qh>zDpXr@7xDZ|MmtYAK&Z7w#B;#PKtZ+d|210 zVK=t*zi51O?0c`y8-8y2SXGB>+YIX4Dc>4i={54I?!X)4xBl^*%C%87o1$Erq%Fs- zv9YmJ9kEJxTUV=N#lo_5-w3I}ATiTxGzow7cr+>cFgW#5TJfRCjv^_n@wf z_j^nMMNjJUH`2tS{97$8muM{{*!GHrIm`yII;p1OIQ(}EefB0~x<8(K?x9Sjn&(PM zrt7OL>%5&UQoBq0HJz`gEYR7~Fl85LFdKco(wC(D`3&mRx7SsjnW|CLR)slIuLa3s z>)pFob&4%p+-zkfde}Ez<~ony=WA_o%EY+`F)Qj2ybrJ(IU^w z7RzjIV{>yeJW^a->`5stD}!3MHa8oz_n;EASWa|{4kDdZM0R;roGU_{qEvU2R_lNj&zJQJ0EFF(QNfh-+Y z&YpdX%(G(;+;mNhKK)Ksm@gvVTnn#xPfttJii6p-x!qe5_>Pj267iQ%MT61!+}Qju z>gs_OL7-JIm^xL^u81cL>qN?$gm}o4ggKa60Y%m2G$L-tFwXsZ+Se zJFYY|G(f5IYyTom>6#jg#p3ZOpaV)oT&L5?VzJha=uXSuWEuo%Nmp`~t(?5t@D)jL z3f~dIVim}C*KCk>8m$^ z4fz`O?f(>XsMBJ+IRzS?L^io4%TGlQUYxJK$l6_#N7Q*!mEHvf^f9v+=$;9HnNI_I z7pxr1S-C*O`y`f}Rok%~B*xzljvlAa%U9>+-%li*!5OrN_j`5QeCUA%Nkk(n$U4uQ z87fIti^^+JL5n5#47|O+?Sif?{w43YHcMXq)h~q^{yguOdF~?x?$rtR#`UIB1D&Mr z`chYmx2t9qv>noZy>Dtu@x{VrE4Pmu%y7@YH?EZdxb%FMT9`pU3T~!1b$8gAN@4jhtvGEP zsnoQfH3WXaU~^7PH1F;we2N1%y>c1`4t&;;o8iZl2{)7b`vmV-^@981rz-~T4?Z&N z3|J97l6$uD*%qO&GC?@~Qh8-t$?L(I)T_f@n#9ZN@HP?kGJUwBn&cPX7k9$ z5V|1Y6y@bbNXZA69{FsF>Xc45kAjc+!D3L|u(!nte@F>yxWw=Pc+nxUPPNiD8?eX^^pi z!|CtN8vi5HSqt+mn23^sb1O`!tiPUDOE3w8qde^Bh#uKkyyh9Kh3!hPM=dY%{{(hLlBSW4tQkXH8c$s&Dx2o+n8@V+zfA#e& z;*B;P%*wx&a+aRT+e#RW{mVJTCFQT8=YH9>y1G}OOtRMrn&4cw&m=CXGC|cVSU%G> za7FOrjo=Idhfdr@ziSYe0E9>miyq8K{*|cZ5rRXUAw0HGn4!LLhBxt2f7Qnrg-0N`XnEeLp7SV(wv1p%M!S^4jXgUzrt`1C+^7=s2c?jwNtJ}iO| z(^x26u3-ngda9$-?r*k3yZc+>Un2XV3L?@g5()0!-zijeCvtvuLaECtDTqz~Fp=!t z$)nNru0VTo;2i>eNl8A8m#saVnVD(1z<1FYQWp^sQF+L?U@-Z%#u>s1F#ujTlMdSy ze}GWORwVwC#*#{RXXmQkm~Yv^)9T3v;Kd^)W*g8Aqe3qp*OuEN9+Ja1@tkHGWDhtvFi zK5V~p`~6Eqn-z)*eqef|4osBByky5$A!!3h`+o!e`x^uRt78CGizB6#XWIbs!=AM? z2zn}RK_~+|@O(8jHD{M=<`L_>9~27Jt8}n~uLe+mRo9eV5CO7^;ID=$>#G3EvF6Lx z>VoL{%@%?8tN?5=reU901bgo`Z`zbv6%!K!fhftKA_ZcZu9_lA5LV&x-yAi@{z~j}Oha%~6%I`pRAOc*H(+JcA z&zzjzHOAc8-c+|@VJwvSRt*Gg#3~jfr5)@Sgo49v)D}saXnPGso`mIy(YFEHLdU%> zrAn#v0b>)2u_A*(Z@^~?JP+YV^nu!t`NiIESqSTQ^8D-H<^gHb4$TJ9{T8@_;aqL` z;MbPY@cN5UH4t+etx#tO4+%X~R+b*jTT3eLX=-W$!UpV<4)1}Bxd7I=aR;y=BGSr{ zlZTT{h$o7Qpwh9*u6v}?riAM1YEuHCOzt&Sbcfw!kLCVN_v^fT1Q`YrG6p=rNdTur zD!q2g=FMHt0kPl%o=u?grzsTU=q;Vny(Hoqs&coH&*3B{o(@-R>=vg0j0yNBWjArPmA{w?WjX`usth@E0$LOIV2OD;>x;`#4YL_v+4zK9hF0`PC1 zIb>kj+1WrgY!QoE9#|NepM#NZ$ixn5%aopud-sm7t*@x40E$a2f)Y&GHM+Y5MZDkB zCHRIplWuaF;UJqb$v7fIH5YOOGi&T>tB5j{GfhCOK#LN3AjrWk_(A@mou<&?WaJ9c zgC$t+?r2^lx&`eHAsh_j`}=iw(=9(v50m+U76>#S>^psZVGgipN1NPfk~*;P;)>(+ zunKNJHfXPBO$hqH*LK6Wf|s=f_iHvx=^!2K`D|@e(b{_G^)jbt-AKMeEP}Y$s$faa z(q zfACiax&-xF@nx5f=8wAf@Ght44am#!BYC}6=cxJOD9^lN;X|DE-)K)=>$;JivLfwg zYZd&p^*NXkHXRy6Yz94MaU9{yKu&f*W6+|T=sxD-ZyH1RZ#P`;)&dyO+lCGsL7)Or z(~m=098-UHOkvX@;C~>?!EuC>>V4f58Uq>w8Uyk|$aEohhY>m)!H7g|=pfF&8KM7d zq6x3+_UQ|Ug}iX;qDDAm%m;x$ziAv{-~5PcN4NNf=1wh&mWK{gRk1=#80O0Rr}oF@ z4vJIPeJu)OFG);(=#0yr6z8y3eY17cLG2(;&lwLLsOlCAKDCOxpU8K)*Bvc&!HKPW zI2sf)!RA8i!g0bglk;<3IIZ9WiT(sVqnd|6f{(Zh5hMa`kU(MRfRWa}U(7h~!wogv zS^$iM(ch!LN5%*VHX?aOe~BXQ6l-ZD}kCkLw{7DgDc=MFt1 zdPek&=o!&7qGv?Uh@KJ2XXNT})xm*~2q6(dqMs@xDjM2#o1#Xf#tUC~8COqn9T=C* zgF+`HbCAqIM_ zoFId|F8UMnjOZB=cR6*4AQ5n5=zx*dKNK_idMs$uk0`jr8brYrKPypqG;Znk{{l<- B_WA$- literal 0 HcmV?d00001 diff --git a/gbpplanner-rs/src/config/environment.rs b/gbpplanner-rs/src/config/environment.rs index 1924a6ca..5eadd941 100644 --- a/gbpplanner-rs/src/config/environment.rs +++ b/gbpplanner-rs/src/config/environment.rs @@ -1,6 +1,8 @@ use bevy::ecs::system::Resource; use serde::{Deserialize, Serialize}; +use super::geometry::Shape; + #[derive(Debug, Serialize, Deserialize)] #[serde(rename_all = "kebab-case")] pub struct Grid(Vec); @@ -35,9 +37,9 @@ pub struct Cell { pub col: usize, } -// #[derive(Debug, Serialize, Deserialize)] -// #[serde(rename_all = "kebab-case")] -// pub struct shapes(Vec<(Cell, Shape)>); +#[derive(Debug, Serialize, Deserialize)] +#[serde(rename_all = "kebab-case")] +pub struct shapes(Vec<(Cell, Shape)>); /// **Bevy** [`Resource`] /// The en diff --git a/gbpplanner-rs/src/config/formation.rs b/gbpplanner-rs/src/config/formation.rs index 48af3003..d4bb3ff4 100644 --- a/gbpplanner-rs/src/config/formation.rs +++ b/gbpplanner-rs/src/config/formation.rs @@ -8,6 +8,9 @@ use serde::{Deserialize, Serialize}; use typed_floats::StrictlyPositiveFinite; use unit_interval::UnitInterval; +use super::geometry::Shape; +use crate::{line, polygon}; + #[derive(Debug, Serialize, Deserialize)] #[serde(rename_all = "kebab-case")] pub enum PlacementStrategy { @@ -16,139 +19,6 @@ pub enum PlacementStrategy { Map, } -/// A relative point within the boudaries of the map. -/// ... -#[derive(Debug, Serialize, Deserialize, Clone, Copy)] -pub struct RelativePoint { - pub x: UnitInterval, - pub y: UnitInterval, -} - -impl RelativePoint { - /// Create a new `RelativePoint` from a pair of values. - /// Returns an error if either `x` or `y` is not in the interval [0.0, 1.0]. - pub fn new(x: f64, y: f64) -> Result { - Ok(Self { - x: UnitInterval::new(x)?, - y: UnitInterval::new(y)?, - }) - } - - /// Returns the x and y values as a tuple - pub fn get(&self) -> (f64, f64) { - (self.x.get(), self.y.get()) - } -} - -impl TryFrom<(f64, f64)> for RelativePoint { - type Error = unit_interval::UnitIntervalError; - - fn try_from(value: (f64, f64)) -> Result { - Ok(Self { - x: UnitInterval::new(value.0)?, - y: UnitInterval::new(value.1)?, - }) - } -} - -// #[derive(Debug, thiserror::Error)] -// pub enum PointError { -// #[error("x is out of bounds: {0}")] -// XOutOfBounds(#[from] unit_interval::UnitIntervalError), -// #[error("y is out of bounds: {0}")] -// YOutOfBounds(#[from] unit_interval::UnitIntervalError), -// #[error("both x and y are out of bounds: x: {0}, y: {1}")] -// BothOutOfBounds(f64, f64), -// } -// -// impl From for bevy::math::Vec2 { -// fn from(value: Point) -> Self { -// Self { -// x: value.x, -// y: value.y, -// } -// } -// } -// -// impl From<&Point> for bevy::math::Vec2 { -// fn from(value: &Point) -> Self { -// Self { -// x: value.x, -// y: value.y, -// } -// } -// } -// -// impl From<(f32, f32)> for Point { -// fn from(value: (f32, f32)) -> Self { -// Self { -// x: value.0, -// y: value.1, -// } -// } -// } - -#[derive(Debug, Serialize, Deserialize)] -#[serde(rename_all = "kebab-case")] -pub enum Shape { - Circle { - radius: StrictlyPositiveFinite, - center: RelativePoint, - }, - Polygon(OneOrMore), - Line((RelativePoint, RelativePoint)), -} - -impl Shape { - /// Returns `true` if the shape is [`Polygon`]. - /// - /// [`Polygon`]: Shape::Polygon - #[must_use] - pub fn is_polygon(&self) -> bool { - matches!(self, Self::Polygon(..)) - } - - /// Returns `true` if the shape is [`Circle`]. - /// - /// [`Circle`]: Shape::Circle - #[must_use] - pub fn is_circle(&self) -> bool { - matches!(self, Self::Circle { .. }) - } - - pub fn as_polygon(&self) -> Option<&OneOrMore> { - if let Self::Polygon(v) = self { - Some(v) - } else { - None - } - } -} - -/// Shorthand to construct `Shape::Polygon(vec![Point {x: $x, y: $y}, ... ])` -#[macro_export] -macro_rules! polygon { - [$(($x:expr, $y:expr)),+ $(,)?] => {{ - let vertices = vec![ - $( - RelativePoint::new($x, $y).expect("both x and y are within the interval [0.0, 1.0]") - ),+ - ]; - Shape::Polygon(OneOrMore::new(vertices).expect("at least one vertex")) - }} -} - -/// Shorthand to construct `Shape::Line((Point {x: $x1, y: $y1}, Point {x: $x2, -/// y: $y2}))` -#[macro_export] -macro_rules! line { - [($x1:expr, $y1:expr), ($x2:expr, $y2:expr)] => { - // Shape::Line((Point { x: $x1, y: $y1 }, Point { x: $x2, y: $y2 })) - // Shape::Line((Point { x: ($x1 as f64).try_from().unwrap(), y: ($y1 as f64).try_from().unwrap() }, Point { x: ($x2 as f64).try_from().unwrap(), y: f64::try_from().unwrap() })) - Shape::Line((RelativePoint::new($x1, $y1).expect("both x1 and y1 are within the interval [0.0, 1.0]"), RelativePoint::new($x2, $y2).expect("both x2 and y2 are within the interval [0.0, 1.0]"))) - }; -} - #[derive(Debug, Serialize, Deserialize)] #[serde(rename_all = "kebab-case")] pub struct Waypoint { @@ -409,6 +279,7 @@ mod tests { use pretty_assertions::assert_eq; use super::*; + use crate::polygon; // fn float_eq(lhs: f32, rhs: f32) -> bool { // f32::abs(lhs - rhs) <= f32::EPSILON diff --git a/gbpplanner-rs/src/config/geometry.rs b/gbpplanner-rs/src/config/geometry.rs new file mode 100644 index 00000000..e4d0b84b --- /dev/null +++ b/gbpplanner-rs/src/config/geometry.rs @@ -0,0 +1,100 @@ +use min_len_vec::OneOrMore; +use serde::{Deserialize, Serialize}; +use typed_floats::StrictlyPositiveFinite; +use unit_interval::UnitInterval; + +/// A relative point within the boudaries of the map. +/// ... +#[derive(Debug, Serialize, Deserialize, Clone, Copy)] +pub struct RelativePoint { + pub x: UnitInterval, + pub y: UnitInterval, +} + +impl RelativePoint { + /// Create a new `RelativePoint` from a pair of values. + /// Returns an error if either `x` or `y` is not in the interval [0.0, 1.0]. + pub fn new(x: f64, y: f64) -> Result { + Ok(Self { + x: UnitInterval::new(x)?, + y: UnitInterval::new(y)?, + }) + } + + /// Returns the x and y values as a tuple + pub fn get(&self) -> (f64, f64) { + (self.x.get(), self.y.get()) + } +} + +impl TryFrom<(f64, f64)> for RelativePoint { + type Error = unit_interval::UnitIntervalError; + + fn try_from(value: (f64, f64)) -> Result { + Ok(Self { + x: UnitInterval::new(value.0)?, + y: UnitInterval::new(value.1)?, + }) + } +} + +#[derive(Debug, Serialize, Deserialize)] +#[serde(rename_all = "kebab-case")] +pub enum Shape { + Circle { + radius: StrictlyPositiveFinite, + center: RelativePoint, + }, + Polygon(OneOrMore), + Line((RelativePoint, RelativePoint)), +} + +impl Shape { + /// Returns `true` if the shape is [`Polygon`]. + /// + /// [`Polygon`]: Shape::Polygon + #[must_use] + pub fn is_polygon(&self) -> bool { + matches!(self, Self::Polygon(..)) + } + + /// Returns `true` if the shape is [`Circle`]. + /// + /// [`Circle`]: Shape::Circle + #[must_use] + pub fn is_circle(&self) -> bool { + matches!(self, Self::Circle { .. }) + } + + pub fn as_polygon(&self) -> Option<&OneOrMore> { + if let Self::Polygon(v) = self { + Some(v) + } else { + None + } + } +} + +/// Shorthand to construct `Shape::Polygon(vec![Point {x: $x, y: $y}, ... ])` +#[macro_export] +macro_rules! polygon { + [$(($x:expr, $y:expr)),+ $(,)?] => {{ + let vertices = vec![ + $( + crate::config::geometry::RelativePoint::new($x, $y).expect("both x and y are within the interval [0.0, 1.0]") + ),+ + ]; + Shape::Polygon(OneOrMore::new(vertices).expect("at least one vertex")) + }} +} + +/// Shorthand to construct `Shape::Line((Point {x: $x1, y: $y1}, Point {x: $x2, +/// y: $y2}))` +#[macro_export] +macro_rules! line { + [($x1:expr, $y1:expr), ($x2:expr, $y2:expr)] => { + // Shape::Line((Point { x: $x1, y: $y1 }, Point { x: $x2, y: $y2 })) + // Shape::Line((Point { x: ($x1 as f64).try_from().unwrap(), y: ($y1 as f64).try_from().unwrap() }, Point { x: ($x2 as f64).try_from().unwrap(), y: f64::try_from().unwrap() })) + Shape::Line((crate::config::geometry::RelativePoint::new($x1, $y1).expect("both x1 and y1 are within the interval [0.0, 1.0]"), crate::config::geometry::RelativePoint::new($x2, $y2).expect("both x2 and y2 are within the interval [0.0, 1.0]"))) + }; +} diff --git a/gbpplanner-rs/src/config/mod.rs b/gbpplanner-rs/src/config/mod.rs index 3c5d3314..a0ceb07f 100644 --- a/gbpplanner-rs/src/config/mod.rs +++ b/gbpplanner-rs/src/config/mod.rs @@ -1,5 +1,6 @@ pub mod environment; pub mod formation; +pub mod geometry; use std::num::NonZeroUsize; diff --git a/gbpplanner-rs/src/planner/spawner.rs b/gbpplanner-rs/src/planner/spawner.rs index 38cd6715..53965d56 100644 --- a/gbpplanner-rs/src/planner/spawner.rs +++ b/gbpplanner-rs/src/planner/spawner.rs @@ -10,7 +10,7 @@ use super::robot::VariableTimestepsResource; use crate::{ asset_loader::SceneAssets, config::{ - formation::{RelativePoint, Shape}, + geometry::{RelativePoint, Shape}, Config, Formation, FormationGroup, }, planner::robot::RobotBundle,