今回はrustの勉強としてplottersでgifアニメーションを作成します．具体的には以下のように迷路を解くA*アルゴリズムの描画をします．plottersのバージョンは0.3.1です．
A*アルゴリズム
A*アルゴリズムとは，ダイクストラ法に推定値を導入して一般化したものです．アルゴリズムの詳細はwikipediaを参照してください．実装についてはwikipediaの実際に使われている実装の部分とrustで優先度付きキューとして利用できるbinary_heapのexampleにあるダイクストラ法の実装を参考にしました．まず，計算に必要なNodeとQueueiItemの定義です．NodeはOpenかCloseであるかのタグstate，ヒューリスティックコストh，実コストを含めた全体のコストf，親のidparent_idを保持します．color_stateは描画のために持つ色の情報です．隣接ノードや移動コストは別に持ちます．
fが最小のidと対応するNodeを探索していくため優先度付きキューが必要ですが，rustで利用できるbinary_heapはOrdトレイトが実装されているものしか利用できません．f32はNaNを含むためOrdは実装されておらず，PartialOrdを用いて無理やり実装します．get_min_pathはゴールが見つかった場合に親を辿ってpathに入れていきます．
use std::cmp::Ordering;
use std::collections::BinaryHeap;
use std::collections::HashMap;
const INF: f32 = 10_i32.pow(5) as f32;
#[derive(Debug)]
enum State{ // OpenかCloseを表す
Open,
Close,
}
// Nodeを表す
#[derive(Debug)]
struct Node {
state: State,
h: f32, // ヒューリスティックコスト
f: f32, // トータルのコスト
parent_id: Option<u32>, // 親ノードのid
color_state: ColorState // セルの色を示す
}
// 優先度付きキューの要素(f32はOrdを実装していないため)
#[derive(PartialEq)]
struct QueueItem {
f: f32,
id: u32,
}
// QueueItemのふるまい
impl Eq for QueueItem{
}
impl PartialOrd for QueueItem {
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
Some(self.cmp(other))
}
}
impl Ord for QueueItem {
fn cmp(&self, other: &Self) -> Ordering {
other.f.partial_cmp(&self.f).unwrap() // nanでパニック
.then_with(||other.id.cmp(&self.id)) // fが同じならldで
}
}
// ゴールのノードから再帰してスタートまで親を辿る
fn get_min_path(all_nodes: &HashMap<u32, Node>, id: u32, path: &mut Vec<u32>) {
match all_nodes[&id].parent_id{
None => { return (); }, // 何もしない
Some(parent_id) => {
path.insert(0, parent_id);
return get_min_path(all_nodes, parent_id, path);
}
}
}
以下がアルゴリズムを記述した部分です．parse_strは文字列をパースしてidをキーNodeを値とするHashMapであるall_nodes，同じくidをキーとして隣接ノードに対応するidのVecを値とするadjacent_nodes，それと対応した移動コストを値とするtravel_costsとスタートとゴールのidを返す関数であり後述します．draw_mazeは描画を行う関数で，plotting_areaはルートのDrawingAreaであり後述します．root.presentは1フレームをgifとして保存します．
wikipediaにもありますが，Openの優先度付きキューopen_priory_queにはfを更新するようなNodeと対応するQueueItemを重複を許して挿入します．open_priory_queでは最もfの小さいQueueItemがpopされるため，重複したidのNodeがOpenの状態で取得されることはありません（探索されたNodeはClose状態になります）．wikipediaにある隣接ノードmに対する条件と処理はここでは以下のようになります．
- mがOpenリストにもCloseリストにも含まれていない場合
fをINFで初期化するため，最初にmに到達した段階でfが更新され，open_priory_queに挿入されます - mがOpenリストにある場合・mかCloseリストにある場合
fを更新する場合にopen_priory_queに重複を許して挿入します， そして状態をOpenにします．
let str_maze = vec![
vec!["0", "0", "0", "0", "0","0", "0", "0", "0", "0"],
vec!["0", "s", "1", "1", "0","1", "0", "1", "0", "1"],
vec!["0", "0", "1", "0", "1","0", "0", "1", "0", "0"],
vec!["1", "0", "1", "0", "0","0", "0", "1", "1", "0"],
vec!["0", "0", "0", "1", "0","1", "1", "1", "0", "0"],
vec!["0", "0", "0", "0", "0","1", "0", "0", "0", "0"],
vec!["1", "0", "1", "1", "1","1", "0", "0", "0", "0"],
vec!["1", "0", "0", "1", "0","0", "0", "1", "0", "0"],
vec!["0", "1", "0", "0", "0","0", "0", "1", "g", "0"],
vec!["0", "1", "0", "0", "0","1", "1", "1", "0", "0"]
];
let (mut all_nodes, adjacent_nodes, travel_costs, s, g) = parse_str(&str_maze)?;
let maze_size: (usize, usize) = (str_maze.len(), str_maze[0].len()); // i,j
// 初期の描画
draw_maze(&all_nodes, &plotting_area, maze_size)?;
root.present()?;
// a_starアルゴリズム
let mut open_priory_que: BinaryHeap<QueueItem> = BinaryHeap::new(); // (f, id) の優先度付きキュー
let mut path: Vec<u32> = Vec::new(); // 最適解のパス
// スタートノードの追加
let f = all_nodes[&s].h; // 最初はg=0
open_priory_que.push(QueueItem{f, id:s});
while let Some(QueueItem {f:n_f, id:n_id}) = open_priory_que.pop() { // 最小値を取得
let mut n = all_nodes.get_mut(&n_id).unwrap(); // 探索するノード
if let State::Close = n.state { continue; } // クローズだった場合
let n_h = n.h;
n.state = State::Close; // nを探索済み
if n.color_state != ColorState::Start && n.color_state != ColorState::Goal{
n.color_state = ColorState::Search; // 現在探索中
}
// ゴールだった場合
if n_id == g {
path.insert(0, n_id); // ゴールノードのidを追加
get_min_path(&all_nodes, n_id, &mut path);
break;
}
for (m_id, n_to_m_cost) in (*adjacent_nodes[&n_id]).into_iter().zip((*travel_costs[&n_id]).into_iter()) {
if *m_id == s { continue; } // mがスタートの場合
let mut m = all_nodes.get_mut(&m_id).unwrap(); // 隣接ノード
let pre_m_f = m.f;
let g_n = n_f - n_h; // nまでの移動コスト
let m_f = g_n + n_to_m_cost + m.h;
// 新しいfの方が小さいとき
if m_f < pre_m_f{
m.f = m_f;
open_priory_que.push(QueueItem {f:m_f, id:*m_id}); // 重複を許してOpenに挿入
m.state = State::Open; // mを未探索とする
m.parent_id = Some(n_id);
if m.color_state != ColorState::Goal && m.color_state != ColorState::Start {
m.color_state = ColorState::Opened
}
}
}
// 探索中の描画
draw_maze(&all_nodes, &plotting_area, maze_size)?;
root.present()?;
// 探索中のセルを解除
let mut n = all_nodes.get_mut(&n_id).unwrap(); // 探索したノード
if n.color_state != ColorState::Start && n.color_state != ColorState::Goal{
n.color_state = ColorState::Opened; // Opened
}
}
println!("solution_path:{:?}", path);
文字列のパース
先ほどの迷路をパースするのに利用するparse_strは以下となります．idは左上から右下へ何番目かを表す整数を与えています．Nodeの初期値としてfにはINF，parent_idにはNone，stateにはOpenを与えています．
// ユークリッド距離を計算
fn euclidean_dist((i, j):(f32, f32), (k, l):(f32, f32)) -> f32{
((i-k)*(i-k) + (j-l)*(j-l)).sqrt()
}
// マンハッタン距離を計算
fn manhattan_dist((i,j):(f32, f32), (k,l):(f32, f32)) -> f32{
(i-k).abs() + (j-l).abs()
}
// 文字列の迷路のパース
fn parse_str(str_maze:&Vec<Vec<&str>>) -> Result<(HashMap<u32, Node>, HashMap<u32, Vec<u32>>, HashMap<u32, Vec<f32>>, u32, u32), String>{
let mut all_nodes: HashMap<u32, Node> = HashMap::new();
let mut adjacent_nodes: HashMap<u32, Vec<u32>> = HashMap::new();
let mut travel_costs: HashMap<u32, Vec<f32>> = HashMap::new();
let mut s: u32 = 0;
let mut exist_start = false; // スタートの存在判定に利用するフラッグ
let mut g: u32 = 0;
let mut exist_end = false; // ゴールの存在判定に利用するフラッグ
let one_row_len = str_maze[0].len();
let one_col_len = str_maze.len();
// 次元が正しいかチェック
for strs in str_maze.iter() {
if strs.len() != one_row_len { return Err("This str_maze has wrong dimmension.".to_string()); }
}
for i in 0..one_col_len {
for j in 0..one_row_len {
// id
let id = (i * one_row_len + j) as u32;
let mut color_state = ColorState::Normal;
// str_maze[i][j]の文字列のパース
if str_maze[i][j] == "s"{
s = id;
exist_start = true;
color_state = ColorState::Start;
} else if str_maze[i][j] == "g" {
g = id;
exist_end = true;
color_state = ColorState::Goal;
} else if str_maze[i][j] == "0" {
} else if str_maze[i][j] == "1" {
color_state = ColorState::Block;
} else { return Err("This str_maze has wrong str".to_string()); }
// all_nodesへの追加
let node = Node{state:State::Open, h:INF, f:INF, parent_id:None, color_state};
all_nodes.insert(id, node);
// adjacent_nodesへの追加, travel_costsへの挿入
let mut n_adjacent_nodes: Vec<u32> = Vec::new();
let mut n_travel_costs: Vec<f32> = Vec::new();
let adjacents = vec![(-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 1), (1, -1), (1, 0), (1, 1)]; // 隣接ノードの相対インデックス
for (k, l) in adjacents.iter() {
let adj_i:i32 = i as i32 + k;
let adj_j:i32 = j as i32 + l;
if 0 <= adj_i && adj_i < one_col_len as i32 && 0 <= adj_j && adj_j < one_row_len as i32 {
let adj_id = (adj_i as usize * one_row_len + adj_j as usize) as u32;
n_adjacent_nodes.push(adj_id); // 隣接ノードのidを挿入
if str_maze[adj_i as usize][adj_j as usize] == "1" { // 壁がある場合
n_travel_costs.push(INF);
} else {
n_travel_costs.push(1.0);
}
}
}
adjacent_nodes.insert(id, n_adjacent_nodes);
travel_costs.insert(id, n_travel_costs);
}
}
// スタートかエンドがない場合の処理
if !(exist_start && exist_end) { return Err("This str_maze does not heve 's' or 'g'.".to_string()); }
// ヒューリスティックコストの計算
for i in 0..one_col_len {
for j in 0..one_row_len {
// id
let id = (i * one_row_len + j) as u32;
let g_i = g as usize / one_row_len;
let g_j = g as usize % one_row_len;
let mut n = all_nodes.get_mut(&id).unwrap();
//n.h = euclidean_dist((g_i as f32, g_j as f32), (i as f32, j as f32)); // a_star
n.h = manhattan_dist((g_i as f32, g_j as f32), (i as f32, j as f32)); // a_star
//n.h = 0.0_f32; // dijktra
}
}
Ok((all_nodes, adjacent_nodes, travel_costs, s, g))
}
描画
描画はbackendをgifメソッドで作ること以外は通常のplottersの描画と変わりません．以下はグラフの設定です．BigMapBackend::gifの第三引数は1フレーム当たりの時間です．座標はf32じゃなくても大丈夫です（文字を描画しようとした名残です）．
//let out_file = "plotters-doc-data/dijkstra_maze_animation.gif";
let out_file = "plotters-doc-data/a_star_maze_animation.gif";
let root = BitMapBackend::gif(out_file, (200, 200), 500)?.into_drawing_area();
root.fill(&WHITE)?;
let mut chart = ChartBuilder::on(&root)
.margin(5)
.x_label_area_size(0)
.y_label_area_size(0)
.build_cartesian_2d(0 as f32..maze_size.0 as f32, 0 as f32..maze_size.1 as f32)?;
chart
.configure_mesh()
.disable_x_mesh()
.disable_y_mesh()
.disable_x_axis()
.disable_y_axis()
.draw()?;
let plotting_area = chart.plotting_area();
また，Nodeの色の状態を示すColorStateの定義は以下です．
#[derive(Debug, Clone, Copy, Eq, PartialEq)]
enum ColorState{ // 描画の色分けに利用
Normal, // 通常のセル
Block, // 壁のセル
Start, // スタートのセル
Goal, // ゴールのセル
Opened, // Openに入ったことのあるセル
Search, // 現在探索中のセル
Solution // 最適解のパスのセル
}
迷路を描画する関数は以下です．最初，探索時，結果を描画するのに呼ばれます．結果の描画は上では省いていますので後述します．
DrawingAreaに対してdrawメソッドで矩形をNodeのcolor_Stateに応じて描画しています．縦線と横線はPathElementで線を描画します．draw_seriesでもできますが，DrawingAreaに対するapiに統一したかったのでdrawをfor文で回しています．
use plotters::prelude::*;
use plotters::drawing::DrawingArea;
use plotters::coord::types::RangedCoordf32;
// 迷路の描画(上塗りしていく)
fn draw_maze(
all_nodes: &HashMap<u32, Node>,
plotting_area: &DrawingArea<BitMapBackend, Cartesian2d<RangedCoordf32, RangedCoordf32>>,
maze_size: (usize, usize)
) -> Result<(), Box<dyn std::error::Error>>{
// セルの描画
for id in all_nodes.keys() {
let i = (maze_size.0-1) - (*id as usize / maze_size.1);
let j = *id as usize % maze_size.1;
let node = &all_nodes[id];
let color = match node.color_state {
ColorState::Start => &YELLOW,
ColorState::Goal => &GREEN,
ColorState::Block => &BLACK,
ColorState::Opened => &BLUE,
ColorState::Search => &CYAN,
ColorState::Solution => &RED,
ColorState::Normal => &WHITE,
};
plotting_area.draw(&Rectangle::new(
[(j as f32, i as f32), (j as f32 + 1.0, i as f32 + 1.0)],
Into::<ShapeStyle>::into(color).filled()
))?;
}
// 縦線の描画
for i in 0..maze_size.1+1{
plotting_area.draw(&PathElement::new([(i as f32, 0.0_f32), (i as f32, maze_size.0 as f32)], &BLACK))?;
}
// 横線の描画
for i in 0..maze_size.0+1{
plotting_area.draw(&PathElement::new([(0_f32, i as f32), (maze_size.1 as f32, i as f32)], &BLACK))?;
}
Ok(())
}
先ほど述べたように，1フレームのgifへの保存はroot.presentメソッドを呼びます．root.fill(&WHITE)を呼ばないとグラフがリセットされないので，今回は全て上書きで塗っています．結果の描画は以下となります．少し長めになるようにroot.presentを何回か呼んでいます．
// 結果の描画
for id in path.into_iter() {
let mut n = all_nodes.get_mut(&id).unwrap();
if n.color_state != ColorState::Start && n.color_state != ColorState::Goal{
n.color_state = ColorState::Solution;
}
}
draw_maze(&all_nodes, &plotting_area, maze_size)?;
root.present()?;
// 追加で同じグラフを描画
root.present()?;
root.present()?;
println!("Result has been saved to {}", out_file);
Ok(())
グラフのサイズを小さくとればgifアニメーションを作るのもそれほど時間はかからないので，plottersはgifアニメーションをサクッと作るには向いていると思います．
プログラム全体
最後にプログラム全体を載せます．
use std::cmp::Ordering;
use std::collections::BinaryHeap;
use std::collections::HashMap;
use plotters::prelude::*;
use plotters::drawing::DrawingArea;
use plotters::coord::types::RangedCoordf32;
const INF: f32 = 10_i32.pow(5) as f32;
#[derive(Debug)]
enum State{ // OpenかCloseを表す
Open,
Close,
}
// Nodeを表す
#[derive(Debug)]
struct Node {
state: State,
h: f32, // ヒューリスティックコスト
f: f32, // トータルのコスト
parent_id: Option<u32>, // 親ノードのid
color_state: ColorState // セルの色を示す
}
// 優先度付きキューの要素(f32はOrdを実装していないため)
#[derive(PartialEq)]
struct QueueItem {
f: f32,
id: u32,
}
// QueueItemのふるまい
impl Eq for QueueItem{
}
impl PartialOrd for QueueItem {
fn partial_cmp(&self, other: &Self) -> Option<Ordering> {
Some(self.cmp(other))
}
}
impl Ord for QueueItem {
fn cmp(&self, other: &Self) -> Ordering {
other.f.partial_cmp(&self.f).unwrap() // nanでパニック
.then_with(||other.id.cmp(&self.id)) // fが同じならldで
}
}
// ゴールのノードから再帰してスタートまで親を辿る
fn get_min_path(all_nodes: &HashMap<u32, Node>, id: u32, path: &mut Vec<u32>) {
match all_nodes[&id].parent_id{
None => { return (); }, // 何もしない
Some(parent_id) => {
path.insert(0, parent_id);
return get_min_path(all_nodes, parent_id, path);
}
}
}
#[derive(Debug, Clone, Copy, Eq, PartialEq)]
enum ColorState{ // 描画の色分けに利用
Normal, // 通常のセル
Block, // 壁のセル
Start, // スタートのセル
Goal, // ゴールのセル
Opened, // Openに入ったことのあるセル
Search, // 現在探索中のセル
Solution // 最適解のパスのセル
}
// ユークリッド距離を計算
fn euclidean_dist((i, j):(f32, f32), (k, l):(f32, f32)) -> f32{
((i-k)*(i-k) + (j-l)*(j-l)).sqrt()
}
fn manhattan_dist((i,j):(f32, f32), (k,l):(f32, f32)) -> f32{
(i-k).abs() + (j-l).abs()
}
// 文字列の迷路のパース
fn parse_str(str_maze:&Vec<Vec<&str>>) -> Result<(HashMap<u32, Node>, HashMap<u32, Vec<u32>>, HashMap<u32, Vec<f32>>, u32, u32), String>{
let mut all_nodes: HashMap<u32, Node> = HashMap::new();
let mut adjacent_nodes: HashMap<u32, Vec<u32>> = HashMap::new();
let mut travel_costs: HashMap<u32, Vec<f32>> = HashMap::new();
let mut s: u32 = 0;
let mut exist_start = false; // スタートの存在判定に利用するフラッグ
let mut g: u32 = 0;
let mut exist_end = false; // ゴールの存在判定に利用するフラッグ
let one_row_len = str_maze[0].len();
let one_col_len = str_maze.len();
// 次元が正しいかチェック
for strs in str_maze.iter() {
if strs.len() != one_row_len { return Err("This str_maze has wrong dimmension.".to_string()); }
}
for i in 0..one_col_len {
for j in 0..one_row_len {
// id
let id = (i * one_row_len + j) as u32;
let mut color_state = ColorState::Normal;
// str_maze[i][j]の文字列のパース
if str_maze[i][j] == "s"{
s = id;
exist_start = true;
color_state = ColorState::Start;
} else if str_maze[i][j] == "g" {
g = id;
exist_end = true;
color_state = ColorState::Goal;
} else if str_maze[i][j] == "0" {
} else if str_maze[i][j] == "1" {
color_state = ColorState::Block;
} else { return Err("This str_maze has wrong str".to_string()); }
// all_nodesへの追加
let node = Node{state:State::Open, h:INF, f:INF, parent_id:None, color_state};
all_nodes.insert(id, node);
// adjacent_nodesへの追加, travel_costsへの挿入
let mut n_adjacent_nodes: Vec<u32> = Vec::new();
let mut n_travel_costs: Vec<f32> = Vec::new();
let adjacents = vec![(-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 1), (1, -1), (1, 0), (1, 1)]; // 隣接ノードの相対インデックス
for (k, l) in adjacents.iter() {
let adj_i:i32 = i as i32 + k;
let adj_j:i32 = j as i32 + l;
if 0 <= adj_i && adj_i < one_col_len as i32 && 0 <= adj_j && adj_j < one_row_len as i32 {
let adj_id = (adj_i as usize * one_row_len + adj_j as usize) as u32;
n_adjacent_nodes.push(adj_id); // 隣接ノードのidを挿入
if str_maze[adj_i as usize][adj_j as usize] == "1" { // 壁がある場合
n_travel_costs.push(INF);
} else {
n_travel_costs.push(1.0);
}
}
}
adjacent_nodes.insert(id, n_adjacent_nodes);
travel_costs.insert(id, n_travel_costs);
}
}
// スタートかエンドがない場合の処理
if !(exist_start && exist_end) { return Err("This str_maze does not heve 's' or 'g'.".to_string()); }
// ヒューリスティックコストの計算
for i in 0..one_col_len {
for j in 0..one_row_len {
// id
let id = (i * one_row_len + j) as u32;
let g_i = g as usize / one_row_len;
let g_j = g as usize % one_row_len;
let mut n = all_nodes.get_mut(&id).unwrap();
//n.h = euclidean_dist((g_i as f32, g_j as f32), (i as f32, j as f32)); // a_star
n.h = manhattan_dist((g_i as f32, g_j as f32), (i as f32, j as f32)); // a_star
//n.h = 0.0_f32; // dijktra
}
}
Ok((all_nodes, adjacent_nodes, travel_costs, s, g))
}
// 迷路の描画(上塗りしていく)
fn draw_maze(
all_nodes: &HashMap<u32, Node>,
plotting_area: &DrawingArea<BitMapBackend, Cartesian2d<RangedCoordf32, RangedCoordf32>>,
maze_size: (usize, usize)
) -> Result<(), Box<dyn std::error::Error>>{
// セルの描画
for id in all_nodes.keys() {
let i = (maze_size.0-1) - (*id as usize / maze_size.1);
let j = *id as usize % maze_size.1;
let node = &all_nodes[id];
let color = match node.color_state {
ColorState::Start => &YELLOW,
ColorState::Goal => &GREEN,
ColorState::Block => &BLACK,
ColorState::Opened => &BLUE,
ColorState::Search => &CYAN,
ColorState::Solution => &RED,
ColorState::Normal => &WHITE,
};
plotting_area.draw(&Rectangle::new(
[(j as f32, i as f32), (j as f32 + 1.0, i as f32 + 1.0)],
Into::<ShapeStyle>::into(color).filled()
))?;
}
// 縦線の描画
for i in 0..maze_size.1+1{
plotting_area.draw(&PathElement::new([(i as f32, 0.0_f32), (i as f32, maze_size.0 as f32)], &BLACK))?;
}
// 横線の描画
for i in 0..maze_size.0+1{
plotting_area.draw(&PathElement::new([(0_f32, i as f32), (maze_size.1 as f32, i as f32)], &BLACK))?;
}
Ok(())
}
fn main() -> Result<(), Box<dyn std::error::Error>> {
let str_maze = vec![
vec!["0", "0", "0", "0", "0","0", "0", "0", "0", "0"],
vec!["0", "s", "1", "1", "0","1", "0", "1", "0", "1"],
vec!["0", "0", "1", "0", "1","0", "0", "1", "0", "0"],
vec!["1", "0", "1", "0", "0","0", "0", "1", "1", "0"],
vec!["0", "0", "0", "1", "0","1", "1", "1", "0", "0"],
vec!["0", "0", "0", "0", "0","1", "0", "0", "0", "0"],
vec!["1", "0", "1", "1", "1","1", "0", "0", "0", "0"],
vec!["1", "0", "0", "1", "0","0", "0", "1", "0", "0"],
vec!["0", "1", "0", "0", "0","0", "0", "1", "g", "0"],
vec!["0", "1", "0", "0", "0","1", "1", "1", "0", "0"]
];
let (mut all_nodes, adjacent_nodes, travel_costs, s, g) = parse_str(&str_maze)?;
//let out_file = "plotters-doc-data/dijkstra_maze_animation.gif";
let out_file = "plotters-doc-data/a_star_maze_animation.gif";
let root = BitMapBackend::gif(out_file, (200, 200), 500)?.into_drawing_area();
root.fill(&WHITE)?;
let maze_size: (usize, usize) = (str_maze.len(), str_maze[0].len()); // i,j
let mut chart = ChartBuilder::on(&root)
.margin(5)
.x_label_area_size(0)
.y_label_area_size(0)
.build_cartesian_2d(0 as f32..maze_size.0 as f32, 0 as f32..maze_size.1 as f32)?;
chart
.configure_mesh()
.disable_x_mesh()
.disable_y_mesh()
.disable_x_axis()
.disable_y_axis()
.draw()?;
let plotting_area = chart.plotting_area();
// 初期の描画
draw_maze(&all_nodes, &plotting_area, maze_size)?;
root.present()?;
// a_starアルゴリズム
let mut open_priory_que: BinaryHeap<QueueItem> = BinaryHeap::new(); // (f, id) の優先度付きキュー
let mut path: Vec<u32> = Vec::new(); // 最適解のパス
// スタートノードの追加
let f = all_nodes[&s].h; // 最初はg=0
open_priory_que.push(QueueItem{f, id:s});
while let Some(QueueItem {f:n_f, id:n_id}) = open_priory_que.pop() { // 最小値を取得
let mut n = all_nodes.get_mut(&n_id).unwrap(); // 探索するノード
if let State::Close = n.state { continue; } // クローズだった場合
let n_h = n.h;
n.state = State::Close; // nを探索済み
if n.color_state != ColorState::Start && n.color_state != ColorState::Goal{
n.color_state = ColorState::Search; // 現在探索中
}
// ゴールだった場合
if n_id == g {
path.insert(0, n_id); // ゴールノードのidを追加
get_min_path(&all_nodes, n_id, &mut path);
break;
}
for (m_id, n_to_m_cost) in (*adjacent_nodes[&n_id]).into_iter().zip((*travel_costs[&n_id]).into_iter()) {
if *m_id == s { continue; } // mがスタートの場合
let mut m = all_nodes.get_mut(&m_id).unwrap(); // 隣接ノード
let pre_m_f = m.f;
let g_n = n_f - n_h; // nまでの移動コスト
let m_f = g_n + n_to_m_cost + m.h;
// 新しいfの方が小さいとき
if m_f < pre_m_f{
m.f = m_f;
open_priory_que.push(QueueItem {f:m_f, id:*m_id}); // 重複を許してOpenに挿入
m.state = State::Open; // mを未探索とする
m.parent_id = Some(n_id);
if m.color_state != ColorState::Goal && m.color_state != ColorState::Start {
m.color_state = ColorState::Opened
}
}
}
// 探索中の描画
draw_maze(&all_nodes, &plotting_area, maze_size)?;
root.present()?;
// 探索中のセルを解除
let mut n = all_nodes.get_mut(&n_id).unwrap(); // 探索したノード
if n.color_state != ColorState::Start && n.color_state != ColorState::Goal{
n.color_state = ColorState::Opened; // Opened
}
}
println!("solution_path:{:?}", path);
// 結果の描画
for id in path.into_iter() {
let mut n = all_nodes.get_mut(&id).unwrap();
if n.color_state != ColorState::Start && n.color_state != ColorState::Goal{
n.color_state = ColorState::Solution;
}
}
draw_maze(&all_nodes, &plotting_area, maze_size)?;
root.present()?;
// 追加で同じグラフを描画
root.present()?;
root.present()?;
println!("Result has been saved to {}", out_file);
Ok(())
}