From b606d62a9281052a16bb55c28dacfb46ae68d5e3 Mon Sep 17 00:00:00 2001 From: rondlh <77279634+rondlh@users.noreply.github.com> Date: Tue, 13 Jan 2026 00:08:26 +0800 Subject: [PATCH] Update snap grid support in Manipulation.java Update snap grid support in Manipulation.java --- .../bowlerkernel/Bezier3d/Manipulation.java | 273 +++++++++++------- 1 file changed, 165 insertions(+), 108 deletions(-) diff --git a/src/main/java/com/neuronrobotics/bowlerkernel/Bezier3d/Manipulation.java b/src/main/java/com/neuronrobotics/bowlerkernel/Bezier3d/Manipulation.java index 1afb7432..5338c746 100644 --- a/src/main/java/com/neuronrobotics/bowlerkernel/Bezier3d/Manipulation.java +++ b/src/main/java/com/neuronrobotics/bowlerkernel/Bezier3d/Manipulation.java @@ -21,13 +21,17 @@ public class Manipulation { public HashMap, EventHandler> map = new HashMap<>(); - double startx = 0; - double starty = 0; + double startx = 0; // drag X-start position on screen + double starty = 0; // drag Y-start position on screen double newx = 0; double newy = 0; double newz = 0; boolean dragging = false; - private double increment = 0.000001; + boolean snapGridEnabled = true; + private boolean startCorrected = false; // Keep track if starting point was corrected + + public static final double SNAP_GRID_OFF = 0.000001; + private double snapGridValue = SNAP_GRID_OFF; private static IInteractiveUIElementProvider ui = new IInteractiveUIElementProvider() { @Override @@ -41,75 +45,81 @@ public PerspectiveCamera getCamera() { private ArrayList saveListeners = new ArrayList<>(); private ArrayList dependants = new ArrayList<>(); private Affine manipulationMatrix; - private TransformNR orintation; - private TransformNR globalPose= new TransformNR(); - private TransformNR currentPose=new TransformNR(); + private TransformNR orientation; + private TransformNR globalPose = new TransformNR(); + private TransformNR currentPose = new TransformNR(); private IFrameProvider frameOfReference = ()->new TransformNR(); - //private PhongMaterial color;// = new PhongMaterial(getColor()); - //private PhongMaterial highlight = new PhongMaterial(Color.GOLD); -private double gridOffsetX = 0; -private double gridOffsetY = 0; -private double gridOffsetZ = 0; -private Point3D startingAbsolutePosition; + private double gridOffsetX = 0; + private double gridOffsetY = 0; + private double gridOffsetZ = 0; + private Point3D startingWorkplanePosition = null; + + public boolean isWorkplaneRotated() { + TransformNR workplane = getFrameOfReference(); + RotationNR workplaneRotation = workplane.getRotation(); + double[][] rm = workplaneRotation.getRotationMatrix(); + return (Math.abs(rm[0][2]) > 1e-9) || (Math.abs(rm[1][2]) > 1e-9) || (Math.abs(rm[2][2] - 1.0) > 1e-9); + } - public void setStartingAbsolutePosition(Point3D position) { + // Calculate the starting point based on the active work plane + public void setStartingWorkplanePosition(Point3D startingPoint) { gridOffsetX = 0; gridOffsetY = 0; gridOffsetZ = 0; + snapGridEnabled = true; // Auto reset to enabled, disable lasts only for one drag + startCorrected = false; try { - // Store the raw global position for grid offset calculation - - // Transform to workplane space for snapping - TransformNR global = new TransformNR(position.getX(), position.getY(), position.getZ(), new RotationNR()); - TransformNR wp = getFrameOfReference().copy(); - wp.setX(0); - wp.setY(0); - wp.setZ(0); - global = wp.inverse().times(global); - - double x = global.getX() * orintation.getX(); - double y = global.getY() * orintation.getY(); - double z = global.getZ() * orintation.getZ(); - - this.startingAbsolutePosition = new Point3D(x, y, z); - - } catch(Throwable t) { + + TransformNR workplane = getFrameOfReference(); + Vector3d origin = new Vector3d(workplane.getX(), workplane.getY(), workplane.getZ()); + Vector3d clicked = new Vector3d(startingPoint.getX(), startingPoint.getY(), startingPoint.getZ()); + Vector3d diff = clicked.minus(origin); + + RotationNR workplaneRotation = workplane.getRotation(); + double[][] rm = workplaneRotation.getRotationMatrix(); + + boolean rotated = (Math.abs(rm[0][2]) > 1e-9) || (Math.abs(rm[1][2]) > 1e-9) || (Math.abs(rm[2][2] - 1.0) > 1e-9); + + // Get active work plane axis + Vector3d xAxis = new Vector3d(rm[0][0], rm[1][0], rm[2][0]); + Vector3d yAxis = new Vector3d(rm[0][1], rm[1][1], rm[2][1]); + Vector3d zAxis = new Vector3d(rm[0][2], rm[1][2], rm[2][2]); + + // Get only the perpendicular parts + double x = diff.dot(xAxis); + double y = diff.dot(yAxis); + double z = diff.dot(zAxis); + + // Don't use XY-offsets on rotated work planes + // Or perhaps, use one corner of the object as origin (0, 0)? + if (rotated) { + x = 0; + y = 0; + } + + startingWorkplanePosition = new Point3D(x, y, z); + + } catch (Throwable t) { t.printStackTrace(); } } - public void calculateGridOffsets() { + private void calculateGridOffsets() { + if ((startingWorkplanePosition == null) || (snapGridValue <= 0)) + return; - if (startingAbsolutePosition != null && increment > 0) { - double gridX = Math.round(startingAbsolutePosition.getX() / increment) * increment; - double gridY = Math.round(startingAbsolutePosition.getY() / increment) * increment; - double gridZ = Math.round(startingAbsolutePosition.getZ() / increment) * increment; - - gridOffsetX = gridX - startingAbsolutePosition.getX(); - if (gridOffsetX > increment / 2.0) - gridOffsetX -= increment; - if (gridOffsetX < -increment / 2.0) - gridOffsetX += increment; - - gridOffsetY = gridY - startingAbsolutePosition.getY(); - if (gridOffsetY > increment / 2.0) - gridOffsetY -= increment; - if (gridOffsetY < -increment / 2.0) - gridOffsetY += increment; - - gridOffsetZ = gridZ - startingAbsolutePosition.getZ(); - if (gridOffsetZ > increment / 2.0) - gridOffsetZ -= increment; - if (gridOffsetZ < -increment / 2.0) - gridOffsetZ += increment; - } - } + double gx = Math.round(startingWorkplanePosition.getX() / snapGridValue) * snapGridValue; + double gy = Math.round(startingWorkplanePosition.getY() / snapGridValue) * snapGridValue; + double gz = Math.round(startingWorkplanePosition.getZ() / snapGridValue) * snapGridValue; - public Point3D getStartingAbsolutePosition() { - return startingAbsolutePosition; + gridOffsetX = (gx - startingWorkplanePosition.getX()) * orientation.getX(); + gridOffsetY = (gy - startingWorkplanePosition.getY()) * orientation.getY(); + gridOffsetZ = (gz - startingWorkplanePosition.getZ()) * orientation.getZ(); + + com.neuronrobotics.sdk.common.Log.debug(">>> calculateGridOffsets gridOffsetX:" + gridOffsetX + " Y:" + gridOffsetY + " Z:" + gridOffsetZ); } public enum DragState { @@ -117,24 +127,26 @@ public enum DragState { } private DragState state = DragState.IDLE; - private boolean resizeAllowed=true; + private boolean resizeAllowed = true; public void addEventListener(EventHandler r) { if (eventListeners.contains(r)) return; + eventListeners.add(r); } public void addDependant(Manipulation r) { if (dependants.contains(r)) return; - dependants.add(r); + dependants.add(r); } public void addSaveListener(Runnable r) { if (saveListeners.contains(r)) return; + saveListeners.add(r); } @@ -145,32 +157,31 @@ public void clearListeners() { } private void fireMove(TransformNR trans, MouseEvent event2) { - for (Manipulation R : dependants) { + for (Manipulation R : dependants) R.performMove(trans,event2); - } + //com.neuronrobotics.sdk.common.Log.debug("Mouse event "+event2.getEventType()); - for (EventHandler R : eventListeners) { + for (EventHandler R : eventListeners) R.handle(event2); - } + } public void fireSave() { new Thread(() -> { - for (Runnable R : saveListeners) { + for (Runnable R : saveListeners) R.run(); - } + }).start(); } -// public manipulation(Affine mm, Vector3d o, CSG m, TransformNR p) { -// -// } + public Manipulation(Affine mm, Vector3d o, TransformNR p) { this.manipulationMatrix = mm; - this.orintation = new TransformNR(o.x, o.y, o.z); + this.orientation = new TransformNR(o.x, o.y, o.z); //this.manip = m; //color = new PhongMaterial(m.getColor()); this.setGlobalPose(p); setCurrentPose(p.copy()); + getUi().runLater(() -> { try { TransformFactory.nrToAffine(getGlobalPose(), manipulationMatrix); @@ -187,19 +198,25 @@ public EventHandler getMouseEvents() { @Override public void handle(MouseEvent event) { String name = event.getEventType().getName(); - if(event.isControlDown()) + + if (event.isControlDown()) return; + switch (name) { + case "MOUSE_PRESSED": - if(event.isPrimaryButtonDown()) + if (event.isPrimaryButtonDown()) pressed(event); break; + case "MOUSE_DRAGGED": dragged(event,event); break; + case "MOUSE_RELEASED": release(event); break; + case "MOUSE_MOVED": // ignore break; @@ -221,13 +238,14 @@ public void handle(MouseEvent event) { private void pressed(MouseEvent event) { setState(DragState.Dragging); + new Thread(() -> { event.consume(); dragging = false; - for (Manipulation R : dependants) { + for (Manipulation R : dependants) R.dragging = false; - } + }).start(); } @@ -239,60 +257,69 @@ private void release(MouseEvent event) { mouseRelease(event); for (Manipulation R : dependants) R.mouseRelease(event); + setState(DragState.IDLE); //manip.getMesh().setMaterial(color); } private void dragged(MouseEvent event, MouseEvent event2) { - if(resizeAllowed) - if(getState()==DragState.Dragging) { + + if (resizeAllowed && (getState() == DragState.Dragging)) { + getUi().runLater(() -> { setDragging(event); double deltx = (startx - event.getScreenX()); double delty = (starty - event.getScreenY()); - double x = deltx/ getDepthNow() ; - double y = delty/ getDepthNow() ; + double x = deltx / getDepthNow() ; + double y = delty / getDepthNow() ; + //com.neuronrobotics.sdk.common.Log.error("Moved "+x+" "+y); - if(Double.isFinite(y) && Double.isFinite(x)) { + if (Double.isFinite(y) && Double.isFinite(x)) { TransformNR trans = new TransformNR(x, y, 0, new RotationNR()); performMove(trans,event2); - }else { + } else com.neuronrobotics.sdk.common.Log.error("ERROR?"); - } + }); event.consume(); } } public boolean isMoving() { - return getState() == DragState.Dragging; + return (getState() == DragState.Dragging); } private void mouseRelease(MouseEvent event) { + if (dragging) { dragging = false; getGlobalPose().setX(newx); getGlobalPose().setY(newy); getGlobalPose().setZ(newz); - if(event!=null) + + if (event != null) event.consume(); + fireSave(); } } private void setDragging(MouseEvent event) { - if (dragging == false) { + + if (!dragging) { startx = event.getScreenX(); starty = event.getScreenY(); dragging = true; + startCorrected = false; } - for (Manipulation R : dependants) { + for (Manipulation R : dependants) R.setDragging(event); - } + } private void performMove(TransformNR trans, MouseEvent event2) { + TransformNR camerFrame = getUi().getCamerFrame(); TransformNR globalTMP = new TransformNR(camerFrame.getRotation()); @@ -306,11 +333,23 @@ private void performMove(TransformNR trans, MouseEvent event2) { wp.setZ(0); global = wp.inverse().times(global); - calculateGridOffsets(); // Calculate only AFTER the first call! - - newx = snapToGrid(global.getX() * orintation.getX()) + gridOffsetX * orintation.getX(); - newy = snapToGrid(global.getY() * orintation.getY()) + gridOffsetY * orintation.getY(); - newz = snapToGrid(global.getZ() * orintation.getZ()) + gridOffsetZ * orintation.getZ(); + if (snapGridEnabled) { + newx = snapToGrid(global.getX() * orientation.getX()) + gridOffsetX; + newy = snapToGrid(global.getY() * orientation.getY()) + gridOffsetY; + newz = snapToGrid(global.getZ() * orientation.getZ()) + gridOffsetZ; + } else { + newx = global.getX() * orientation.getX(); + newy = global.getY() * orientation.getY(); + newz = global.getZ() * orientation.getZ(); + } + + if (!startCorrected && dragging) { + calculateGridOffsets(); // Calculate only AFTER the first call! + + startx += gridOffsetX; + starty += gridOffsetY; + startCorrected = true; + } TransformNR globalTrans = globalPose.copy().setRotation(new RotationNR()); @@ -328,17 +367,26 @@ private void performMove(TransformNR trans, MouseEvent event2) { } catch(Throwable t) { t.printStackTrace(); } + fireMove(trans, event2); + } private double snapToGrid(double in) { - return Math.round(in / increment) * increment; + if (!snapGridEnabled) + return in; + + if (snapGridValue <= SNAP_GRID_OFF) + snapGridValue = SNAP_GRID_OFF; + + return Math.round(in / snapGridValue) * snapGridValue; } - + + public void setSnapGridStatus(boolean status) { + this.snapGridEnabled = status; + } + public void setGlobal(TransformNR global) { -// newx = global.getX(); -// newy = global.getY(); -// newz = global.getZ(); getCurrentPose().setX(newx); getCurrentPose().setY(newy); getCurrentPose().setZ(newz); @@ -354,6 +402,7 @@ public static IInteractiveUIElementProvider getUi() { public static void setUi(IInteractiveUIElementProvider ui) { Manipulation.ui = ui; } + public void reset() { newx = 0; newy = 0; @@ -363,58 +412,66 @@ public void reset() { getGlobalPose().setZ(0); setGlobal(new TransformNR(0, 0, 0, new RotationNR())); } + public void set(double newX, double newY, double newZ) { + newx = newX; newy = newY; newz = newZ; + getGlobalPose().setX(newX); getGlobalPose().setY(newY); getGlobalPose().setZ(newZ); setGlobal(new TransformNR(newX, newY, newZ, new RotationNR())); - for (EventHandler R : eventListeners) { + + for (EventHandler R : eventListeners) R.handle(null); - } } + public void setInReferenceFrame(double newX, double newY, double newZ) { TransformNR inLocal = new TransformNR(newX, newY, newZ); - TransformNR wp = new TransformNR( getFrameOfReference() .getRotation()); - inLocal=wp.times(inLocal); + TransformNR wp = new TransformNR(getFrameOfReference().getRotation()); + inLocal = wp.times(inLocal); inLocal.setRotation(new RotationNR()); //com.neuronrobotics.sdk.common.Log.error("Setting in reference frame:"+inLocal.toSimpleString()); setGlobal(inLocal); - for (EventHandler R : eventListeners) { + + for (EventHandler R : eventListeners) R.handle(null); - } } + public TransformNR getGlobalPose() { return globalPose; } + public TransformNR getGlobalPoseInReferenceFrame() { TransformNR globalPose = getGlobalPose().copy(); - TransformNR wp = new TransformNR( getFrameOfReference() .getRotation()); + TransformNR wp = new TransformNR(getFrameOfReference().getRotation()); globalPose=wp.times(globalPose); globalPose.setRotation(new RotationNR()); return globalPose; } + public TransformNR getCurrentPoseInReferenceFrame() { TransformNR globalPose = getCurrentPose().copy(); - TransformNR wp = new TransformNR( getFrameOfReference() .getRotation()); - globalPose=wp.times(globalPose); + TransformNR wp = new TransformNR(getFrameOfReference().getRotation()); + globalPose = wp.times(globalPose); globalPose.setRotation(new RotationNR()); return globalPose; } + public void setGlobalPose(TransformNR globalPose) { this.globalPose = globalPose; } public double getIncrement() { - return increment; + return snapGridValue; } - public void setIncrement(double increment) { - this.increment = increment; + public void setIncrement(double value) { + this.snapGridValue = value; } public TransformNR getCurrentPose() { @@ -449,4 +506,4 @@ public void setState(DragState state) { this.state = state; } -} +} \ No newline at end of file