///<summary> ///コンストラクタ ///<param name="Image">xamlにあるImageコントロール</param> /// </summary> public MainWindowViewModel(Image image) { station_ = new StationLayoutParam(); nodes_ = new List <Node>(); image_ = image; agents_ = new List <AgentBase>(); ekiin_ = new List <AgentBase>(); Bitmap = new RenderTargetBitmap( 1000, 1000, 96, 96, PixelFormats.Default); DrawVisual = new DrawingVisual(); LoadLayoutCommand = new ReactiveCommand(); LoadLayoutCommand.Subscribe(_ => OpenFile()); MoveCommand = new ReactiveCommand(); MoveCommand.Subscribe(_ => Move()); }
//中央線 private static bool CheckCollisionDeterminationWithAllBench(Point r1, Point r2, StationLayoutParam station) { bool t1, t2; foreach (var bench in station.Benchs) { //中央の線の上辺 var p1 = new Point(bench.PositionX - (bench.Width), bench.PositionY - (bench.Height / 2)); var p2 = new Point(bench.PositionX + (bench.Width), bench.PositionY - (bench.Height / 2)); t1 = CheckCollisionSide(r1, r2, p1, p2); t2 = CheckCollisionSide(p1, p2, r1, r2); if (t1 && t2) { return(true); } //中央の線の右辺 p1 = new Point(bench.PositionX + (bench.Width), bench.PositionY - (bench.Height / 2)); p2 = new Point(bench.PositionX + (bench.Width), bench.PositionY + (bench.Height / 2)); t1 = CheckCollisionSide(r1, r2, p1, p2); t2 = CheckCollisionSide(p1, p2, r1, r2); if (t1 && t2) { return(true); } //中央の線の下辺 p1 = new Point(bench.PositionX + (bench.Width), bench.PositionY + (bench.Height / 2)); p2 = new Point(bench.PositionX - (bench.Width), bench.PositionY + (bench.Height / 2)); t1 = CheckCollisionSide(r1, r2, p1, p2); t2 = CheckCollisionSide(p1, p2, r1, r2); if (t1 && t2) { return(true); } //中央の線の左辺 p1 = new Point(bench.PositionX - (bench.Width), bench.PositionY + (bench.Height / 2)); p2 = new Point(bench.PositionX - (bench.Width), bench.PositionY - (bench.Height / 2)); t1 = CheckCollisionSide(r1, r2, p1, p2); t2 = CheckCollisionSide(p1, p2, r1, r2); if (t1 && t2) { return(true); } } return(false); }
//駅員室 private static bool CheckCollisionDeterminationWithAllroom(Point r1, Point r2, StationLayoutParam station) { bool t1, t2; foreach (var room in station.Rooms) { //駅員室の上辺 var p1 = new Point(room.PositionX - (room.Width), room.PositionY - (room.Height / 2)); var p2 = new Point(room.PositionX + (room.Width), room.PositionY - (room.Height / 2)); t1 = CheckCollisionSide(r1, r2, p1, p2); t2 = CheckCollisionSide(p1, p2, r1, r2); if (t1 && t2) { return(true); } //駅員室の右辺 p1 = new Point(room.PositionX + (room.Width), room.PositionY - (room.Height / 2)); p2 = new Point(room.PositionX + (room.Width), room.PositionY + (room.Height / 2)); t1 = CheckCollisionSide(r1, r2, p1, p2); t2 = CheckCollisionSide(p1, p2, r1, r2); if (t1 && t2) { return(true); } //駅員室の下辺 p1 = new Point(room.PositionX + (room.Width), room.PositionY + (room.Height / 2)); p2 = new Point(room.PositionX - (room.Width), room.PositionY + (room.Height / 2)); t1 = CheckCollisionSide(r1, r2, p1, p2); t2 = CheckCollisionSide(p1, p2, r1, r2); if (t1 && t2) { return(true); } //駅員室の左辺 p1 = new Point(room.PositionX - (room.Width), room.PositionY + (room.Height / 2)); p2 = new Point(room.PositionX - (room.Width), room.PositionY - (room.Height / 2)); t1 = CheckCollisionSide(r1, r2, p1, p2); t2 = CheckCollisionSide(p1, p2, r1, r2); if (t1 && t2) { return(true); } } return(false); }
///<summary> ///辺と席との衝突判定 /// </summary> /// <param name="r1">辺の座標1</param> /// <param name="r2">辺の座標2</param> /// <returns> /// true:衝突してる /// false:衝突してない /// </returns> /// 改札 private static bool CheckCollisionDeterminationWithAllkaisatu(Point r1, Point r2, StationLayoutParam station) { bool t1, t2; foreach (var kaisatu in station.Kaisatus) { //改札の上辺 var p1 = new Point(kaisatu.PositionX - (kaisatu.Width / 2), kaisatu.PositionY - (kaisatu.Height / 2)); var p2 = new Point(kaisatu.PositionX + (kaisatu.Width / 2), kaisatu.PositionY - (kaisatu.Height / 2)); t1 = CheckCollisionSide(r1, r2, p1, p2); t2 = CheckCollisionSide(p1, p2, r1, r2); if (t1 && t2) { return(true); } //改札の右辺 p1 = new Point(kaisatu.PositionX + (kaisatu.Width / 2), kaisatu.PositionY - (kaisatu.Height / 2)); p2 = new Point(kaisatu.PositionX + (kaisatu.Width / 2), kaisatu.PositionY + (kaisatu.Height / 2)); t1 = CheckCollisionSide(r1, r2, p1, p2); t2 = CheckCollisionSide(p1, p2, r1, r2); if (t1 && t2) { return(true); } //改札の下辺 p1 = new Point(kaisatu.PositionX + (kaisatu.Width / 2), kaisatu.PositionY + (kaisatu.Height / 2)); p2 = new Point(kaisatu.PositionX - (kaisatu.Width / 2), kaisatu.PositionY + (kaisatu.Height / 2)); t1 = CheckCollisionSide(r1, r2, p1, p2); t2 = CheckCollisionSide(p1, p2, r1, r2); if (t1 && t2) { return(true); } //改札の左辺 p1 = new Point(kaisatu.PositionX - (kaisatu.Width / 2), kaisatu.PositionY + (kaisatu.Height / 2)); p2 = new Point(kaisatu.PositionX - (kaisatu.Width / 2), kaisatu.PositionY - (kaisatu.Height / 2)); t1 = CheckCollisionSide(r1, r2, p1, p2); t2 = CheckCollisionSide(p1, p2, r1, r2); if (t1 && t2) { return(true); } } return(false); }
public static bool IsColidedSomething(Node node1, Node node2, double tolerance, StationLayoutParam station) { ///<summary> ///ノード間を中心線とする短径とその辺と全ての席との衝突判定をチェックする関数 /// </summary> /// ///<remarks> /// /// ノード1○-----------○ノード2 /// /// </remarks> /// ///<param name="node1">ノード1</param> ///<param name="node2">ノード2</param> ///<param name="tolerance">許容範囲</param> ///<param name="station">駅のレイアウト</param> ///<returns> ///true:衝突している ///false:衝突していない /// </returns> var x1 = node1.X; var x2 = node2.X; var y1 = node1.Y; var y2 = node2.Y; double theta = Math.Atan2(y2 - y1, x2 - x1); var sin = tolerance * Math.Sin(theta); var cos = tolerance * Math.Cos(theta); //改札上辺 if (CheckCollisionDeterminationWithAllkaisatu( new Point(x1 - tolerance * Math.Sin(theta), y1 + tolerance * Math.Cos(theta)), new Point(x2 - tolerance * Math.Sin(theta), y2 + tolerance * Math.Cos(theta)), station)) { return(false); } //右辺 if (CheckCollisionDeterminationWithAllkaisatu( new Point(x2 - tolerance * Math.Sin(theta), y2 + tolerance * Math.Cos(theta)), new Point(x2 + tolerance * Math.Sin(theta), y2 - tolerance * Math.Cos(theta)), station)) { return(false); } //下辺 if (CheckCollisionDeterminationWithAllkaisatu( new Point(x2 + tolerance * Math.Sin(theta), y2 - tolerance * Math.Cos(theta)), new Point(x1 + tolerance * Math.Sin(theta), y1 - tolerance * Math.Cos(theta)), station)) { return(false); } //左辺 if (CheckCollisionDeterminationWithAllkaisatu( new Point(x1 + tolerance * Math.Sin(theta), y1 - tolerance * Math.Cos(theta)), new Point(x1 - tolerance * Math.Sin(theta), y1 + tolerance * Math.Cos(theta)), station)) { return(false); } //駅員室上辺 if (CheckCollisionDeterminationWithAllroom( new Point(x1 - tolerance * Math.Sin(theta), y1 + tolerance * Math.Cos(theta)), new Point(x2 - tolerance * Math.Sin(theta), y2 + tolerance * Math.Cos(theta)), station)) { return(false); } //右辺 if (CheckCollisionDeterminationWithAllroom( new Point(x2 - tolerance * Math.Sin(theta), y2 + tolerance * Math.Cos(theta)), new Point(x2 + tolerance * Math.Sin(theta), y2 - tolerance * Math.Cos(theta)), station)) { return(false); } //下辺 if (CheckCollisionDeterminationWithAllroom( new Point(x2 + tolerance * Math.Sin(theta), y2 - tolerance * Math.Cos(theta)), new Point(x1 + tolerance * Math.Sin(theta), y1 - tolerance * Math.Cos(theta)), station)) { return(false); } //左辺 if (CheckCollisionDeterminationWithAllroom( new Point(x1 + tolerance * Math.Sin(theta), y1 - tolerance * Math.Cos(theta)), new Point(x1 - tolerance * Math.Sin(theta), y1 + tolerance * Math.Cos(theta)), station)) { return(false); } //中央の線 //上辺 if (CheckCollisionDeterminationWithAllBench( new Point(x1 - tolerance * Math.Sin(theta), y1 + tolerance * Math.Cos(theta)), new Point(x2 - tolerance * Math.Sin(theta), y2 + tolerance * Math.Cos(theta)), station)) { return(false); } //右辺 if (CheckCollisionDeterminationWithAllBench( new Point(x2 - tolerance * Math.Sin(theta), y2 + tolerance * Math.Cos(theta)), new Point(x2 + tolerance * Math.Sin(theta), y2 - tolerance * Math.Cos(theta)), station)) { return(false); } //下辺 if (CheckCollisionDeterminationWithAllBench( new Point(x2 + tolerance * Math.Sin(theta), y2 - tolerance * Math.Cos(theta)), new Point(x1 + tolerance * Math.Sin(theta), y1 - tolerance * Math.Cos(theta)), station)) { return(false); } //左辺 if (CheckCollisionDeterminationWithAllBench( new Point(x1 + tolerance * Math.Sin(theta), y1 - tolerance * Math.Cos(theta)), new Point(x1 - tolerance * Math.Sin(theta), y1 + tolerance * Math.Cos(theta)), station)) { return(false); } return(true); }
/// <summary> /// Jsonファイルからレイアウト情報を読み込む関数 /// </summary> public void OpenFile() { // 駅員の座標 int ekiinX = 700; int ekiinY = 700; using (var fileDialog = new OpenFileDialog()) { fileDialog.Filter = "Json File(.json)|*.json"; fileDialog.Title = "開くファイルを選択してください"; fileDialog.Multiselect = false; //ダイアログを表示する if (fileDialog.ShowDialog() == DialogResult.OK) { using (var streamReader = new StreamReader(fileDialog.FileName)) { var json = streamReader.ReadToEnd(); station_ = JsonConvert.DeserializeObject <StationLayoutParam>(json); nodes_.Clear(); agents_.Clear(); ekiin_.Clear(); //ノードの設定 //改札 foreach (var kaisatu in station_.Kaisatus) { double distance = 25; SetUpNodes(new Node( kaisatu.PositionX - (kaisatu.Width / 2) - distance, kaisatu.PositionY - (kaisatu.Height / 2) - distance)); SetUpNodes(new Node( kaisatu.PositionX - (kaisatu.Width / 2) - distance, kaisatu.PositionY + (kaisatu.Height / 2) + distance)); } //駅員室やエレベーター foreach (var room in station_.Rooms) { double distance = 15; //左上 SetUpNodes(new Node( room.PositionX - (room.Width / 2) - distance, room.PositionY - (room.Height / 2) - distance)); //右上 SetUpNodes(new Node( room.PositionX + room.Width + (room.Width / 2) + distance, room.PositionY - (room.Height / 2) - distance)); //左下 SetUpNodes(new Node( room.PositionX - (room.Width / 2) - distance, room.PositionY + room.Height + (room.Height / 2) + distance)); //左上 SetUpNodes(new Node( room.PositionX + room.Width + (room.Width / 2) + distance, room.PositionY + room.Height + (room.Height / 2) + distance)); } //上に線がある階段 foreach (var stairsUp in station_.StairsUp) { double distance = 15; SetUpNodes(new Node( stairsUp.PositionX - distance, stairsUp.PositionY - distance)); SetUpNodes(new Node( stairsUp.PositionX + stairsUp.Width + distance, stairsUp.PositionY - distance)); SetUpNodes(new Node( stairsUp.PositionX - distance, stairsUp.PositionY + stairsUp.Height + distance)); SetUpNodes(new Node( stairsUp.PositionX + stairsUp.Width + distance, stairsUp.PositionY + stairsUp.Height + distance)); //仮ゴール nodes_.Add(new Node( stairsUp.PositionX + stairsUp.Width / 2, stairsUp.PositionY + stairsUp.Height / 4, NodeKind.Temporary1)); //真ん中下 SetUpNodes(new Node( stairsUp.PositionX + stairsUp.Width / 2, stairsUp.PositionY + stairsUp.Height + distance)); } //下に線がある階段 foreach (var stairsDown in station_.StairsDown) { double distance = 15; SetUpNodes(new Node( stairsDown.PositionX - distance, stairsDown.PositionY - distance)); SetUpNodes(new Node( stairsDown.PositionX + stairsDown.Width + distance, stairsDown.PositionY - distance)); SetUpNodes(new Node( stairsDown.PositionX - distance, stairsDown.PositionY + stairsDown.Height + distance)); SetUpNodes(new Node( stairsDown.PositionX + stairsDown.Width + distance, stairsDown.PositionY + stairsDown.Height + distance)); //真ん中上 SetUpNodes(new Node( stairsDown.PositionX + stairsDown.Width / 2, stairsDown.PositionY - distance)); //仮ゴール nodes_.Add(new Node( stairsDown.PositionX + stairsDown.Width / 2, stairsDown.PositionY + stairsDown.Height - distance, NodeKind.Temporary2)); } //出口の設定 foreach (var goal in station_.Goals) { nodes_.Add(new Node(goal.PositionX, goal.PositionY, NodeKind.Goal)); } //エージェントの設定(座標) agents_.Add(new AgentBase(800, 800)); ekiin_.Add(new AgentBase(ekiinX, ekiinY)); //全てのエージェントで移動可能かどうかを調べる foreach (var agent in agents_) { //最初にエージェントの現在地から移動可能な候補を探す foreach (var node in nodes_) { if (Djikstra.IsColidedSomething(agent.Node, node, 25, station_)) { agent.Node.NextNodes.Add(node); } } Node goalNode = null; while (true) { Node determinedNode = null; //ダイクストラ法で探索 agent.Node.DoDijikstra(agent.Node.NextNodes, ref determinedNode, agent, agents_); //狭い通路での探索 agent.Node.DoDijikstra(agent.Node.NextNodesWithNarrow, ref determinedNode, agent, agents_, true); //誘導範囲内 if (Math.Pow((ekiinX - agent.Node.X), 2) + Math.Pow((ekiinY - agent.Node.Y), 2) <= Math.Pow(200, 2)) { //ダイクストラ法で探索 agent.Node.DoDijikstra(agent.Node.NextNodes, ref determinedNode, agent, agents_); } //1Fにいるエージェントが階段を使って2Fにいく //上に線がある階段 if (determinedNode.NodeStatus == NodeKind.Temporary1) { agent.Node.X = 300; agent.Node.Y = 300; } //下に線がある階段 if (determinedNode.NodeStatus == NodeKind.Temporary2) { //エージェントの座標を上の階段に変更する agent.Node.X = 300; agent.Node.Y = 300; } //ゴールにたどり着けないエージェントがいるとき //エージェントの初期位置がしっかりしていれば起きない if (determinedNode == null) { throw new Exception("ゴールにいけません"); } //ゴールなら終了 if (determinedNode.NodeStatus == NodeKind.Goal) { goalNode = determinedNode.Clone(); break; } //ノードを確定ノードに determinedNode.NodeStatus = NodeKind.Determined; //確定ノードから移動可能なノードを全てNextNodesに保存 foreach (var node in nodes_) { if (determinedNode == node || node.NodeStatus == NodeKind.Determined) { continue; } if (Djikstra.IsColidedSomething(determinedNode, node, 25, station_)) { determinedNode.NextNodes.Add(node); } } } ///出口までの経路をエージェントに保持させる //この後レイアウト内のノードは全て初期化されるのでディープコピーしたものを渡す //ゴールを見つけた時にゴールノードとゴールまでに通るノードをディープコピーしてあるためここでCloneしなくてよい while (true) { agent.RouteNode.Add(goalNode); goalNode = goalNode.PreviousNode; if (goalNode.NodeStatus == NodeKind.Start) { agent.RouteNode.Reverse(); break; } } //ノードの初期化 foreach (var node in nodes_) { node.NextNodes.Clear(); if (node.NodeStatus != NodeKind.Goal) { node.NodeStatus = NodeKind.Unsearched; } node.PreviousNode = null; node.DistanceCost = double.MaxValue; } } DrawLayout(); } } } }