From c32263675a14e3692719a4eb476b94f91f56c993 Mon Sep 17 00:00:00 2001
From: austin <austin@austinhoover.net>
Date: Wed, 28 May 2025 13:39:03 -0400
Subject: [PATCH] fix upright tree

---
 docs/src/progress/renderertodo.md             |  1 +
 .../collision/CollisionEngine.java            | 79 +++++++++++++------
 .../collision/PhysicsEntityUtils.java         |  4 +-
 .../electrosphere/collision/PhysicsUtils.java | 14 ++++
 .../upright/ClientAlwaysUprightTree.java      |  3 +-
 .../upright/ServerAlwaysUprightTree.java      |  3 +-
 6 files changed, 75 insertions(+), 29 deletions(-)

diff --git a/docs/src/progress/renderertodo.md b/docs/src/progress/renderertodo.md
index 0582f7c9..c1b2fce6 100644
--- a/docs/src/progress/renderertodo.md
+++ b/docs/src/progress/renderertodo.md
@@ -2020,6 +2020,7 @@ Properly show loading state when waiting on character list
 Fix standard uniform buffer interaction with actor panel
 Fix foundation generation for structures in chunkgen
 Body synchronization includes enabled state
+Fix upright tree continuously re-enabling bodies
 
 
 
diff --git a/src/main/java/electrosphere/collision/CollisionEngine.java b/src/main/java/electrosphere/collision/CollisionEngine.java
index 36b125aa..555ff12e 100644
--- a/src/main/java/electrosphere/collision/CollisionEngine.java
+++ b/src/main/java/electrosphere/collision/CollisionEngine.java
@@ -440,31 +440,46 @@ public class CollisionEngine {
         try {
             //null out the contact buffer
             contacts.nullify();
-            SurfaceParams surfaceParams = c1.getSurfaceParams();
+            SurfaceParams surfaceParams1 = c1.getSurfaceParams();
+            SurfaceParams surfaceParams2 = c2.getSurfaceParams();
             for (int i=0; i<MAX_CONTACTS; i++) {
                 DContact contact = contacts.get(i);
-                contact.surface.mode = surfaceParams.getMode();
-                contact.surface.mu = surfaceParams.getMu();
-                if(surfaceParams.getRho() != null){
-                    contact.surface.rho = surfaceParams.getRho();
+                contact.surface.mode = surfaceParams1.getMode();
+                contact.surface.mu = surfaceParams1.getMu();
+                if(surfaceParams1.getRho() != null){
+                    contact.surface.rho = surfaceParams1.getRho();
+                } else if(surfaceParams2.getRho() != null){
+                    contact.surface.rho = surfaceParams2.getRho();
                 }
-                if(surfaceParams.getRho2() != null){
-                    contact.surface.rho2 = surfaceParams.getRho2();
+                if(surfaceParams1.getRho2() != null){
+                    contact.surface.rho2 = surfaceParams1.getRho2();
+                } else if(surfaceParams2.getRho2() != null){
+                    contact.surface.rho = surfaceParams2.getRho2();
                 }
-                if(surfaceParams.getRhoN() != null){
-                    contact.surface.rhoN = surfaceParams.getRhoN();
+                if(surfaceParams1.getRhoN() != null){
+                    contact.surface.rhoN = surfaceParams1.getRhoN();
+                } else if(surfaceParams2.getRhoN() != null){
+                    contact.surface.rho = surfaceParams2.getRhoN();
                 }
-                if(surfaceParams.getBounce() != null){
-                    contact.surface.bounce = surfaceParams.getBounce();
+                if(surfaceParams1.getBounce() != null){
+                    contact.surface.bounce = surfaceParams1.getBounce();
+                } else if(surfaceParams2.getBounce() != null){
+                    contact.surface.rho = surfaceParams2.getBounce();
                 }
-                if(surfaceParams.getBounceVel() != null){
-                    contact.surface.bounce_vel = surfaceParams.getBounceVel();
+                if(surfaceParams1.getBounceVel() != null){
+                    contact.surface.bounce_vel = surfaceParams1.getBounceVel();
+                } else if(surfaceParams2.getBounceVel() != null){
+                    contact.surface.rho = surfaceParams2.getBounceVel();
                 }
-                if(surfaceParams.getSoftErp() != null){
-                    contact.surface.soft_erp = surfaceParams.getSoftErp();
+                if(surfaceParams1.getSoftErp() != null){
+                    contact.surface.soft_erp = surfaceParams1.getSoftErp();
+                } else if(surfaceParams2.getSoftErp() != null){
+                    contact.surface.rho = surfaceParams2.getSoftErp();
                 }
-                if(surfaceParams.getSoftCfm() != null){
-                    contact.surface.soft_cfm = surfaceParams.getSoftCfm();
+                if(surfaceParams1.getSoftCfm() != null){
+                    contact.surface.soft_cfm = surfaceParams1.getSoftCfm();
+                } else if(surfaceParams2.getSoftCfm() != null){
+                    contact.surface.rho = surfaceParams2.getSoftCfm();
                 }
             }
             //calculate collisions
@@ -1276,7 +1291,7 @@ public class CollisionEngine {
     protected void setAutoDisableFlags(DBody body, boolean autoDisable, double linearThreshold, double angularThreshold, int steps){
         spaceLock.lock();
         body.setAutoDisableFlag(autoDisable);
-        body.setAutoDisableLinearThreshold(0.1);
+        body.setAutoDisableLinearThreshold(linearThreshold);
         body.setAutoDisableAngularThreshold(angularThreshold);
         body.setAutoDisableSteps(steps);
         spaceLock.unlock();
@@ -1319,6 +1334,29 @@ public class CollisionEngine {
      * @param enabled Whether the body is enabled or not -- true to enable, false to disable
      */
     protected void synchronizeData(DBody body, Vector3d position, Quaterniond rotation, Vector3d linearVel, Vector3d angularVel, Vector3d linearForce, Vector3d angularForce, boolean enabled){
+        if(body != null){
+            spaceLock.lock();
+            this.synchronizeData(body, position, rotation, linearVel, angularVel, linearForce, angularForce);
+            if(enabled){
+                body.enable();
+            } else {
+                body.disable();
+            }
+            spaceLock.unlock();
+        }
+    }
+
+    /**
+     * Synchronizes the data on a body
+     * @param body The body
+     * @param position The position
+     * @param rotation The rotation
+     * @param linearVel The linear velocity
+     * @param angularVel The angular velocity
+     * @param linearForce The linear force
+     * @param angularForce The angular force
+     */
+    protected void synchronizeData(DBody body, Vector3d position, Quaterniond rotation, Vector3d linearVel, Vector3d angularVel, Vector3d linearForce, Vector3d angularForce){
         if(body != null){
             spaceLock.lock();
             body.setPosition(position.x - this.floatingOrigin.x, position.y - this.floatingOrigin.y, position.z - this.floatingOrigin.z);
@@ -1327,11 +1365,6 @@ public class CollisionEngine {
             body.setAngularVel(PhysicsUtils.jomlVecToOdeVec(angularVel));
             body.setForce(PhysicsUtils.jomlVecToOdeVec(linearForce));
             body.setTorque(PhysicsUtils.jomlVecToOdeVec(angularForce));
-            if(enabled){
-                body.enable();
-            } else {
-                body.disable();
-            }
             spaceLock.unlock();
         }
     }
diff --git a/src/main/java/electrosphere/collision/PhysicsEntityUtils.java b/src/main/java/electrosphere/collision/PhysicsEntityUtils.java
index 5fce4fb0..0115ea0c 100644
--- a/src/main/java/electrosphere/collision/PhysicsEntityUtils.java
+++ b/src/main/java/electrosphere/collision/PhysicsEntityUtils.java
@@ -35,12 +35,12 @@ public class PhysicsEntityUtils {
     /**
      * The linear velocity threshold to disable under
      */
-    static final double LINEAR_THRESHOLD = 0.1;
+    static final double LINEAR_THRESHOLD = 3;
 
     /**
      * The angular velocity threshold to disable under
      */
-    static final double ANGULAR_THRESHOLD = 0.1;
+    static final double ANGULAR_THRESHOLD = 3;
 
     /**
      * The number of steps below threshold to disable after
diff --git a/src/main/java/electrosphere/collision/PhysicsUtils.java b/src/main/java/electrosphere/collision/PhysicsUtils.java
index b13e500a..34316175 100644
--- a/src/main/java/electrosphere/collision/PhysicsUtils.java
+++ b/src/main/java/electrosphere/collision/PhysicsUtils.java
@@ -122,6 +122,20 @@ public class PhysicsUtils {
         collisionEngine.synchronizeData(body, position, rotation, linearVel, angularVel, linearForce, angularForce, enabled);
     }
 
+    /**
+     * Synchronizes the data on a body
+     * @param body The body
+     * @param position The position
+     * @param rotation The rotation
+     * @param linearVel The linear velocity
+     * @param angularVel The angular velocity
+     * @param linearForce The linear force
+     * @param angularForce The angular force
+     */
+    public static void synchronizeData(CollisionEngine collisionEngine, DBody body, Vector3d position, Quaterniond rotation, Vector3d linearVel, Vector3d angularVel, Vector3d linearForce, Vector3d angularForce){
+        collisionEngine.synchronizeData(body, position, rotation, linearVel, angularVel, linearForce, angularForce);
+    }
+
 
     /**
      * Sets the position + rotation + scale of a body
diff --git a/src/main/java/electrosphere/entity/state/physicssync/upright/ClientAlwaysUprightTree.java b/src/main/java/electrosphere/entity/state/physicssync/upright/ClientAlwaysUprightTree.java
index 3eb2bcd3..2ae0883b 100644
--- a/src/main/java/electrosphere/entity/state/physicssync/upright/ClientAlwaysUprightTree.java
+++ b/src/main/java/electrosphere/entity/state/physicssync/upright/ClientAlwaysUprightTree.java
@@ -37,14 +37,13 @@ public class ClientAlwaysUprightTree implements BehaviorTree {
             Vector3d angularVelocity = new Vector3d();
             Vector3d linearForce = PhysicsUtils.odeVecToJomlVec(body.getForce());
             Vector3d angularForce = new Vector3d();
-            boolean enabled = body.isEnabled();
 
             //make sure rotation is vertical
             // sourceRotation = sourceRotation.mul(0.001, 1, 0.001, 1).normalize();
 
             EntityUtils.getPosition(parent).set(position);
             EntityUtils.getRotation(parent).set(sourceRotation);
-            PhysicsUtils.synchronizeData(Globals.clientState.clientSceneWrapper.getCollisionEngine(), body, position, sourceRotation, linearVelocity, angularVelocity, linearForce, angularForce, enabled);
+            PhysicsUtils.synchronizeData(Globals.clientState.clientSceneWrapper.getCollisionEngine(), body, position, sourceRotation, linearVelocity, angularVelocity, linearForce, angularForce);
         }
     }
 
diff --git a/src/main/java/electrosphere/entity/state/physicssync/upright/ServerAlwaysUprightTree.java b/src/main/java/electrosphere/entity/state/physicssync/upright/ServerAlwaysUprightTree.java
index d48c6da4..3dc0137c 100644
--- a/src/main/java/electrosphere/entity/state/physicssync/upright/ServerAlwaysUprightTree.java
+++ b/src/main/java/electrosphere/entity/state/physicssync/upright/ServerAlwaysUprightTree.java
@@ -42,7 +42,6 @@ public class ServerAlwaysUprightTree implements BehaviorTree {
             Vector3d angularVelocity = new Vector3d();
             Vector3d linearForce = PhysicsUtils.odeVecToJomlVec(body.getForce());
             Vector3d angularForce = new Vector3d();
-            boolean enabled = body.isEnabled();
 
             //make sure rotation is vertical
             // sourceRotation = sourceRotation.mul(0.001, 0.001, 0.001, 1).normalize();
@@ -54,7 +53,7 @@ public class ServerAlwaysUprightTree implements BehaviorTree {
 
             EntityUtils.getPosition(parent).set(position);
             EntityUtils.getRotation(parent).set(sourceRotation);
-            PhysicsUtils.synchronizeData(realm.getCollisionEngine(), body, position, sourceRotation, linearVelocity, angularVelocity, linearForce, angularForce, enabled);
+            PhysicsUtils.synchronizeData(realm.getCollisionEngine(), body, position, sourceRotation, linearVelocity, angularVelocity, linearForce, angularForce);
         }
     }