83 lines
4.6 KiB
JavaScript
83 lines
4.6 KiB
JavaScript
|
/* 离线生成关节空间安全路网（PRM），写入 docs/collide.json 的 roadmap 字段。
|
||
|
|
* node webapp/tools/gen_roadmap.cjs [节点数=2500] [k=10]
|
||
|
|
* 障碍是静态的（立柱 + 自身零件），路网只依赖碰撞模型与限位；重新拟合胶囊 / 改限位后必须重跑
|
||
|
|
* （求解器按指纹校验，不符自动弃用路网，退回在线 RRT —— 只是变慢，不会算错）。
|
||
|
|
* 多线程：worker_threads 按 CPU 数并行细验边。 */
|
||
|
|
const fs = require('fs'), path = require('path'), os = require('os');
|
||
|
|
const { Worker, isMainThread, parentPort, workerData } = require('worker_threads');
|
||
|
|
const ROOT = path.resolve(__dirname, '..', '..');
|
||
|
|
const COLF = path.join(ROOT, 'docs/collide.json');
|
||
|
|
function stripped() { // 去掉旧 roadmap 字段（文本级，保持原格式）
|
||
|
|
let text = fs.readFileSync(COLF, 'utf8');
|
||
|
|
const cut = text.indexOf(',\n "roadmap":');
|
||
|
|
if (cut >= 0) text = text.slice(0, cut) + '\n}\n';
|
||
|
|
return text;
|
||
|
|
}
|
||
|
|
if (!isMainThread) {
|
||
|
|
const { S } = require(path.join(ROOT, 'tests/route_bench_setup.cjs'))(undefined, workerData.tmp);
|
||
|
|
parentPort.on('message', ({ Q, edges }) => {
|
||
|
|
const ok = edges.map(([a, b]) => S.edgeFree(Q[a], S.unwrapTo ? S.unwrapTo(Q[a], Q[b]) : Q[b]) ? 1 : 0);
|
||
|
|
parentPort.postMessage(ok);
|
||
|
|
});
|
||
|
|
return;
|
||
|
|
}
|
||
|
|
(async () => {
|
||
|
|
const N = +process.argv[2] || 2500, K = +process.argv[3] || 10;
|
||
|
|
const text = stripped(), tmp = COLF + '.rmtmp';
|
||
|
|
fs.writeFileSync(tmp, text);
|
||
|
|
const { S } = require(path.join(ROOT, 'tests/route_bench_setup.cjs'))(undefined, tmp);
|
||
|
|
const O = S.ORDER, L = S.LIM;
|
||
|
|
let seed = 20261015;
|
||
|
|
const rnd = () => { seed = (seed * 1664525 + 1013904223) >>> 0; return seed / 4294967296; };
|
||
|
|
const Q = [];
|
||
|
|
let tries = 0;
|
||
|
|
while (Q.length < N) {
|
||
|
|
tries++;
|
||
|
|
const q = O.map(id => { const t = L[id]; return t.cont ? -180 + 360 * rnd() : t.lo + (t.hi - t.lo) * rnd(); });
|
||
|
|
const qr = q.map(v => Math.round(v * 100) / 100);
|
||
|
|
if (S.collideFree(qr)) Q.push(qr);
|
||
|
|
}
|
||
|
|
console.error('nodes', N, 'free ratio', (N / tries).toFixed(3));
|
||
|
|
const E = new Set();
|
||
|
|
for (let i = 0; i < N; i++) {
|
||
|
|
const d = [];
|
||
|
|
for (let j = 0; j < N; j++) if (j !== i) d.push([j, S.wd(Q[i], Q[j])]);
|
||
|
|
d.sort((a, b) => a[1] - b[1]);
|
||
|
|
for (let m = 0; m < K; m++) { const j = d[m][0]; E.add(i < j ? i * 65536 + j : j * 65536 + i); }
|
||
|
|
}
|
||
|
|
const edges = [...E].map(k => [Math.floor(k / 65536), k % 65536]);
|
||
|
|
console.error('candidate edges', edges.length);
|
||
|
|
const nw = Math.max(1, os.cpus().length), ws = [];
|
||
|
|
for (let w = 0; w < nw; w++) ws.push(new Worker(__filename, { workerData: { tmp } }));
|
||
|
|
const CH = 200, okAll = new Uint8Array(edges.length);
|
||
|
|
let next = 0, done = 0; const t0 = Date.now();
|
||
|
|
await new Promise(res => {
|
||
|
|
const feed = w => {
|
||
|
|
if (next >= edges.length) { if (done >= edges.length) res(); return; }
|
||
|
|
const lo = next; next = Math.min(edges.length, next + CH);
|
||
|
|
w.once('message', ok => { okAll.set(ok, lo); done += ok.length;
|
||
|
|
if ((done / CH) % 10 === 0) console.error('edges', done + '/' + edges.length, ((Date.now() - t0) / 1000).toFixed(0) + 's');
|
||
|
|
if (done >= edges.length) res(); else feed(w); });
|
||
|
|
w.postMessage({ Q, edges: edges.slice(lo, next) });
|
||
|
|
};
|
||
|
|
ws.forEach(feed);
|
||
|
|
});
|
||
|
|
ws.forEach(w => w.terminate());
|
||
|
|
fs.unlinkSync(tmp);
|
||
|
|
const adjL = Array.from({ length: N }, () => []);
|
||
|
|
let ne = 0;
|
||
|
|
edges.forEach(([a, b], i) => { if (okAll[i]) { adjL[a].push(b); adjL[b].push(a); ne++; } });
|
||
|
|
/* 连通分量统计 */
|
||
|
|
const comp = new Int32Array(N).fill(-1); let nc = 0, big = 0;
|
||
|
|
for (let i = 0; i < N; i++) if (comp[i] < 0) { let st = [i], sz = 0; comp[i] = nc; while (st.length) { const u = st.pop(); sz++; for (const v of adjL[u]) if (comp[v] < 0) { comp[v] = nc; st.push(v); } } big = Math.max(big, sz); nc++; }
|
||
|
|
console.error('valid edges', ne, 'components', nc, 'largest', big);
|
||
|
|
const qb = Buffer.alloc(N * 12); Q.forEach((q, i) => q.forEach((v, k) => qb.writeInt16LE(Math.round(v * 100), (i * 6 + k) * 2)));
|
||
|
|
const off = Buffer.alloc((N + 1) * 4), adj = Buffer.alloc(ne * 2 * 2);
|
||
|
|
let p = 0; for (let i = 0; i < N; i++) { off.writeUInt32LE(p, i * 4); for (const v of adjL[i]) { adj.writeUInt16LE(v, p * 2); p++; } } off.writeUInt32LE(p, N * 4);
|
||
|
|
const rm = { n: N, k: K, res: 1.5, fp: S.modelFingerprint(), q: qb.toString('base64'), off: off.toString('base64'), adj: adj.toString('base64'),
|
||
|
|
generated: new Date().toISOString(), stats: { edges: ne, components: nc, largest: big } };
|
||
|
|
const body = text.replace(/\s*\}\s*$/, '');
|
||
|
|
fs.writeFileSync(COLF, body + ',\n "roadmap": ' + JSON.stringify(rm) + '\n}\n');
|
||
|
|
console.log('roadmap written', rm.stats);
|
||
|
|
})();
|