diff --git a/curveEditor.js b/curveEditor.js index 4546d16..c52e611 100644 --- a/curveEditor.js +++ b/curveEditor.js @@ -215,21 +215,21 @@ export class CurveEditor { } // -yToMotorPosition(yCanvas) { - const [minOut, maxOut] = this.exportRange; // Output bounds (0 - 4095) + yToMotorPosition(yCanvas) { + const [minOut, maxOut] = this.exportRange; // Output bounds (0 - 4095) - // Transform yCanvas to the original logical space by adjusting for the current offset and scale. - const yLogical = (yCanvas - this.offset.y) / this.scaleY; + // Transform yCanvas to the original logical space by adjusting for the current offset and scale. + const yLogical = (yCanvas - this.offset.y) / this.scaleY; - // Normalize the logical value based on the actual height of the logical range - const normalized = yLogical / 800; // This assumes yLogical ranges from 0 to 800 + // Normalize the logical value based on the actual height of the logical range + const normalized = yLogical / 800; // This assumes yLogical ranges from 0 to 800 - // Flip normalized to convert to desired motor range - const flipped = normalized; // This gives the inverted mapping + // Flip normalized to convert to desired motor range + const flipped = normalized; // This gives the inverted mapping - // Calculate the final motor position - return Math.round(flipped * (maxOut - minOut) + minOut); -} + // Calculate the final motor position + return Math.round(flipped * (maxOut - minOut) + minOut); + } // exportRangeToY(value) { @@ -663,14 +663,27 @@ yToMotorPosition(yCanvas) { } for (let curve of this.curves) { - for (let key of ['startPointHandle', 'endPointHandle', 'startPoint', 'endPoint']) { - const p = curve[key]; - if (Math.hypot(p.x - mouse.x, p.y - mouse.y) < 10 / this.scale) { - this.dragging = { curve, key }; - return; - } - } - } + // Pass 1: check handles first + for (let key of ['startPointHandle', 'endPointHandle']) { + const p = curve[key]; + if (Math.hypot(p.x - mouse.x, p.y - mouse.y) < 10 / this.scale) { + this.dragging = { curve, key }; + return; + } + } +} + +// If no handle was hit, then check points +for (let curve of this.curves) { + for (let key of ['startPoint', 'endPoint']) { + const p = curve[key]; + if (Math.hypot(p.x - mouse.x, p.y - mouse.y) < 10 / this.scale) { + this.dragging = { curve, key }; + return; + } + } +} + }); @@ -758,76 +771,106 @@ yToMotorPosition(yCanvas) { } splitCurveAtTime(motorID, timeX, yPosition = null) { - const curves = this.curveSets[motorID]; - if (!curves) return; + const curves = this.curveSets[motorID]; + if (!curves) return; - for (let i = 0; i < curves.length; i++) { - const curve = curves[i]; + const tolerance = 2; // pixels/frames tolerance for matching existing points - // --- if timeX matches an existing startPoint --- - if (Math.abs(curve.startPoint.x - timeX) < 1e-3) { - if (yPosition !== null) { - const yEditor = (yPosition * 800 / 4095); - curve.startPoint.y = yEditor; - if (curve.startPointHandle) curve.startPointHandle.y = yEditor; - } - this.draw(); - return; + for (let i = 0; i < curves.length; i++) { + const curve = curves[i]; + + // --- check startPoint --- + if (Math.abs(curve.startPoint.x - timeX) < tolerance) { + if (yPosition !== null) { + // move existing point + const yEditor = (yPosition * 800 / 4095); + curve.startPoint.y = yEditor; + if (curve.startPointHandle) curve.startPointHandle.y = yEditor; + } else { + // remove point by merging with previous + if (i > 0) { + const prev = curves[i - 1]; + const merged = { + startPoint: prev.startPoint, + startPointHandle: prev.startPointHandle, + endPointHandle: curve.endPointHandle, + endPoint: curve.endPoint + }; + curves.splice(i - 1, 2, merged); + } else { + curves.splice(i, 1); // drop if it's the very first + } + } + this.curveSets[motorID] = curves; + this.draw(); + return; + } + + // --- check endPoint --- + if (Math.abs(curve.endPoint.x - timeX) < tolerance) { + if (yPosition !== null) { + // move existing point + const yEditor = (yPosition * 800 / 4095); + curve.endPoint.y = yEditor; + if (curve.endPointHandle) curve.endPointHandle.y = yEditor; + } else { + // remove point by merging with next + if (i < curves.length - 1) { + const next = curves[i + 1]; + const merged = { + startPoint: curve.startPoint, + startPointHandle: curve.startPointHandle, + endPointHandle: next.endPointHandle, + endPoint: next.endPoint + }; + curves.splice(i, 2, merged); + } else { + curves.splice(i, 1); // drop if it's the very last + } + } + this.curveSets[motorID] = curves; + this.draw(); + return; + } + + // --- split inside curve if timeX lies between start and end --- + const x0 = curve.startPoint.x; + const x3 = curve.endPoint.x; + + if (timeX > x0 && timeX < x3) { + // binary search for t where curve.x ≈ timeX + let t = 0.5, minT = 0, maxT = 1; + for (let j = 0; j < 10; j++) { + const pt = this.cubicBezier( + t, + curve.startPoint, + curve.startPointHandle, + curve.endPointHandle, + curve.endPoint + ); + if (pt.x < timeX) minT = t; + else maxT = t; + t = (minT + maxT) / 2; + } + + let [left, right] = this.splitCurve(curve, t); + + if (yPosition !== null) { + const yEditor = (yPosition * 800 / 4095); + left.endPoint.y = yEditor; + right.startPoint.y = yEditor; + if (left.endPointHandle) left.endPointHandle.y = yEditor; + if (right.startPointHandle) right.startPointHandle.y = yEditor; + } + + curves.splice(i, 1, left, right); + this.curveSets[motorID] = curves; + this.draw(); + return; + } + } } - // --- if timeX matches an existing endPoint --- - if (Math.abs(curve.endPoint.x - timeX) < 1e-3) { - if (yPosition !== null) { - const yEditor = (yPosition * 800 / 4095); - curve.endPoint.y = yEditor; - if (curve.endPointHandle) curve.endPointHandle.y = yEditor; - } - this.draw(); - return; - } - - // --- split inside curve if timeX lies between start and end --- - const x0 = curve.startPoint.x; - const x3 = curve.endPoint.x; - - if (timeX > x0 && timeX < x3) { - // binary search for t where curve.x ≈ timeX - let t = 0.5, minT = 0, maxT = 1; - for (let j = 0; j < 10; j++) { - const pt = this.cubicBezier( - t, - curve.startPoint, - curve.startPointHandle, - curve.endPointHandle, - curve.endPoint - ); - if (pt.x < timeX) minT = t; - else maxT = t; - t = (minT + maxT) / 2; - } - - let [left, right] = this.splitCurve(curve, t); - - // ✅ If yPosition provided, override the split point’s Y - if (yPosition !== null) { - const yEditor = (yPosition * 800 / 4095); - left.endPoint.y = yEditor; - right.startPoint.y = yEditor; - if (left.endPointHandle) left.endPointHandle.y = yEditor; - if (right.startPointHandle) right.startPointHandle.y = yEditor; - } - - curves.splice(i, 1, left, right); - this.curveSets[motorID] = curves; - this.draw(); - return; - } - } -} - - - - adjustAllCurvesToDuration(newTime) { for (const [motorID, curves] of Object.entries(this.curveSets)) { diff --git a/ros_robot_visualiser/URDFEditor.js b/ros_robot_visualiser/URDFEditor.js index 2f2211e..6819918 100644 --- a/ros_robot_visualiser/URDFEditor.js +++ b/ros_robot_visualiser/URDFEditor.js @@ -547,6 +547,8 @@ export class URDFEditor { } this.selectedJoint = this.hoveredJoint; this.selectedJoint.material.emissive.setHex(0x555555); // selection color + let motorID = this.findJointAncestor(this.selectedJoint, 0).transmission.motorID; + this.curveEditor.selectMotor(motorID); } this.isDragging = true;