chenlin
3ad29356c9
集成通用机器人数值接口、本机控制桥、LeRobot 插件和统一键盘遥操作。采用离线 CoACD 全臂碰撞配方 revision 4、局部装配区切分与结构自接触，限制直接关节位姿写入并保留安全看门狗。同步版本号、变更记录、来源许可证和兼容性验证。
72 lines
2.7 KiB
TypeScript
72 lines
2.7 KiB
TypeScript
import { Ray, Triangle, Vector3 } from 'three';
|
|
|
|
export type Surface = { vertices: Vector3[]; faces: number[][] };
|
|
|
|
/** Independent, unsigned triangle-surface distance oracle for the small E2E tip hulls.
|
|
* Not a runtime collision detector. Includes vertex/face, edge/edge and edge/face cases.
|
|
*/
|
|
export function surfaceDistance(a: Surface, b: Surface) {
|
|
let distanceSq = Infinity;
|
|
let from = new Vector3(),
|
|
to = new Vector3();
|
|
const candidate = new Vector3(),
|
|
rayPoint = new Vector3(),
|
|
segmentPoint = new Vector3();
|
|
const consider = (p: Vector3, q: Vector3, reversed = false) => {
|
|
const distance = p.distanceToSquared(q);
|
|
if (distance < distanceSq) {
|
|
distanceSq = distance;
|
|
from = (reversed ? q : p).clone();
|
|
to = (reversed ? p : q).clone();
|
|
}
|
|
};
|
|
const triangles = (surface: Surface) =>
|
|
surface.faces.map(
|
|
([i, j, k]) => new Triangle(surface.vertices[i], surface.vertices[j], surface.vertices[k]),
|
|
);
|
|
const edges = (surface: Surface) => {
|
|
const pairs = new Map<string, [Vector3, Vector3]>();
|
|
for (const f of surface.faces)
|
|
for (let i = 0; i < 3; i++) {
|
|
const p = f[i],
|
|
q = f[(i + 1) % 3];
|
|
pairs.set([p, q].sort((x, y) => x - y).join(','), [
|
|
surface.vertices[p],
|
|
surface.vertices[q],
|
|
]);
|
|
}
|
|
return [...pairs.values()];
|
|
};
|
|
const ta = triangles(a),
|
|
tb = triangles(b),
|
|
ea = edges(a),
|
|
eb = edges(b);
|
|
for (const v of a.vertices) for (const t of tb) consider(v, t.closestPointToPoint(v, candidate));
|
|
for (const v of b.vertices)
|
|
for (const t of ta) consider(v, t.closestPointToPoint(v, candidate), true);
|
|
for (const [p, q] of ea) {
|
|
const length = p.distanceTo(q);
|
|
if (length === 0) continue;
|
|
const ray = new Ray(p, q.clone().sub(p).divideScalar(length));
|
|
for (const [v, w] of eb) {
|
|
ray.distanceSqToSegment(v, w, rayPoint, segmentPoint);
|
|
if (rayPoint.distanceTo(p) <= length) consider(rayPoint, segmentPoint);
|
|
}
|
|
for (const t of tb) {
|
|
const point = ray.intersectTriangle(t.a, t.b, t.c, false, candidate);
|
|
if (point && point.distanceTo(p) <= length) consider(point, point);
|
|
}
|
|
}
|
|
for (const [p, q] of eb) {
|
|
const length = p.distanceTo(q);
|
|
if (length === 0) continue;
|
|
const ray = new Ray(p, q.clone().sub(p).divideScalar(length));
|
|
for (const t of ta) {
|
|
const point = ray.intersectTriangle(t.a, t.b, t.c, false, candidate);
|
|
if (point && point.distanceTo(p) <= length) consider(point, point);
|
|
}
|
|
}
|
|
if (!Number.isFinite(distanceSq)) throw new Error('表面距离查询缺少有效三角形');
|
|
return { distance: Math.sqrt(distanceSq), from: from.toArray(), to: to.toArray() };
|
|
}
|