eomsangdonandClaude Opus 5.5
e2da944c78
- 새 A00_Common/map/logic/map_guide_route.ts — 지점 차례 구간마다 A* · 20 m 걸음 56 방향 · ±14 % 막음 · 급경사 35°+ · 기울기 · 꺾임 벌점 · 가중 어림 × 3 · 상자 두 점 + 여유(끝에 닿았을 때만 두 배 한 번) · 펼침 30 만 + 450 ms 상한 · 첫 · 끝 걸음 정확 · 1 m 격자 2 칸 솎음 · 못 닿음 = 가장 가까이 간 데까지 - 화면 가이드 전용 · 파이썬 짝 없음 · 저장 없음 - 지킴 시험 test_636_guide_route — 평지 · 지그재그 · 못 닿음 · 지점 셋 · 급경사 벌점 · 1 km · 솎음 · 지형 밖 Co-Authored-By: Claude Opus 5.5 <noreply@anthropic.com>
435 lines
16 KiB
TypeScript
435 lines
16 KiB
TypeScript
/* =============================================================================
|
|
* map_guide_route.ts(logic)
|
|
* 예정노선 가이드 추천선 셈(PLAN 636-1 · 요구 546) — 기준지점을 차례로 거치는 참고용 선.
|
|
* 화면 가이드 전용 · 파이썬 짝 없음 · 저장 안 함(설계 값 아님 · 선례 `D03_PlanAnalysis_Fan.ts:8`).
|
|
* 2 m 격자 A*: 걸음 = 반지름 걸음/칸 ± 0.5 격자점(20 m = 56 방향) · |Δz| > 상한 × 걸음 길이 막음 ·
|
|
* 비용 = 길이 × (1 + 급경사 × 급경사 표본 비율 + 기울기 × (g/상한)²) + 꺾임 × 걸음 × (꺾임각/π)² ·
|
|
* 어림 = 직선 거리 × 가중(가중 A*) · 구간마다 상자 = 두 점 + 여유(못 닿으면 두 배 한 번) · 펼침 상한.
|
|
* 첫 걸음 = 정확한 시작점에서 · 끝 걸음도 기울기 검사 · 꼭짓점 그대로(20 m 다시 나누기 금지).
|
|
* 1 m 격자 = 2 칸 솎아 2 m · 급경사 = 격자 높이에서 바로 셈 · 못 닿음 = 가장 가까이 간 데까지.
|
|
* 근거 = `docs/raw/요구_1101/_분석_546-C_추천노선_라이다.md` C1 · DOM · fetch · three 없음.
|
|
* ========================================================================== */
|
|
|
|
import {
|
|
heightAt as gridHeightAt,
|
|
type DtmGrid,
|
|
} from "../../../common_util/common_util_surface_sampler";
|
|
|
|
export type Point2 = [number, number];
|
|
export type Point3 = [number, number, number];
|
|
|
|
/** 높이 원천 — heightAt(NaN = 밖 · 빈칸) · 원천 격자 칸(m) · 격자 원점(칸 꼭짓점 맞춤). */
|
|
export interface GuideTerrain {
|
|
heightAt: (x: number, y: number) => number;
|
|
/** 원천 격자 칸 m(기본 2) — 2 m 보다 잘면 솎아 2 m 안팎으로. */
|
|
cell?: number;
|
|
origin?: Point2;
|
|
}
|
|
|
|
export interface GuideWeights {
|
|
/** 급경사 표본 비율 벌점(기본 3) */
|
|
steep: number;
|
|
/** (g/상한)² 벌점(기본 0.3) */
|
|
grade: number;
|
|
/** 꺾임 벌점(기본 1) */
|
|
turn: number;
|
|
/** 어림 가중(기본 3 · 1 = 정확 A*) */
|
|
heuristic: number;
|
|
}
|
|
|
|
export interface GuideOptions {
|
|
/** 걸음 m(기본 20 · 그리기 「길이·기울기 고정」 칸) */
|
|
step?: number;
|
|
/** 기울기 상한 비율(기본 0.14) */
|
|
maxGrade?: number;
|
|
/** 급경사 문턱 %(기본 70 = 35° · 급경사 겹 둘째 경계) */
|
|
steepPct?: number;
|
|
weights?: Partial<GuideWeights>;
|
|
/** 구간마다 펼침 상한(기본 30 만 ≈ 0.4 s · 못 닿음 ≤ 0.5 s) */
|
|
maxExpand?: number;
|
|
/** 구간마다 시간 상한 ms(기본 450 · 못 닿음 ≤ 0.5 s · 느린 기계 막음) */
|
|
maxMs?: number;
|
|
}
|
|
|
|
export interface GuideSegment {
|
|
/** 꼭짓점 [x, y, z] — 닿음이면 끝 = 다음 지점 · 아니면 가장 가까이 간 데까지 */
|
|
vertices: Point3[];
|
|
reached: boolean;
|
|
/** 평면 길이 m */
|
|
length: number;
|
|
/** 걸음 가운데 가장 큰 |기울기| 비율 */
|
|
maxGrade: number;
|
|
/** 급경사(문턱 이상) 지나는 길이 비율 0~1 */
|
|
steepRatio: number;
|
|
expanded: number;
|
|
}
|
|
|
|
export interface GuideResult {
|
|
segments: GuideSegment[];
|
|
ms: number;
|
|
}
|
|
|
|
export const GUIDE_DEFAULTS = {
|
|
step: 20,
|
|
maxGrade: 0.14,
|
|
steepPct: 70,
|
|
weights: { steep: 3, grade: 0.3, turn: 1, heuristic: 3 } as GuideWeights,
|
|
maxExpand: 300_000,
|
|
maxMs: 450,
|
|
};
|
|
|
|
const MARGIN_MIN = 200;
|
|
const MARGIN_MAX = 800;
|
|
const STEEP_SAMPLES = [0.25, 0.5, 0.75, 1];
|
|
|
|
/** 서버 지형 격자(`parseDtmGrid`) → 가이드 높이 원천(원천 칸 · 원점 그대로). */
|
|
export function guideTerrainFromGrid(grid: DtmGrid): GuideTerrain {
|
|
const n = grid.x.length;
|
|
const cell = n > 1 ? (grid.x[n - 1] - grid.x[0]) / (n - 1) : 2;
|
|
return {
|
|
heightAt: (x, y) => gridHeightAt(grid, x, y),
|
|
cell,
|
|
origin: [grid.x[0], grid.y[0]],
|
|
};
|
|
}
|
|
|
|
/** 지점 차례로 잇는 가이드 선 — 지점 사이 구간마다 A*. */
|
|
export function guideRoute(
|
|
points: Point2[],
|
|
terrain: GuideTerrain,
|
|
options: GuideOptions = {},
|
|
): GuideResult {
|
|
const started = performance.now();
|
|
const segments: GuideSegment[] = [];
|
|
for (let index = 0; index + 1 < points.length; index++) {
|
|
segments.push(guideSegment(points[index], points[index + 1], terrain, options));
|
|
}
|
|
return { segments, ms: performance.now() - started };
|
|
}
|
|
|
|
/** 두 점 사이 가이드 선 하나. */
|
|
export function guideSegment(
|
|
start: Point2,
|
|
goal: Point2,
|
|
terrain: GuideTerrain,
|
|
options: GuideOptions = {},
|
|
): GuideSegment {
|
|
const step = options.step ?? GUIDE_DEFAULTS.step;
|
|
const gmax = options.maxGrade ?? GUIDE_DEFAULTS.maxGrade;
|
|
const weights = { ...GUIDE_DEFAULTS.weights, ...options.weights };
|
|
const steepGrade = (options.steepPct ?? GUIDE_DEFAULTS.steepPct) / 100;
|
|
const source = terrain.cell ?? 2;
|
|
const cell = source * Math.max(1, Math.round(2 / source));
|
|
const origin = terrain.origin ?? [0, 0];
|
|
const za = terrain.heightAt(start[0], start[1]);
|
|
const zb = terrain.heightAt(goal[0], goal[1]);
|
|
if (!Number.isFinite(za) || !Number.isFinite(zb)) {
|
|
return measure([[start[0], start[1], za]], false, 0, null);
|
|
}
|
|
const span = Math.hypot(goal[0] - start[0], goal[1] - start[1]);
|
|
const margin = Math.min(MARGIN_MAX, Math.max(MARGIN_MIN, span / 2));
|
|
const limit = options.maxExpand ?? GUIDE_DEFAULTS.maxExpand;
|
|
let spent = 0; // 두 상자 합친 펼침 수
|
|
let late = false;
|
|
const deadline = performance.now() + (options.maxMs ?? GUIDE_DEFAULTS.maxMs);
|
|
let edge = false; // 상자 끝에 닿음 = 넓히면 길이 있을 수 있음
|
|
let found = search(margin);
|
|
if (!found.reached && edge && spent < limit && !late) found = search(margin * 2);
|
|
found.expanded = spent;
|
|
return found;
|
|
|
|
function search(pad: number): GuideSegment {
|
|
const i0 = Math.floor((Math.min(start[0], goal[0]) - pad - origin[0]) / cell);
|
|
const j0 = Math.floor((Math.min(start[1], goal[1]) - pad - origin[1]) / cell);
|
|
const w = Math.ceil((Math.max(start[0], goal[0]) + pad - origin[0]) / cell) - i0 + 1;
|
|
const h = Math.ceil((Math.max(start[1], goal[1]) + pad - origin[1]) / cell) - j0 + 1;
|
|
const n = w * h;
|
|
const START = n;
|
|
const GOAL = n + 1;
|
|
const zs = new Float32Array(n).fill(Infinity); // Infinity = 아직 안 읽음 · NaN = 밖
|
|
const steepState = new Uint8Array(n); // 0 모름 · 1 급경사 · 2 아님
|
|
const g = new Float64Array(n + 2).fill(Infinity);
|
|
const parent = new Int32Array(n + 2).fill(-1);
|
|
const dir = new Float32Array(n + 2).fill(NaN);
|
|
const closed = new Uint8Array(n + 2);
|
|
const heap = new MinHeap();
|
|
const nx = (id: number) => origin[0] + (i0 + (id % w)) * cell;
|
|
const ny = (id: number) => origin[1] + (j0 + Math.floor(id / w)) * cell;
|
|
const zAt = (id: number): number => {
|
|
let value = zs[id];
|
|
if (value === Infinity) {
|
|
value = terrain.heightAt(nx(id), ny(id));
|
|
zs[id] = Number.isFinite(value) ? value : NaN;
|
|
}
|
|
return value;
|
|
};
|
|
const steepSq = (2 * cell * steepGrade) ** 2;
|
|
const steepAt = (id: number): boolean => {
|
|
if (steepState[id] === 0) {
|
|
const i = id % w;
|
|
const j = (id - i) / w;
|
|
if (i < 1 || j < 1 || i >= w - 1 || j >= h - 1) steepState[id] = 2;
|
|
else {
|
|
const gx = zAt(id + 1) - zAt(id - 1);
|
|
const gy = zAt(id + w) - zAt(id - w);
|
|
steepState[id] = gx * gx + gy * gy >= steepSq ? 1 : 2; // NaN = 아님
|
|
}
|
|
}
|
|
return steepState[id] === 1;
|
|
};
|
|
const radius = step / cell;
|
|
// 걸음 표 — 격자 차 · 길이 · 방위 · 급경사 표본 차(1/4 · 1/2 · 3/4 · 끝)
|
|
const offI: number[] = [];
|
|
const offJ: number[] = [];
|
|
const offLen: number[] = [];
|
|
const offDir: number[] = [];
|
|
const sampleD: number[] = []; // 칸 번호 차
|
|
const reach = Math.ceil(radius + 0.5);
|
|
for (let dj = -reach; dj <= reach; dj++) {
|
|
for (let di = -reach; di <= reach; di++) {
|
|
if (Math.abs(Math.hypot(di, dj) - radius) > 0.5) continue;
|
|
offI.push(di);
|
|
offJ.push(dj);
|
|
offLen.push(Math.hypot(di, dj) * cell);
|
|
offDir.push(Math.atan2(dj, di));
|
|
for (const t of STEEP_SAMPLES) {
|
|
sampleD.push(Math.round(dj * t) * w + Math.round(di * t));
|
|
}
|
|
}
|
|
}
|
|
const steepShare = weights.steep / STEEP_SAMPLES.length;
|
|
const turnWeight = (weights.turn * step) / (Math.PI * Math.PI);
|
|
// 걸음 비용 — 막히면 Infinity
|
|
const cost = (
|
|
ax: number,
|
|
ay: number,
|
|
az: number,
|
|
bx: number,
|
|
by: number,
|
|
bz: number,
|
|
inDir: number,
|
|
): number => {
|
|
const len = Math.hypot(bx - ax, by - ay);
|
|
const dz = Math.abs(bz - az);
|
|
if (!(dz <= gmax * len)) return Infinity;
|
|
let hits = 0;
|
|
for (const t of STEEP_SAMPLES) {
|
|
const si = Math.round((ax + (bx - ax) * t - origin[0]) / cell) - i0;
|
|
const sj = Math.round((ay + (by - ay) * t - origin[1]) / cell) - j0;
|
|
if (si >= 0 && sj >= 0 && si < w && sj < h && steepAt(sj * w + si)) hits++;
|
|
}
|
|
const grade = len > 0 ? dz / len / gmax : 0;
|
|
let value = len * (1 + steepShare * hits + weights.grade * grade * grade);
|
|
if (Number.isFinite(inDir) && len > 0) {
|
|
let turn = Math.abs(Math.atan2(by - ay, bx - ax) - inDir);
|
|
if (turn > Math.PI) turn = 2 * Math.PI - turn;
|
|
value += turnWeight * turn * turn;
|
|
}
|
|
return value;
|
|
};
|
|
const relax = (
|
|
from: number,
|
|
to: number,
|
|
c: number,
|
|
ax: number,
|
|
ay: number,
|
|
bx: number,
|
|
by: number,
|
|
) => {
|
|
if (!Number.isFinite(c) || g[from] + c >= g[to]) return;
|
|
g[to] = g[from] + c;
|
|
parent[to] = from;
|
|
dir[to] = Math.atan2(by - ay, bx - ax);
|
|
heap.push(g[to] + weights.heuristic * Math.hypot(goal[0] - bx, goal[1] - by), to);
|
|
};
|
|
// 끝 걸음 — 한 걸음 안이면 정확한 끝점으로(기울기 검사)
|
|
const tryGoal = (from: number, ax: number, ay: number, az: number) => {
|
|
if (Math.hypot(goal[0] - ax, goal[1] - ay) <= step + 0.5 * cell) {
|
|
relax(
|
|
from,
|
|
GOAL,
|
|
cost(ax, ay, az, goal[0], goal[1], zb, dir[from]),
|
|
ax,
|
|
ay,
|
|
goal[0],
|
|
goal[1],
|
|
);
|
|
}
|
|
};
|
|
g[START] = 0;
|
|
tryGoal(START, start[0], start[1], za);
|
|
// 첫 걸음 — 정확한 시작점에서 반지름 걸음 고리
|
|
const ci = Math.round((start[0] - origin[0]) / cell) - i0;
|
|
const cj = Math.round((start[1] - origin[1]) / cell) - j0;
|
|
for (let j = cj - reach - 1; j <= cj + reach + 1; j++) {
|
|
for (let i = ci - reach - 1; i <= ci + reach + 1; i++) {
|
|
if (i < 0 || j < 0 || i >= w || j >= h) continue;
|
|
const id = j * w + i;
|
|
const bx = nx(id);
|
|
const by = ny(id);
|
|
if (Math.abs(Math.hypot(bx - start[0], by - start[1]) - step) > 0.5 * cell) continue;
|
|
const bz = zAt(id);
|
|
if (Number.isNaN(bz)) continue;
|
|
relax(START, id, cost(start[0], start[1], za, bx, by, bz, NaN), start[0], start[1], bx, by);
|
|
}
|
|
}
|
|
let best = START;
|
|
let bestDist = span;
|
|
while (heap.size > 0 && spent < limit && !late) {
|
|
const u = heap.pop();
|
|
if (closed[u]) continue;
|
|
closed[u] = 1;
|
|
if (u === GOAL) return finish(GOAL, true);
|
|
spent++;
|
|
if ((spent & 1023) === 0 && performance.now() > deadline) late = true;
|
|
const ui = u % w;
|
|
const uj = Math.floor(u / w);
|
|
const ux = nx(u);
|
|
const uy = ny(u);
|
|
const uz = zs[u];
|
|
const left = Math.hypot(goal[0] - ux, goal[1] - uy);
|
|
if (left < bestDist) {
|
|
bestDist = left;
|
|
best = u;
|
|
}
|
|
tryGoal(u, ux, uy, uz);
|
|
const gu = g[u];
|
|
const du = dir[u];
|
|
for (let k = 0; k < offI.length; k++) {
|
|
const vi = ui + offI[k];
|
|
const vj = uj + offJ[k];
|
|
if (vi < 0 || vj < 0 || vi >= w || vj >= h) {
|
|
edge = true;
|
|
continue;
|
|
}
|
|
const v = vj * w + vi;
|
|
if (closed[v]) continue;
|
|
let vz = zs[v];
|
|
if (vz === Infinity) vz = zAt(v);
|
|
if (vz !== vz) continue; // NaN = 밖
|
|
const len = offLen[k];
|
|
const dz = Math.abs(vz - uz);
|
|
if (dz > gmax * len) continue;
|
|
let hits = 0;
|
|
for (let s = 4 * k; s < 4 * k + 4; s++) {
|
|
const at = u + sampleD[s];
|
|
const state = steepState[at];
|
|
if (state === 1 || (state === 0 && steepAt(at))) hits++;
|
|
}
|
|
const grade = dz / len / gmax;
|
|
let c = len * (1 + steepShare * hits + weights.grade * grade * grade);
|
|
if (du === du) {
|
|
let turn = Math.abs(offDir[k] - du);
|
|
if (turn > Math.PI) turn = 2 * Math.PI - turn;
|
|
c += turnWeight * turn * turn;
|
|
}
|
|
const next = gu + c;
|
|
if (next >= g[v]) continue;
|
|
g[v] = next;
|
|
parent[v] = u;
|
|
dir[v] = offDir[k];
|
|
const ex = goal[0] - (ux + offI[k] * cell);
|
|
const ey = goal[1] - (uy + offJ[k] * cell);
|
|
heap.push(next + weights.heuristic * Math.sqrt(ex * ex + ey * ey), v);
|
|
}
|
|
}
|
|
return finish(best, false);
|
|
|
|
function finish(last: number, reached: boolean): GuideSegment {
|
|
const chain: Point3[] = [];
|
|
for (let id = last; id !== -1; id = parent[id]) {
|
|
if (id === START) chain.push([start[0], start[1], za]);
|
|
else if (id === GOAL) chain.push([goal[0], goal[1], zb]);
|
|
else chain.push([nx(id), ny(id), zs[id]]);
|
|
}
|
|
chain.reverse();
|
|
return measure(chain, reached, 0, (x, y) => {
|
|
const si = Math.round((x - origin[0]) / cell) - i0;
|
|
const sj = Math.round((y - origin[1]) / cell) - j0;
|
|
return si >= 0 && sj >= 0 && si < w && sj < h && steepAt(sj * w + si);
|
|
});
|
|
}
|
|
}
|
|
|
|
// 길이 · 최대 기울기 · 급경사 지남 비율(걸음마다 칸 간격 표본)
|
|
function measure(
|
|
vertices: Point3[],
|
|
reached: boolean,
|
|
expanded: number,
|
|
steepAt: ((x: number, y: number) => boolean) | null,
|
|
): GuideSegment {
|
|
let length = 0;
|
|
let maxGrade = 0;
|
|
let steepLength = 0;
|
|
for (let k = 1; k < vertices.length; k++) {
|
|
const [ax, ay, az] = vertices[k - 1];
|
|
const [bx, by, bz] = vertices[k];
|
|
const len = Math.hypot(bx - ax, by - ay);
|
|
if (len <= 0) continue;
|
|
length += len;
|
|
maxGrade = Math.max(maxGrade, Math.abs(bz - az) / len);
|
|
if (!steepAt) continue;
|
|
const parts = Math.max(1, Math.ceil(len / cell));
|
|
for (let p = 0; p < parts; p++) {
|
|
const t = (p + 0.5) / parts;
|
|
if (steepAt(ax + (bx - ax) * t, ay + (by - ay) * t)) steepLength += len / parts;
|
|
}
|
|
}
|
|
return {
|
|
vertices,
|
|
reached,
|
|
length,
|
|
maxGrade,
|
|
steepRatio: length > 0 ? steepLength / length : 0,
|
|
expanded,
|
|
};
|
|
}
|
|
}
|
|
|
|
/** 이진 힙(값 작은 것 먼저) — 타입 배열 · 늘어남. */
|
|
class MinHeap {
|
|
private keys = new Float64Array(1024);
|
|
private ids = new Int32Array(1024);
|
|
size = 0;
|
|
|
|
push(key: number, id: number): void {
|
|
if (this.size === this.keys.length) {
|
|
const keys = new Float64Array(this.size * 2);
|
|
keys.set(this.keys);
|
|
const ids = new Int32Array(this.size * 2);
|
|
ids.set(this.ids);
|
|
this.keys = keys;
|
|
this.ids = ids;
|
|
}
|
|
let at = this.size++;
|
|
while (at > 0) {
|
|
const up = (at - 1) >> 1;
|
|
if (this.keys[up] <= key) break;
|
|
this.keys[at] = this.keys[up];
|
|
this.ids[at] = this.ids[up];
|
|
at = up;
|
|
}
|
|
this.keys[at] = key;
|
|
this.ids[at] = id;
|
|
}
|
|
|
|
pop(): number {
|
|
const top = this.ids[0];
|
|
const key = this.keys[--this.size];
|
|
const id = this.ids[this.size];
|
|
let at = 0;
|
|
for (;;) {
|
|
let child = 2 * at + 1;
|
|
if (child >= this.size) break;
|
|
if (child + 1 < this.size && this.keys[child + 1] < this.keys[child]) child++;
|
|
if (this.keys[child] >= key) break;
|
|
this.keys[at] = this.keys[child];
|
|
this.ids[at] = this.ids[child];
|
|
at = child;
|
|
}
|
|
this.keys[at] = key;
|
|
this.ids[at] = id;
|
|
return top;
|
|
}
|
|
}
|