feat(lekiwi): release V0.10.1 初步集成 LeKiwi,优化碰撞模型
集成通用机器人数值接口、本机控制桥、LeRobot 插件和统一键盘遥操作。采用离线 CoACD 全臂碰撞配方 revision 4、局部装配区切分与结构自接触,限制直接关节位姿写入并保留安全看门狗。同步版本号、变更记录、来源许可证和兼容性验证。
This commit is contained in:
@@ -0,0 +1,71 @@
|
||||
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() };
|
||||
}
|
||||
Reference in New Issue
Block a user