@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
