Dijkstra, Branch & Bound, Floyd pathfinding demo

[Whistler]

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);
} 


[Pierre-Marie Baty]

Very interesting, and very useful. Be sure to check out this one also.