第K最短路探索プログラムについて

#include <stdio.h>
#include <stdlib.h>
#include <math.h>
#include <string.h>

#define MAXN 1100  // 最大ノード数
#define MAXM 500   // 最大クエリ数
#define MAXQ 100   // 最大クエリ数
#define MAX_K 5    // 最多経路数
#define INF 1e18   // 無限大
#define EPS 1e-8   // 浮動小数点比較の誤差許容範囲

// 座標点構造体
typedef struct {
    double x, y;
} Point;

// 線分
typedef struct {
    int a, b;
} Segment;

// グラフの隣接リスト用エッジ構造体
typedef struct Edge {
    int to;
    double cost;
    struct Edge* next;
} Edge;

typedef struct {
    double cost;
    int path[MAXN];
    int length;
} Path;

// グローバル変数
int N,M;   // ノード数、クエリ数
Point points[MAXN];   // 座標点一覧
Segment segs[MAXM];   // 線分一覧
Edge* graph[MAXN];   // グラフの隣接リスト
Path paths[MAXQ * MAX_K];
int node_count = 0;   // ノードの総数
int cur_path[MAXN];
int cur_len = 0;
int vis[MAXN] = {0};
int path_count_total = 0;

// 2点が同じ座標かを比較（誤差考慮）
int equal(Point p1, Point p2) {
    return fabs(p1.x - p2.x) < EPS && fabs(p1.y - p2.y) < EPS;
}

// 2点間のユークリッド距離
double distance(Point p1, Point p2) {
    double dx = p1.x - p2.x, dy = p1.y - p2.y;
    return sqrt(dx * dx + dy * dy);
}

// 点pが線分ab上にあるか判定
int point_on_segment(Point p, Point a, Point b) {
    
    double dx1 = p.x - a.x;
    double dy1 = p.y - a.y;
    double dx2 = b.x - a.x;
    double dy2 = b.y - a.y;
  
    // 外積で一直線上にあるか判定し、内積で範囲内か確認
    if (fabs(dx1 * dy2 - dy1 * dx2) < EPS) {
        double dot = dx1 * dx2 + dy1 * dy2;
        double len_sq = dx2 * dx2 + dy2 * dy2;
        return (0 <= dot && dot <= len_sq) || fabs(dot) < EPS || fabs(dot - len_sq) < EPS;
    }
    return 0;
}

// 線分同士の交差判定と交点計算
int intersect(Point p1, Point q1, Point p2, Point q2, double* ix, double* iy) {
    
    // 端点同士が重なっているか確認
    if (point_on_segment(p1, p2, q2)) {
        *ix = p1.x;
        *iy = p1.y;
        return 1;
    }

    if (point_on_segment(q1, p2, q2)) {
        *ix = q1.x;
        *iy = q1.y;
        return 1;
    }

    if (point_on_segment(p2, p1, q1)) {
        *ix = p2.x;
        *iy = p2.y;
        return 1;
    }

    if (point_on_segment(q2, p1, q1)) {
        *ix = q2.x;
        *iy = q2.y;
        return 1;
    }

    // 行列式で交点計算
    double a = q1.x - p1.x;
    double b = -(q2.x - p2.x);
    double c = q1.y - p1.y;
    double d = -(q2.y - p2.y);
    double e = p2.x - p1.x;
    double f = p2.y - p1.y;
    double det = a * d - b * c;

    if (fabs(det) < EPS) return 0;   // 平行または一致

    double s = (d * e - b * f) / det;
    double t = (-c * e + a * f) / det;

    // 交差範囲のチェック
    if (s < EPS || s > 1 - EPS || t < EPS || t > 1 - EPS) return 0;

    *ix = p1.x + s * (q1.x - p1.x);
    *iy = p1.y + s * (q1.y - p1.y);

    // 既存点との重複回避
    for (int i = 0; i < node_count; i++) {
        if (equal((Point){*ix, *iy}, points[i])) {
            *ix = points[i].x;
            *iy = points[i].y;
            break;
        }
    }

    return 1;
}

// グラフに無向エッジを追加
void add_edge(int u, int v, double cost) {
    Edge* e = malloc(sizeof(Edge));
    e->to = v;
    e->cost = cost;
    e->next = graph[u];
    graph[u] = e;
}

// 点の比較関数（x優先 → y）
int cmp_point(const void* a, const void* b) {
    Point* p = (Point*)a;
    Point* q = (Point*)b;
    if (fabs(p->x - q->x) > EPS) return p->x < q->x ? -1 : 1;
    if (fabs(p->y - q->y) > EPS) return p->y < q->y ? -1 : 1;
    return 0;
}

// 既存ノードのインデックス取得（なければ追加）
int get_node_index(Point p) {
    for (int i = 0; i < node_count; i++) {
        if (equal(points[i], p)) return i;
    }
    points[node_count++] = p;
    return node_count - 1;
}

// 線分の交差を考慮しながらグラフ構築
void build_graph(int M) {

    // グラフ初期化
    for (int i = 0; i < MAXN; i++) {
        graph[i] = NULL;
    }
    
    for (int i = 0; i < M; i++) {
        Point p1 = points[segs[i].a];
        Point p2 = points[segs[i].b];
        Point seg_pts[MAXN];   // 分割された点列
        int count = 0;
        seg_pts[count++] = p1;
        seg_pts[count++] = p2;

        // 他の線分との交点を探す
        for (int j = 0; j < M; j++) {
            if (i == j) continue;
            double ix, iy;
            if (intersect(p1, p2, points[segs[j].a], points[segs[j].b], &ix, &iy)) {
                Point p = {ix, iy};
                int found = 0;
                for (int k = 0; k < count; k++) {
                    if (equal(p, seg_pts[k])) {
                        found = 1;
                        break;
                    }
                }  
                if (!found) {
                    seg_pts[count++] = p;
                }
            }
        }

        // 点をソートして順番に辺を張る
        qsort(seg_pts, count, sizeof(Point), cmp_point);

        // 重複を除去
        int unique_count = 1;

        for (int j = 1; j < count; j++) {
            if (!equal(seg_pts[j], seg_pts[unique_count-1])) {
                seg_pts[unique_count++] = seg_pts[j];
            }
        }

        count = unique_count;

        for (int k = 0; k < count - 1; k++) {
            int u = get_node_index(seg_pts[k]);
            int v = get_node_index(seg_pts[k + 1]);
            if (u == v) continue;  
            double d = distance(seg_pts[k], seg_pts[k + 1]);
            // 重複エッジチェック
            int edge_exists = 0;
            for (Edge* e = graph[u]; e; e = e->next) {
                if (e->to == v) {
                    edge_exists = 1;
                    break;
                }
            }
            if (!edge_exists) {
                add_edge(u, v, d);
                add_edge(v, u, d);   // 無向グラフ
            }
        }
    }
}

// 幹線道路検出用変数
int tin[MAXN],low[MAXN],visited[MAXN],timer = 0;

// 橋検出用DFS
void bridge_dfs(int v, int parent) {
    visited[v] = 1;
    tin[v] = low[v] = ++timer;

    for (Edge* e = graph[v]; e != NULL; e = e->next) {
        int to = e->to;
        if (to == parent) continue;

        if (visited[to]) {
            if (low[v] > tin[to]) low[v] = tin[to];
        } else {
            bridge_dfs(to, v);
            if (low[v] > low[to]) low[v] = low[to];

            if (low[to] > tin[v]) {
                printf("%d %d\n", v + 1, to + 1);  // 両端のノード番号を出力
            }
        }
    }
}


// ノード名（数値 or C+数値）からノードインデックスを取得
int parse_node(char* s, int N) {
    if (s[0] == 'C') {
        int id = atoi(s + 1);
        if (id < 1 || id > node_count - N) return -1;
        return N + id - 1;
    } else {
        int id = atoi(s);
        if (id < 1 || id > N) return -1;
        return id - 1;
    }
}

// 昇順にdoubleを比較するための関数（誤差考慮）
int comp_double(const void* A, const void* B) {
    double a = *(double*)A, b = *(double*)B;
    if (fabs(a - b) > EPS) return a < b ? -1 : 1;
    return 0;
}

// 深さ優先探索で全経路を探索し、目的地までの距離と経路を記録
void dfs(int u, int goal, double cost) {
    cur_path[cur_len++] = u;

    if (u == goal) {
        paths[path_count_total].cost = cost;
        paths[path_count_total].length = cur_len;
        for (int i = 0; i < cur_len; i++) {
            paths[path_count_total].path[i] = cur_path[i];
        }
        path_count_total++;
        cur_len--;
        return;
    }

    for (Edge* e = graph[u]; e; e = e->next) {
        int v = e->to;
        if (vis[v]) continue;
        vis[v] = 1;
        dfs(v, goal, cost + e->cost);
        vis[v] = 0;
    }

    cur_len--;
}

// 経路を距離の昇順にソート
int compare_path(const void* a, const void* b) {
    double d1 = ((Path*)a)->cost;
    double d2 = ((Path*)b)->cost;
    if (fabs(d1 - d2) > EPS) return d1 < d2 ? -1 : 1;
    return 0;
}

// DFSを呼び出し、結果をソートして準備
void solve(int start, int goal, int k, int output_path) {
    memset(vis, 0, sizeof(vis));
    path_count_total = 0;
    cur_len = 0;

    vis[start] = 1;
    dfs(start, goal, 0);
    vis[start] = 0;

    qsort(paths, path_count_total, sizeof(Path), compare_path);

    for (int i = 0; i < k && i < path_count_total; i++) {
        printf("%.5f\n", paths[i].cost);
        if (output_path) {
            for (int j = 0; j < paths[i].length; j++) {
                int node = paths[i].path[j];
                // 数字ノードとCラベルノードの区別
                if (node < N) {
                    printf("%d ", node + 1);   // ノード1〜Nはそのまま出力
                } else {
                    printf("C%d ", node - N + 1);   // ノードN以降は "C" + 番号で出力
                }
            }
            printf("\n");
        }
    }

    // 経路が存在しない場合
    if (path_count_total == 0) {
        printf("NA\n");
    }
}

void connect_new_points(int P) {
    for (int i = 0; i < P; i++) {
        Point p;
        scanf("%lf %lf", &p.x, &p.y);

        double min_dist = INF;
        Point best_point;

        for (int j = 0; j < M; j++) {
            Point a = points[segs[j].a];
            Point b = points[segs[j].b];

            double dx = b.x - a.x;
            double dy = b.y - a.y;
            double t = ((p.x - a.x) * dx + (p.y - a.y) * dy) / (dx * dx + dy * dy);
            if (t < 0) t = 0;
            if (t > 1) t = 1;

            Point proj = { a.x + t * dx, a.y + t * dy };
            double d = distance(p, proj);

            if (d + EPS < min_dist) {
                min_dist = d;
                best_point = proj;
            }
        }

        // 出力
        printf("%.5f %.5f\n", best_point.x, best_point.y);

        // グラフに接続
        int pi = get_node_index(p);
        int ci = get_node_index(best_point);
        double d = distance(p, best_point);
        add_edge(pi, ci, d);
        add_edge(ci, pi, d);

        // 線分にも記録（順番どおりにMを増やす）
        segs[M].a = pi;
        segs[M].b = ci;
        M++;
    }
}

// 入力読み込みと各クエリに対する出力
int main() {
    int P, Q;

    scanf("%d %d %d %d", &N, &M, &P, &Q);   // 点数、線分数、新たに加えられる点の数、クエリ数
    
    for (int i = 0; i < N; i++) {
        scanf("%lf %lf", &points[i].x, &points[i].y);
    }

    node_count = N;

    for (int i = 0; i < M; i++) {
        scanf("%d %d", &segs[i].a, &segs[i].b);
        segs[i].a--; segs[i].b--;   // 0-indexed に変換
    }

    build_graph(M);   // グラフ構築

    memset(visited, 0, sizeof(visited));   // 幹線道路検出
    timer = 0;

    for (int i = 0; i < node_count; i++) {
        if (!visited[i]) {
        bridge_dfs(i, -1);
        }
    }

    for (int q = 0; q < Q; q++) {
        char s1[10], s2[10];
        int k;
        scanf("%s %s %d", s1, s2, &k);
        int start = parse_node(s1, N);
        int goal = parse_node(s2, N);
        if (start == -1 || goal == -1) {
            printf("NA\n");
            continue;
        }

        solve(start, goal,k,1);   
        
    }

    connect_new_points(P);   

    return 0;
}