initial commit

.gitignore

@@ -0,0 +1,3 @@
+Debug/*
+*.cproject
+*.project

Agent.cpp

@@ -0,0 +1,146 @@
+#include <stdio.h>
+#include <algorithm>
+#include "Agent.h"
+
+#define SEARCH_DEPTH 1
+
+void Agent::reset()
+{
+	state = Agent::IDLE;
+	path.clear();
+	distIndexList.clear();
+}
+
+Direction Agent::calcNextDirection(const IndexVec &cur, const IndexVec &_dist)
+{
+	maze->updateStepMap(_dist);
+	const uint8_t curStep = maze->stepMap[cur.y][cur.x];
+	if (curStep == 255) return Direction(0);
+
+	Direction result(0);
+	int nFoundWall = 10;
+	const Direction cur_wall = maze->wall[cur.y][cur.x];
+	for (int i=0;i<4;i++) {
+		if (cur.canSum(IndexVec::vecDir[i])) {
+			IndexVec neighbor(cur + IndexVec::vecDir[i]);
+			if (!cur_wall[i] && maze->stepMap[neighbor.y][neighbor.x] < curStep ) {
+				//北優先
+				//return Direction(NORTH << i);
+				//未探索の壁優先
+				if (nFoundWall > maze->wall[neighbor.y][neighbor.x].nDoneWall()) {
+					nFoundWall = maze->wall[neighbor.y][neighbor.x].nDoneWall();
+					result = Direction(NORTH << i);
+				}
+			}
+		}
+	}
+	if (result) return result;
+
+	for (int i=0;i<4;i++) {
+		if (cur.canSum(IndexVec::vecDir[i])) {
+			IndexVec neighbor(cur + IndexVec::vecDir[i]);
+			if (!cur_wall[i] && maze->stepMap[neighbor.y][neighbor.x] == curStep ) {
+				//北優先
+				//return Direction(NORTH << i);
+				//未探索壁優先
+				if (nFoundWall > maze->wall[neighbor.y][neighbor.x].nDoneWall()) {
+					nFoundWall = maze->wall[neighbor.y][neighbor.x].nDoneWall();
+					result = Direction(NORTH << i);
+				}
+			}
+		}
+	}
+
+	return result;
+}
+
+
+void Agent::update(const IndexVec &cur, const Direction &cur_wall)
+{
+	maze->updateWall(cur, cur_wall);
+
+	if (state == Agent::IDLE) {
+		distIndexList.clear();
+		distIndexList.assign(mazeGoalList.begin(), mazeGoalList.end());
+
+		dist = distIndexList.front();
+		state = Agent::SEARCHING_NOT_GOAL;
+	}
+
+
+	if (state == Agent::SEARCHING_NOT_GOAL) {
+		for (auto it = distIndexList.begin();it!=distIndexList.end();) {
+			if (*it == cur){
+				it = distIndexList.erase(it);
+				continue;
+			}
+			it++;
+		}
+		if (distIndexList.empty()) {
+			state = Agent::SEARCHING_REACHED_GOAL;
+		}
+		else dist = distIndexList.front();
+	}
+
+
+	if (state == Agent::SEARCHING_REACHED_GOAL) {
+		//distでは無いが、distIndexListの一部を通過したからそれをリストから削除する
+		//TODO:distIndexListに含まれる壁情報が更新されることになるが、distIndexListは更新されない dist==curでなく、cur in distを条件にすべき
+		for (auto it = distIndexList.begin();it!=distIndexList.end();) {
+			if (*it == cur){
+				it = distIndexList.erase(it);
+				continue;
+			}
+			it++;
+		}
+		//TODO:到達不可能な壁がdistIndexListに入りっぱなしになっている可能性
+		//TODO:到達不可能な壁が連続で出てくる可能性 詰みマスに囲まれた場合はどうなるのだろう
+		if (dist == cur || calcNextDirection(cur, dist) == 0) {
+			distIndexList.clear();
+			path.calcKShortestDistancePath(IndexVec(0,0), mazeGoalList,SEARCH_DEPTH, false);
+			path.calcNeedToSearchWallIndex();
+			distIndexList.assign(path.getNeedToSearchIndex().begin(), path.getNeedToSearchIndex().end());
+			if (distIndexList.empty()) {
+				distIndexList.clear();
+				distIndexList.push_back(IndexVec(0,0));
+				dist = distIndexList.front();
+				state = Agent::BACK_TO_START;
+			}
+			else {
+				maze->updateStepMap(dist);
+				distIndexList.sort(
+						[&](const IndexVec& lhs, const IndexVec& rhs)
+						{
+					const unsigned curStep = maze->stepMap[cur.y][cur.x];
+					const unsigned lhsStep = maze->stepMap[lhs.y][lhs.x];
+					const unsigned rhsStep = maze->stepMap[rhs.y][rhs.x];
+					return (lhsStep - curStep) < (rhsStep - curStep);
+						}
+				);
+				distIndexList.unique();
+				dist = distIndexList.front();
+			}
+		}
+	}
+
+
+	if (state == Agent::BACK_TO_START) {
+		if (dist == cur) {
+			state = Agent::FINISHED;
+			nextDir = 0;
+
+			//最終的に走る最短経路を計算
+			path.calcShortestTimePath(IndexVec(0,0), mazeGoalList, 20, true);
+			return;
+		}
+		nextDir = calcNextDirection(cur, dist);
+	}
+
+
+	if (state == Agent::FINISHED) {
+
+	}
+
+
+	nextDir = calcNextDirection(cur, dist);
+}

Agent.h

@@ -0,0 +1,85 @@
+#ifndef AGENT_H_
+#define AGENT_H_
+
+#include <stdint.h>
+
+#include <list>
+#include <vector>
+
+#include "Maze.h"
+#include "ShortestPath.h"
+
+/*
+ * 探索時にロボットに動きの指示を与える
+ * 探索・最短経路の計算において最も上位に位置する
+ *
+ * 一区画進むごとにupdateを呼び出して今の座標と壁情報を入力していく
+ * updateを実行すると次に進むべき方向が計算される
+ *
+ * 迷路情報は外に保存をするが、Agent::updateを通して更新をしていく
+ */
+class Agent {
+public:
+	typedef enum {
+		IDLE, 					//まだ実行されていない
+		SEARCHING_NOT_GOAL, 	//探索中　まだ一度もゴールに到達していない
+		SEARCHING_REACHED_GOAL, //探索中　一度ゴールに到達したが、追加で探索をしている
+		BACK_TO_START, 			//スタートに戻っている
+		FINISHED 				//スタート地点に到着をし、最短経路の計算が終わった
+	} State;
+
+private:
+	Maze* maze;
+	State state;
+	IndexVec dist;
+	std::list<IndexVec> distIndexList;
+	Direction nextDir;
+	ShortestPath path;
+	std::list<IndexVec> mazeGoalList;
+
+	Direction calcNextDirection(const IndexVec &cur, const IndexVec &dist);
+
+public:
+	Agent(Maze &_maze) :maze(&_maze), state(Agent::IDLE), path(_maze)
+	{
+		mazeGoalList.push_back(IndexVec(7,7));
+		mazeGoalList.push_back(IndexVec(7,8));
+		mazeGoalList.push_back(IndexVec(8,7));
+		mazeGoalList.push_back(IndexVec(8,8));
+	}
+
+	//状態を更新する
+	//cur:今の座標
+	//cur_wall:今の座標における壁情報(Done bitは無視される)
+	void update(const IndexVec &cur, const Direction &cur_wall);
+
+	//現在の状態を返す
+	//updateを呼び出したあとは必ずこれを読んで状態を確認する
+	const State &getState() const {return state;}
+
+	//次にロボットが動くべき方向を返す
+	//0が帰ってきた場合はバグったときか、終了した時(おそらくロボットは停止すべき)
+	//今の向きと180度逆方向が出てくる場合もある
+	//その場合はおそらく一旦停止して切り返す必要がある
+	const Direction &getNextDirection() const {return nextDir;}
+
+
+	//強制的にゴールに向かわせる
+	//探索に時間がかかりすぎている場合につかう(2分たったら呼び出すとか)
+	void forceGotoStart() { dist = IndexVec(0,0); state = Agent::BACK_TO_START; }
+
+	const Path &getShortestPath() const {return path.getShortestTimePath();}
+	const std::vector<Path> &getKShortestPath() const {return path.getKShortestDistancePath();}
+	const IndexVec& getDist() const { return dist; }
+	const std::list<IndexVec> &getDistList() const { return distIndexList; }
+
+	const std::vector<Operation> &getRunSequence() const { return path.getShortestTimePathOperation(); }
+	//TODO:途中から再開できるようにしたい
+
+	void reset();
+
+};
+
+
+
+#endif /* AGENT_H_ */