Bounding boxes and road stroke path memoization #19

lib/descartes/src/intersect.rs

@@ -1,4 +1,4 @@
-use super::{N, P2, RoughlyComparable, Curve, FiniteCurve, THICKNESS, WithUniqueOrthogonal};
+use super::{N, V2, P2, RoughlyComparable, Curve, FiniteCurve, THICKNESS, WithUniqueOrthogonal};
 use super::primitives::{Line, Circle, Segment};
 use super::nalgebra::{Dot, Norm};
 use super::path::Path;
@@ -128,10 +128,46 @@ impl<'a> Intersect for (&'a Circle, &'a Circle) {
     }
 }
 
+struct BoundingBox {
+    min: P2,
+    max: P2
+}
+
+impl BoundingBox{
+    fn overlaps(&self, other: &BoundingBox) -> bool {
+        self.max.x >= other.min.x && other.max.x >= self.min.x
+        && self.max.y >= other.min.y && other.max.y >= self.min.y
+    }
+}
+
+trait HasBoundingBox {
+    fn bounding_box(&self) -> BoundingBox;
+}
+
+impl HasBoundingBox for Segment {
+    fn bounding_box(&self) -> BoundingBox {
+        if self.is_linear() {
+            BoundingBox{
+                min: P2::new(self.start.x.min(self.end.x), self.start.y.min(self.end.y)),
+                max: P2::new(self.start.x.max(self.end.x), self.start.y.max(self.end.y)),
+            }
+        } else {
+            let half_diagonal = V2::new(self.radius(), self.radius());
+            BoundingBox{
+                min: self.center() - half_diagonal,
+                max: self.center() + half_diagonal
+            }
+        }
+    }
+}
+
 // TODO: optimize: use something better than .includes()
 impl<'a> Intersect for (&'a Segment, &'a Segment) {
     fn intersect(&self) -> Vec<Intersection> {
         let (a, b) = *self;
+        if !a.bounding_box().overlaps(&b.bounding_box()) {
+            return vec![]
+        }
         match (a.is_linear(), b.is_linear()) {
             (true, true) => (
                     &Line{start: a.start(), direction: a.start_direction()},

src/game/lanes_and_cars/planning/current_plan_rendering.rs

@@ -21,7 +21,7 @@ impl Recipient<RenderToScene> for CurrentPlan {
         RenderToScene{renderer_id, scene_id} => {
             if self.ui_state.dirty {
                 let thing : Thing = self.current_plan_result_delta.trimmed_strokes.to_create.values()
-                    .filter(|stroke| stroke.nodes.len() > 1)
+                    .filter(|stroke| stroke.nodes().len() > 1)
                     .map(RoadStroke::preview_thing)
                     .sum();
                 renderer_id << UpdateThing{

src/game/lanes_and_cars/planning/mod.rs

@@ -84,21 +84,21 @@ impl Recipient<PlanControl> for CurrentPlan {
                     } else {
                         let new_current_nodes = current_nodes.clone().iter().map(|&RoadStrokeNodeRef(stroke_idx, node_idx)| {
                             let stroke = &mut self.delta.new_strokes[stroke_idx];
-                            let node = stroke.nodes[node_idx];
+                            let node = stroke.nodes()[node_idx];
                             let relative_position_in_basis = (node.position - previous_add).to_basis(node.direction);
 
-                            if node_idx == stroke.nodes.len() - 1 {
+                            if node_idx == stroke.nodes().len() - 1 {
                                 // append
                                 let new_direction = Segment::arc_with_direction(previous_add, node.direction, position).end_direction();
-                                stroke.nodes.push(RoadStrokeNode{
+                                stroke.nodes_mut().push(RoadStrokeNode{
                                     position: position + relative_position_in_basis.from_basis(new_direction),
                                     direction: new_direction
                                 });
-                                RoadStrokeNodeRef(stroke_idx, stroke.nodes.len() - 1)
+                                RoadStrokeNodeRef(stroke_idx, stroke.nodes().len() - 1)
                             } else if node_idx == 0 {
                                 // prepend
                                 let new_direction = -Segment::arc_with_direction(previous_add, -node.direction, position).end_direction();
-                                stroke.nodes.insert(0, RoadStrokeNode{
+                                stroke.nodes_mut().insert(0, RoadStrokeNode{
                                     position: position + relative_position_in_basis.from_basis(new_direction),
                                     direction: new_direction
                                 });
@@ -118,36 +118,36 @@ impl Recipient<PlanControl> for CurrentPlan {
         PlanControl::MoveRoadStrokeNodesTo(ref node_refs, handle_from, handle_to) =>  {
             for &RoadStrokeNodeRef(stroke_idx, node_idx) in node_refs {
                 let stroke = &mut self.delta.new_strokes[stroke_idx];
-                let node = stroke.nodes[node_idx];
-                if node_idx == stroke.nodes.len() - 1 {
-                    let previous_node = stroke.nodes[node_idx - 1];
+                let node = stroke.nodes()[node_idx];
+                if node_idx == stroke.nodes().len() - 1 {
+                    let previous_node = stroke.nodes()[node_idx - 1];
                     let relative_position_in_basis = (node.position - handle_from).to_basis(node.direction);
                     let previous_node_relative_position = relative_position_in_basis.from_basis(previous_node.direction);
                     let imaginary_previous_add = previous_node.position - previous_node_relative_position;
                     let new_direction = Segment::arc_with_direction(imaginary_previous_add, previous_node.direction, handle_to).end_direction();
                     let new_position = handle_to + relative_position_in_basis.from_basis(new_direction);
                     if (previous_node.position - new_position).norm() > MIN_NODE_DISTANCE {
-                        stroke.nodes[node_idx].position = new_position;
-                        stroke.nodes[node_idx].direction = new_direction
+                        stroke.nodes_mut()[node_idx].position = new_position;
+                        stroke.nodes_mut()[node_idx].direction = new_direction
                     }
                 } else if node_idx == 0 {
-                    let next_node = stroke.nodes[node_idx + 1];
+                    let next_node = stroke.nodes()[node_idx + 1];
                     let relative_position_in_basis = (node.position - handle_from).to_basis(node.direction);
                     let next_node_relative_position = relative_position_in_basis.from_basis(next_node.direction);
                     let imaginary_next_add = next_node.position - next_node_relative_position;
                     let new_direction = -Segment::arc_with_direction(imaginary_next_add, -next_node.direction, handle_to).end_direction();
                     let new_position = handle_to + relative_position_in_basis.from_basis(new_direction);
                     if (next_node.position - new_position).norm() > MIN_NODE_DISTANCE {
-                        stroke.nodes[node_idx].position = new_position;
-                        stroke.nodes[node_idx].direction = new_direction
+                        stroke.nodes_mut()[node_idx].position = new_position;
+                        stroke.nodes_mut()[node_idx].direction = new_direction
                     }
                 } else {
-                    let previous_node = stroke.nodes[node_idx - 1];
-                    let next_node = stroke.nodes[node_idx + 1];
+                    let previous_node = stroke.nodes()[node_idx - 1];
+                    let next_node = stroke.nodes()[node_idx + 1];
                     let new_position = node.position + (handle_to - handle_from);
                     if (previous_node.position - new_position).norm() > MIN_NODE_DISTANCE
                     && (next_node.position - new_position).norm() > MIN_NODE_DISTANCE {
-                        stroke.nodes[node_idx].position = new_position;
+                        stroke.nodes_mut()[node_idx].position = new_position;
                     }
                 }
             }
@@ -158,7 +158,7 @@ impl Recipient<PlanControl> for CurrentPlan {
         PlanControl::MaybeMakeCurrent(ref node_refs, handle_position) => {
             let strokes = &mut self.delta.new_strokes;
             let is_first_or_last = |&RoadStrokeNodeRef(stroke_idx, node_idx)| {
-                node_idx == 0 || node_idx == strokes[stroke_idx].nodes.len() - 1
+                node_idx == 0 || node_idx == strokes[stroke_idx].nodes().len() - 1
             };
 
             if node_refs.iter().all(is_first_or_last) {

src/game/lanes_and_cars/planning/plan_result_steps.rs

@@ -55,7 +55,7 @@ pub fn trim_strokes_and_add_incoming_outgoing(strokes: &CVec<RoadStroke>, inters
         let new_strokes = strokes_todo.iter().flat_map(|&(stroke_ref, ref stroke)| {
             let stroke_path = stroke.path();
             let mut maybe_trimmed_strokes = intersections.iter_mut().filter_map(|intersection| {
-                let intersection_points = (&stroke_path, &intersection.shape).intersect();
+                let intersection_points = (stroke_path, &intersection.shape).intersect();
                 if intersection_points.len() >= 2 {
                     let entry_distance = intersection_points.iter().map(|p| OrderedFloat(p.along_a)).min().unwrap();
                     let exit_distance = intersection_points.iter().map(|p| OrderedFloat(p.along_a)).max().unwrap();
@@ -77,16 +77,16 @@ pub fn trim_strokes_and_add_incoming_outgoing(strokes: &CVec<RoadStroke>, inters
                         (None, None) => None
                     }
                 } else if intersection_points.len() == 1 {
-                    if intersection.shape.contains(stroke.nodes[0].position) {
+                    if intersection.shape.contains(stroke.nodes()[0].position) {
                         let exit_distance = intersection_points[0].along_a;
                         if let Some(after_intersection) = stroke.cut_after(exit_distance + 0.05) {
-                            intersection.outgoing.insert(stroke_ref, after_intersection.nodes[0]);
+                            intersection.outgoing.insert(stroke_ref, after_intersection.nodes()[0]);
                             Some(vec![(stroke_ref, after_intersection)])
                         } else {None}
-                    } else if intersection.shape.contains(stroke.nodes.last().unwrap().position) {
+                    } else if intersection.shape.contains(stroke.nodes().last().unwrap().position) {
                         let entry_distance = intersection_points[0].along_a;
                         if let Some(before_intersection) = stroke.cut_before(entry_distance - 0.05) {
-                            intersection.incoming.insert(stroke_ref, *before_intersection.nodes.last().unwrap());
+                            intersection.incoming.insert(stroke_ref, *before_intersection.nodes().last().unwrap());
                             Some(vec![(stroke_ref, before_intersection)])
                         } else {None}
                     } else {None}
@@ -186,12 +186,11 @@ pub fn find_transfer_strokes(trimmed_strokes: &CVec<RoadStroke>) -> Vec<RoadStro
                 }
             );
             // TODO: connect consecutive aligned segments
-            aligned_paths.map(|segment| RoadStroke{
-                nodes: vec![
+            aligned_paths.map(|segment| RoadStroke::new(vec![
                     RoadStrokeNode{position: segment.start(), direction: segment.start_direction()},
                     RoadStrokeNode{position: segment.end(), direction: segment.end_direction()},
                 ].into()
-            }).collect::<Vec<_>>()
+            )).collect::<Vec<_>>()
         }).collect::<Vec<_>>()
     }).collect::<Vec<_>>()
 }

src/game/lanes_and_cars/planning/road_stroke.rs

@@ -8,7 +8,8 @@ use super::super::{Lane, TransferLane, AdvertiseForConnectionAndReport};
 
 #[derive(Compact, Clone)]
 pub struct RoadStroke{
-    pub nodes: CVec<RoadStrokeNode>
+    nodes: CVec<RoadStrokeNode>,
+    _memoized_path: CPath
 }
 
 #[derive(Copy, Clone, PartialEq, Eq)]
@@ -24,17 +25,32 @@ impl RoadStroke {
         if nodes.len() <= 1 {
             panic!("Invalid stroke")
         }
-        RoadStroke{nodes: nodes}
+        RoadStroke{nodes: nodes, _memoized_path: CPath::new(vec![])}
     }
 
-    pub fn path(&self) -> CPath {
-        Path::new(self.nodes.windows(2).flat_map(|window|
-            Segment::biarc(window[0].position, window[0].direction, window[1].position, window[1].direction)
-        ).collect::<Vec<_>>())
+    pub fn nodes(&self) -> &CVec<RoadStrokeNode> {
+        &self.nodes
+    }
+
+    pub fn nodes_mut(&mut self) -> &mut CVec<RoadStrokeNode> {
+        &mut self.nodes
+    }
+
+    pub fn path(&self) -> &CPath {
+        // TODO: replace by proper Option
+        if self._memoized_path.segments().len() == 0 {
+            // TODO: maybe there is something less damn dangerous
+            #[allow(mutable_transmutes)]
+            let unsafe_memoized_path : &mut CPath = unsafe{::std::mem::transmute(&self._memoized_path)};
+            *unsafe_memoized_path = Path::new(self.nodes.windows(2).flat_map(|window|
+                Segment::biarc(window[0].position, window[0].direction, window[1].position, window[1].direction)
+            ).collect::<Vec<_>>())
+        }
+        &self._memoized_path
     }
 
     pub fn preview_thing(&self) -> Thing {
-        band_to_thing(&Band::new(Band::new(self.path(), 3.0).outline(), 0.3), 0.0)
+        band_to_thing(&Band::new(Band::new(self.path().clone(), 3.0).outline(), 0.3), 0.0)
     }
 
     #[allow(needless_lifetimes)]
@@ -103,7 +119,7 @@ impl RoadStroke {
 
     pub fn build(&self, report_to: ID, report_as: BuildableRef) {
         Swarm::<Lane>::all() << CreateWith(
-            Lane::new(self.path(), match report_as {
+            Lane::new(self.path().clone(), match report_as {
                 BuildableRef::Intersection(_) => true,
                 _ => false,
             }),
@@ -113,7 +129,7 @@ impl RoadStroke {
 
     pub fn build_transfer(&self, report_to: ID, report_as: BuildableRef) {
         Swarm::<TransferLane>::all() << CreateWith(
-            TransferLane::new(self.path()),
+            TransferLane::new(self.path().clone()),
             AdvertiseForConnectionAndReport(report_to, report_as)
         );
     }

src/main.rs

@@ -6,8 +6,8 @@
 #![allow(no_effect, unnecessary_operation)]
 // Enable this for memory tracking with Instruments/MacOS
 // and for much better stacktraces for memory issues
-// #![feature(alloc_system)]
-// extern crate alloc_system;
+#![feature(alloc_system)]
+extern crate alloc_system;
 
 extern crate ordered_float;
 extern crate itertools;