# 依頼 v9 の報告：ロボットが地図の上で自分で道を引く（2026-10-24）
書いた日時: 2026-10-24 11:55
依頼: unity/claude_code_request_v9_map_planner.md
やったこと: 0〜6 章を入れて、7 章 A（人のいない店で道を引く確かめ）を回した。
7 章 B（③ ロボット2台・種0 を1回）はエイイチが押す。
**先に「決めた値」と「気になる所」を書きます（6 章のあと）。読んでから B を押してください。**
====================================================================
1. 直したファイル
====================================================================
## 新しく作ったファイル（全文はこの報告のいちばん後ろ）
Assets/ServiceSim/Robot/GridPlanner.cs
地図（RobotMap）と、その上で道を引く道探し（GridPlanner）。
Assets/ServiceSim/Robot/MapPlanCheck.cs
確かめ A の中身。Unity のメニューからも、Unity の外からも同じものを呼ぶ。
Assets/ServiceSim/Robot/RobotPathLine.cs
いま引いている道を床に細い線で出す（6 章）。見た目だけ。
Assets/ServiceSim/Editor/ServiceSimMapPlanner.cs
メニュー「ServiceSim/Map Planner/③⑦ 確かめ A（地図で道を引く・回さない）」。
## 直したファイル
Store/JsonStoreLayout.cs
・地図の欄 mapJson（TextAsset）を足し、店の JSON と一緒に読む。
・読んだあとの確かめ（1 章）：2・1・0 の数が JSON の counts と同じ／
版の番号が店の形と同じ／1 マスの大きさが meta.settings.res と同じ／
受け渡し口の点・全卓の止まる位置・待機場所が 1 以上のマス。
どれか合わなければ Compose が止まる（店を建てない）。前と同じ決まり。
・Build Store の報告に地図の行が出る。
・MapGeneration を外から読めるようにした（run_info.json に出すため）。
Store/StorePlan.cs / Store/StoreBuilder.cs
読んだ地図を plan.robotMap に持たせ、StoreBuilder.RobotMap で読めるようにした。
Robot/RobotSpec.cs
・enum PathMode { MapPlanner, SiteRoutes } を足し、pathMode を足した（既定 MapPlanner）。
・2〜4 章の数字を足した（narrowCellCost 3 / centerWeight 2 / preferClearM 0.6 /
replanMinIntervalSec 0.5 / detourAfterSec 3 / robotPathAheadM 4 など）。
・UseMapPlanner と PlannerSettings() を足した。
・走り・加速・減速・向きを変える速さ・前のセンサー・動き出すまでの1秒・
卓での25秒・60秒・600秒は**何も変えていない**。
Robot/ServingRobotController.cs（いちばん大きく直した）
・MapPlanner のときは SetDestination が NavMesh ではなく GridPlanner を呼ぶ。
行き先は卓の止まる位置（または受け渡し口）そのもので、サイトの routes の点は
たどらない。NavMesh はロボットの道探しには使わない。
・PathAhead・CurrentLegSegment は、サイトの道順でも地図で引いた道でも
「これから進む道」を返すようにした（人のよけ方は変えていない）。
まとめて見る WayCount / WayIndex / WayPoint を足した。
・3 章：前がふさがって 3 秒たったら回り込む（UpdateDetour / TryDetour）。
近く 3 m の人・ロボットをその 1 回だけ通れないマスとして置く
（人は 人の半径＋ロボットの半径＋0.1 m、ロボットは 2 台の半径の和＋0.1 m）。
延びた距離が もとの残り×2＋5 m より長い道は取らずに待つ。
引けなければ 3 秒ごとに探し直す。止まる位置そのものに人がいるときは回り込まない。
・4 章：YieldRank（料理を載せている 0 ＞ 空で戻る 1、同じなら 1 号機が先）と
UpdateRobotYieldMap。優先が低い側は、相手が 3 m 以内に来て先の道（4 m 分）が
かぶったら、相手の道を重くして引き直す。かぶらない道が引けないときは、
2.5 m 以内で相手の道から 0.6 m 以上離れた通れるマスへ動いて待つ（最大 20 秒）。
向かい合って 10 秒止まったら、引き直しを待たずによける場所へ動く。
・引き直しは 1 台につき 0.5 秒に 1 回まで（replanMinIntervalSec）。
・MapPlanner では「止まって10秒ごとに引き直す」と escape を使わない。
・SiteRoutes のときだけ 戻りの線（useReturnLine）・横へどく点（yieldOrder）が働く。
・記録：Replans / Detours / RobotYields / PlanMsMax / SiteRouteM / TripEndS /
MapSnapStarts / PlanFails を足した。
・6 章：UpdatePathLine で床の線を出す。
Experiment/TripLogger.cs
・trips.csv の**後ろに**足した（今の列は 1 つも動かしていない）:
trip_end_s, path_mode, replans, detours, robot_yields,
site_route_m, detour_extra_m, narrow_used, plan_ms_max
・stops.csv の resolved に「回り込んで動き出した」「よける場所へ動いた」を足した。
止まりの原因の分け方は今のまま。
・run_info.json に path_mode / map_generation / narrow_cell_cost / center_weight /
prefer_clear_m / detour_after_sec / plan_ms_max_per_trip / map_snap_starts /
plan_fails を足した。
Experiment/ExperimentRunner.cs
・上の列に値を入れる。
・角の箱は MapPlanner のときは入れない（下の「気になる所 (4)」）。
Store/KitchenMonitor.cs
6 章：ロボットのいまの様子に 走る／止まる／回り込む／よける を出す。
Service/ServiceOps.cs
1 行だけ：yields.csv の in_corner を MapPlanner では立てない（角の箱と同じ理由）。
## 写したファイル（0 章）
Store/Layouts/summary.json ← site_copy_2fa10547a2b7/summary.json
Store/Layouts/p1_efficient_corner.json ← 同 unity/
Store/Layouts/p5_crammed.json ← 同 unity/
Store/Layouts/p1_efficient_corner_map.json ← 同 unity/（新しい）
Store/Layouts/p5_crammed_map.json ← 同 unity/（新しい）
Experiment/serving-robot-params.json ← site_copy_2fa10547a2b7/
新しい 2 本に .meta を作り、StoreLayout_p1_efficient_corner.asset と
StoreLayout_p5_crammed.asset の mapJson に結び、fetchedAt を
「2026-10-24 11:00:00 JST（版 2fa10547a2b7）」に直した。
RobotSpec.asset に v9 の値を書き足した。
4 つの版の番号はぜんぶ 2fa10547a2b7 でそろっている（写したファイルを読んで確かめた）:
params の layout_generation 2fa10547a2b7
p1: 店 2fa10547a2b7 / summary 2fa10547a2b7 / 地図 2fa10547a2b7（res 0.025 / 0.025）
p5: 店 2fa10547a2b7 / summary 2fa10547a2b7 / 地図 2fa10547a2b7（res 0.025 / 0.025）
回すときの確かめも 3 か所でかかる:
店 ↔ summary JsonStoreLayout.CheckGeneration（前からある）
店 ↔ 地図 JsonStoreLayout.ReadMap（今回足した）
店 ↔ 試算の値 ExperimentRunner の simParams.Load（前からある）
そろわなければ建てない・回さない。
====================================================================
2. 確かめ A の結果（回した）
====================================================================
回した所: Unity の外（mono）。
Unity Editor がプロジェクトを押さえていて 2 つめの Unity を立てられないので、
Assets の MapPlanCheck.cs / GridPlanner.cs / SimpleJson.cs を**そのまま**
Unity 同梱の mono で組み立てて回した。Unity のメニュー
「ServiceSim/Map Planner/③⑦ 確かめ A」が呼ぶのと**同じ MapPlanCheck.Run** なので、
Unity で押しても同じ数字が出るはずです（押して確かめてください）。
## ③ p1_efficient_corner
地図: 版 2fa10547a2b7 / 960 × 640 = 614400 マス / 1 マス 0.025 m
マスの数: 2（ふだんの道）148223 / 1（通れる幅だけ）30397 / 0（通れない）435780
→ 地図の JSON の counts と **一致**
受け渡し口の前 (15.3, 5.95) → マスの種類 2
狭い道を使った卓 引いた道 0 卓 / サイトの routes の use 0 卓 → **一致**
道が引けない卓 0 / 42
長さの比（引いた道 ÷ サイトの length） 最小 0.959 / 平均 1.019 / 最大 1.057
道探しの時間 平均 13.2 ms / 中央 11.2 ms / 最大 35.5 ms
出発点・行き先を寄せた卓 0 / 42
待機場所1 (16.9, 6.05) マスの種類 1 → 引けた（1.605 m・0.5 ms・狭い道を使った）
待機場所2 (17.7, 6.05) マスの種類 1 → 引けた（2.403 m・0.9 ms・狭い道を使った）
待機場所3 (18.5, 6.05) マスの種類 1 → 引けた（3.202 m・1.1 ms・狭い道を使った）
→ 確かめ A は通った
## ⑦ p5_crammed
地図: 版 2fa10547a2b7 / 960 × 640 = 614400 マス（counts と一致）
狭い道を使った卓 引いた道 24 卓 / サイトの routes の use 24 卓 → **一致**
道が引けない卓 0 / 42
長さの比 最小 0.959 / 平均 1.037 / 最大 1.227
道探しの時間 平均 30.3 ms / 中央 22.6 ms / 最大 79.4 ms
出発点・行き先を寄せた卓 0 / 42
待機場所1〜3 ぜんぶ 引けた（1.6 / 2.4 / 3.2 m・0.5〜1.1 ms・狭い道を使った）
→ 確かめ A は通った
長さの比が 1 を少し超えるのは、真ん中寄りにした分です（依頼どおり）。
最小が 0.959（サイトより少し短い）になるのは、サイトの折れ線を
マス目の 8 方向でたどり直すと斜めが少しまっすぐになるからです。
## 道探しそのものの確かめ（確かめ A では通らない道を見た）
○ 1. 同じ条件なら毎回同じ道 点 9 個・10.825 m を 2 回引いて同じ（乱数なし）
○ 2. 人を通れないマスにすると道が変わる 10.825 m → 11.697 m
○ 3. 重いマスを差し込んでも引ける 10.825 m → 16.145 m（別の道を選んだ）
○ 4. 行き先をふさぐと引けない 「つながっていない」
○ 5. 出発点が 0 のマスでも寄せて引ける (10.0, 4.5) マスの種類 0 → 0.088 m 寄せて引けた
○ 6. 曲がり角の点がすべて通れるマスの上 はみ出し 0 点
○ 7. 曲がり角だけにしても長さが暴れない マスをたどった 11.122 m → 縮めた 10.825 m
## 出力フォルダ
unity/screens/
20261024_1150_v9_checkA_map_planner.txt 確かめ A の全文
20261024_1150_v9_planner_unit_checks.txt 上の 7 つの確かめ
20261024_1150_v9_map_plan_p1_efficient_corner.csv 卓ごと（use・長さ・比・角の数・ms）
20261024_1150_v9_map_plan_p5_crammed.csv
====================================================================
3. 決めきれなかった値・この依頼どおりにすると困る所
====================================================================
（依頼の「報告」のとおり、ここを読んでから B を押してください。
(2) と (4) はエイイチに決めてほしいところです）
## (1) 地図の free_class は「中心を置ける所」で、通路の幅のぶんもう削ってある
free_class は体の半径（0.25 m）ではなく、**道の幅の半分**（1.0 m の道なら 0.5 m、
0.8 m の道なら 0.4 m）で削ってあります。地図を横断して測った実際の値:
③ 通路 A01（幅 1.2 m） 中心を置ける帯 0.45 m（うち 2 のマスは 0.25 m）
⑦ 通路 A01（幅 0.95 m） 中心を置ける帯 0.20 m（ぜんぶ 1 のマス）
2 台がすれ違うには中心どうし 0.5 m 要るので、**通路の中では 2 台は並べません**。
体だけで見れば 1.2 m の通路には 0.75 m の帯があって並べるのですが、
依頼の「地図を塗り直さない・通れる所を足さない」に従って free_class だけを使いました。
その結果、4 章の「相手の道とかぶらない道を選ぶ」「0.6 m 横へどく」は
**細い通路の中では成り立ちません**（どいても 0.225 m / 0.1 m しか離れられない）。
通路の中で正面から出会ったら、片方が通路の外へ出るか、20 秒で待つのをやめて進むか、
60 秒で立ち往生になります。
ただし、v8b の比べる回（20261024_1004）の stops.csv を見ると、
**60 秒の止まり 51 回（ロボットどうし）は全部が細い通路の外**でした
（aisle の欄が空・in_narrow が 0）。止まった所の内訳:
(10.0, 4.5) 11回 (10.5, 4.5) 9回 ← 通路 A03 の入口の手前
(17.0, 2.5) 6回 (19.0, 2.5) 6回 ← レジ・入口のあたり
(13.0, 8.0) 5回 (13.0, 10.0) 5回 (13.5, 7.5) 4回 ← 太い通路
(17.5, 2.5) 3回 (16.0, 4.0) 1回 (5.0, 4.5) 1回
どの所でも 2.5 m 以内に「0.6 m どける余地のあるマス（clear ≥ 850 mm）」が
8〜17257 個ありました。だから 4 章は**働く場所で働く**見込みです。
これは「このままでよい」という報告で、直してほしいことではありません。
## (2) ロボットが、地図では通れないマス（free_class 0）に立っていることがある ★決めてほしい
いちばん大きな見落としだと思います。依頼に決まりが無かったので、こちらで決めました。
上の 51 回のうち **36 回**は、地図では free_class 0 の所で止まっていました
（clear は 287〜309 mm しかない）。(10.0,4.5) (10.5,4.5) (17.0,2.5) (19.0,2.5)
(17.5,2.5) (5.0,4.5) がそれです。
なぜそうなるか: NavMesh は体の半径 0.25 m で焼いてあるので、卓の角から 0.25 m まで
面があります。いっぽう free_class は道の幅（0.4 / 0.5 m）で削ってあるので、
通路の入口で待つ点（AisleWaitPoint）や横へどいた点は NavMesh には乗るけれど
free_class では 0 です。そこから MapPlanner で道を引こうとすると
「出発点が通れない」で 1 本も引けません。
決めたこと（勝手に決めた分）:
地図は塗り直さず、**出発点だけ近くの通れるマスへ寄せて引く**（mapSnapM 1.0 m 以内、
いちばん近いマス。同じ距離ならマスの番号の小さい方）。
寄せた回数は run_info.json の map_snap_starts に出します。
確かめ 5（上）で、(10.0, 4.5) から 0.088 m 寄せて引けることを見ています。
残っている気になる所:
通路の入口で 1 台ずつ入る決まり（aisleYielding）は「変えないもの」なので、
**待つ点はいまも NavMesh で決めています**（AisleWaitPoint）。つまり
その点は free_class 0 のことがあり、毎回こちらで寄せが入ります。
入口の待つ点も地図のマスから選ぶようにするなら、言ってください
（「変えないもの」に入っているので、勝手には変えませんでした）。
## (3) 依頼に数字が無かったので、こちらで決めた値
headOnDotMax −0.5 「向かい合っている」と見る 2 台の向きの内積の上限。
−1 が真正面、−0.5 は 120 度より正面寄り
robotPathCostAdd 6 相手の先の道を重くする強さ（1 歩の重さに足す）。
narrowCellCost 3 より大きくして、相手の道を避けさせた
planMaxExpand 200000 1 回の道探しで広げるマスの上限。超えたら引けないことにする。
回が止まらないようにするための保険（ふだんは当たらない）
mapSnapM 1.0 m 上の (2) の寄せる半径
planSimplifyMaxM 6 m 曲がり角だけにするとき、近道 1 本の長さの上限
よける場所の候補 3 つまで・互いに 0.5 m 以上離して、近い順に試す
robot_yields の数え方
「よける場所へ動いた回数」＋「相手の道をよけた道に引き直した回数」。
同じ相手との 1 回の出会いは 1 回だけ数える（近くにいるあいだ 0.5 秒ごとに
引き直すので、毎回数えると膨らむ）。
よける場所へ動いた分は yields.csv にも 1 行ずつ出ます（前と同じ形）。
## (4) 角の箱（cornerMin/Max）を MapPlanner で切ると、比べる手がかりが 1 つ減る ★決めてほしい
依頼どおり MapPlanner では働かないようにしました。ただしこれは
**測る範囲だけで、動きには何も効きません**（止まった所が角の中かどうかを数えるだけ）。
切ると summary.json の corner_stops / corner_stop_s / corner_stop_time_share が 0 になり、
v8b の「止まった時間の約3分の1がこの角」と並べられなくなります。
残したいなら、ExperimentRunner の 1 行（`&& !robotSpec.UseMapPlanner`）と
ServiceOps の 1 行を外すだけです。どちらにしますか。
（今は依頼のとおり「切る」で入っています）
## (5) 回す時間が延びる見込み
1 回の道探しは ③ 平均 13 ms・最大 35 ms、⑦ 平均 30 ms・最大 79 ms（上の確かめ A）。
引き直しを 1 台 0.5 秒に 1 回までに抑えたので、最悪で
2 台 × 2 回/秒 × 30 ms = 120 ms（店の中の 1 秒あたり）。
v8b の回は 10825 秒を 828 秒（13.1 倍）で回していたので、最悪で **+22 分**くらい。
ふだんは出会っていないので引き直さないので、実際はもっと少ないはずです。
run_info.json の plan_ms_max_per_trip で実際の値が見られます。
重くならないように入れた工夫:
・「2 のマスだけで行けるか」は、地図を読んだときに作る「つながりの組」で
一瞬で見る（A* を空振りさせない）。⑦は卓の半分が狭い道なので、これが効きます。
・作業用の配列は作り直さず使い回す。道探しは 2 台で 1 つを使い回す。
## (6) 待機場所は 1 のマス（狭い道）
待機場所（16.9 / 17.7 / 18.5, 6.05）は壁ぎわなので free_class 1（clear 462 mm）です。
依頼の「待機場所の真ん中も1以上（壁ぎわなので1になる）」のとおりでした。
そこへ引くと「狭い道を使った」になりますが、MapPlanner の往復は
受け渡し口の点（2 のマス）で終わるので、trips.csv の narrow_used には出ません。
別件で気づいたこと（v9 では触っていません）:
GoStandby は NavMeshAgent.SetDestination で待機場所へ向かわせていますが、
NavMeshAgent は updatePosition = false で、Update() は Idle / Done のときに
先に return します。つまり **transform は動かず、待機場所へ実際には動いていない**
はずです。4 章の優先の順に「待機場所へ動いている」があるので、
一度確かめた方がよさそうです。今回は「変えないもの」なので手を入れていません。
====================================================================
4. 組み立ての確かめ
====================================================================
Unity Editor がプロジェクトを押さえているので、Unity を立てずに
Unity 同梱の csc（Roslyn）で ServiceSim の .cs 49 本を**ぜんぶ**組み立てて、
エラーが 0 であることを確かめています（直すたびに毎回）。
なので Unity に切り替えたときに赤い字が出る心配は小さいですが、
Console は見てください（.meta の作られ方など、Unity 側でしか分からない分があります）。
====================================================================
5. エイイチにお願いすること
====================================================================
1. Unity に切り替える（新しい .cs が取り込まれる）。Console にエラーが無いか見る。
2. ServiceSim/Build Store で ③ を建て直す。
報告（store_report）に「地図 (map.json): 版 2fa10547a2b7 / マス 960 × 640 …
JSON の counts と 一致」と「止まる位置: 42 卓すべて 1 以上のマス」が出るはず。
3. ServiceSim/Map Planner/③⑦ 確かめ A（地図で道を引く・回さない）を押す。
上の「2. 確かめ A の結果」と同じ数字が出るはず（③ 0 卓 / ⑦ 24 卓・一致）。
4. 上の「3.」の (2) と (4) を決める。
5. ServiceSim/Site Run/③ スタッフ4人・ロボット2台 を 1 回（7 章 B）。
B で比べる相手は 20261024_p1_efficient_corner_fam_p1_s4_r2_a42_s0_1004（v8b）。
見るのは:
60 秒の止まりの数と、そのうちロボットどうしの数 v8b: 57 回・51 回
逃がし弁 v8b: あきらめた 61・移した 54・卓を空けた 19
1 往復で止まった秒（平均）、ロボットが運んだ皿の割合
detours・robot_yields・detour_extra_m の平均（新しい列）
最後まで回り切る（全員が店を出る）。来た組は 64 組
B の結果を見せてもらったら、段階Bの18回の回し直しを頼むかどうかを相談します。
B が最後まで回り切ったら Check-in して番号を報告します。
====================================================================
6. 新しいファイルの全文
====================================================================
-------------------------------------------------------------
--- Assets/ServiceSim/Robot/GridPlanner.cs
-------------------------------------------------------------
using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Globalization;
using System.IO;
using System.IO.Compression;
using System.Text;
using UnityEngine;
namespace ServiceSim
{
///
/// ロボットが走れる所の地図（サイトの {名前}_map.json）。
///
/// 2026-10-24 にエイイチと決めたこと（依頼 v9）:
/// ロボットは**動ける範囲を地図で持ち、道はその都度自分で引く**。
/// 地図はサイトが正（版 2fa10547a2b7 で公開）。Unity は塗り直さない・通れる所を足さない。
/// NavMesh はロボットの道探しには使わない（人に自分をよけてもらうためだけに残す）。
///
/// マスの意味（JSON の free_class_values そのまま）:
/// 2 … ふだん走らせる幅の道（normal_width 1.0 m）が通れる
/// 1 … 通れる幅の道だけ（pass_width 0.8 m）通れる
/// 0 … 通れない
/// clear_mm … そのマスから、いちばん近い障害物までの距離（mm）
///
/// 大事な読み方: free_class は「**ロボットの中心を置ける所**」で、
/// 道の幅のぶんをもう削ってある（体の半径 0.25 m ではなく、道の幅の半分で削ってある）。
/// だから 1.2 m の通路でも、中心を置ける帯は 0.45 m しかない。
/// 2 台（中心どうし 0.5 m）は通路の中では並べない。
///
/// 座標: マス (ix, iy) の中心は x=(ix+0.5)*res、y=(iy+0.5)*res。
/// JSON の x → Unity の X、y → Unity の Z。番号は iy*nx+ix。
///
public class RobotMap
{
public readonly string generation = "";
public readonly float res;
public readonly int nx, ny;
public readonly float normalWidthM, passWidthM;
readonly byte[] _class;
readonly ushort[] _clearMm;
/// JSON が持っている counts（読んだものと突き合わせる）
readonly int[] _jsonCounts = new int[3];
readonly int[] _realCounts = new int[3];
/// つながりの番号。2 だけでたどれる組と、1 以上でたどれる組
int[] _region2, _region1;
GridPlanner _planner;
RobotMap(string generation, float res, int nx, int ny,
float normalWidthM, float passWidthM,
byte[] cls, ushort[] clearMm, int[] jsonCounts)
{
this.generation = generation ?? "";
this.res = res;
this.nx = nx;
this.ny = ny;
this.normalWidthM = normalWidthM;
this.passWidthM = passWidthM;
_class = cls;
_clearMm = clearMm;
for (int i = 0; i < 3; i++) _jsonCounts[i] = jsonCounts[i];
for (int i = 0; i < cls.Length; i++)
{
int c = cls[i];
if (c >= 0 && c <= 2) _realCounts[c]++;
}
}
public int CellCount { get { return nx * ny; } }
/// この地図を使う道探し（1つを使い回す。配列が大きいので台ごとに作らない）
public GridPlanner Planner
{
get
{
if (_planner == null) _planner = new GridPlanner(this);
return _planner;
}
}
// ---------------- 読む ----------------
///
/// 地図の JSON を読む。読めなければ error に理由を入れて null を返す。
/// free_class・clear_mm は base64 → gzip を解く（JSON の encoding のとおり）。
///
public static RobotMap Read(string json, out string error)
{
error = null;
JsonValue root = JsonValue.Parse(json, out error);
if (root == null || !root.IsObject)
{
error = "地図の JSON を読めません: " + error;
return null;
}
float res = root["res"].F(0f);
int nx = root["nx"].I(0);
int ny = root["ny"].I(0);
if (res <= 0.0001f || nx <= 0 || ny <= 0)
{
error = "地図の res / nx / ny がありません";
return null;
}
int want = nx * ny;
byte[] cls = Unpack(root["free_class"].S(""), 1, want, "free_class", out error);
if (cls == null) return null;
byte[] raw = Unpack(root["clear_mm"].S(""), 2, want, "clear_mm", out error);
if (raw == null) return null;
ushort[] clear = new ushort[want];
for (int i = 0; i < want; i++)
clear[i] = (ushort)(raw[i * 2] | (raw[i * 2 + 1] << 8)); // 小さい桁が先
int[] counts = new int[3];
JsonValue cj = root["counts"];
for (int c = 0; c <= 2; c++)
counts[c] = cj[c.ToString(CultureInfo.InvariantCulture)].I(-1);
return new RobotMap(root["meta"]["generation"].S(""), res, nx, ny,
root["normal_width"].F(0f), root["pass_width"].F(0f),
cls, clear, counts);
}
static byte[] Unpack(string b64, int bytesPerCell, int cells, string what, out string error)
{
error = null;
if (string.IsNullOrEmpty(b64))
{
error = what + " がありません";
return null;
}
byte[] packed;
try { packed = Convert.FromBase64String(b64); }
catch (Exception e)
{
error = what + " の base64 を解けません: " + e.Message;
return null;
}
byte[] data;
try
{
using (MemoryStream src = new MemoryStream(packed))
using (GZipStream gz = new GZipStream(src, CompressionMode.Decompress))
using (MemoryStream dst = new MemoryStream(cells * bytesPerCell))
{
byte[] buf = new byte[1 << 16];
int n;
while ((n = gz.Read(buf, 0, buf.Length)) > 0) dst.Write(buf, 0, n);
data = dst.ToArray();
}
}
catch (Exception e)
{
error = what + " の gzip を解けません: " + e.Message;
return null;
}
if (data.Length != cells * bytesPerCell)
{
error = what + " の長さが合いません: " + data.Length + " バイト / 要るのは " +
(cells * bytesPerCell) + " バイト（nx × ny × " + bytesPerCell + "）";
return null;
}
return data;
}
// ---------------- マスを引く ----------------
public int Index(int ix, int iy) { return iy * nx + ix; }
/// その点のマスの番号。店の外なら -1
public int CellAt(Vector3 p)
{
int ix = Mathf.FloorToInt(p.x / res);
int iy = Mathf.FloorToInt(p.z / res);
if (ix < 0 || ix >= nx || iy < 0 || iy >= ny) return -1;
return iy * nx + ix;
}
/// マスの真ん中（Unity の XZ。y は 0）
public Vector3 Center(int index)
{
int iy = index / nx;
int ix = index - iy * nx;
return new Vector3((ix + 0.5f) * res, 0f, (iy + 0.5f) * res);
}
/// そのマスの種類（2 / 1 / 0）。店の外は 0
public int ClassOf(int index)
{
return index < 0 || index >= _class.Length ? 0 : _class[index];
}
/// その点の種類（店の外は 0）
public int ClassAt(Vector3 p) { return ClassOf(CellAt(p)); }
/// いちばん近い障害物までの距離（m）。店の外は 0
public float ClearM(int index)
{
return index < 0 || index >= _clearMm.Length ? 0f : _clearMm[index] * 0.001f;
}
/// 読んだマスの数（種類ごと）
public int CountOf(int cls) { return cls >= 0 && cls <= 2 ? _realCounts[cls] : 0; }
/// JSON の counts と読んだ数が合っているか
public bool CountsMatch
{
get
{
for (int c = 0; c <= 2; c++)
if (_jsonCounts[c] != _realCounts[c]) return false;
return true;
}
}
///
/// 斜めに進めるか（両隣が通れるときだけ。サイトの道の引き方と同じ）。
/// ix,iy → jx,jy は斜め隣のこと
///
public bool DiagonalOpen(int ix, int iy, int jx, int jy, int minClass)
{
return _class[iy * nx + jx] >= minClass && _class[jy * nx + ix] >= minClass;
}
// ---------------- つながり（2 だけで行けるかを一瞬で見る）----------------
///
/// 「行き先まで 2 のマスだけで行けるか」を、毎回 A* を空振りさせずに見るための組分け。
/// 道の引き方（8方向・斜めは両隣が通れるときだけ）とまったく同じつなぎ方で塗る。
///
public void BuildRegions()
{
if (_region2 != null) return;
_region2 = Flood(2);
_region1 = Flood(1);
}
int[] Flood(int minClass)
{
int n = nx * ny;
int[] region = new int[n];
for (int i = 0; i < n; i++) region[i] = -1;
int[] stack = new int[n];
int next = 0;
for (int s = 0; s < n; s++)
{
if (region[s] >= 0 || _class[s] < minClass) continue;
int id = next++;
int top = 0;
stack[top++] = s;
region[s] = id;
while (top > 0)
{
int i = stack[--top];
int iy = i / nx;
int ix = i - iy * nx;
for (int d = 0; d < 8; d++)
{
int jx = ix + GridPlanner.Dx[d];
int jy = iy + GridPlanner.Dy[d];
if (jx < 0 || jx >= nx || jy < 0 || jy >= ny) continue;
int j = jy * nx + jx;
if (region[j] >= 0 || _class[j] < minClass) continue;
if (GridPlanner.Dx[d] != 0 && GridPlanner.Dy[d] != 0 &&
!DiagonalOpen(ix, iy, jx, jy, minClass)) continue;
region[j] = id;
stack[top++] = j;
}
}
}
return region;
}
/// 2 つのマスが、その種類だけでつながっているか
public bool SameRegion(int a, int b, int minClass)
{
BuildRegions();
int[] r = minClass >= 2 ? _region2 : _region1;
if (a < 0 || b < 0 || a >= r.Length || b >= r.Length) return false;
return r[a] >= 0 && r[a] == r[b];
}
// ---------------- 通れるマスへ寄せる ----------------
///
/// その点のいちばん近い「通れるマス」。
///
/// なぜ要るか: ロボットがいま立っている所が、地図では 0（通れない）のことがある。
/// NavMesh は体の半径 0.25 m で焼いてあるのに、free_class は道の幅（0.4 m／0.5 m）で
/// 削ってあるので、通路の端・通路の入口で待つ点・横へどいた点は 0 になる。
/// （v8b の 20261024_1004 の回では、60 秒の止まり 51 回のうち 36 回がそういう所だった）
/// 地図は塗り直さず、**出発点だけを近くの通れるマスへ寄せる**。寄せた回数は記録に出す。
///
/// 見つからなければ -1。
///
public int NearestPassable(Vector3 p, int minClass, float searchM)
{
int here = CellAt(p);
if (here >= 0 && _class[here] >= minClass) return here;
int ix0 = Mathf.FloorToInt(p.x / res);
int iy0 = Mathf.FloorToInt(p.z / res);
int maxR = Mathf.Max(1, Mathf.CeilToInt(searchM / res));
int best = -1;
float bestD2 = float.MaxValue;
for (int r = 1; r <= maxR; r++)
{
for (int iy = iy0 - r; iy <= iy0 + r; iy++)
{
if (iy < 0 || iy >= ny) continue;
for (int ix = ix0 - r; ix <= ix0 + r; ix++)
{
if (ix < 0 || ix >= nx) continue;
// 輪の上だけ見る（中は前の回で見た）
if (Mathf.Abs(ix - ix0) != r && Mathf.Abs(iy - iy0) != r) continue;
int i = iy * nx + ix;
if (_class[i] < minClass) continue;
Vector3 c = Center(i);
float dx = c.x - p.x, dz = c.z - p.z;
float d2 = dx * dx + dz * dz;
// 同じ距離なら番号の小さい方（毎回同じ答えにする）
if (d2 < bestD2 - 1e-9f) { bestD2 = d2; best = i; }
}
}
if (best >= 0) return best; // 輪を1つ見終わって見つかれば、それが最短
}
return best;
}
// ---------------- 報告 ----------------
public string Report()
{
StringBuilder sb = new StringBuilder();
sb.AppendLine(" 地図 (map.json): 版 " + generation +
" / マス " + nx + " × " + ny + " = " + CellCount +
" / 1 マス " + F(res) + " m");
sb.AppendLine(" ふだんの道の幅 " + F(normalWidthM) + " m / 通れる幅 " + F(passWidthM) + " m");
sb.AppendLine(" マスの数: 2（ふだんの道）" + _realCounts[2] +
" / 1（通れる幅だけ）" + _realCounts[1] +
" / 0（通れない）" + _realCounts[0] +
" … JSON の counts と " + (CountsMatch ? "一致" : "ちがう") +
(CountsMatch ? "" : "（JSON: 2=" + _jsonCounts[2] + " 1=" + _jsonCounts[1] +
" 0=" + _jsonCounts[0] + "）"));
return sb.ToString().TrimEnd();
}
static string F(float v) { return v.ToString("0.###", CultureInfo.InvariantCulture); }
}
///
/// 地図の上で道を引く（依頼 v9 の 2 章）。ロボットだけが使う。NavMesh は使わない。
///
/// 決まり:
/// ・8 方向。斜めは両隣が通れるときだけ（サイトと同じ）
/// ・行き先まで 2 のマスだけで行けるなら 2 だけで引く。行けなければ 1 も使い、
/// 1 のマスを narrowCellCost 倍（目安 3 倍）重くする
/// ・通路の真ん中寄りを走る: 1 歩の重さに
/// centerWeight × max(0, preferClearM − clear) / preferClearM を足す
/// ・乱数を使わない。同じ条件なら毎回同じ道（同じ重さのときは h → マスの番号の順）
/// ・引けた道は曲がり角の点だけに縮める。近道は「もとの道で通っていた所より
/// 障害物に近づかない所」だけ（端に寄った線に戻さないため）
///
/// 回り込み・よけのために、その 1 回だけ「通れないマス」「重いマス」を差し込める。
/// 配列が大きいので、1 つを台どうしで使い回す（道探しはその場で終わるので取り合わない）。
///
public class GridPlanner
{
/// 8 方向の順（決まった順。乱数を使わない）
public static readonly int[] Dx = { 1, 1, 0, -1, -1, -1, 0, 1 };
public static readonly int[] Dy = { 0, 1, 1, 1, 0, -1, -1, -1 };
/// 道探しの結果
public enum Status
{
Ok, // 引けた
StartBlocked, // 出発点が通れないマス（寄せても見つからない）
GoalBlocked, // 行き先が通れないマス
NoPath, // つながっていない
TooMany // 探しすぎ（maxExpand を超えた）
}
/// 道の引き方の数字（RobotSpec から写す）
public struct Settings
{
public float narrowCellCost;
public float centerWeight;
public float preferClearM;
public int maxExpand;
public float snapM;
/// 曲がり角だけに縮めるとき、1 本の近道の長さの上限（m）
public float simplifyMaxM;
public static Settings Default
{
get
{
Settings s = new Settings();
s.narrowCellCost = 3f;
s.centerWeight = 2f;
s.preferClearM = 0.6f;
s.maxExpand = 200000;
s.snapM = 1.0f;
s.simplifyMaxM = 6f;
return s;
}
}
}
readonly RobotMap _map;
readonly int _nx, _ny, _n;
readonly float _res;
// ---- 道探しの作業場（毎回作り直さない）----
readonly float[] _g;
readonly int[] _from;
readonly int[] _seen; // この回で見たか（_runStamp と比べる）
readonly int[] _closed;
int _runStamp;
// ---- 差し込んだ「通れないマス」「重いマス」----
readonly int[] _blockStamp;
readonly int[] _costStamp;
readonly float[] _costAdd;
int _markStamp;
bool _hasMarks;
// ---- 二分の山（f が小さい順。同じなら h → 番号）----
int[] _heap;
float[] _heapF, _heapH;
int _heapCount;
readonly List _points = new List();
readonly List _cells = new List();
readonly Stopwatch _watch = new Stopwatch();
public GridPlanner(RobotMap map)
{
_map = map;
_nx = map.nx; _ny = map.ny; _n = map.nx * map.ny; _res = map.res;
_g = new float[_n];
_from = new int[_n];
_seen = new int[_n];
_closed = new int[_n];
_blockStamp = new int[_n];
_costStamp = new int[_n];
_costAdd = new float[_n];
_heap = new int[1024];
_heapF = new float[1024];
_heapH = new float[1024];
// 印は 1 から始める。0 のままだと、何も印をつけていないマス（0）が
// 「印がついている」に見えてしまう
_markStamp = 1;
map.BuildRegions();
}
public RobotMap Map { get { return _map; } }
// ---- 直前の結果 ----
public Status LastStatus { get; private set; }
/// 引けた道の曲がり角の点（最初は出発のマスの真ん中、最後は行き先のマスの真ん中）
public IReadOnlyList Points { get { return _points; } }
/// 道に 1 のマスが入ったか（＝狭い道を使った）
public bool UsedNarrow { get; private set; }
/// この 1 回にかかった時間（ミリ秒）
public float LastPlanMs { get; private set; }
/// 広げたマスの数（重さを調べるため）
public int LastExpanded { get; private set; }
/// マスをたどった長さ（縮める前, m）
public float LastRawLengthM { get; private set; }
/// 縮めたあとの長さ（m）
public float LastLengthM { get; private set; }
/// 出発のマスの真ん中と、渡された点のあいだの距離（m）
public float LastStartSnapM { get; private set; }
/// 行き先のマスの真ん中と、渡された点のあいだの距離（m）
public float LastGoalSnapM { get; private set; }
///
/// 出発点が、地図では通れないマス（0）だったか。
/// ロボットは NavMesh（体の半径 0.25 m）で焼いた面を歩くので、
/// 通路の端・通路の入口で待つ点・横へどいた点は free_class では 0 になる。
/// そういうときは近くの通れるマスへ寄せて引く。何回あったかを記録に出す
///
public bool LastStartWasBlocked { get; private set; }
/// 行き先が、地図では通れないマス（0）だったか
public bool LastGoalWasBlocked { get; private set; }
public string StatusText
{
get
{
switch (LastStatus)
{
case Status.Ok: return UsedNarrow ? "引けた（狭い道）" : "引けた";
case Status.StartBlocked: return "出発点が通れない";
case Status.GoalBlocked: return "行き先が通れない";
case Status.NoPath: return "つながっていない";
default: return "探しすぎ";
}
}
}
// ---------------- 差し込み（その 1 回だけ）----------------
/// 差し込んだものをぜんぶ消す（道を引く前に必ず呼ぶ）
public void ClearMarks()
{
_markStamp++;
_hasMarks = false;
}
/// 丸い所を「通れないマス」にする（人・ほかのロボットの体）
public void BlockCircle(Vector3 center, float radiusM)
{
MarkCapsule(center, center, radiusM, true, 0f);
}
/// 線のまわりを「通れないマス」にする
public void BlockLine(Vector3 a, Vector3 b, float radiusM)
{
MarkCapsule(a, b, radiusM, true, 0f);
}
/// 線のまわりを重くする（相手の先の道から離れて走らせる）
public void CostLine(Vector3 a, Vector3 b, float radiusM, float add)
{
MarkCapsule(a, b, radiusM, false, add);
}
void MarkCapsule(Vector3 a, Vector3 b, float radiusM, bool block, float add)
{
if (radiusM <= 0f) return;
_hasMarks = true;
float minX = Mathf.Min(a.x, b.x) - radiusM, maxX = Mathf.Max(a.x, b.x) + radiusM;
float minZ = Mathf.Min(a.z, b.z) - radiusM, maxZ = Mathf.Max(a.z, b.z) + radiusM;
int ix0 = Mathf.Max(0, Mathf.FloorToInt(minX / _res));
int ix1 = Mathf.Min(_nx - 1, Mathf.FloorToInt(maxX / _res));
int iy0 = Mathf.Max(0, Mathf.FloorToInt(minZ / _res));
int iy1 = Mathf.Min(_ny - 1, Mathf.FloorToInt(maxZ / _res));
float r2 = radiusM * radiusM;
for (int iy = iy0; iy <= iy1; iy++)
{
for (int ix = ix0; ix <= ix1; ix++)
{
float cx = (ix + 0.5f) * _res, cz = (iy + 0.5f) * _res;
if (SegDist2(cx, cz, a.x, a.z, b.x, b.z) > r2) continue;
int i = iy * _nx + ix;
if (block)
{
_blockStamp[i] = _markStamp;
continue;
}
if (_costStamp[i] == _markStamp) _costAdd[i] = Mathf.Max(_costAdd[i], add);
else { _costStamp[i] = _markStamp; _costAdd[i] = add; }
}
}
}
static float SegDist2(float px, float pz, float ax, float az, float bx, float bz)
{
float vx = bx - ax, vz = bz - az;
float l2 = vx * vx + vz * vz;
float t = 0f;
if (l2 > 1e-12f)
{
t = ((px - ax) * vx + (pz - az) * vz) / l2;
t = t < 0f ? 0f : (t > 1f ? 1f : t);
}
float dx = px - (ax + t * vx), dz = pz - (az + t * vz);
return dx * dx + dz * dz;
}
// ---------------- 道を引く ----------------
///
/// from から to まで道を引く。引けたら Status.Ok で、Points に曲がり角の点が入る。
///
/// 「2 のマスだけで行けるなら 2 だけ」は、つながりの組（RobotMap.SameRegion）で
/// 一瞬で見る。A* を空振りさせない（⑦では卓の半分が狭い道なので、空振りが重い）。
///
public Status Plan(Vector3 from, Vector3 to, Settings st)
{
_watch.Reset();
_watch.Start();
_points.Clear();
_cells.Clear();
UsedNarrow = false;
LastExpanded = 0;
LastRawLengthM = 0f;
LastLengthM = 0f;
LastStartSnapM = 0f;
LastGoalSnapM = 0f;
LastStartWasBlocked = _map.ClassAt(from) < 1;
LastGoalWasBlocked = _map.ClassAt(to) < 1;
// 行き先は、まず 1 以上のマスに乗っているかを見る（サイトが通れる所に置いている）
int goal = _map.NearestPassable(to, 1, st.snapM);
if (goal < 0)
{
LastStatus = Status.GoalBlocked;
return Done();
}
LastGoalSnapM = Flat(_map.Center(goal) - to).magnitude;
int start = _map.NearestPassable(from, 1, st.snapM);
if (start < 0)
{
LastStatus = Status.StartBlocked;
return Done();
}
LastStartSnapM = Flat(_map.Center(start) - from).magnitude;
// 2 のマスだけで行けるか（出発点・行き先がどちらも 2 で、同じ組）
int minClass = 1;
if (_map.ClassOf(start) >= 2 && _map.ClassOf(goal) >= 2 &&
_map.SameRegion(start, goal, 2) && !_hasMarks)
{
minClass = 2;
}
else if (_map.ClassOf(start) >= 2 && _map.ClassOf(goal) >= 2 &&
_map.SameRegion(start, goal, 2))
{
// 差し込み（回り込み・よけ）があるときは、つながりの組では決められない。
// まず 2 だけで試し、だめなら 1 も使う
if (Search(start, goal, 2, st) == Status.Ok)
{
Finish(st, 2);
LastStatus = Status.Ok;
return Done();
}
}
if (minClass == 1 && !_map.SameRegion(start, goal, 1))
{
LastStatus = Status.NoPath;
return Done();
}
Status s = Search(start, goal, minClass, st);
if (s != Status.Ok)
{
LastStatus = s;
return Done();
}
Finish(st, minClass);
LastStatus = Status.Ok;
return Done();
}
Status Done()
{
_watch.Stop();
LastPlanMs = (float)(_watch.Elapsed.TotalMilliseconds);
return LastStatus;
}
Status Search(int start, int goal, int minClass, Settings st)
{
_runStamp++;
_heapCount = 0;
_cells.Clear();
if (Blocked(start, minClass) || Blocked(goal, minClass)) return Status.NoPath;
int gy = goal / _nx, gx = goal - gy * _nx;
float prefMm = Mathf.Max(0.001f, st.preferClearM);
float d2 = _res * 1.41421356f;
int expanded = 0;
_g[start] = 0f;
_from[start] = -1;
_seen[start] = _runStamp;
Push(start, Heuristic(start, gx, gy, d2), Heuristic(start, gx, gy, d2));
while (_heapCount > 0)
{
int i = Pop();
if (_closed[i] == _runStamp) continue;
_closed[i] = _runStamp;
expanded++;
if (i == goal)
{
LastExpanded = expanded;
Trace(start, goal);
return Status.Ok;
}
if (expanded > st.maxExpand)
{
LastExpanded = expanded;
return Status.TooMany;
}
int iy = i / _nx, ix = i - iy * _nx;
float gi = _g[i];
for (int d = 0; d < 8; d++)
{
int jx = ix + Dx[d], jy = iy + Dy[d];
if (jx < 0 || jx >= _nx || jy < 0 || jy >= _ny) continue;
int j = jy * _nx + jx;
if (_closed[j] == _runStamp) continue;
if (Blocked(j, minClass)) continue;
float step;
if (Dx[d] != 0 && Dy[d] != 0)
{
// 斜めは両隣が通れるときだけ
if (Blocked(iy * _nx + jx, minClass) || Blocked(jy * _nx + ix, minClass)) continue;
step = d2;
}
else step = _res;
float mult = _map.ClassOf(j) == 1 ? st.narrowCellCost : 1f;
float clearM = _map.ClearM(j);
if (clearM < st.preferClearM)
mult += st.centerWeight * (st.preferClearM - clearM) / prefMm;
if (_costStamp[j] == _markStamp) mult += _costAdd[j];
float ng = gi + step * mult;
if (_seen[j] == _runStamp && ng >= _g[j] - 1e-9f) continue;
_g[j] = ng;
_from[j] = i;
_seen[j] = _runStamp;
float h = Heuristic(j, gx, gy, d2);
Push(j, ng + h, h);
}
}
LastExpanded = expanded;
return Status.NoPath;
}
/// そのマスは通れないか（地図の種類＋差し込んだ「通れないマス」）
bool Blocked(int i, int minClass)
{
if (_map.ClassOf(i) < minClass) return true;
return _hasMarks && _blockStamp[i] == _markStamp;
}
/// 残りの見込み（8 方向のまっすぐな長さ）。1 歩の重さの下限は 1 倍なので、下から見ている
float Heuristic(int i, int gx, int gy, float d2)
{
int iy = i / _nx, ix = i - iy * _nx;
int dx = ix > gx ? ix - gx : gx - ix;
int dy = iy > gy ? iy - gy : gy - iy;
int lo = dx < dy ? dx : dy;
return _res * (dx + dy) + (d2 - 2f * _res) * lo;
}
void Trace(int start, int goal)
{
_cells.Clear();
int i = goal;
while (i != start)
{
_cells.Add(i);
i = _from[i];
if (i < 0) break;
}
_cells.Add(start);
_cells.Reverse();
}
void Finish(Settings st, int minClass)
{
// 1 のマスが入ったか
for (int k = 0; k < _cells.Count; k++)
if (_map.ClassOf(_cells[k]) == 1) { UsedNarrow = true; break; }
float raw = 0f;
for (int k = 1; k < _cells.Count; k++)
raw += Flat(_map.Center(_cells[k]) - _map.Center(_cells[k - 1])).magnitude;
LastRawLengthM = raw;
Simplify(st, minClass);
float len = 0f;
for (int k = 1; k < _points.Count; k++)
len += Flat(_points[k] - _points[k - 1]).magnitude;
LastLengthM = len;
}
// ---------------- 曲がり角の点だけにする ----------------
///
/// マスの並びを曲がり角の点だけに縮める。
/// 近道してよいのは「もとの道で通っていた所より障害物に近づかない」ときだけ
/// （＝近道の上のどのマスも、飛ばしたマスのいちばん狭い所より clear が大きい）。
/// 端に寄った線に戻らないようにするため。
///
void Simplify(Settings st, int minClass)
{
_points.Clear();
if (_cells.Count == 0) return;
_points.Add(_map.Center(_cells[0]));
if (_cells.Count == 1) return;
int step = Mathf.Max(1, Mathf.RoundToInt(0.1f / _res)); // 0.1 m ごとに試す
int maxCells = Mathf.Max(step, Mathf.RoundToInt(st.simplifyMaxM / _res));
int i = 0;
while (i < _cells.Count - 1)
{
int best = i + 1;
float floorM = Mathf.Min(_map.ClearM(_cells[i]), _map.ClearM(_cells[i + 1]));
int j = i + step;
float runFloor = floorM;
while (j < _cells.Count && j - i <= maxCells)
{
for (int k = Mathf.Max(i + 1, j - step + 1); k <= j; k++)
runFloor = Mathf.Min(runFloor, _map.ClearM(_cells[k]));
if (!SegmentOk(_cells[i], _cells[j], runFloor, minClass)) break;
best = j;
j += step;
}
// 最後の点までちょうど届くなら、そこで終わる
int last = _cells.Count - 1;
if (best < last && last - i <= maxCells)
{
float f = runFloor;
for (int k = best + 1; k <= last; k++) f = Mathf.Min(f, _map.ClearM(_cells[k]));
if (SegmentOk(_cells[i], _cells[last], f, minClass)) best = last;
}
_points.Add(_map.Center(_cells[best]));
i = best;
}
}
/// まっすぐ結んだ線が通れて、もとの道より障害物に近づかないか
bool SegmentOk(int a, int b, float floorM, int minClass)
{
Vector3 pa = _map.Center(a), pb = _map.Center(b);
float len = Flat(pb - pa).magnitude;
int n = Mathf.Max(1, Mathf.CeilToInt(len / (_res * 0.5f)));
for (int k = 0; k <= n; k++)
{
float t = (float)k / n;
float x = pa.x + (pb.x - pa.x) * t;
float z = pa.z + (pb.z - pa.z) * t;
int ix = Mathf.FloorToInt(x / _res);
int iy = Mathf.FloorToInt(z / _res);
if (ix < 0 || ix >= _nx || iy < 0 || iy >= _ny) return false;
int i = iy * _nx + ix;
if (Blocked(i, minClass)) return false;
if (_map.ClearM(i) < floorM - 1e-6f) return false;
if (_hasMarks && _costStamp[i] == _markStamp) return false; // 重い所へは近道しない
}
return true;
}
// ---------------- 二分の山 ----------------
void Push(int index, float f, float h)
{
if (_heapCount == _heap.Length) Grow();
int k = _heapCount++;
_heap[k] = index; _heapF[k] = f; _heapH[k] = h;
while (k > 0)
{
int p = (k - 1) >> 1;
if (!Less(k, p)) break;
Swap(k, p);
k = p;
}
}
int Pop()
{
int top = _heap[0];
_heapCount--;
if (_heapCount > 0)
{
_heap[0] = _heap[_heapCount];
_heapF[0] = _heapF[_heapCount];
_heapH[0] = _heapH[_heapCount];
int k = 0;
while (true)
{
int l = (k << 1) + 1, r = l + 1, m = k;
if (l < _heapCount && Less(l, m)) m = l;
if (r < _heapCount && Less(r, m)) m = r;
if (m == k) break;
Swap(k, m);
k = m;
}
}
return top;
}
/// 同じ重さのときの並べ方を決めておく（f → h → マスの番号）。乱数を使わない
bool Less(int a, int b)
{
if (_heapF[a] < _heapF[b] - 1e-9f) return true;
if (_heapF[a] > _heapF[b] + 1e-9f) return false;
if (_heapH[a] < _heapH[b] - 1e-9f) return true;
if (_heapH[a] > _heapH[b] + 1e-9f) return false;
return _heap[a] < _heap[b];
}
void Swap(int a, int b)
{
int i = _heap[a]; _heap[a] = _heap[b]; _heap[b] = i;
float f = _heapF[a]; _heapF[a] = _heapF[b]; _heapF[b] = f;
float h = _heapH[a]; _heapH[a] = _heapH[b]; _heapH[b] = h;
}
void Grow()
{
int size = _heap.Length * 2;
int[] hi = new int[size];
float[] hf = new float[size];
float[] hh = new float[size];
Array.Copy(_heap, hi, _heapCount);
Array.Copy(_heapF, hf, _heapCount);
Array.Copy(_heapH, hh, _heapCount);
_heap = hi; _heapF = hf; _heapH = hh;
}
static Vector3 Flat(Vector3 v) { v.y = 0f; return v; }
}
}
-------------------------------------------------------------
--- Assets/ServiceSim/Robot/MapPlanCheck.cs
-------------------------------------------------------------
using System.Collections.Generic;
using System.Globalization;
using System.Text;
using UnityEngine;
namespace ServiceSim
{
///
/// 確かめ A（依頼 v9 の 7 章 A）：人のいない店で、受け渡し口から全卓へ道を引く。
///
/// 見るもの:
/// ・狭い道を使った卓が、サイトの routes の use と同じか（③は 0 卓・⑦は 24 卓）
/// ・引いた道の長さと、サイトの routes の length の比（最小・平均・最大）
/// ・道探しの時間（平均・中央・最大）
/// ・道が引けない卓が無いこと。待機場所へも引けること
///
/// 店も人も建てずに、JSON と地図だけで回せるようにしてある。
/// Unity のメニュー（ServiceSim/Map Planner）と、Unity の外（mono）の両方から
/// **同じこの関数**を呼ぶので、出る数字は必ず同じになる。
///
public static class MapPlanCheck
{
/// 1 卓ぶんの結果
public struct Row
{
public string tableId;
public string siteUse; // サイトの routes の use（normal / narrow）
public string planUse; // 引いた道（normal / narrow）
public float siteLengthM;
public float planLengthM;
public float ratio; // 引いた道 ÷ サイトの length
public int corners;
public float planMs;
public string status;
}
/// 1 店ぶんの結果
public class Result
{
public bool ok;
public string note = "";
public string report = "";
public readonly List rows = new List();
public int narrowMine, narrowSite, failed, mismatch, snapped;
public float ratioMin, ratioMean, ratioMax;
public float planMsMean, planMsMedian, planMsMax;
public int standbyOk, standbyCount;
/// CSV（table,...）。そのまま書き出せる
public string Csv()
{
StringBuilder sb = new StringBuilder();
sb.Append("table,site_use,plan_use,site_length_m,plan_length_m,ratio,corners,plan_ms,status\n");
for (int i = 0; i < rows.Count; i++)
{
Row r = rows[i];
sb.Append(r.tableId).Append(',').Append(r.siteUse).Append(',').Append(r.planUse).Append(',')
.Append(F(r.siteLengthM)).Append(',').Append(F(r.planLengthM)).Append(',')
.Append(F(r.ratio)).Append(',').Append(r.corners).Append(',')
.Append(F(r.planMs)).Append(',').Append(r.status).Append('\n');
}
return sb.ToString();
}
}
///
/// 回す。storeJson は店の形の JSON、mapJsonText は地図の JSON。
/// spec が null なら GridPlanner.Settings.Default で引く。
///
public static Result Run(string label, string storeJson, string mapJsonText, RobotSpec spec)
{
Result res = new Result();
StringBuilder sb = new StringBuilder();
sb.AppendLine("## " + label);
string err;
JsonValue store = JsonValue.Parse(storeJson, out err);
if (store == null || !store.IsObject)
{
res.note = "店の形の JSON を読めません: " + err;
sb.AppendLine(" " + res.note);
res.report = sb.ToString().TrimEnd();
return res;
}
RobotMap map = RobotMap.Read(mapJsonText, out err);
if (map == null)
{
res.note = "地図を読めません: " + err;
sb.AppendLine(" " + res.note);
res.report = sb.ToString().TrimEnd();
return res;
}
sb.AppendLine(map.Report());
GridPlanner.Settings st = spec != null ? spec.PlannerSettings() : GridPlanner.Settings.Default;
sb.AppendLine(" 道の引き方: narrowCellCost " + F(st.narrowCellCost) +
" / centerWeight " + F(st.centerWeight) +
" / preferClearM " + F(st.preferClearM) +
" / maxExpand " + st.maxExpand +
" / snapM " + F(st.snapM) +
" / simplifyMaxM " + F(st.simplifyMaxM));
Vector3 pass = Vector3.zero;
bool hasPass = false;
List standby = new List();
JsonValue places = store["places"];
for (int i = 0; i < places.Count; i++)
{
string kind = places[i]["kind"].S("");
Vector3 p = new Vector3(places[i]["point"][0].F(0f), 0f, places[i]["point"][1].F(0f));
if (kind == "FoodPass") { pass = p; hasPass = true; }
else if (kind == "RobotStandby") standby.Add(p);
}
if (!hasPass)
{
res.note = "places に FoodPass（受け渡し口の前）がありません";
sb.AppendLine(" " + res.note);
res.report = sb.ToString().TrimEnd();
return res;
}
sb.AppendLine(" 受け渡し口の前 " + P(pass) + " → マスの種類 " + map.ClassAt(pass));
GridPlanner planner = map.Planner;
JsonValue fromPass = store["routes"]["from_pass"];
JsonValue tables = store["tables"];
List ratios = new List();
List times = new List();
List mismatchList = new List();
for (int i = 0; i < tables.Count; i++)
{
Row row = new Row();
row.tableId = tables[i]["id"].S("T" + (i + 1).ToString("00"));
Vector3 stop = new Vector3(tables[i]["stop"][0].F(0f), 0f, tables[i]["stop"][1].F(0f));
JsonValue route = fromPass[row.tableId];
row.siteUse = route["use"].S("");
row.siteLengthM = route["length"].F(-1f);
if (row.siteUse == "narrow") res.narrowSite++;
planner.ClearMarks();
GridPlanner.Status s = planner.Plan(pass, stop, st);
row.planMs = planner.LastPlanMs;
times.Add(row.planMs);
if (planner.LastStartWasBlocked || planner.LastGoalWasBlocked) res.snapped++;
if (s != GridPlanner.Status.Ok)
{
res.failed++;
row.status = planner.StatusText;
res.rows.Add(row);
sb.AppendLine(" × " + row.tableId + " 道が引けません: " + row.status);
continue;
}
row.planUse = planner.UsedNarrow ? "narrow" : "normal";
if (planner.UsedNarrow) res.narrowMine++;
row.planLengthM = planner.LastLengthM;
row.corners = planner.Points.Count;
row.status = "ok";
row.ratio = row.siteLengthM > 0.0005f ? row.planLengthM / row.siteLengthM : -1f;
if (row.ratio > 0f) ratios.Add(row.ratio);
if (row.planUse != row.siteUse)
{
res.mismatch++;
mismatchList.Add(row.tableId + "（サイト " + row.siteUse + " / 引いた道 " + row.planUse + "）");
}
res.rows.Add(row);
}
sb.AppendLine(" 狭い道を使った卓: 引いた道 " + res.narrowMine +
" 卓 / サイトの routes の use " + res.narrowSite + " 卓 → " +
(res.mismatch == 0 ? "一致" : "ちがう（" + res.mismatch + " 卓）"));
for (int i = 0; i < mismatchList.Count && i < 12; i++)
sb.AppendLine(" - " + mismatchList[i]);
if (mismatchList.Count > 12)
sb.AppendLine(" - ほか " + (mismatchList.Count - 12) + " 卓");
sb.AppendLine(" 道が引けない卓: " + res.failed + " / " + tables.Count);
if (ratios.Count > 0)
{
ratios.Sort();
float sum = 0f;
for (int i = 0; i < ratios.Count; i++) sum += ratios[i];
res.ratioMin = ratios[0];
res.ratioMean = sum / ratios.Count;
res.ratioMax = ratios[ratios.Count - 1];
sb.AppendLine(" 長さの比（引いた道 ÷ サイトの length）: 最小 " + F(res.ratioMin) +
" / 平均 " + F(res.ratioMean) + " / 最大 " + F(res.ratioMax) +
"（真ん中寄りにした分だけ延びる）");
}
if (times.Count > 0)
{
times.Sort();
float sum = 0f;
for (int i = 0; i < times.Count; i++) sum += times[i];
res.planMsMean = sum / times.Count;
res.planMsMedian = times[times.Count / 2];
res.planMsMax = times[times.Count - 1];
sb.AppendLine(" 道探しの時間: 平均 " + F(res.planMsMean) + " ms / 中央 " +
F(res.planMsMedian) + " ms / 最大 " + F(res.planMsMax) + " ms");
}
sb.AppendLine(" 出発点・行き先が地図で通れないマス（0）だったので寄せた卓: " +
res.snapped + " / " + tables.Count);
res.standbyCount = standby.Count;
for (int i = 0; i < standby.Count; i++)
{
planner.ClearMarks();
GridPlanner.Status s = planner.Plan(pass, standby[i], st);
bool good = s == GridPlanner.Status.Ok;
if (good) res.standbyOk++;
sb.AppendLine(" 待機場所" + (i + 1) + " " + P(standby[i]) +
" マスの種類 " + map.ClassAt(standby[i]) + " → " +
(good ? "引けた（" + F(planner.LastLengthM) + " m・" +
F(planner.LastPlanMs) + " ms・" +
(planner.UsedNarrow ? "狭い道を使った" : "ふだんの道") + "）"
: "引けない：" + planner.StatusText));
}
res.ok = res.failed == 0 && res.mismatch == 0 &&
res.standbyOk == res.standbyCount && map.CountsMatch;
sb.AppendLine(" → " + (res.ok ? "確かめ A は通った" : "確かめ A は通っていない"));
res.report = sb.ToString().TrimEnd();
return res;
}
static string F(float v) { return v.ToString("0.###", CultureInfo.InvariantCulture); }
static string P(Vector3 v) { return "(" + F(v.x) + ", " + F(v.z) + ")"; }
}
}
-------------------------------------------------------------
--- Assets/ServiceSim/Robot/RobotPathLine.cs
-------------------------------------------------------------
using System.Collections.Generic;
using UnityEngine;
namespace ServiceSim
{
///
/// ロボットがいま引いている道を、床の上に細い線で出す（依頼 v9 の 6 章）。
///
/// 色は仕事で変える:
/// 料理を載せているとき（卓へ向かっている）… だいだい
/// 空で戻るとき・待機 … 水色
///
/// 見た目だけ。当たりも NavMesh も持たないので、
/// ロボットのセンサー・人のよけ方・道探しには入らない。
/// RobotSpec.showPathLine を外すと出さない（測る回でも出してよいが、切れるようにする）。
///
/// 置き場所: ロボットの prefab には足さない。ロボットが自分で子として作る
/// （材料も prefab も要らないようにして、シーンの組み立てを変えないため）。
///
[RequireComponent(typeof(LineRenderer))]
public class RobotPathLine : MonoBehaviour
{
/// この部品を置く子の名前
public const string RootName = "PathLine";
static readonly Color CarryColor = new Color(1f, 0.55f, 0.1f, 0.9f); // 料理を載せている
static readonly Color EmptyColor = new Color(0.25f, 0.75f, 1f, 0.9f); // 空で戻る
LineRenderer _line;
readonly List _points = new List();
///
/// ロボットの下に線を作る（無ければ）。材料はその場で作るので、
/// prefab や .asset をいじらなくていい。URP でも読める shader を順に試す
///
public static RobotPathLine Attach(Transform robot, float widthM)
{
if (robot == null) return null;
Transform t = robot.Find(RootName);
GameObject go;
if (t != null) go = t.gameObject;
else
{
go = new GameObject(RootName);
go.transform.SetParent(robot, false);
}
if (go.GetComponent() == null) go.AddComponent();
RobotPathLine p = go.GetComponent();
if (p == null) p = go.AddComponent();
p.Setup(widthM);
return p;
}
void Setup(float widthM)
{
if (_line == null) _line = GetComponent();
_line.useWorldSpace = true; // 親が回っても線は曲がらない
_line.alignment = LineAlignment.View;
_line.textureMode = LineTextureMode.Stretch;
_line.numCapVertices = 2;
_line.numCornerVertices = 2;
_line.shadowCastingMode = UnityEngine.Rendering.ShadowCastingMode.Off;
_line.receiveShadows = false;
_line.widthMultiplier = Mathf.Max(0.005f, widthM);
_line.positionCount = 0;
if (_line.sharedMaterial == null)
{
Shader s = Shader.Find("Universal Render Pipeline/Unlit");
if (s == null) s = Shader.Find("Sprites/Default");
if (s == null) s = Shader.Find("Unlit/Color");
if (s != null)
{
Material m = new Material(s);
m.name = "Mat_RobotPathLine";
_line.sharedMaterial = m;
}
}
}
///
/// 線を出し直す。points は道の曲がり角（世界の座標）。
/// carrying は料理を載せているか。heightM は床から浮かせる高さ。
///
public void Show(IReadOnlyList points, int fromIndex, Vector3 here,
bool carrying, float heightM)
{
if (_line == null) _line = GetComponent();
_points.Clear();
_points.Add(new Vector3(here.x, heightM, here.z));
if (points != null)
for (int i = Mathf.Max(0, fromIndex); i < points.Count; i++)
_points.Add(new Vector3(points[i].x, heightM, points[i].z));
if (_points.Count < 2)
{
_line.positionCount = 0;
return;
}
_line.positionCount = _points.Count;
for (int i = 0; i < _points.Count; i++) _line.SetPosition(i, _points[i]);
Color c = carrying ? CarryColor : EmptyColor;
_line.startColor = c;
_line.endColor = new Color(c.r, c.g, c.b, c.a * 0.35f);
if (_line.sharedMaterial != null && _line.sharedMaterial.HasProperty("_BaseColor"))
_line.sharedMaterial.SetColor("_BaseColor", c);
}
/// 線を消す
public void Hide()
{
if (_line == null) _line = GetComponent();
_line.positionCount = 0;
}
}
}
-------------------------------------------------------------
--- Assets/ServiceSim/Editor/ServiceSimMapPlanner.cs
-------------------------------------------------------------
using System;
using System.IO;
using System.Text;
using UnityEditor;
using UnityEngine;
namespace ServiceSim.EditorTools
{
///
/// 地図の上で道を引く確かめ（依頼 v9 の 7 章 A）。
///
/// 店も人も建てず、回さない。サイトの JSON と地図だけで、
/// 受け渡し口から全卓へ道を引いて、サイトの routes と突き合わせる。
/// 中身は ServiceSim.MapPlanCheck（Unity の外からも同じものを呼べる）。
///
public static class ServiceSimMapPlanner
{
const string P1Path = "Assets/ServiceSim/Store/StoreLayout_p1_efficient_corner.asset";
const string P5Path = "Assets/ServiceSim/Store/StoreLayout_p5_crammed.asset";
[MenuItem("ServiceSim/Map Planner/③⑦ 確かめ A（地図で道を引く・回さない）", false, 34)]
public static void CheckBoth()
{
StringBuilder sb = new StringBuilder();
sb.AppendLine("# 確かめ A：人のいない店で、受け渡し口から全卓へ道を引く（依頼 v9 の 7 章 A）");
sb.AppendLine("作った日時: " + DateTime.Now.ToString("yyyy-MM-dd HH:mm:ss"));
sb.AppendLine("店も人も建てていない（JSON と地図だけで引いた）");
sb.AppendLine();
RobotSpec spec = AssetDatabase.LoadAssetAtPath(ServiceSimSetup.RobotSpecPath);
if (spec == null)
sb.AppendLine("※ RobotSpec が見つからないので、道の引き方は既定の値で引きました");
bool ok = true;
ok &= One(sb, P1Path, "③ 効率重視型・入口が左下の角", spec);
sb.AppendLine();
ok &= One(sb, P5Path, "⑦ ぎっしり型", spec);
sb.AppendLine();
sb.AppendLine(ok ? "→ ③⑦ ともに確かめ A は通った" : "→ 通っていないものがある（上の × を見てください）");
ExperimentPlan plan = AssetDatabase.LoadAssetAtPath(ServiceSimSetup.ExperimentPlanPath);
string outDir = plan != null ? plan.screenshotDir : null;
if (!string.IsNullOrEmpty(outDir))
{
try
{
Directory.CreateDirectory(outDir);
string txt = Path.Combine(outDir,
DateTime.Now.ToString("yyyyMMdd_HHmm") + "_map_planner_checkA.txt");
File.WriteAllText(txt, sb.ToString(), new UTF8Encoding(false));
sb.AppendLine("書き出し: " + txt);
}
catch (Exception e) { sb.AppendLine("書き出せません: " + e.Message); }
}
if (ok) Debug.Log("[ServiceSim] " + sb.ToString());
else Debug.LogError("[ServiceSim] " + sb.ToString());
}
static bool One(StringBuilder sb, string assetPath, string label, RobotSpec spec)
{
JsonStoreLayout layout = AssetDatabase.LoadAssetAtPath(assetPath);
if (layout == null)
{
sb.AppendLine("## " + label + "\n 店の形が見つかりません: " + assetPath);
return false;
}
if (layout.layoutJson == null || layout.mapJson == null)
{
sb.AppendLine("## " + label + "（" + layout.layoutFileName + "）");
sb.AppendLine(" " + (layout.layoutJson == null ? "layoutJson" : "mapJson") +
" が入っていません（依頼 v9 の 0 章を確かめてください）");
return false;
}
MapPlanCheck.Result res = MapPlanCheck.Run(
label + "（" + layout.layoutFileName + "）",
layout.layoutJson.text, layout.mapJson.text, spec);
sb.AppendLine(res.report);
ExperimentPlan plan = AssetDatabase.LoadAssetAtPath(ServiceSimSetup.ExperimentPlanPath);
string outDir = plan != null ? plan.screenshotDir : null;
if (!string.IsNullOrEmpty(outDir) && res.rows.Count > 0)
{
try
{
Directory.CreateDirectory(outDir);
string csv = Path.Combine(outDir,
DateTime.Now.ToString("yyyyMMdd_HHmm") + "_map_plan_" +
layout.layoutFileName + ".csv");
File.WriteAllText(csv, res.Csv(), new UTF8Encoding(false));
sb.AppendLine(" CSV: " + csv);
}
catch (Exception e) { sb.AppendLine(" CSV を書けません: " + e.Message); }
}
return res.ok;
}
}
}