| Whistler |
25-11-2004 10:12 |
Dijkstra, Branch & Bound, Floyd pathfinding demo
Branch & Bound:
PHP Code:
#include <stdio.h>
#include <stdlib.h>
int paths[5][5] = {
{-1, 32, -1, 56, -1},
{-1, -1, 61, -1, -1},
{61, 67, -1, 61, 91},
{-1, -1, 12, -1, 67},
{21, -1, -1, 81, -1}
};
typedef struct node_s {
int i;
node_s *parent;
node_s *next;
int cost;
} node_t;
main()
{
int from, to;
printf("Enter start point: ");
scanf("%d", &from);
printf("Enter end point: ");
scanf("%d", &to);
node_t *queue = NULL, *closed = NULL;
queue = new node_t;
queue->i = from;
queue->cost = 0;
queue->parent = NULL;
queue->next = NULL;
while (queue) {
node_t *current = queue;
if (current->i == to) {
node_t *p = queue;
while (p) {
printf("%d, ", p->i);
p = p->parent;
}
printf("\n");
goto endprogram;
}
queue = queue->next;
for (int i = 0; i < 5; i++) {
if (paths[current->i][i] < 0)
continue;
node_t *curchild = new node_t;
curchild->parent = current;
curchild->i = i;
curchild->cost = current->cost + paths[current->i][i];
node_t *p = queue, *prev = NULL;
while (p && p->cost < curchild->cost) {
prev = p;
p = p->next;
}
if (prev) {
curchild->next = p;
prev->next = curchild;
} else {
curchild->next = queue;
queue = curchild;
}
}
if (closed) {
closed->next = current;
current->next = NULL;
} else {
closed = current;
}
}
printf("No path found!\n");
endprogram:
while (queue) {
node_t *p = queue->next;
delete queue;
queue = p;
}
while (closed) {
node_t *p = closed->next;
delete closed;
closed = p;
}
}
Dijkstra:
PHP Code:
#include <stdio.h>
#include <stdlib.h>
int paths[5][5] = {
{-1, 32, -1, 56, -1},
{-1, -1, 61, -1, -1},
{61, 67, -1, 61, 91},
{-1, -1, 12, -1, 67},
{21, -1, -1, 81, -1}
};
typedef struct node_s {
int i;
node_s *parent;
node_s *next;
int cost;
} node_t;
main()
{
int from, to;
printf("Enter start point: ");
scanf("%d", &from);
printf("Enter end point: ");
scanf("%d", &to);
node_t *open = NULL, *closed = NULL;
open = new node_t;
open->i = from;
open->cost = 0;
open->parent = NULL;
open->next = NULL;
while (open) {
node_t *current = open;
if (current->i == to) {
node_t *p = open;
while (p) {
printf("%d, ", p->i);
p = p->parent;
}
printf("\n");
goto endprogram;
}
open = open->next;
for (int i = 0; i < 5; i++) {
if (paths[current->i][i] < 0)
continue;
node_t *curchild = new node_t;
curchild->parent = current;
curchild->i = i;
curchild->cost = current->cost + paths[current->i][i];
node_t *p = open, *prev = NULL;
bool is_continue = true;
while (p) {
if (p->i == curchild->i && p->cost <= curchild->cost) {
delete curchild;
is_continue = false;
break;
}
p = p->next;
}
if (!is_continue)
continue;
p = closed;
is_continue = true;
while (p) {
if (p->i == curchild->i && p->cost <= curchild->cost) {
delete curchild;
is_continue = false;
break;
}
p = p->next;
}
if (!is_continue)
continue;
p = open;
while (p && p->cost < curchild->cost) {
prev = p;
p = p->next;
}
if (prev) {
curchild->next = p;
prev->next = curchild;
} else {
curchild->next = open;
open = curchild;
}
}
if (closed) {
closed->next = current;
current->next = NULL;
} else {
closed = current;
}
}
printf("No path found!\n");
endprogram:
while (open) {
node_t *p = open->next;
delete open;
open = p;
}
while (closed) {
node_t *p = closed->next;
delete closed;
closed = p;
}
}
Floyd-Warshall:
PHP Code:
#include <stdio.h>
#include <stdlib.h>
int paths[5][5] = {
{0, 32, -1, 56, -1},
{-1, 0, 61, -1, -1},
{61, 67, 0, 61, 91},
{-1, -1, 12, 0, 67},
{21, -1, -1, 81, 0}
};
main()
{
int path_matrix[5][5];
int dist_matrix[5][5];
int i, j, k;
for (i = 0; i < 5; i++) {
for (j = 0; j < 5; j++) {
if (paths[i][j] != -1) {
path_matrix[i][j] = j;
dist_matrix[i][j] = paths[i][j];
} else {
path_matrix[i][j] = -1;
dist_matrix[i][j] = 32767;
}
}
}
for (i = 0; i < 5; i++) {
for (j = 0; j < 5; j++) {
for (k = 0; k < 5; k++) {
if (dist_matrix[j][i] + dist_matrix[i][k] < dist_matrix[j][k]) {
path_matrix[j][k] = i;
dist_matrix[j][k] = dist_matrix[j][i] + dist_matrix[i][k];
}
}
}
}
printf("Enter start point: ");
scanf("%d", &i);
printf("Enter end point: ");
scanf("%d", &j);
if (dist_matrix[i][j] == 32767) {
printf("NO PATH FOUND !\n");
exit(1);
}
printf("Total cost = %d; Path is: ", dist_matrix[i][j]);
while (i != j) {
printf("%d, ", i);
i = path_matrix[i][j];
}
printf("%d\n", j);
}
|
