mirror of
https://github.com/allaunthefox/Research-Stack.git
synced 2026-07-31 03:05:21 +00:00
436 lines
16 KiB
JavaScript
436 lines
16 KiB
JavaScript
const IDENTITY_TRANSFORM = Object.freeze([
|
|
1, 0, 0, 0,
|
|
0, 1, 0, 0,
|
|
0, 0, 1, 0,
|
|
0, 0, 0, 1
|
|
]);
|
|
const HEX_COLOR_PATTERN = /^#(?:[0-9a-fA-F]{3}){1,2}$/;
|
|
|
|
function srgbToLinear(value) {
|
|
return value <= 0.04045
|
|
? value / 12.92
|
|
: ((value + 0.055) / 1.055) ** 2.4;
|
|
}
|
|
|
|
function parseHexColorToLinearRgb(value) {
|
|
const normalized = String(value || "").trim();
|
|
if (!HEX_COLOR_PATTERN.test(normalized)) {
|
|
return null;
|
|
}
|
|
const expanded = normalized.length === 4
|
|
? `#${normalized[1]}${normalized[1]}${normalized[2]}${normalized[2]}${normalized[3]}${normalized[3]}`
|
|
: normalized;
|
|
return [
|
|
srgbToLinear(Number.parseInt(expanded.slice(1, 3), 16) / 255),
|
|
srgbToLinear(Number.parseInt(expanded.slice(3, 5), 16) / 255),
|
|
srgbToLinear(Number.parseInt(expanded.slice(5, 7), 16) / 255)
|
|
];
|
|
}
|
|
|
|
function toTransformArray(value, fallback = IDENTITY_TRANSFORM) {
|
|
if (!Array.isArray(value) || value.length !== 16) {
|
|
return [...fallback];
|
|
}
|
|
return value.map((component, index) => Number.isFinite(Number(component)) ? Number(component) : fallback[index]);
|
|
}
|
|
|
|
function toVector3(value, fallback = [0, 0, 1]) {
|
|
if (!Array.isArray(value) || value.length < 3) {
|
|
return [...fallback];
|
|
}
|
|
return [
|
|
Number.isFinite(Number(value[0])) ? Number(value[0]) : fallback[0],
|
|
Number.isFinite(Number(value[1])) ? Number(value[1]) : fallback[1],
|
|
Number.isFinite(Number(value[2])) ? Number(value[2]) : fallback[2]
|
|
];
|
|
}
|
|
|
|
function normalizeVector(vector) {
|
|
const [x, y, z] = toVector3(vector, [0, 0, 1]);
|
|
const length = Math.hypot(x, y, z);
|
|
if (length <= 1e-9) {
|
|
return [0, 0, 1];
|
|
}
|
|
return [x / length, y / length, z / length];
|
|
}
|
|
|
|
export function multiplyTransforms(left, right) {
|
|
const a = toTransformArray(left);
|
|
const b = toTransformArray(right);
|
|
const product = new Array(16).fill(0);
|
|
for (let row = 0; row < 4; row += 1) {
|
|
for (let column = 0; column < 4; column += 1) {
|
|
let total = 0;
|
|
for (let offset = 0; offset < 4; offset += 1) {
|
|
total += a[(row * 4) + offset] * b[(offset * 4) + column];
|
|
}
|
|
product[(row * 4) + column] = total;
|
|
}
|
|
}
|
|
return product;
|
|
}
|
|
|
|
export function axisAngleTransform(axis, angleRad) {
|
|
const [x, y, z] = normalizeVector(axis);
|
|
const cosine = Math.cos(angleRad);
|
|
const sine = Math.sin(angleRad);
|
|
const oneMinusCosine = 1 - cosine;
|
|
return [
|
|
cosine + (x * x * oneMinusCosine), (x * y * oneMinusCosine) - (z * sine), (x * z * oneMinusCosine) + (y * sine), 0,
|
|
(y * x * oneMinusCosine) + (z * sine), cosine + (y * y * oneMinusCosine), (y * z * oneMinusCosine) - (x * sine), 0,
|
|
(z * x * oneMinusCosine) - (y * sine), (z * y * oneMinusCosine) + (x * sine), cosine + (z * z * oneMinusCosine), 0,
|
|
0, 0, 0, 1
|
|
];
|
|
}
|
|
|
|
export function transformPoint(transform, point) {
|
|
const matrix = toTransformArray(transform);
|
|
const [x, y, z] = toVector3(point, [0, 0, 0]);
|
|
return [
|
|
(matrix[0] * x) + (matrix[1] * y) + (matrix[2] * z) + matrix[3],
|
|
(matrix[4] * x) + (matrix[5] * y) + (matrix[6] * z) + matrix[7],
|
|
(matrix[8] * x) + (matrix[9] * y) + (matrix[10] * z) + matrix[11]
|
|
];
|
|
}
|
|
|
|
export function transformBounds(bounds, transform) {
|
|
const min = Array.isArray(bounds?.min) ? bounds.min : [0, 0, 0];
|
|
const max = Array.isArray(bounds?.max) ? bounds.max : [0, 0, 0];
|
|
const corners = [
|
|
[min[0], min[1], min[2]],
|
|
[min[0], min[1], max[2]],
|
|
[min[0], max[1], min[2]],
|
|
[min[0], max[1], max[2]],
|
|
[max[0], min[1], min[2]],
|
|
[max[0], min[1], max[2]],
|
|
[max[0], max[1], min[2]],
|
|
[max[0], max[1], max[2]]
|
|
];
|
|
const transformed = corners.map((corner) => transformPoint(transform, corner));
|
|
const xs = transformed.map((point) => point[0]);
|
|
const ys = transformed.map((point) => point[1]);
|
|
const zs = transformed.map((point) => point[2]);
|
|
return {
|
|
min: [Math.min(...xs), Math.min(...ys), Math.min(...zs)],
|
|
max: [Math.max(...xs), Math.max(...ys), Math.max(...zs)]
|
|
};
|
|
}
|
|
|
|
export function mergeBounds(boundsList) {
|
|
const normalized = (Array.isArray(boundsList) ? boundsList : []).filter(Boolean);
|
|
if (!normalized.length) {
|
|
return {
|
|
min: [0, 0, 0],
|
|
max: [0, 0, 0]
|
|
};
|
|
}
|
|
const xs = normalized.flatMap((bounds) => [bounds.min[0], bounds.max[0]]);
|
|
const ys = normalized.flatMap((bounds) => [bounds.min[1], bounds.max[1]]);
|
|
const zs = normalized.flatMap((bounds) => [bounds.min[2], bounds.max[2]]);
|
|
return {
|
|
min: [Math.min(...xs), Math.min(...ys), Math.min(...zs)],
|
|
max: [Math.max(...xs), Math.max(...ys), Math.max(...zs)]
|
|
};
|
|
}
|
|
|
|
export function buildDefaultUrdfJointValues(urdfData) {
|
|
const joints = Array.isArray(urdfData?.joints) ? urdfData.joints : [];
|
|
return Object.fromEntries(
|
|
joints
|
|
.filter((joint) => String(joint?.type || "") !== "fixed" && !joint?.mimic)
|
|
.map((joint) => [String(joint?.name || ""), Number(joint?.defaultValueDeg) || 0])
|
|
.filter(([name]) => name)
|
|
);
|
|
}
|
|
|
|
export function clampJointValueDeg(joint, valueDeg) {
|
|
const defaultValue = Number(joint?.defaultValueDeg) || 0;
|
|
const jointType = String(joint?.type || "");
|
|
if (jointType === "fixed") {
|
|
return defaultValue;
|
|
}
|
|
const numericValue = Number.isFinite(Number(valueDeg)) ? Number(valueDeg) : defaultValue;
|
|
if (jointType === "continuous") {
|
|
return numericValue;
|
|
}
|
|
const minValue = Number.isFinite(Number(joint?.minValueDeg)) ? Number(joint.minValueDeg) : numericValue;
|
|
const maxValue = Number.isFinite(Number(joint?.maxValueDeg)) ? Number(joint.maxValueDeg) : numericValue;
|
|
return Math.min(Math.max(numericValue, minValue), Math.max(minValue, maxValue));
|
|
}
|
|
|
|
function isAngularJoint(joint) {
|
|
const jointType = String(joint?.type || "fixed");
|
|
return jointType === "continuous" || jointType === "revolute";
|
|
}
|
|
|
|
function jointValueToNative(joint, value) {
|
|
const clampedValue = clampJointValueDeg(joint, value);
|
|
return isAngularJoint(joint) ? (clampedValue * Math.PI) / 180 : clampedValue;
|
|
}
|
|
|
|
function nativeToJointValue(joint, value) {
|
|
const numericValue = Number.isFinite(Number(value)) ? Number(value) : 0;
|
|
return isAngularJoint(joint) ? (numericValue * 180) / Math.PI : numericValue;
|
|
}
|
|
|
|
function translationAlongAxisTransform(axis, distance) {
|
|
const [x, y, z] = normalizeVector(axis);
|
|
const safeDistance = Number.isFinite(Number(distance)) ? Number(distance) : 0;
|
|
return [
|
|
1, 0, 0, x * safeDistance,
|
|
0, 1, 0, y * safeDistance,
|
|
0, 0, 1, z * safeDistance,
|
|
0, 0, 0, 1
|
|
];
|
|
}
|
|
|
|
export function posedJointLocalTransform(joint, valueDeg) {
|
|
const jointType = String(joint?.type || "fixed");
|
|
const originTransform = toTransformArray(joint?.originTransform);
|
|
if (jointType === "fixed") {
|
|
return originTransform;
|
|
}
|
|
const axis = toVector3(joint?.axis ?? joint?.axisInJointFrame ?? joint?.axisInParentFrame, [0, 0, 1]);
|
|
if (jointType === "prismatic") {
|
|
return multiplyTransforms(originTransform, translationAlongAxisTransform(axis, clampJointValueDeg(joint, valueDeg)));
|
|
}
|
|
const angleRad = (clampJointValueDeg(joint, valueDeg) * Math.PI) / 180;
|
|
// URDF axes are defined in the joint frame, so the static origin rotation
|
|
// must be applied before the animated axis-angle motion.
|
|
return multiplyTransforms(originTransform, axisAngleTransform(axis, angleRad));
|
|
}
|
|
|
|
function resolveJointValue(joint, jointByName, jointValuesByName, resolving = new Set()) {
|
|
const jointName = String(joint?.name || "");
|
|
if (!joint?.mimic) {
|
|
return clampJointValueDeg(joint, jointValuesByName?.[jointName]);
|
|
}
|
|
if (resolving.has(jointName)) {
|
|
return clampJointValueDeg(joint, joint?.defaultValueDeg);
|
|
}
|
|
resolving.add(jointName);
|
|
const mimic = joint.mimic;
|
|
const masterJoint = jointByName.get(String(mimic.joint || ""));
|
|
const masterValue = masterJoint
|
|
? resolveJointValue(masterJoint, jointByName, jointValuesByName, resolving)
|
|
: Number(jointValuesByName?.[mimic.joint]) || 0;
|
|
resolving.delete(jointName);
|
|
|
|
const masterNativeValue = masterJoint ? jointValueToNative(masterJoint, masterValue) : masterValue;
|
|
const multiplier = Number.isFinite(Number(mimic.multiplier)) ? Number(mimic.multiplier) : 1;
|
|
const offset = Number.isFinite(Number(mimic.offset)) ? Number(mimic.offset) : 0;
|
|
return clampJointValueDeg(joint, nativeToJointValue(joint, (multiplier * masterNativeValue) + offset));
|
|
}
|
|
|
|
export function solveUrdfLinkWorldTransforms(urdfData, jointValuesByName = {}) {
|
|
const rootLink = String(urdfData?.rootLink || "");
|
|
const rootWorldTransform = toTransformArray(urdfData?.rootWorldTransform);
|
|
const joints = Array.isArray(urdfData?.joints) ? urdfData.joints : [];
|
|
const jointByName = new Map(joints.map((joint) => [String(joint?.name || ""), joint]).filter(([name]) => name));
|
|
const linkTransforms = new Map();
|
|
if (!rootLink) {
|
|
return linkTransforms;
|
|
}
|
|
const jointsByParent = new Map();
|
|
for (const joint of joints) {
|
|
const parentLink = String(joint?.parentLink || "");
|
|
if (!parentLink) {
|
|
continue;
|
|
}
|
|
const current = jointsByParent.get(parentLink) || [];
|
|
current.push(joint);
|
|
jointsByParent.set(parentLink, current);
|
|
}
|
|
|
|
const visit = (linkName, worldTransform) => {
|
|
linkTransforms.set(linkName, worldTransform);
|
|
const childJoints = jointsByParent.get(linkName) || [];
|
|
for (const joint of childJoints) {
|
|
const childLink = String(joint?.childLink || "");
|
|
if (!childLink) {
|
|
continue;
|
|
}
|
|
const jointValueDeg = resolveJointValue(joint, jointByName, jointValuesByName);
|
|
const childWorldTransform = multiplyTransforms(worldTransform, posedJointLocalTransform(joint, jointValueDeg));
|
|
visit(childLink, childWorldTransform);
|
|
}
|
|
};
|
|
|
|
visit(rootLink, rootWorldTransform);
|
|
return linkTransforms;
|
|
}
|
|
|
|
function resolveVisualMesh(meshesByUrl, meshUrl, partFileRef) {
|
|
const lookupKey = meshUrl || partFileRef;
|
|
if (!lookupKey) {
|
|
return null;
|
|
}
|
|
if (meshesByUrl instanceof Map) {
|
|
return meshesByUrl.get(lookupKey) || null;
|
|
}
|
|
if (meshesByUrl && typeof meshesByUrl === "object") {
|
|
return meshesByUrl[lookupKey] || null;
|
|
}
|
|
return null;
|
|
}
|
|
|
|
function resolveUrdfVisuals(urdfData, meshesByUrl) {
|
|
const links = Array.isArray(urdfData?.links) ? urdfData.links : [];
|
|
const resolvedVisuals = [];
|
|
for (const link of links) {
|
|
const linkName = String(link?.name || "");
|
|
const visuals = Array.isArray(link?.visuals) ? link.visuals : [];
|
|
for (const visual of visuals) {
|
|
const meshUrl = String(visual?.meshUrl || "");
|
|
const partFileRef = String(visual?.partFileRef || "");
|
|
const partMesh = resolveVisualMesh(meshesByUrl, meshUrl, partFileRef);
|
|
if (!partMesh) {
|
|
continue;
|
|
}
|
|
resolvedVisuals.push({
|
|
linkName,
|
|
meshUrl,
|
|
partFileRef,
|
|
visual,
|
|
partMesh
|
|
});
|
|
}
|
|
}
|
|
return resolvedVisuals;
|
|
}
|
|
|
|
export function buildUrdfMeshGeometry(urdfData, meshesByUrl) {
|
|
const resolvedVisuals = resolveUrdfVisuals(urdfData, meshesByUrl);
|
|
let totalVertexCount = 0;
|
|
let totalIndexCount = 0;
|
|
let hasSourceColors = false;
|
|
for (const resolvedVisual of resolvedVisuals) {
|
|
const partMesh = resolvedVisual.partMesh;
|
|
totalVertexCount += Math.floor((partMesh.vertices?.length || 0) / 3);
|
|
totalIndexCount += partMesh.indices?.length || 0;
|
|
const visualColor = String(resolvedVisual.visual?.color || "").trim();
|
|
hasSourceColors ||= urdfVisualHasDisplayColors(visualColor, partMesh);
|
|
}
|
|
|
|
const vertices = new Float32Array(totalVertexCount * 3);
|
|
const normals = new Float32Array(totalVertexCount * 3);
|
|
const indices = new Uint32Array(totalIndexCount);
|
|
const colors = hasSourceColors ? new Float32Array(totalVertexCount * 3).fill(1) : new Float32Array(0);
|
|
const parts = [];
|
|
let vertexOffset = 0;
|
|
let indexOffset = 0;
|
|
|
|
for (const resolvedVisual of resolvedVisuals) {
|
|
const { linkName, meshUrl, partFileRef, partMesh, visual } = resolvedVisual;
|
|
const sourceVertices = partMesh.vertices || new Float32Array(0);
|
|
const sourceNormals = partMesh.normals || new Float32Array(0);
|
|
const sourceColors = partMesh.colors || new Float32Array(0);
|
|
const sourceIndices = partMesh.indices || new Uint32Array(0);
|
|
const partVertexOffset = vertexOffset;
|
|
const partTriangleOffset = Math.floor(indexOffset / 3);
|
|
const vertexCount = Math.floor(sourceVertices.length / 3);
|
|
const triangleCount = Math.floor(sourceIndices.length / 3);
|
|
const visualColor = String(visual?.color || "").trim();
|
|
const visualRgb = parseHexColorToLinearRgb(visualColor);
|
|
const partHasSourceColors = !!visualRgb || urdfMeshHasSourceColors(partMesh);
|
|
|
|
vertices.set(sourceVertices, partVertexOffset * 3);
|
|
if (sourceNormals.length === sourceVertices.length) {
|
|
normals.set(sourceNormals, partVertexOffset * 3);
|
|
}
|
|
if (hasSourceColors && partHasSourceColors) {
|
|
if (visualRgb) {
|
|
for (let colorIndex = 0; colorIndex < vertexCount; colorIndex += 1) {
|
|
const offset = (partVertexOffset + colorIndex) * 3;
|
|
colors[offset] = visualRgb[0];
|
|
colors[offset + 1] = visualRgb[1];
|
|
colors[offset + 2] = visualRgb[2];
|
|
}
|
|
} else if (sourceColors.length === sourceVertices.length) {
|
|
colors.set(sourceColors, partVertexOffset * 3);
|
|
}
|
|
}
|
|
for (let index = 0; index < sourceIndices.length; index += 1) {
|
|
indices[indexOffset + index] = sourceIndices[index] + partVertexOffset;
|
|
}
|
|
|
|
const partLabel = String(visual?.label || visual?.instanceId || visual?.id || meshUrl || partFileRef).trim();
|
|
parts.push({
|
|
id: String(visual?.id || `${linkName}:${meshUrl || partFileRef}`),
|
|
name: partLabel,
|
|
label: partLabel,
|
|
color: visualColor,
|
|
meshUrl,
|
|
partFileRef,
|
|
linkName,
|
|
localTransform: toTransformArray(visual?.localTransform),
|
|
sourceBounds: partMesh.bounds,
|
|
bounds: partMesh.bounds,
|
|
transform: [...IDENTITY_TRANSFORM],
|
|
hasSourceColors: partHasSourceColors,
|
|
vertexOffset: partVertexOffset,
|
|
vertexCount,
|
|
triangleOffset: partTriangleOffset,
|
|
triangleCount,
|
|
edgeIndexOffset: 0,
|
|
edgeIndexCount: 0
|
|
});
|
|
|
|
vertexOffset += vertexCount;
|
|
indexOffset += sourceIndices.length;
|
|
}
|
|
|
|
return {
|
|
vertices,
|
|
indices,
|
|
normals,
|
|
colors,
|
|
edge_indices: new Uint32Array(0),
|
|
bounds: mergeBounds(parts.map((part) => part.bounds)),
|
|
parts,
|
|
has_source_colors: hasSourceColors
|
|
};
|
|
}
|
|
|
|
function urdfMeshHasSourceColors(partMesh) {
|
|
return !!partMesh?.has_source_colors &&
|
|
partMesh.colors?.length > 0 &&
|
|
partMesh.colors.length === partMesh.vertices?.length;
|
|
}
|
|
|
|
function urdfVisualHasDisplayColors(visualColor, partMesh) {
|
|
return !!parseHexColorToLinearRgb(visualColor) || (!visualColor && urdfMeshHasSourceColors(partMesh));
|
|
}
|
|
|
|
export function poseUrdfMeshData(urdfData, meshData, jointValuesByName = {}) {
|
|
const linkWorldTransforms = solveUrdfLinkWorldTransforms(urdfData, jointValuesByName);
|
|
const sourceParts = Array.isArray(meshData?.parts) ? meshData.parts : [];
|
|
const posedParts = sourceParts.map((part) => {
|
|
const linkWorldTransform = linkWorldTransforms.get(String(part?.linkName || "")) || [...IDENTITY_TRANSFORM];
|
|
const localTransform = toTransformArray(part?.localTransform);
|
|
const worldTransform = multiplyTransforms(linkWorldTransform, localTransform);
|
|
const sourceBounds = part?.sourceBounds || part?.bounds;
|
|
return {
|
|
...part,
|
|
transform: worldTransform,
|
|
bounds: transformBounds(sourceBounds, worldTransform)
|
|
};
|
|
});
|
|
|
|
return {
|
|
meshData: {
|
|
...meshData,
|
|
bounds: mergeBounds(posedParts.map((part) => part.bounds)),
|
|
parts: posedParts
|
|
},
|
|
linkWorldTransforms
|
|
};
|
|
}
|
|
|
|
export function buildUrdfMeshData(urdfData, meshesByUrl, jointValuesByName = {}) {
|
|
return poseUrdfMeshData(
|
|
urdfData,
|
|
buildUrdfMeshGeometry(urdfData, meshesByUrl),
|
|
jointValuesByName
|
|
);
|
|
}
|