@@ -48,17 +48,20 @@ impl Graph {
48
48
49
49
/// returns closes point of given long & lat
50
50
pub fn get_point_id ( & self , lat : f32 , long : f32 ) -> usize {
51
- let lat_grid = ( lat * GRID_MULTIPLICATOR as f32 ) as i32 ;
52
- let lng_grid = ( long * GRID_MULTIPLICATOR as f32 ) as i32 ;
53
51
let mut min_distance: f32 = std:: f32:: MAX ;
54
52
let mut min_distance_id: usize = 0 ;
55
-
56
- for i in 0 ..self . nodes . len ( ) {
57
- let distance =
58
- calc_distance ( lat, long, self . nodes [ i] . latitude , self . nodes [ i] . longitude ) ;
59
- if distance < min_distance {
60
- min_distance = distance;
61
- min_distance_id = i;
53
+ let adjacent_nodes = self . get_adjacent_node_ids ( lat, long) ;
54
+ for node_id in adjacent_nodes {
55
+ match self . nodes . get ( node_id) {
56
+ Some ( node) => {
57
+ let distance =
58
+ calc_distance ( lat, long, node. latitude , node. longitude ) ;
59
+ if distance < min_distance {
60
+ min_distance = distance;
61
+ min_distance_id = node_id;
62
+ }
63
+ } ,
64
+ None => continue
62
65
}
63
66
}
64
67
return min_distance_id;
@@ -81,8 +84,37 @@ impl Graph {
81
84
}
82
85
}
83
86
84
- fn get_adjacent_grids ( current_grid : ( usize , usize ) , grid : & HashMap < ( usize , usize ) , Vec < usize > > ) {
85
-
87
+ /// returns node_ids in adjacent grid cells
88
+ /// goes from most inner cell to cells with distance 1 to n until a node is found
89
+ fn get_adjacent_node_ids ( & self , lat : f32 , lng : f32 ) -> Vec < usize > {
90
+ let lat_grid = ( lat * GRID_MULTIPLICATOR as f32 ) as usize ;
91
+ let lng_grid = ( lng * GRID_MULTIPLICATOR as f32 ) as usize ;
92
+ let mut node_ids = Vec :: < usize > :: new ( ) ;
93
+ match self . grid . get ( & ( lat_grid, lng_grid) ) {
94
+ Some ( adjacent_node_ids) => node_ids. extend ( adjacent_node_ids) ,
95
+ None => ( )
96
+ }
97
+ let mut in_dist: usize = 1 ;
98
+ loop {
99
+ for x in lat_grid-in_dist..lat_grid+in_dist+1 {
100
+ for y in lng_grid-in_dist..lng_grid+in_dist+1 {
101
+ if ( x < lat_grid+in_dist || x > lat_grid-in_dist) && ( y < lng_grid-in_dist || y > lng_grid+in_dist) {
102
+ // both coordinates are bigger or smaller than the outer bounds => in inner square => already investigated
103
+ continue ;
104
+ }
105
+ match self . grid . get ( & ( x, y) ) {
106
+ Some ( adjacent_node_ids) => node_ids. extend ( adjacent_node_ids) ,
107
+ None => continue
108
+ }
109
+ }
110
+ }
111
+ if node_ids. len ( ) > 0 {
112
+ return node_ids
113
+ } else {
114
+ // search in next level
115
+ in_dist += 1 ;
116
+ }
117
+ }
86
118
}
87
119
88
120
/// executes dijkstra
0 commit comments