33 lines
2.1 KiB
JavaScript
33 lines
2.1 KiB
JavaScript
|
/* 任务采样（方案 §4.3）：A 均匀 45% / B 直线被挡 30% / C 贴立柱 15% / D 目标不可达 10%；pos/pose 各半（D 只做 pos） */
|
|||
|
|
'use strict';
|
|||
|
|
module.exports = function makeSampler(S, seed) {
|
|||
|
|
const L = S.LIM, O = S.ORDER, w = S.wrap180;
|
|||
|
|
let st = (seed >>> 0) || 1; const rnd = () => { st = (st * 1664525 + 1013904223) >>> 0; return st / 4294967296; };
|
|||
|
|
const rq = () => O.map(id => { const t = L[id]; return t.lo + rnd() * (t.hi - t.lo); });
|
|||
|
|
const d = (a, b) => a.map((v, i) => L[O[i]].cont ? w(b[i] - v) : b[i] - v);
|
|||
|
|
const segOK = (a, b) => { const dd = d(a, b); const n = Math.max(1, Math.ceil(Math.max(...dd.map(Math.abs)))); for (let i = 1; i <= n; i++) if (!S.collideFree(a.map((v, k) => v + dd[k] * i / n))) return false; return true; };
|
|||
|
|
const hitsColumn = q => (S.collideDetail(q) || []).some(p => /立柱/.test(p.cnA + p.cnB));
|
|||
|
|
const nearColumn = q => { for (const k of [0, 1, 2]) for (const sg of [-4, 4]) { const p = q.slice(); p[k] += sg; if (hitsColumn(p)) return true; } return false; };
|
|||
|
|
const freeQ = () => { for (;;) { const q = rq(); if (S.collideFree(q)) return q; } };
|
|||
|
|
function sample() {
|
|||
|
|
const u = rnd();
|
|||
|
|
const cat = u < 0.45 ? 'A' : u < 0.75 ? 'B' : u < 0.90 ? 'C' : 'D';
|
|||
|
|
let a, b, T, R = null;
|
|||
|
|
if (cat === 'A') { a = freeQ(); b = freeQ(); }
|
|||
|
|
else if (cat === 'B') { for (;;) { a = freeQ(); b = freeQ(); if (!segOK(a, b)) break; } }
|
|||
|
|
else if (cat === 'C') {
|
|||
|
|
for (;;) { b = rq(); if (!S.collideFree(b) || !nearColumn(b)) continue; a = null;
|
|||
|
|
for (let t = 0; t < 200; t++) { const c = rq(); if (S.collideFree(c) && !segOK(c, b)) { a = c; break; } } if (a) break; }
|
|||
|
|
} else {
|
|||
|
|
a = freeQ();
|
|||
|
|
if (rnd() < 0.6) { for (;;) { b = rq(); if (hitsColumn(b)) break; } T = S.fk(b, 0); } // 目标位置落在立柱附近/内部
|
|||
|
|
else { b = rq(); const p = S.fk(b, 0), o = S.fk(O.map(() => 0), 0); T = p.map((v, i) => o[i] + (v - o[i]) * 1.6); } // 工作空间外
|
|||
|
|
}
|
|||
|
|
const mode = cat === 'D' ? 'pos' : (rnd() < 0.5 ? 'pos' : 'pose');
|
|||
|
|
if (!T) T = S.fk(b, 0);
|
|||
|
|
if (mode === 'pose') R = S.orient(b);
|
|||
|
|
return { cat, mode, q0: a, T, R, qb: b };
|
|||
|
|
}
|
|||
|
|
return { sample, rnd, segOK };
|
|||
|
|
};
|