サーチを利用したマーカーの移動

演習1では、マーカーが通る経路上の交差点全てを、配列path[]により指定していました。カーナビゲーションでは、この経路上の交差点を自動的に見つけだす必要があります。この演習では、出発地と目的地を指定するとその間の経路(上の交差点)を見つけ出す簡単なプログラムを作成します。

出発地と目的地間の交差点を捜し出すために、ここでは次のアルゴリズムを用います。このアルゴリズムには致命的な問題がありますが、完全なカーナビゲーションプログラムに一歩近付くために、まずはこの簡単なアルゴリズムをプログラミングしてみましょう。

が与えられているとき、start から end へ至る交差点の ID を次のようにして求める。

現在地交差点に隣接する交差点全てから最も近い交差点は、次のようにして選ぶことができます。

/* 最近傍隣接交差点探索 */
int search_nearest(int id, int goal)

演習のヒント

この課題プログラムは、演習１で作成したプログラムmobile.cに関数を追加し、また、幾つか修正をすると楽に作成できます。ソースファイル名をmobile_s.cとして、課題プログラムを作成して下さい。追加する関数と、修正点は以下の通りです。

/* 交差点goalに最も近い交差点idの隣接交差点を探す */
int search_nearest(int id, int goal) {
    int i, nearest;                /* nearestは最も近い交差点のID */
    double min_distance = 1e100;   /* とりあえず大きな数値に初期化しておく */
    double distance;

    for (i = 0; i < cross[id].points; i++) {
        /* 交差点idのi番目の隣接交差点 と 交差点 goal 間の距離を計算し、
         * distanceに代入。距離の計算には hypot(x, y)が使えます。
         * 計算した距離が今までの最短距離(min_distance)よりも小さかったら...
         * 上の説明を基に考えてみて下さい。
         */
    }
    return nearest;
}

int vehicle_goal;        /* 移動体の最終目的地 */
int vehicle_edgeFrom;    /* 移動体の経路上の位置 (何個目の道路か) */
int vehicle_edgeTo;      /* 移動体の経路上の位置 (何個目の道路か) */

/* 移動体の状態を初期化 */
    vehicle_edgeFrom = 5; /* 泉大橋から */
    vehicle_goal = 19;    /* 道路資料館へ */

//    vehicle_edgeFrom = 5; /* 泉大橋から */
//    vehicle_goal = 71;    /* 八木山動物公園へ */

    vehicle_edgeTo = vehicle_edgeFrom;
    vehicle_stepOnEdge = 0

また、main関数にはマーカの移動アニメーション用のコードを追加します。これは、mobile.cで作成したものを改造することでできます。

プログラム例

時間が来たら表示します。その時は、再読み込みボタンを押して下さい。

ひな形を こちら に用意しましたので、適宜使用ください。

/* mobile_s.c --- サーチを利用した移動体の表示
 *
 * cc mobile_s.c -g -O2 -Wall -o mobile_s \
 *   -I/usr/include/freetype2 -lftgl -lglfw -lGLU -lGL -lX11 -lXrandr
 */

#include <stdio.h>
#include <math.h>
#include <unistd.h>
#include <GL/glfw.h>
#include <FTGL/ftgl.h>

#define CROSSING_SIZE 100  /* 交差点数=100 */
#define MAX_NAME_SIZE  50  /* 最大文字数50文字(半角) */

#define MARKER_RADIUS 0.2  /* マーカーの半径 */

/* 座標変換マクロの定義 */
#define ORIGIN_X     -2.0
#define ORIGIN_Y      3.0
#define REAL_SIZE_X  10.0
#define REAL_SIZE_Y  10.0

#ifndef FONT_FILENAME
/* 演習サーバに用意されているフォントのファイル名 */
#define FONT_FILENAME "/usr/share/fonts/truetype/takao-gothic/TakaoGothic.ttf"
#endif
static FTGLfont *font; /* 読み込んだフォントを差すポインタ */

/* データ構造の定義 */
typedef struct {
    double x, y;           /* 位置 x, y */
} Position;                /* 位置を表す構造体 */

typedef struct {
    int id;                /* 交差点番号 */
    Position pos;          /* 位置を表す構造体 */
    double wait;           /* 平均待ち時間 */
    char jname[MAX_NAME_SIZE];   /* 交差点名(日本語) */
    char ename[MAX_NAME_SIZE];   /* 交差点名(ローマ字) */
    int points;            /* 交差道路数 */
    int next[5];           /* 隣接する交差点番号 */
} Crossing;


/* データを格納する変数の定義 */
static Crossing cross[CROSSING_SIZE];


/* 円を描画 */
void draw_circle(double x, double y, double r) {
    int const N = 12;             /* 円周を 12分割して線分で描画することにする */
    int i;

    glBegin(GL_LINE_LOOP);
    for (i = 0; i < N; i++)
        glVertex2d(x + cos(2 * M_PI * i / N) * r,
                   y + sin(2 * M_PI * i / N) * r);
    glEnd();
}

/* 文字列を描画 */
void draw_outtextxy(double x, double y, char const *text) {
    double const scale = 0.01;
    glPushMatrix();
    glTranslated(x, y, 0.0);
    glScaled(scale, scale, scale);
    ftglRenderFont(font, text, FTGL_RENDER_ALL);
    glPopMatrix();
}


/* 交差点goalに最も近い交差点idの隣接交差点を探す */
int search_nearest(int id, int goal) {
    int i;
    int nearest = -1;
    double min_distance = 1e100;
    double distance;

    for (i = 0; i < cross[id].points; i++) {
        /* 交差点idのi番目の隣接交差点 と 交差点 goal 間の距離を計算し、
         * distanceに代入。距離の計算には hypot(x, y)が使えます。
         * 計算した距離が今までの最短距離(min_distance)よりも小さかったら...
         * 完成させてください
         */
      distance = hypot(cross[goal].pos.x - cross[cross[id].next[i]].pos.x, 
                       cross[goal].pos.y - cross[cross[id].next[i]].pos.y);

      if(min_distance > distance){
         min_distance = distance;
         nearest = cross[id].next[i];

      }
    }

    return nearest;
}


/* ファイルの読み込み */
int map_read(char *filename) {
    FILE *fp;
    int i, j;
    int crossing_number;          /* 交差点数 */

    fp = fopen(filename, "r");
    if (fp == NULL) {
        perror(filename);
        return -1;
    }

    /* はじめに交差点数を読み込む */
    fscanf(fp, "%d", &crossing_number);

    for (i = 0; i < crossing_number; i++) {

        fscanf(fp, "%d,%lf,%lf,%lf,%[^,],%[^,],%d",
                     &(cross[i].id), &(cross[i].pos.x), &(cross[i].pos.y),
                     &(cross[i].wait), cross[i].jname,
                     cross[i].ename, &(cross[i].points));

        for (j = 0; j < cross[i].points; j++) {
            fscanf(fp, ",%d", &(cross[i].next[j]));
        }

    }
    fclose(fp);

    /* ファイルから読み込んだ交差点数を返す */
    return crossing_number;
}


/* 道路網の表示 */
void map_show(int crossing_number) {
    int i, j;
    double x0, y0, x1, y1;

    for (i = 0; i < crossing_number; i++) {     /* 交差点毎のループ */
        x0 = cross[i].pos.x;
        y0 = cross[i].pos.y;

        /* 交差点を表す円を描く */
        glColor3d(1.0, 0.5, 0.5);
        draw_circle(x0, y0, 0.1);

        /* 交差点の名前を描く */
        glColor3d(1.0, 1.0, 0.0);
        draw_outtextxy(x0, y0, cross[i].jname);

        /* 交差点から伸びる道路を描く */
        glColor3d(1.0, 1.0, 1.0);
        glBegin(GL_LINES);
        for(j = 0; j < cross[i].points; j++) {
            x1 = cross[ cross[i].next[j] ].pos.x;
            y1 = cross[ cross[i].next[j] ].pos.y;
            glVertex2d(x0, y0);
            glVertex2d(x1, y1);
        }
        glEnd();
    }
}


int main(void) {
    int crossing_number;     /* 交差点数 */
    int vehicle_goal;        /* 移動体の最終目的地 */
    int vehicle_edgeFrom;    /* 移動体の経路上の位置 (何個目の道路か) */
    int vehicle_edgeTo;      /* 移動体の経路上の位置 (何個目の道路か) */
    int vehicle_stepOnEdge;  /* 移動体の道路上の位置 (何ステップ目か) */
    double vehicle_x = 0.0, vehicle_y = 0.0; /* 移動体の座標 */
    double x0, y0, x1, y1;
    double distance;
    int steps;
    int width, height;

    /* グラフィック環境を初期化して、ウィンドウを開く */
    glfwInit();
    glfwOpenWindow(800, 800, 0, 0, 0, 0, 0, 0, GLFW_WINDOW);

    /* (ORIGIN_X, ORIGIN_Y) を中心に、REAL_SIZE_X * REAL_SIZE_Y の範囲の
         空間をビューポートに投影する */
    glMatrixMode(GL_PROJECTION);
    glLoadIdentity();
    glOrtho(ORIGIN_X + REAL_SIZE_X * -0.5, ORIGIN_X + REAL_SIZE_X * 0.5,
            ORIGIN_Y + REAL_SIZE_Y * -0.5, ORIGIN_Y + REAL_SIZE_Y * 0.5,
            -1.0, 1.0);

    glMatrixMode(GL_MODELVIEW);
    glLoadIdentity();             /* それ以外の座標変換は行わない */

    /* 文字列描画のためのフォントの読み込みと設定 */
    font = ftglCreateExtrudeFont(FONT_FILENAME);
    if (font == NULL) {
        perror(FONT_FILENAME);
        fprintf(stderr, "could not load font\n");
        exit(1);
    }
    ftglSetFontFaceSize(font, 24, 24);
    ftglSetFontDepth(font, 0.01);
    ftglSetFontOutset(font, 0, 0.1);
    ftglSetFontCharMap(font, ft_encoding_unicode);

    /* マップファイルの読み込み */
    crossing_number = map_read("map2.dat");
    if (crossing_number < 0) {
        fprintf(stderr, "couldn't read map file\n");
        exit(1);
    }

    /* 移動体の状態を初期化 */
    vehicle_edgeFrom = 5; /* 泉大橋から */
    vehicle_goal = 19;    /* 道路資料館へ */

//    vehicle_edgeFrom = 5; /* 泉大橋から */
//    vehicle_goal = 71;    /* 八木山動物公園へ */

    vehicle_edgeTo = vehicle_edgeFrom;
    vehicle_stepOnEdge = 0;

    while (1) {

        /* Esc が押されるかウィンドウが閉じられたらおしまい */
        if (glfwGetKey(GLFW_KEY_ESC) || !glfwGetWindowParam(GLFW_OPENED))
            break;

        glfwGetWindowSize(&width, &height); /* 現在のウィンドウサイズを取得する */
        glViewport(0, 0, width, height); /* ウィンドウ全面をビューポートにする */

        glClearColor(0.0f, 0.0f, 0.0f, 0.0f);
        glClear(GL_COLOR_BUFFER_BIT); /* バックバッファを黒で塗り潰す */

        map_show(crossing_number);                /* 道路網の表示 */

        /* 移動体を進めて座標を計算する */
        if (vehicle_edgeFrom != vehicle_goal) {
            /* まだゴールに達していないので、移動体の位置を進める */

           x0 = cross[vehicle_edgeFrom].pos.x;
           y0 = cross[vehicle_edgeFrom].pos.y;
           x1 = cross[vehicle_edgeTo].pos.x;
           y1 = cross[vehicle_edgeTo].pos.y;
           distance = hypot(x1-x0, y1-y0);
           steps = (int)(distance/0.1);

           /* 道路上を進んで、座標を更新 */
           vehicle_stepOnEdge++;
           vehicle_x = x0 + (x1 - x0) / steps * vehicle_stepOnEdge;
           vehicle_y = y0 + (y1 - y0) / steps * vehicle_stepOnEdge;

            if (vehicle_stepOnEdge >= steps) {
                /* 交差点に達したので次の道路へ入る */
                vehicle_edgeFrom = vehicle_edgeTo;
                vehicle_edgeTo = search_nearest(vehicle_edgeFrom,
                                                vehicle_goal);
                vehicle_stepOnEdge = 0;
            }
        }

        /* ゴールと移動体を表示 */
        glColor3d(1.0, 1.0, 1.0);
        draw_circle(cross[vehicle_goal].pos.x,
               cross[vehicle_goal].pos.y,
               MARKER_RADIUS);
        draw_circle(vehicle_x, vehicle_y, MARKER_RADIUS);

        glfwSwapBuffers(); /* フロントバッファとバックバッファを入れ替える */
        usleep(50 * 1000); /* 50ミリ秒くらい待つ */
    }

    glfwTerminate();

    return 0;
}

完成したプログラム mobile_s_ans.c を示しますので、参考にして下さい。

さあ、ここまでくればもうカーナビらしくなりましたね。あとは出発地点、目的地点を指定する部分(インターフェース)を付け足せばさらに完成度は高まるでしょう。

しかし、ここで用いた経路探索には、致命的な問題があります。例えば、main()関数中のvehicle_edgeFromとvehicle_edgeToを5と71に変更してみて下さい。この場合、出発地は泉大橋、目的地は八木山動物公園となりますが、うまく目的地まで到達できるでしょうか？恐らく、目的地に至る前に2つの交差点間を行ったり来たりしてしまうでしょう。

これは、ここで用いた経路探索アルゴリズムが、その場だけでの最適経路しか探していないからです。場合によっては、最初遠回りした方が最終的に短い経路となることがありますね。このアルゴリズムではそのような場合には最短経路を探し出すことができません。木を見て森を見ず、といったところでしょうか。

では、本当の最短経路を見つけだすにはどうしたら良いでしょうか。その答の一つは、次週やるDijkstra(ダイクストラ)のアルゴリズムです。今日は、このアルゴリズムを自力で理解してみるか、あるいは自分で新しいアルゴリズムを考え、上記の問題が起こらないようにプログラムを改良してみて下さい。

サーチを利用したトレースプログラム

演習のヒント

プログラム例