com-croftsoft-core 0.14.0

A core library of highly reusable code
Documentation 
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161

// =============================================================================
//! - Trait implementations for the A* algorithm
//!
//! # Metadata
//! - Copyright: © 2022-2023 [`CroftSoft Inc`]
//! - Author: [`David Wallace Croft`]
//! - Java created: 2002-04-21
//! - Java updated: 2003-05-10
//! - Rust created: 2022-10-24
//! - Rust updated: 2023-06-25
//!
//! # History
//! - Adapted from the classes in the Java-based [`CroftSoft Core Library`]
//!   - com.croftsoft.core.ai.astar.GridCartographer
//!
//! [`CroftSoft Core Library`]: https://www.croftsoft.com/library/code/
//! [`CroftSoft Inc`]: https://www.croftsoft.com/
//! [`David Wallace Croft`]: https://www.croftsoft.com/people/david/
// =============================================================================

#[cfg(test)]
mod test;

use super::{
  structures::{GradientCartographer, GridCartographer, NodeInfo},
  traits::Cartographer,
};
use crate::math::geom::point_xy::PointXY;
use core::cmp::Ordering;
use core::f64::consts::TAU;

impl<N: PointXY> Cartographer<N> for GradientCartographer<N> {
  fn estimate_cost_to_goal(
    &self,
    node: &N,
  ) -> f64 {
    node.distance_xy(&self.goal_node)
  }

  fn get_adjacent_nodes(
    &self,
    node: &N,
  ) -> Vec<N> {
    let mut adjacent_list = Vec::new();
    let distance_to_goal: f64 = node.distance_xy(&self.goal_node);
    let distance_from_start: f64 = node.distance_xy(&self.start_node);
    let step_size =
      (distance_from_start / self.init_step_size).trunc() * self.init_step_size;
    let step_size = step_size.max(self.init_step_size);
    if distance_to_goal <= step_size {
      let x: f64 = self.goal_node.get_x();
      let y: f64 = self.goal_node.get_y();
      let goal_node_copy: N = (self.make_node_fn)(x, y);
      adjacent_list.push(goal_node_copy);
      return adjacent_list;
    }
    let x: f64 = node.get_x();
    let y: f64 = node.get_y();
    let heading_to_goal =
      (self.goal_node.get_y() - y).atan2(self.goal_node.get_x() - x);
    let directions_f64 = self.directions as f64;
    for i in 0..self.directions {
      let heading = heading_to_goal + (i as f64) * TAU / directions_f64;
      let step: N = (self.make_node_fn)(
        x + step_size * heading.cos(),
        y + step_size * heading.sin(),
      );
      if (self.is_space_available_fn)(&step) {
        adjacent_list.push(step);
      }
    }
    adjacent_list
  }

  fn get_cost_to_adjacent_node(
    &self,
    from_node: &N,
    to_node: &N,
  ) -> f64 {
    from_node.distance_xy(to_node)
  }

  fn is_goal_node(
    &self,
    node: &N,
  ) -> bool {
    self.goal_node.distance_xy(node) == 0.0
  }
}

impl<N: PointXY> Cartographer<N> for GridCartographer<N> {
  fn estimate_cost_to_goal(
    &self,
    node: &N,
  ) -> f64 {
    node.distance_xy(&self.goal_node)
  }

  fn get_adjacent_nodes(
    &self,
    node: &N,
  ) -> Vec<N> {
    let mut adjacent_list = Vec::new();
    let distance_to_goal: f64 = node.distance_xy(&self.goal_node);
    if distance_to_goal <= self.step_size {
      let x: f64 = self.goal_node.get_x();
      let y: f64 = self.goal_node.get_y();
      let goal_node_copy: N = (self.make_node_fn)(x, y);
      adjacent_list.push(goal_node_copy);
      return adjacent_list;
    }
    let x: f64 = node.get_x();
    let y: f64 = node.get_y();
    for ix in -1..2 {
      for iy in -1..2 {
        if ix == 0 && iy == 0 {
          continue;
        }
        let step: N = (self.make_node_fn)(
          ((x / self.step_size).trunc() + ix as f64) * self.step_size,
          ((y / self.step_size).trunc() + iy as f64) * self.step_size,
        );
        if (self.is_space_available_fn)(&step) {
          adjacent_list.push(step);
        }
      }
    }
    adjacent_list
  }

  fn get_cost_to_adjacent_node(
    &self,
    from_node: &N,
    to_node: &N,
  ) -> f64 {
    from_node.distance_xy(to_node)
  }

  fn is_goal_node(
    &self,
    node: &N,
  ) -> bool {
    self.goal_node.distance_xy(node) == 0.0
  }
}

impl Eq for NodeInfo {}

impl Ord for NodeInfo {
  fn cmp(
    &self,
    other: &Self,
  ) -> std::cmp::Ordering {
    if self.total_cost < other.total_cost {
      return Ordering::Less;
    } else if self.total_cost > other.total_cost {
      return Ordering::Greater;
    }
    Ordering::Equal
  }
}