プログラマ専用SNS ミクプラ

コード:

#include <stdio.h>
#include <conio.h> /* getch */
#include <vector>
#include <string>
#include <math.h>
#include <crtdbg.h>


#define NODE1 0
#define NODE2 1
#define NODE3 2
#define NODE4 3
#define NODE5 4
#define NODE6 5


/*ノードとノードの距離を測ります*/
int getDist( int x1, int y1, int x2, int y2 )
{

	int ret = (int)pow( (x2-x1)*(x2-x1) + (y2-y1)*(y2-y1), 0.5 );

	return ret;

}

/*ノードです*/
class Node
{

public:
	int x,
		y,
		id,
		cost;
	bool   done;
	Node*  previousNode; //最短でつながっているノード

	std::vector<int> edgesCost; //繋がっているエッジのコスト
	std::vector<Node*> edgesTo; //エッジがどこに繋がっているか

	Node( int i_x, int i_y, int i_id )
	{
		x    = i_x;
		y    = i_y;
		id   = i_id;
		cost = -1;
		done = false;
		previousNode = nullptr;
	}
	~Node()
	{
	
	}
	void addNode( Node* node )
	{
		int inputCost = getDist( x, y, node->x, node->y );

		edgesTo.push_back( node );
		edgesCost.push_back( inputCost );
	}
	void resetNode()
	{
		cost = -1;
		done = false;
		previousNode = nullptr;
	}

};

/*すべてのノードをリセット*/
void resetNodes( std::vector<Node*> nodes )
{
	for( int i=0; i<nodes.size(); i++ )
	{
		nodes[i]->resetNode();
	}
}

/*ノードを配置してエッジをつなげます*/
std::vector<Node*> getNodes()
{

	std::vector<Node*> ret;
	Node* node1 = new Node( 100, 100, NODE1 );
	Node* node2 = new Node( 150, 50,  NODE2 );
	Node* node3 = new Node( 200, 150, NODE3 );
	Node* node4 = new Node( 200, 200, NODE4 );
	Node* node5 = new Node( 300, 300, NODE5 );
	Node* node6 = new Node( 50,  50,  NODE6 );

	node1->addNode( node2 );
	node1->addNode( node3 );
	node1->addNode( node6 );

	node2->addNode( node1 );
	node2->addNode( node3 );
	node2->addNode( node4 );

	node3->addNode( node1 );
	node3->addNode( node2 );
	node3->addNode( node4 );
	node3->addNode( node5 );
	node3->addNode( node6 );

	node4->addNode( node2 );
	node4->addNode( node3 );
	node4->addNode( node5 );

	node5->addNode( node4 );
	node5->addNode( node3 );

	node6->addNode( node1 );
	node6->addNode( node3 );

	ret.push_back( node1 );
	ret.push_back( node2 );
	ret.push_back( node3 );
	ret.push_back( node4 );
	ret.push_back( node5 );
	ret.push_back( node6 );

	return ret;

}

/*最短経路探索*/
std::vector<int> getPass( int start, int goal, std::vector<Node*> i_nodes )
{

	std::vector<Node*> nodes = i_nodes;
	std::vector<int> nodesId;
	Node*           goalNode = nullptr;

	/*スタートノード、ゴールノードを設定*/
	for( int i=0; i<nodes.size(); i++ )
	{
		if( nodes[i]->id == start )
		{
			nodes[i]->cost = 0;
		}
		else if( nodes[i]->id == goal )
		{
			goalNode = nodes[i];
		}
	}


	while( true)
	{
		printf( "\n\n探索中...\n" );

		Node* processNode = nullptr;

		//ノードを決定する
		for( int i=0; i<nodes.size(); i++ )
		{

			Node *node = nodes[i];

			if( node->done || node->cost < 0 )
			{
				continue;
			}

			if( processNode == nullptr )
			{
				processNode = node;
			}

			if( node->cost < processNode->cost )
			{
				processNode = node;
			}

		}

		if( processNode == nullptr )
		{
			break;
		}

		processNode->done = true;

		//コストを設定
		for( int i=0; i<processNode->edgesTo.size(); i++)
		{

			Node* node = processNode->edgesTo[i];
			int   cost = node->cost + processNode->edgesCost[i];

			if( node->cost < 0 || node->cost > cost )
			{
				node->cost         = cost;
				node->previousNode = processNode;
			}

		}

	}


	/*ノードの状態を表示*/
	/*printf( "ノードの一覧を表示します\n\n" );
	for( int i= 0; i<nodes.size(); i++ )
	{
		Node* node = nodes[i];
		printf( "\n座標 x=%d y=%d\n", node->x, node->y );
		printf( "ID  %d\n",node->id );

		for( int j=0; j<node->edgesTo.size(); j++ )
		{
			printf( "繋がっているノード  ID %d\n", node->edgesTo[j]->id );
			printf( "距離 %d\n", node->edgesCost[j] );

		}
	}*/


	/*最短経路発表*/
	printf( "\n\n最短経路は\n\n" );
	printf( "Goal ID=" );

	Node* curNode = goalNode;
	while( true )
	{

		if( curNode->previousNode == nullptr )
		{
			printf( "Start ID=%d", start );
			nodesId.push_back( curNode->id );
			break;
		}
		printf( "%d -> ", curNode->id );
		nodesId.push_back( curNode->id );
		curNode = curNode->previousNode;

	}

	

	return nodesId;

}


/*最短経路テーブルを作ります*/
std::vector< std::vector< int > > makeTable()
{

	std::vector<Node*> nodes = getNodes();
	int length               = nodes.size();
	std::vector< std::vector< int > > table = std::vector< std::vector< int > >( length, std::vector<int>( length, 0 ) );

	/*スタート地点*/
	for( int i=0; i<length; i++ )
	{
		/*ゴール地点*/
		for( int j=0; j<length; j++ )
		{

			if( i != j )
			{
				/*スタートとゴールの経路順が逆になるので引数を逆に入れる*/
				std::vector<int> pass = getPass( j, i, nodes );
				resetNodes( nodes );

				for( int n=0; n<pass.size() -1; n++ )
				{
					table[j][ pass[n] ] = pass[n+1];

				}
			}else
			{
				table[i][j] = -1;
			}
		}
	}

	for( int i=0; i<length; i++ )
	{
		delete nodes[i];
		nodes[i] = nullptr;
	}


	return table;

}


int main(){
	_CrtSetDbgFlag(_CRTDBG_ALLOC_MEM_DF|_CRTDBG_DELAY_FREE_MEM_DF|_CRTDBG_CHECK_ALWAYS_DF|_CRTDBG_LEAK_CHECK_DF);
	
	
	std::vector< std::vector< int > > table = makeTable();

	printf( "\n\n 最短経路表です\n" );
	for( int i=0; i<table.size(); i++ )
	{
		for( int j=0; j<table[0].size(); j++ )
		{
			printf( "%d ,", table[i][j] );
		}
		printf( "\n");
	}


	getch();

	return 0;
}