order geometry transforms properly

This fixes shapes changing depending on their rotation; if we rotate before
scaling, the shape is rotated along different axes!

Author: garfieldnate

src/main/java/edu/umich/soar/svsviewer/command/UpdateGeometryCommand.java

@@ -10,14 +10,14 @@
 import java.util.List;
 import java.util.Map;
 
-import javafx.application.Platform;
-import javafx.event.Event;
 import javafx.geometry.Point3D;
 import javafx.scene.Node;
 import javafx.scene.paint.Color;
 import javafx.scene.paint.PhongMaterial;
 import javafx.scene.shape.*;
 import javafx.scene.transform.Rotate;
+import javafx.scene.transform.Scale;
+import javafx.scene.transform.Translate;
 
 public record UpdateGeometryCommand(
     NameMatcher sceneMatcher,
@@ -43,30 +43,21 @@ public void interpret(GeometryManager geoManager, SceneController sceneControlle
     // part of this list.
     for (Geometry geometry : geoManager.findGeometries(sceneMatcher, geometryMatcher)) {
       if (position != null) {
-        geometry.modifyGroups(
-            node -> {
-              node.setTranslateX(position.get(0));
-              node.setTranslateY(position.get(1));
-              node.setTranslateZ(position.get(2));
-            });
+        geometry.setTranslation(new Translate(position.get(0), position.get(1), position.get(2)));
         rerenderedScenes.put(geometry.getParent().name(), geometry.getParent());
       }
-      if (rotation != null) {
-        rotateWithQuaternion(rotation, geometry);
+      if (scale != null) {
+        geometry.setScale(new Scale(scale.get(0), scale.get(1), scale.get(2)));
         rerenderedScenes.put(geometry.getParent().name(), geometry.getParent());
       }
-      if (scale != null) {
-        geometry.modifyGroups(
-            node -> {
-              node.setScaleX(scale.get(0));
-              node.setScaleY(scale.get(1));
-              node.setScaleZ(scale.get(2));
-            });
+      if (rotation != null) {
+        Rotate r = quaternionToRotation(rotation);
+        geometry.setRotation(r);
         rerenderedScenes.put(geometry.getParent().name(), geometry.getParent());
       }
 
       if (vertices != null) {
-        geometry.clearChildren();
+        geometry.clear();
         TriangleMesh mesh = verticesToTriangleMesh(vertices);
         MeshView meshView = new MeshView(mesh);
         meshView.setCullFace(CullFace.NONE);
@@ -88,7 +79,7 @@ public void interpret(GeometryManager geoManager, SceneController sceneControlle
         rerenderedScenes.put(geometry.getParent().name(), geometry.getParent());
       }
       if (radius != null) {
-        geometry.clearChildren();
+        geometry.clear();
         Sphere s = new Sphere(radius);
         geometry.getGroup().getChildren().add(s);
 
@@ -99,7 +90,7 @@ public void interpret(GeometryManager geoManager, SceneController sceneControlle
         rerenderedScenes.put(geometry.getParent().name(), geometry.getParent());
       }
       if (text != null) {
-        geometry.clearChildren();
+        geometry.clear();
         // svs_viewer calls draw_text(g->text, 0, 0), which would draw at the origin no matter where
         // the geometry is;
         // that seems wrong. I think maybe it was never implemented properly because it's not

src/main/java/edu/umich/soar/svsviewer/scene/Geometry.java

@@ -4,8 +4,14 @@
 
 import edu.umich.soar.svsviewer.util.DrawingMode;
 import java.util.function.Consumer;
+
+import javafx.collections.ObservableList;
 import javafx.scene.Group;
 import javafx.scene.Node;
+import javafx.scene.transform.Rotate;
+import javafx.scene.transform.Scale;
+import javafx.scene.transform.Transform;
+import javafx.scene.transform.Translate;
 
 public class Geometry {
   private final SVSScene parent;
@@ -20,18 +26,42 @@ public class Geometry {
 
   private final Node label;
 
+  private final Scale scale;
+  private final Rotate rotation;
+  private final Translate translation;
+
+  // The ordering here is significant (transform matrices are not commutative)
+  private static final int TRANSLATE_TRANSFORM_INDEX = 0;
+  private static final int ROTATE_TRANSFORM_INDEX = 1;
+  private static final int SCALE_TRANSFORM_INDEX = 2;
+
   public Geometry(String name, SVSScene parent) {
     this.name = name;
     this.parent = parent;
     group = new Group();
     lineGroup = new Group();
     label = createLabel(name);
+
+    rotation = new Rotate();
+    translation = new Translate();
+    scale = new Scale();
+    modifyGroups(
+        g -> {
+          ObservableList<Transform> transforms = g.getTransforms();
+          transforms.add(TRANSLATE_TRANSFORM_INDEX, translation);
+          transforms.add(ROTATE_TRANSFORM_INDEX, rotation);
+          transforms.add(SCALE_TRANSFORM_INDEX, scale);
+        });
   }
 
   public Group getGroup() {
     return group;
   }
 
+  public Group getLineGroup() {
+    return lineGroup;
+  }
+
   /**
    * If the client doesn't need the underlying main Group or line Group, it's recommended to operate
    * on them via this method to ensure that they stay in sync.
@@ -41,12 +71,20 @@ public void modifyGroups(Consumer<Group> modifier) {
     modifier.accept(getLineGroup());
   }
 
-  public void clearChildren() {
+  public void clear() {
     modifyGroups(g -> g.getChildren().clear());
   }
 
-  public Group getLineGroup() {
-    return lineGroup;
+  public void setTranslation(Translate t) {
+    modifyGroups(g -> g.getTransforms().set(TRANSLATE_TRANSFORM_INDEX, t));
+  }
+
+  public void setRotation(Rotate r) {
+    modifyGroups(g -> g.getTransforms().set(ROTATE_TRANSFORM_INDEX, r));
+  }
+
+  public void setScale(Scale s) {
+    modifyGroups(g -> g.getTransforms().set(SCALE_TRANSFORM_INDEX, s));
   }
 
   public Node getLabel() {