cleaning

MarginallyClever · Nov 14, 2024 · 0135876 · 0135876
1 parent 9bc8f1a
commit 0135876
Show file tree

Hide file tree

Showing 4 changed files with 28 additions and 31 deletions.
diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/odenode/ODEAngularMotor.java b/src/main/java/com/marginallyclever/ro3/node/nodes/odenode/ODEAngularMotor.java
@@ -114,32 +114,35 @@ private void updatePhysicsFromWorld() {
         var xAxis = MatrixHelper.getXAxis(mat);        motor.setAxis(0,rel,xAxis.x, xAxis.y, xAxis.z);
         var yAxis = MatrixHelper.getYAxis(mat);        motor.setAxis(1,rel,yAxis.x, yAxis.y, yAxis.z);
         var zAxis = MatrixHelper.getZAxis(mat);        motor.setAxis(2,rel,zAxis.x, zAxis.y, zAxis.z);
+        logger.debug("{} setAxis {}",getAbsolutePath(),zAxis);
     }
 
     @Override
     public void update(double dt) {
         super.update(dt);
-        if(!Registry.getPhysics().isPaused()) {
-            // if the physics simulation is running then the motor will behave as normal.
-            DBody body = motor.getBody(0);
-            if(body==null) body = motor.getBody(1);
-            if(body==null) return;
-
-            DVector3C anchor = body.getPosition();
-            DVector3 axis = new DVector3();
-            motor.getAxis(2,axis);
-            // use axis and anchor to set the world matrix.
-            Matrix3d m3 = MatrixHelper.lookAt(
-                    new Vector3d(0,0,0),
-                    new Vector3d(axis.get0(),axis.get1(),axis.get2())
-            );
-            Matrix4d m4 = new Matrix4d();
-            m4.set(m3);
-            m4.setTranslation(new Vector3d(anchor.get0(),anchor.get1(),anchor.get2()));
-            setWorld(m4);
+        if (!Registry.getPhysics().isPaused()) {
+            updateWorldFromPhysics();
         }
     }
 
+    private void updateWorldFromPhysics() {
+        if(motor==null) return;
+
+        DBody body = motor.getBody(0);
+        if(body==null) body = motor.getBody(1);
+        if(body==null) return;
+
+        var anchor = body.getPosition();
+        var axis = new DVector3();
+        motor.getAxis(2,axis);
+        // use axis and anchor to set the world matrix.
+        Matrix3d m3 = MatrixHelper.lookAt(
+                new Vector3d(),  // from
+                new Vector3d(axis.get0(),axis.get1(),axis.get2())  // to
+        );
+        setWorld(new Matrix4d(m3,new Vector3d(anchor.get0(),anchor.get1(),anchor.get2()),1));
+    }
+
     @Override
     public JSONObject toJSON() {
         var json = super.toJSON();
@@ -175,7 +178,6 @@ public double getAngleMin() {
     public void setForceMax(double force) {
         forceMax = force;
         if(motor==null) return;
-        System.out.println("ODEAngularMotor.setForceMax("+force+")");
         motor.setParamFMax(force);
         motor.setParamFMax2(force);
         motor.setParamFMax3(force);

diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/odenode/ODEHinge.java b/src/main/java/com/marginallyclever/ro3/node/nodes/odenode/ODEHinge.java
@@ -126,20 +126,16 @@ public void update(double dt) {
     private void updatePoseFromPhysics() {
         if(hinge==null) return;
 
-        // if the physics simulation is running then the hinge will behave as normal.
-        DVector3 anchor = new DVector3();
-        DVector3 axis = new DVector3();
+        var anchor = new DVector3();
+        var axis = new DVector3();
         hinge.getAnchor(anchor);
         hinge.getAxis(axis);
         // use axis and anchor to set the world matrix.
         Matrix3d m3 = MatrixHelper.lookAt(
-                new Vector3d(0,0,0),
-                new Vector3d(axis.get0(),axis.get1(),axis.get2())
+                new Vector3d(),  // from
+                new Vector3d(axis.get0(),axis.get1(),axis.get2())  // to
         );
-        Matrix4d m4 = new Matrix4d();
-        m4.set(m3);
-        m4.setTranslation(new Vector3d(anchor.get0(),anchor.get1(),anchor.get2()));
-        setWorld(m4);
+        setWorld(new Matrix4d(m3,new Vector3d(anchor.get0(),anchor.get1(),anchor.get2()),1));
     }
 
 

diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/odenode/ODEJoint.java b/src/main/java/com/marginallyclever/ro3/node/nodes/odenode/ODEJoint.java
@@ -117,7 +117,6 @@ public void linkDetached(ODENode body) {
      * Examines the bodies of this link and connects them if possible.
      */
     protected void connectInternal() {
-        //logger.debug(getAbsolutePath()+" connectInternal");
         var as = partA.getSubject();
         var bs = partB.getSubject();
         if(as==null && bs==null) {
@@ -131,7 +130,7 @@ protected void connectInternal() {
         DBody a = as.getODEBody();
         DBody b = bs == null ? null : bs.getODEBody();
 
-        //logger.debug(this.getName()+" connect "+ as.getName() +" to "+(bs == null ?"null":bs.getName()));
+        logger.debug(getAbsolutePath()+" connectInternal "+ as.getAbsolutePath() +" to "+(bs == null ?"null":bs.getAbsolutePath()));
         connect(a,b);
     }
 

diff --git a/src/main/java/com/marginallyclever/ro3/node/nodes/odenode/ODESlider.java b/src/main/java/com/marginallyclever/ro3/node/nodes/odenode/ODESlider.java
@@ -118,7 +118,7 @@ public void update(double dt) {
     private void updateWorldFromPhysics() {
         if(sliderJoint==null) return;
 
-        DVector3 axis = new DVector3();
+        var axis = new DVector3();
         sliderJoint.getAxis(axis);
         Matrix3d m3 = MatrixHelper.lookAt(
                 new Vector3d(),  // from