@z3dev if there is anything I can do to help, I'm willing.
I'm happy to share my code as well.
Here is PosableGeom3.js:
"use strict"; const jscad = require('@jscad/modeling'); const { Pose } = require('./Pose'); const { mat4, vec3, vec4 } = jscad.maths; const { geom3 } = jscad.geometries; const { PI } = Math; // Base class for geom3 objects with named Poses class PosableGeom3 { constructor(geometry, poses) { this._geometry = geometry || geom3.create(); this._poses = {}; if (poses) { Object.keys(poses).forEach(key => { this._poses[key] = poses[key].clone(); }); } } get polygons() { return this._geometry.polygons; } set polygons() { this._geometry.polygons = value; } get transforms() { return this._geometry.transforms; } set transforms(value) { this._geometry.transforms = value; } clone() { // Create a new geom3 with deep-copied polygons and transforms const clonedGeom = geom3.clone(this._geometry); // Clone the poses const clonedPoses = {}; Object.keys(this._poses).forEach(key => { clonedPoses[key] = this._poses[key].clone(); }); // Create a new PosableGeom3 with the cloned geometry and poses return new PosableGeom3(clonedGeom, clonedPoses); } transform(matrix) { this._geometry = geom3.transform(matrix, this._geometry); Object.keys(this._poses).forEach(key => { this._poses[key].transform(matrix); }); return this; } getPose(name) { return this._poses[name] || new Pose(); } applyTransforms(){ this._geometry = geom3.create(geom3.toPolygons(this._geometry)); return this; } alignTo(port, targetPose) { const sourcePose = this.getPose(port); if (!sourcePose) { throw new Error(`Invalid port ${port}`); } if (!targetPose) { throw new Error(`Invalid targetPose`); } return this.transform( sourcePose.getMatrix(targetPose) ); } } module.exports = { PosableGeom3 };And here is Pose.js:
"use strict"; const jscad = require('@jscad/modeling'); const { vec3, vec4, mat4 } = jscad.maths; const { geom3 } = jscad.geometries; const { abs } = Math; const { translate } = jscad.transforms; const { union } = jscad.booleans; const { cylinder, sphere, cylinderElliptic } = jscad.primitives; const x = 0; const y = 1; const z = 2; const w = 3; class Pose { constructor(point, heading, up) { // Default position to origin if not provided this._point = point ? vec3.clone(point) : vec3.create(); // Default heading to Y+ (0, 1, 0) if not provided this._heading = vec3.normalize( vec3.create(), heading ? vec3.clone(heading) : [0, 1, 0] ); // Default up to Z+ (0, 0, 1) if not provided this._up = up ? vec3.clone(up) : [0, 0, 1]; // Project up onto the plane perpendicular to heading const dot = vec3.dot(this._up, this._heading); const projectedUp = vec3.subtract( vec3.create(), this._up, vec3.scale(vec3.create(), this._heading, dot) ); // Check if up is parallel to heading // (length of projectedUp near zero) if (vec3.length(projectedUp) < 0.00001) { // Choose a perpendicular vector based on heading if (abs(this._heading[z]) < 0.99999) { this._up = vec3.cross( vec3.create(), this._heading, [0, 0, 1] ); } else { this._up = vec3.cross( vec3.create(), this._heading, [1, 0, 0] ); } } else { this._up = vec3.normalize(vec3.create(), projectedUp); } // Ensure up is normalized vec3.normalize(this._up, this._up); } get point() { return vec3.clone(this._point); } get heading() { return vec3.clone(this._heading); } get up() { return vec3.clone(this._up); } clone() { return new Pose(this._point, this._heading, this._up); } transform(matrix) { // Transform point (w = 1) let v = this._point.concat(1); // Convert vec3 to vec4 v = vec4.transform(vec4.create(), v, matrix); this._point = vec3.clone(v); // Convert back to vec3 // Transform heading (w = 0) v = this._heading.concat(0); // Convert vec3 to vec4 v = vec4.transform(vec4.create(), v, matrix); this._heading = vec3.clone(v); // Convert back to vec3 // Transform up (w = 0) v = this._up.concat(0); // Convert vec3 to vec4 v = vec4.transform(vec4.create(), v, matrix); this._up = vec3.clone(v); // Convert back to vec3 return this; } getMatrix(targetPose) { let t = mat4.create(); // Step 1: Translate to origin t = mat4.multiply( mat4.create(), mat4.fromTranslation(mat4.create(), vec3.scale(vec3.create(), this._point, -1)), t ); // Step 2: Align heading with targetPose t = mat4.multiply( mat4.create(), mat4.fromVectorRotation(mat4.create(), this._heading, targetPose._heading), t ); // Step 3: Roll around heading to align up vector const p = this.clone().transform(t); const dot = vec3.dot( vec3.cross(vec3.create(), p._up, targetPose._up), p._heading ); let angle = vec3.angle(p._up, targetPose._up); if (dot < 0) angle = -angle; if (Math.abs(angle) > 1e-6) { t = mat4.multiply( mat4.create(), mat4.fromRotation(mat4.create(), angle, targetPose._heading), t ); } // Step 4: Translate to targetPose.point t = mat4.multiply( mat4.create(), mat4.fromTranslation(mat4.create(), targetPose._point), t ); return t; } render() { const vecGeom = (vector) => { const vectorLength = vec3.length(vector) || 1;; const vectorRadius = vectorLength / 10; const arrowLength = vectorLength / 5; const arrowRadius = vectorLength / 5; let out = union( cylinder({ // arrow body center: [0, 0, vectorLength / 2], height: vectorLength, radius: vectorRadius }), cylinderElliptic({ // arrow head center: [0, 0, vectorLength], startRadius: [arrowRadius, arrowRadius], endRadius: [0, 0], height: arrowLength }) ); out = geom3.transform( mat4.fromVectorRotation( mat4.create(), [0, 0, vectorLength], vector ), out ); return out; }; let g = translate( this._point, union( sphere({ radius: 0.2 }), vecGeom(this._heading), vecGeom(this._up) ) ); return g; } roll(angle) { if (angle === 0) return this; // Create a rotation matrix around the heading vector const rotationMatrix = mat4.create(); mat4.rotate( rotationMatrix, rotationMatrix, angle, this._heading ); // Transform the up vector using the rotation matrix (w = 0) const transformedUp = vec4.transform( vec4.create(), this._up.concat(0), rotationMatrix ); this._up = vec3.clone(transformedUp); return this; } translate(vector) { this._point = vec3.add( vec3.create(), vector, this._point ); } } module.exports = { Pose }; // end of file