Refactor collision resolution logic in RapierCollisionWorld

src/runtime-three/rapier-collision-world.ts

@@ -473,6 +473,21 @@ export class RapierCollisionWorld {
       this.characterController.disableAutostep();
     }
 
+    const computeResolvedMovement = (allowAutostep: boolean) => {
+      this.playerCollider.setTranslation(currentCenter);
+
+      if (autostepWasEnabled) {
+        if (allowAutostep) {
+          this.characterController.enableAutostep(
+            this.autostepHeight,
+            this.autostepMinWidth,
+            false
+          );
+        } else {
+          this.characterController.disableAutostep();
+        }
+      }
+
       this.characterController.computeColliderMovement(this.playerCollider, motion);
 
       if (snapToGroundWasEnabled) {
@@ -519,20 +534,42 @@ export class RapierCollisionWorld {
         groundCollisionNormal = toVec3(collision.normal1);
       }
 
-    this.playerCollider.setTranslation(nextCenter);
+      return {
+        correctedMovement,
+        collidedAxes,
+        nextCenter,
+        nextFeetPosition,
+        collisionCount,
+        groundCollisionNormal
+      };
+    };
+
+    const allowAutostep = autostepWasEnabled && supportGrounded;
+    let resolved = computeResolvedMovement(allowAutostep);
+
+    if (
+      allowAutostep &&
+      motion.y <= COLLISION_EPSILON &&
+      resolved.correctedMovement.y > COLLISION_EPSILON &&
+      (resolved.collidedAxes.x || resolved.collidedAxes.z)
+    ) {
+      resolved = computeResolvedMovement(false);
+    }
+
+    this.playerCollider.setTranslation(resolved.nextCenter);
 
     const groundedProbe = this.probePlayerGround(
-      nextFeetPosition,
+      resolved.nextFeetPosition,
       shape,
       this.snapToGroundDistance
     );
 
     return {
-      feetPosition: nextFeetPosition,
+      feetPosition: resolved.nextFeetPosition,
       grounded: groundedProbe.grounded,
-      collisionCount,
-      groundCollisionNormal,
-      collidedAxes
+      collisionCount: resolved.collisionCount,
+      groundCollisionNormal: resolved.groundCollisionNormal,
+      collidedAxes: resolved.collidedAxes
     };
   }