@onerjs/core 8.41.5 → 8.41.7

package/Physics/v2/Plugins/havokPlugin.js

@@ -11,6 +11,7 @@ import { InstancedMesh } from "../../../Meshes/instancedMesh.js";
 import { VertexBuffer } from "../../../Buffers/buffer.js";
 import { BuildArray } from "../../../Misc/arrayTools.js";
 import { Observable } from "../../../Misc/observable.js";
+import { FloatingOriginCurrentScene } from "../../../Materials/floatingOriginMatrixOverrides.js";
 class MeshAccumulator {
     /**
      * Constructor of the mesh accumulator
@@ -210,6 +211,137 @@ class TriggerEvent {
  * The Havok Physics plugin
  */
 export class HavokPlugin {
+    /**
+     * Finds an existing world region that contains the given world position,
+     * or creates a new world region centered at that position.
+     *
+     * When floatingOriginMode is enabled, we use multiple Havok worlds to maintain
+     * float32 precision across a large world. Each world region has its own fixed
+     * floating origin, and bodies within that region are simulated relative to it.
+     *
+     * @param worldPosition - The world position of the body being created
+     * @returns The world region to use for this body
+     */
+    _getOrCreateWorldRegion(worldPosition) {
+        // Check if floating origin mode is enabled
+        const scene = FloatingOriginCurrentScene.getScene();
+        if (!scene?.floatingOriginMode) {
+            // When floating origin mode is disabled, use the default world region
+            return this._worldRegions[0];
+        }
+        // Find an existing region that contains this position
+        for (const region of this._worldRegions) {
+            const distance = Vector3.Distance(worldPosition, region.floatingOrigin);
+            if (distance <= this._floatingOriginWorldRadius) {
+                return region;
+            }
+        }
+        // No existing region found - create a new one centered at this position
+        const newWorld = this._hknp.HP_World_Create()[1];
+        // Apply stored gravity to new world
+        this._hknp.HP_World_SetGravity(newWorld, this._currentGravity);
+        // Copy velocity limits from main world
+        this._hknp.HP_World_SetSpeedLimit(newWorld, this.getMaxLinearVelocity(), this.getMaxAngularVelocity());
+        const newRegion = {
+            world: newWorld,
+            floatingOrigin: worldPosition.clone(),
+            gravity: [...this._currentGravity],
+        };
+        this._worldRegions.push(newRegion);
+        return newRegion;
+    }
+    /**
+     * Checks if a body's world position has left its current region and, if so,
+     * moves it to the correct region (existing or newly created).
+     * This preserves linear and angular velocity across the transition.
+     *
+     * @param pluginData - The plugin data for the body (or instance) to check
+     */
+    _reRegionBodyPluginData(pluginData) {
+        const currentRegion = pluginData.worldRegion;
+        if (!currentRegion) {
+            return;
+        }
+        // Read the body's current local transform from Havok
+        const bodyTransform = this._hknp.HP_Body_GetQTransform(pluginData.hpBodyId)[1];
+        const localPos = bodyTransform[0];
+        const orientation = bodyTransform[1];
+        // Compute world position = local position + floating origin
+        const worldPos = TmpVectors.Vector3[2];
+        worldPos.set(localPos[0] + currentRegion.floatingOrigin._x, localPos[1] + currentRegion.floatingOrigin._y, localPos[2] + currentRegion.floatingOrigin._z);
+        // Check if still within the current region.
+        // Use a 20% hysteresis margin so that bodies near the boundary
+        // don't oscillate between regions every frame.
+        const distToCurrent = Vector3.Distance(worldPos, currentRegion.floatingOrigin);
+        if (distToCurrent <= this._floatingOriginWorldRadius * 1.2) {
+            return; // still in region (with hysteresis), nothing to do
+        }
+        // Body has left its region. Use velocity to look ahead and find the best
+        // region to join. This prevents creating throwaway intermediate regions
+        // when a fast body is heading toward an existing region (e.g. a target).
+        const linVel = this._hknp.HP_Body_GetLinearVelocity(pluginData.hpBodyId)[1];
+        const angVel = this._hknp.HP_Body_GetAngularVelocity(pluginData.hpBodyId)[1];
+        // Project position forward by one second of travel to find a suitable existing region.
+        // This is purely for region selection — the body's actual position is not changed.
+        const lookAheadPos = TmpVectors.Vector3[3];
+        lookAheadPos.set(worldPos._x + linVel[0], worldPos._y + linVel[1], worldPos._z + linVel[2]);
+        // Try to find the best region: first check if look-ahead position falls in an existing region
+        let newRegion = this._findExistingRegion(lookAheadPos);
+        if (!newRegion || newRegion === currentRegion) {
+            // Fall back to current position
+            newRegion = this._findExistingRegion(worldPos);
+        }
+        if (!newRegion || newRegion === currentRegion) {
+            // No existing region works — create one at the current position
+            newRegion = this._getOrCreateWorldRegion(worldPos);
+        }
+        if (newRegion === currentRegion) {
+            return;
+        }
+        // Remove from old world
+        this._hknp.HP_World_RemoveBody(currentRegion.world, pluginData.hpBodyId);
+        // Set transform relative to the new region's floating origin
+        const newOffset = newRegion.floatingOrigin;
+        const newLocalPos = [worldPos._x - newOffset._x, worldPos._y - newOffset._y, worldPos._z - newOffset._z];
+        this._hknp.HP_Body_SetQTransform(pluginData.hpBodyId, [newLocalPos, orientation]);
+        // Add to new world
+        this._hknp.HP_World_AddBody(newRegion.world, pluginData.hpBodyId, false);
+        // Restore velocity
+        this._hknp.HP_Body_SetLinearVelocity(pluginData.hpBodyId, linVel);
+        this._hknp.HP_Body_SetAngularVelocity(pluginData.hpBodyId, angVel);
+        // Update the region reference and world transform offset
+        pluginData.worldRegion = newRegion;
+        pluginData.worldTransformOffset = this._hknp.HP_Body_GetWorldTransformOffset(pluginData.hpBodyId)[1];
+        // Garbage-collect the old region if it has no bodies left and it's not the default world
+        if (currentRegion !== this._worldRegions[0]) {
+            const bodyCount = this._hknp.HP_World_GetNumBodies(currentRegion.world)[1];
+            if (bodyCount === 0) {
+                this._hknp.HP_World_Release(currentRegion.world);
+                const idx = this._worldRegions.indexOf(currentRegion);
+                if (idx > 0) {
+                    this._worldRegions.splice(idx, 1);
+                }
+            }
+        }
+    }
+    /**
+     * Searches existing world regions for one that contains the given position.
+     * @param worldPosition - The world position to find a region for
+     * @returns null if no existing region contains it (does NOT create a new one).
+     */
+    _findExistingRegion(worldPosition) {
+        const scene = FloatingOriginCurrentScene.getScene();
+        if (!scene?.floatingOriginMode) {
+            return this._worldRegions[0];
+        }
+        for (const region of this._worldRegions) {
+            const distance = Vector3.Distance(worldPosition, region.floatingOrigin);
+            if (distance <= this._floatingOriginWorldRadius) {
+                return region;
+            }
+        }
+        return null;
+    }
     constructor(_useDeltaForWorldStep = true, hpInjection = HK, parameters = {}) {
         this._useDeltaForWorldStep = _useDeltaForWorldStep;
         /**
@@ -230,6 +362,20 @@ export class HavokPlugin {
         // Map from constraint id to the pair of bodies, where the first is the parent and the second is the child
         this._constraintToBodyIdPair = new Map();
         this._bodyCollisionEndedObservable = new Map();
+        /**
+         * Array of world regions. The first region is always the default world with origin at Vector3.Zero.
+         * Additional regions are created as needed for floating origin mode.
+         */
+        this._worldRegions = [];
+        /**
+         * Stored gravity value to apply to new world regions.
+         */
+        this._currentGravity = [0, -9.81, 0];
+        /**
+         * Radius of each floating origin world region.
+         * Bodies within this radius of a world region's origin will use that world.
+         */
+        this._floatingOriginWorldRadius = 100000;
         /**
          * Observable for collision started and collision continued events
          */
@@ -254,8 +400,15 @@ export class HavokPlugin {
             return;
         }
         this.world = this._hknp.HP_World_Create()[1];
+        // Add the default world as the first region with origin at zero
+        this._worldRegions.push({
+            world: this.world,
+            floatingOrigin: Vector3.Zero(),
+            gravity: [...this._currentGravity],
+        });
         this._queryCollector = this._hknp.HP_QueryCollector_Create(1)[1];
         this.setMaxQueryCollectorHits(parameters.maxQueryCollectorHits ?? 1);
+        this._floatingOriginWorldRadius = parameters.floatingOriginWorldRadius ?? 100000;
     }
     /**
      * If this plugin is supported
@@ -268,10 +421,41 @@ export class HavokPlugin {
      * Sets the gravity of the physics world.
      *
      * @param gravity - The gravity vector to set.
+     * @param worldPosition - Optional world position to specify which region's gravity to set.
+     *                        If provided, only the region containing this position will be updated.
+     *                        If not provided, all regions will be updated (default behavior).
+     *                        This is useful for planetary scenarios where gravity direction varies by location.
+     */
+    setGravity(gravity, worldPosition) {
+        const gravityArray = this._bVecToV3(gravity);
+        if (worldPosition) {
+            // Set gravity for a specific region based on world position
+            const region = this._getOrCreateWorldRegion(worldPosition);
+            region.gravity = gravityArray;
+            this._hknp.HP_World_SetGravity(region.world, gravityArray);
+        }
+        else {
+            // Set gravity for all regions (default behavior)
+            this._currentGravity = gravityArray;
+            for (const region of this._worldRegions) {
+                region.gravity = gravityArray;
+                this._hknp.HP_World_SetGravity(region.world, gravityArray);
+            }
+        }
+    }
+    /**
+     * Gets the gravity of the physics world or a specific region.
      *
+     * @param worldPosition - Optional world position to get the gravity for that region.
+     *                        If not provided, returns the default gravity.
+     * @returns The gravity vector.
      */
-    setGravity(gravity) {
-        this._hknp.HP_World_SetGravity(this.world, this._bVecToV3(gravity));
+    getGravity(worldPosition) {
+        if (worldPosition) {
+            const region = this._getOrCreateWorldRegion(worldPosition);
+            return new Vector3(region.gravity[0], region.gravity[1], region.gravity[2]);
+        }
+        return new Vector3(this._currentGravity[0], this._currentGravity[1], this._currentGravity[2]);
     }
     /**
      * Sets the fixed time step for the physics engine.
@@ -327,6 +511,21 @@ export class HavokPlugin {
      * to accurately simulate the physics bodies in the world.
      */
     executeStep(delta, physicsBodies) {
+        // Re-region bodies that have moved outside their current world region
+        // BEFORE pre-step and stepping, so the body participates in the correct
+        // world's step and its body buffer transform is valid when sync reads it.
+        if (this._worldRegions.length > 1 || FloatingOriginCurrentScene.getScene()?.floatingOriginMode) {
+            for (const physicsBody of physicsBodies) {
+                if (physicsBody._pluginDataInstances.length > 0) {
+                    for (const instance of physicsBody._pluginDataInstances) {
+                        this._reRegionBodyPluginData(instance);
+                    }
+                }
+                else if (physicsBody._pluginData) {
+                    this._reRegionBodyPluginData(physicsBody._pluginData);
+                }
+            }
+        }
         for (const physicsBody of physicsBodies) {
             if (physicsBody.disablePreStep) {
                 continue;
@@ -334,16 +533,21 @@ export class HavokPlugin {
             this.setPhysicsBodyTransformation(physicsBody, physicsBody.transformNode);
         }
         const deltaTime = this._useDeltaForWorldStep ? delta : this._fixedTimeStep;
-        this._hknp.HP_World_SetIdealStepTime(this.world, deltaTime);
-        this._hknp.HP_World_Step(this.world, deltaTime);
-        this._bodyBuffer = this._hknp.HP_World_GetBodyBuffer(this.world)[1];
+        // Step all world regions
+        for (const region of this._worldRegions) {
+            this._hknp.HP_World_SetIdealStepTime(region.world, deltaTime);
+            this._hknp.HP_World_Step(region.world, deltaTime);
+        }
         for (const physicsBody of physicsBodies) {
             if (!physicsBody.disableSync) {
                 this.sync(physicsBody);
             }
         }
-        this._notifyCollisions();
-        this._notifyTriggers();
+        // Notify collisions and triggers for all world regions
+        for (const region of this._worldRegions) {
+            this._notifyCollisions(region.world);
+            this._notifyTriggers(region.world);
+        }
     }
     /**
      * Returns the version of the physics engine plugin.
@@ -361,7 +565,9 @@ export class HavokPlugin {
      * @param maxAngularVelocity maximum allowed angular velocity
      */
     setVelocityLimits(maxLinearVelocity, maxAngularVelocity) {
-        this._hknp.HP_World_SetSpeedLimit(this.world, maxLinearVelocity, maxAngularVelocity);
+        for (const region of this._worldRegions) {
+            this._hknp.HP_World_SetSpeedLimit(region.world, maxLinearVelocity, maxAngularVelocity);
+        }
     }
     /**
      * @returns maximum allowed linear velocity
@@ -391,9 +597,13 @@ export class HavokPlugin {
     initBody(body, motionType, position, orientation) {
         body._pluginData = new BodyPluginData(this._hknp.HP_Body_Create()[1]);
         this._internalSetMotionType(body._pluginData, motionType);
-        const transform = [this._bVecToV3(position), this._bQuatToV4(orientation)]; //<todo temp transform?
+        // Get the world region for this body's position
+        const worldRegion = this._getOrCreateWorldRegion(position);
+        body._pluginData.worldRegion = worldRegion;
+        const offset = worldRegion.floatingOrigin;
+        const transform = [[position._x - offset._x, position._y - offset._y, position._z - offset._z], this._bQuatToV4(orientation)];
         this._hknp.HP_Body_SetQTransform(body._pluginData.hpBodyId, transform);
-        this._hknp.HP_World_AddBody(this.world, body._pluginData.hpBodyId, body.startAsleep);
+        this._hknp.HP_World_AddBody(worldRegion.world, body._pluginData.hpBodyId, body.startAsleep);
         this._bodies.set(body._pluginData.hpBodyId[0], { body: body, index: 0 });
     }
     /**
@@ -405,13 +615,13 @@ export class HavokPlugin {
         if (body._pluginDataInstances && body._pluginDataInstances.length > 0) {
             for (const instance of body._pluginDataInstances) {
                 this._bodyCollisionObservable.delete(instance.hpBodyId[0]);
-                this._hknp.HP_World_RemoveBody(this.world, instance.hpBodyId);
+                this._hknp.HP_World_RemoveBody(instance.worldRegion.world, instance.hpBodyId);
                 this._bodies.delete(instance.hpBodyId[0]);
             }
         }
         if (body._pluginData) {
             this._bodyCollisionObservable.delete(body._pluginData.hpBodyId[0]);
-            this._hknp.HP_World_RemoveBody(this.world, body._pluginData.hpBodyId);
+            this._hknp.HP_World_RemoveBody(body._pluginData.worldRegion.world, body._pluginData.hpBodyId);
             this._bodies.delete(body._pluginData.hpBodyId[0]);
         }
     }
@@ -443,15 +653,29 @@ export class HavokPlugin {
     _createOrUpdateBodyInstances(body, motionType, matrixData, startIndex, endIndex, update) {
         const rotation = TmpVectors.Quaternion[0];
         const rotationMatrix = Matrix.Identity();
+        const worldPos = TmpVectors.Vector3[0];
         for (let i = startIndex; i < endIndex; i++) {
-            const position = [matrixData[i * 16 + 12], matrixData[i * 16 + 13], matrixData[i * 16 + 14]];
+            // Get world position for this instance
+            worldPos.set(matrixData[i * 16 + 12], matrixData[i * 16 + 13], matrixData[i * 16 + 14]);
             let hkbody;
+            let pluginData;
             if (!update) {
                 hkbody = this._hknp.HP_Body_Create()[1];
+                pluginData = new BodyPluginData(hkbody);
+                if (body._pluginDataInstances.length) {
+                    // If an instance already exists, copy any user-provided mass properties
+                    pluginData.userMassProps = body._pluginDataInstances[0].userMassProps;
+                }
             }
             else {
-                hkbody = body._pluginDataInstances[i].hpBodyId;
+                pluginData = body._pluginDataInstances[i];
+                hkbody = pluginData.hpBodyId;
             }
+            // Get the world region for this instance's position
+            const worldRegion = this._getOrCreateWorldRegion(worldPos);
+            const offset = worldRegion.floatingOrigin;
+            // Subtract floating origin offset to get small coordinates for Havok (float32 precision)
+            const position = [worldPos._x - offset._x, worldPos._y - offset._y, worldPos._z - offset._z];
             rotationMatrix.setRowFromFloats(0, matrixData[i * 16 + 0], matrixData[i * 16 + 1], matrixData[i * 16 + 2], 0);
             rotationMatrix.setRowFromFloats(1, matrixData[i * 16 + 4], matrixData[i * 16 + 5], matrixData[i * 16 + 6], 0);
             rotationMatrix.setRowFromFloats(2, matrixData[i * 16 + 8], matrixData[i * 16 + 9], matrixData[i * 16 + 10], 0);
@@ -459,15 +683,12 @@ export class HavokPlugin {
             const transform = [position, [rotation.x, rotation.y, rotation.z, rotation.w]];
             this._hknp.HP_Body_SetQTransform(hkbody, transform);
             if (!update) {
-                const pluginData = new BodyPluginData(hkbody);
-                if (body._pluginDataInstances.length) {
-                    // If an instance already exists, copy any user-provided mass properties
-                    pluginData.userMassProps = body._pluginDataInstances[0].userMassProps;
-                }
                 this._internalSetMotionType(pluginData, motionType);
                 this._internalUpdateMassProperties(pluginData);
                 body._pluginDataInstances.push(pluginData);
-                this._hknp.HP_World_AddBody(this.world, hkbody, body.startAsleep);
+                // Add to the appropriate world
+                pluginData.worldRegion = worldRegion;
+                this._hknp.HP_World_AddBody(worldRegion.world, hkbody, body.startAsleep);
                 pluginData.worldTransformOffset = this._hknp.HP_Body_GetWorldTransformOffset(hkbody)[1];
             }
         }
@@ -504,7 +725,7 @@ export class HavokPlugin {
             for (let i = 0; i < instancesToRemove; i++) {
                 const hkbody = body._pluginDataInstances.pop();
                 this._bodies.delete(hkbody.hpBodyId[0]);
-                this._hknp.HP_World_RemoveBody(this.world, hkbody.hpBodyId);
+                this._hknp.HP_World_RemoveBody(hkbody.worldRegion.world, hkbody.hpBodyId);
                 this._hknp.HP_Body_Release(hkbody.hpBodyId);
             }
             this._createOrUpdateBodyInstances(body, motionType, matrixData, 0, instancesCount, true);
@@ -535,6 +756,8 @@ export class HavokPlugin {
      * physical behavior of the body.
      */
     syncTransform(body, transformNode) {
+        // Get the floating origin offset - this was subtracted when positions were sent to Havok
+        // We need to add it back to get the correct world position
         if (body._pluginDataInstances.length) {
             // instances
             const m = transformNode;
@@ -544,26 +767,41 @@ export class HavokPlugin {
             }
             const instancesCount = body._pluginDataInstances.length;
             for (let i = 0; i < instancesCount; i++) {
-                const bufOffset = body._pluginDataInstances[i].worldTransformOffset;
-                const transformBuffer = new Float32Array(this._hknp.HEAPU8.buffer, this._bodyBuffer + bufOffset, 16);
+                const pluginData = body._pluginDataInstances[i];
+                // Use instance's world region offset
+                const instanceOffset = pluginData.worldRegion.floatingOrigin;
+                // Get body buffer from the instance's own world region (not always the default world)
+                const bodyBuffer = this._hknp.HP_World_GetBodyBuffer(pluginData.worldRegion.world)[1];
+                const bufOffset = pluginData.worldTransformOffset;
+                const transformBuffer = new Float32Array(this._hknp.HEAPU8.buffer, bodyBuffer + bufOffset, 16);
                 const index = i * 16;
                 for (let mi = 0; mi < 15; mi++) {
                     if ((mi & 3) != 3) {
                         matrixData[index + mi] = transformBuffer[mi];
                     }
                 }
+                // Add back the floating origin offset to get world position
+                // (Havok stores position - offset, so we add offset back)
+                matrixData[index + 12] += instanceOffset._x;
+                matrixData[index + 13] += instanceOffset._y;
+                matrixData[index + 14] += instanceOffset._z;
                 matrixData[index + 15] = 1;
             }
             m.thinInstanceBufferUpdated("matrix");
         }
         else {
             try {
-                // regular
                 const bodyTransform = this._hknp.HP_Body_GetQTransform(body._pluginData.hpBodyId)[1];
                 const bodyTranslation = bodyTransform[0];
                 const bodyOrientation = bodyTransform[1];
                 const quat = TmpVectors.Quaternion[0];
+                // Use body's world region offset
+                const offset = body._pluginData.worldRegion.floatingOrigin;
                 quat.set(bodyOrientation[0], bodyOrientation[1], bodyOrientation[2], bodyOrientation[3]);
+                // Add back the floating origin offset to get world position
+                const worldX = bodyTranslation[0] + offset._x;
+                const worldY = bodyTranslation[1] + offset._y;
+                const worldZ = bodyTranslation[2] + offset._z;
                 const parent = transformNode.parent;
                 // transform position/orientation in parent space
                 if (parent && !parent.getWorldMatrix().isIdentity()) {
@@ -573,7 +811,7 @@ export class HavokPlugin {
                     quat.normalize();
                     const finalTransform = TmpVectors.Matrix[0];
                     const finalTranslation = TmpVectors.Vector3[0];
-                    finalTranslation.copyFromFloats(bodyTranslation[0], bodyTranslation[1], bodyTranslation[2]);
+                    finalTranslation.copyFromFloats(worldX, worldY, worldZ);
                     Matrix.ComposeToRef(transformNode.absoluteScaling, quat, finalTranslation, finalTransform);
                     const parentInverseTransform = TmpVectors.Matrix[1];
                     parent.getWorldMatrix().invertToRef(parentInverseTransform);
@@ -585,7 +823,7 @@ export class HavokPlugin {
                     transformNode.scaling.copyFrom(TmpVectors.Vector3[1]);
                 }
                 else {
-                    transformNode.position.set(bodyTranslation[0], bodyTranslation[1], bodyTranslation[2]);
+                    transformNode.position.set(worldX, worldY, worldZ);
                     if (transformNode.rotationQuaternion) {
                         transformNode.rotationQuaternion.copyFrom(quat);
                     }
@@ -938,7 +1176,8 @@ export class HavokPlugin {
      */
     applyImpulse(body, impulse, location, instanceIndex) {
         this._applyToBodyOrInstances(body, (pluginRef) => {
-            this._hknp.HP_Body_ApplyImpulse(pluginRef.hpBodyId, this._bVecToV3(location), this._bVecToV3(impulse));
+            const offset = pluginRef.worldRegion.floatingOrigin;
+            this._hknp.HP_Body_ApplyImpulse(pluginRef.hpBodyId, this._bVecToV3WithOffset(location, offset), this._bVecToV3(impulse));
         }, instanceIndex);
     }
     /**
@@ -1036,8 +1275,45 @@ export class HavokPlugin {
                 this._createOrUpdateBodyInstances(body, body.getMotionType(), matrixData, 0, instancesCount, true);
             }
             else {
-                // regular
-                this._hknp.HP_Body_SetQTransform(body._pluginData.hpBodyId, this._getTransformInfos(node));
+                // Check if the node's new world position requires a region change.
+                // This ensures teleports (e.g. toggling disablePreStep after moving
+                // the transform node) immediately land in the correct region with
+                // correct local coordinates, avoiding a one-frame precision glitch.
+                const pluginData = body._pluginData;
+                if (pluginData.worldRegion && (this._worldRegions.length > 1 || FloatingOriginCurrentScene.getScene()?.floatingOriginMode)) {
+                    // Get world position of the node
+                    const worldPos = TmpVectors.Vector3[3];
+                    if (node.parent) {
+                        node.computeWorldMatrix(true);
+                        worldPos.copyFrom(node.absolutePosition);
+                    }
+                    else {
+                        worldPos.copyFrom(node.position);
+                    }
+                    const currentRegion = pluginData.worldRegion;
+                    const distToCurrent = Vector3.Distance(worldPos, currentRegion.floatingOrigin);
+                    if (distToCurrent > this._floatingOriginWorldRadius * 1.2) {
+                        // Teleporting outside current region - re-region before setting transform
+                        const newRegion = this._getOrCreateWorldRegion(worldPos);
+                        if (newRegion !== currentRegion) {
+                            // Save velocity before removing from old world
+                            const linVel = this._hknp.HP_Body_GetLinearVelocity(pluginData.hpBodyId)[1];
+                            const angVel = this._hknp.HP_Body_GetAngularVelocity(pluginData.hpBodyId)[1];
+                            // Remove from old world, add to new world
+                            this._hknp.HP_World_RemoveBody(currentRegion.world, pluginData.hpBodyId);
+                            this._hknp.HP_World_AddBody(newRegion.world, pluginData.hpBodyId, false);
+                            // Restore velocity
+                            this._hknp.HP_Body_SetLinearVelocity(pluginData.hpBodyId, linVel);
+                            this._hknp.HP_Body_SetAngularVelocity(pluginData.hpBodyId, angVel);
+                            // Update region reference and cached world transform offset
+                            pluginData.worldRegion = newRegion;
+                            pluginData.worldTransformOffset = this._hknp.HP_Body_GetWorldTransformOffset(pluginData.hpBodyId)[1];
+                        }
+                    }
+                }
+                // Set transform using the (possibly updated) region offset
+                const offset = body._pluginData.worldRegion.floatingOrigin;
+                this._hknp.HP_Body_SetQTransform(body._pluginData.hpBodyId, this._getTransformInfos(node, offset));
             }
         }
         else if (body.getPrestepType() == PhysicsPrestepType.ACTION) {
@@ -1059,7 +1335,8 @@ export class HavokPlugin {
      */
     setTargetTransform(body, position, rotation, instanceIndex) {
         this._applyToBodyOrInstances(body, (pluginRef) => {
-            this._hknp.HP_Body_SetTargetQTransform(pluginRef.hpBodyId, [this._bVecToV3(position), this._bQuatToV4(rotation)]);
+            const offset = pluginRef.worldRegion.floatingOrigin;
+            this._hknp.HP_Body_SetTargetQTransform(pluginRef.hpBodyId, [this._bVecToV3WithOffset(position, offset), this._bQuatToV4(rotation)]);
         }, instanceIndex);
     }
     /**
@@ -1341,12 +1618,13 @@ export class HavokPlugin {
      * It first checks if the node has a rotation quaternion, and if not, it creates one from the node's rotation.
      * It then creates an array containing the position and orientation of the node and returns it.
      * @param node - The transform node.
+     * @param offset - The floating origin offset to apply.
      * @returns An array containing the position and orientation of the node.
      */
-    _getTransformInfos(node) {
+    _getTransformInfos(node, offset) {
         if (node.parent) {
             node.computeWorldMatrix(true);
-            return [this._bVecToV3(node.absolutePosition), this._bQuatToV4(node.absoluteRotationQuaternion)];
+            return [this._bVecToV3WithOffset(node.absolutePosition, offset), this._bQuatToV4(node.absoluteRotationQuaternion)];
         }
         let orientation = TmpVectors.Quaternion[0];
         if (node.rotationQuaternion) {
@@ -1356,7 +1634,7 @@ export class HavokPlugin {
             const r = node.rotation;
             Quaternion.FromEulerAnglesToRef(r.x, r.y, r.z, orientation);
         }
-        const transform = [this._bVecToV3(node.position), this._bQuatToV4(orientation)];
+        const transform = [this._bVecToV3WithOffset(node.position, offset), this._bQuatToV4(orientation)];
         return transform;
     }
     /**
@@ -1905,7 +2183,10 @@ export class HavokPlugin {
         const hitPos = hitData[3];
         const hitNormal = hitData[4];
         const hitTriangle = hitData[5];
-        result.setHitData({ x: hitNormal[0], y: hitNormal[1], z: hitNormal[2] }, { x: hitPos[0], y: hitPos[1], z: hitPos[2] }, hitTriangle);
+        // Add floating origin offset back to hit position using the hit body's world region offset
+        // If no hit body (shouldn't happen), use zero offset from default region
+        const offset = hitBody?.body?._pluginData?.worldRegion?.floatingOrigin ?? this._worldRegions[0].floatingOrigin;
+        result.setHitData({ x: hitNormal[0], y: hitNormal[1], z: hitNormal[2] }, { x: hitPos[0] + offset._x, y: hitPos[1] + offset._y, z: hitPos[2] + offset._z }, hitTriangle);
     }
     /**
      * Performs a raycast from a given start point to a given end point and stores the result in a given PhysicsRaycastResult object.
@@ -1929,9 +2210,15 @@ export class HavokPlugin {
         for (const raycastResult of results) {
             raycastResult.reset(from, to);
         }
-        const hkQuery = [this._bVecToV3(from), this._bVecToV3(to), [queryMembership, queryCollideWith], shouldHitTriggers, bodyToIgnore];
+        // Use the ignored body's world region if available, otherwise use default region
+        const worldRegion = query?.ignoreBody?._pluginData?.worldRegion ?? this._worldRegions[0];
+        const offset = worldRegion.floatingOrigin;
+        const world = worldRegion.world;
+        const offsetFrom = this._bVecToV3WithOffset(from, offset);
+        const offsetTo = this._bVecToV3WithOffset(to, offset);
+        const hkQuery = [offsetFrom, offsetTo, [queryMembership, queryCollideWith], shouldHitTriggers, bodyToIgnore];
         const queryCollector = results.length === 1 || !this._multiQueryCollector ? this._queryCollector : this._multiQueryCollector;
-        this._hknp.HP_World_CastRayWithCollector(this.world, queryCollector, hkQuery);
+        this._hknp.HP_World_CastRayWithCollector(world, queryCollector, hkQuery);
         const numHits = this._hknp.HP_QueryCollector_GetNumHits(queryCollector)[1];
         if (numHits <= 0) {
             return;
@@ -1948,7 +2235,8 @@ export class HavokPlugin {
         for (let i = 0; i < numHits; i++) {
             const [, hitData] = this._hknp.HP_QueryCollector_GetCastRayResult(queryCollector, i)[1];
             const hitPos = hitData[3];
-            from.subtractFromFloatsToRef(hitPos[0], hitPos[1], hitPos[2], this._tmpVec3[0]);
+            // Use offsetFrom for distance calculation since hitPos is in Havok's offset space
+            this._tmpVec3[0].set(offsetFrom[0] - hitPos[0], offsetFrom[1] - hitPos[1], offsetFrom[2] - hitPos[2]);
             const distance = this._tmpVec3[0].lengthSquared();
             hitDatas[i] = {
                 hitData,
@@ -1974,8 +2262,12 @@ export class HavokPlugin {
         const queryCollideWith = query?.collisionFilter?.collideWith ?? ~0;
         result.reset();
         const bodyToIgnore = query.ignoreBody ? [BigInt(query.ignoreBody._pluginData.hpBodyId[0])] : [BigInt(0)];
-        const hkQuery = [this._bVecToV3(query.position), query.maxDistance, [queryMembership, queryCollideWith], query.shouldHitTriggers, bodyToIgnore];
-        this._hknp.HP_World_PointProximityWithCollector(this.world, this._queryCollector, hkQuery);
+        // Use the ignored body's world region if available, otherwise use default region
+        const worldRegion = query.ignoreBody?._pluginData?.worldRegion ?? this._worldRegions[0];
+        const offset = worldRegion.floatingOrigin;
+        const world = worldRegion.world;
+        const hkQuery = [this._bVecToV3WithOffset(query.position, offset), query.maxDistance, [queryMembership, queryCollideWith], query.shouldHitTriggers, bodyToIgnore];
+        this._hknp.HP_World_PointProximityWithCollector(world, this._queryCollector, hkQuery);
         if (this._hknp.HP_QueryCollector_GetNumHits(this._queryCollector)[1] > 0) {
             const [distance, hitData] = this._hknp.HP_QueryCollector_GetPointProximityResult(this._queryCollector, 0)[1];
             this._populateHitData(hitData, result);
@@ -1993,8 +2285,12 @@ export class HavokPlugin {
         hitShapeResult.reset();
         const shapeId = query.shape._pluginData;
         const bodyToIgnore = query.ignoreBody ? [BigInt(query.ignoreBody._pluginData.hpBodyId[0])] : [BigInt(0)];
-        const hkQuery = [shapeId, this._bVecToV3(query.position), this._bQuatToV4(query.rotation), query.maxDistance, query.shouldHitTriggers, bodyToIgnore];
-        this._hknp.HP_World_ShapeProximityWithCollector(this.world, this._queryCollector, hkQuery);
+        // Use the ignored body's world region if available, otherwise use default region
+        const worldRegion = query.ignoreBody?._pluginData?.worldRegion ?? this._worldRegions[0];
+        const offset = worldRegion.floatingOrigin;
+        const world = worldRegion.world;
+        const hkQuery = [shapeId, this._bVecToV3WithOffset(query.position, offset), this._bQuatToV4(query.rotation), query.maxDistance, query.shouldHitTriggers, bodyToIgnore];
+        this._hknp.HP_World_ShapeProximityWithCollector(world, this._queryCollector, hkQuery);
         if (this._hknp.HP_QueryCollector_GetNumHits(this._queryCollector)[1] > 0) {
             const [distance, hitInputData, hitShapeData] = this._hknp.HP_QueryCollector_GetShapeProximityResult(this._queryCollector, 0)[1];
             this._populateHitData(hitInputData, inputShapeResult);
@@ -2014,8 +2310,19 @@ export class HavokPlugin {
         hitShapeResult.reset();
         const shapeId = query.shape._pluginData;
         const bodyToIgnore = query.ignoreBody ? [BigInt(query.ignoreBody._pluginData.hpBodyId[0])] : [BigInt(0)];
-        const hkQuery = [shapeId, this._bQuatToV4(query.rotation), this._bVecToV3(query.startPosition), this._bVecToV3(query.endPosition), query.shouldHitTriggers, bodyToIgnore];
-        this._hknp.HP_World_ShapeCastWithCollector(this.world, this._queryCollector, hkQuery);
+        // Use the ignored body's world region if available, otherwise use default region
+        const worldRegion = query.ignoreBody?._pluginData?.worldRegion ?? this._worldRegions[0];
+        const offset = worldRegion.floatingOrigin;
+        const world = worldRegion.world;
+        const hkQuery = [
+            shapeId,
+            this._bQuatToV4(query.rotation),
+            this._bVecToV3WithOffset(query.startPosition, offset),
+            this._bVecToV3WithOffset(query.endPosition, offset),
+            query.shouldHitTriggers,
+            bodyToIgnore,
+        ];
+        this._hknp.HP_World_ShapeCastWithCollector(world, this._queryCollector, hkQuery);
         if (this._hknp.HP_QueryCollector_GetNumHits(this._queryCollector)[1] > 0) {
             const [fractionAlongRay, hitInputData, hitShapeData] = this._hknp.HP_QueryCollector_GetShapeCastResult(this._queryCollector, 0)[1];
             this._populateHitData(hitInputData, inputShapeResult);
@@ -2093,8 +2400,9 @@ export class HavokPlugin {
             this._hknp.HP_Body_SetEventMask(body._pluginData.hpBodyId, currentCollideEvents);
         }
     }
-    _notifyTriggers() {
-        let eventAddress = this._hknp.HP_World_GetTriggerEvents(this.world)[1];
+    _notifyTriggers(world) {
+        const targetWorld = world ?? this.world;
+        let eventAddress = this._hknp.HP_World_GetTriggerEvents(targetWorld)[1];
         const event = new TriggerEvent();
         while (eventAddress) {
             TriggerEvent.readToRef(this._hknp.HEAPU8.buffer, eventAddress, event);
@@ -2111,20 +2419,30 @@ export class HavokPlugin {
                 };
                 this.onTriggerCollisionObservable.notifyObservers(triggerCollisionInfo);
             }
-            eventAddress = this._hknp.HP_World_GetNextTriggerEvent(this.world, eventAddress);
+            eventAddress = this._hknp.HP_World_GetNextTriggerEvent(targetWorld, eventAddress);
         }
     }
     /**
      * Runs thru all detected collisions and filter by body
+     * @param world optional world to check collisions for (defaults to main world)
      */
-    _notifyCollisions() {
-        let eventAddress = this._hknp.HP_World_GetCollisionEvents(this.world)[1];
+    _notifyCollisions(world) {
+        const targetWorld = world ?? this.world;
+        let eventAddress = this._hknp.HP_World_GetCollisionEvents(targetWorld)[1];
         const event = new CollisionEvent();
-        const worldAddr = Number(this.world);
+        const worldAddr = Number(targetWorld);
+        // Find the region for this world to get the correct floating origin offset.
+        // All bodies in a collision share the same world, so use one offset for both.
+        const region = this._worldRegions.find((r) => Number(r.world) === worldAddr) ?? this._worldRegions[0];
+        const regionOffset = region.floatingOrigin;
         while (eventAddress) {
             CollisionEvent.readToRef(this._hknp.HEAPU8.buffer, eventAddress, event);
             const bodyInfoA = this._bodies.get(event.contactOnA.bodyId);
             const bodyInfoB = this._bodies.get(event.contactOnB.bodyId);
+            // Add floating origin offset back to collision contact positions.
+            // Both contacts are in the same world, so use the region's floating origin.
+            event.contactOnA.position.addInPlace(regionOffset);
+            event.contactOnB.position.addInPlace(regionOffset);
             // Bodies may have been disposed between events. Check both still exist.
             if (bodyInfoA && bodyInfoB) {
                 const collisionInfo = {
@@ -2212,12 +2530,16 @@ export class HavokPlugin {
         }
         if (this._multiQueryCollector) {
             this._hknp.HP_QueryCollector_Release(this._multiQueryCollector);
-            this._multiQueryCollector;
+            this._multiQueryCollector = undefined;
         }
-        if (this.world) {
-            this._hknp.HP_World_Release(this.world);
-            this.world = undefined;
+        // Dispose all world regions (includes the default world)
+        for (const region of this._worldRegions) {
+            if (region.world) {
+                this._hknp.HP_World_Release(region.world);
+            }
         }
+        this._worldRegions.length = 0;
+        this.world = undefined;
     }
     _v3ToBvecRef(v, vec3) {
         vec3.set(v[0], v[1], v[2]);
@@ -2225,6 +2547,19 @@ export class HavokPlugin {
     _bVecToV3(v) {
         return [v._x, v._y, v._z];
     }
+    /**
+     * Converts a Vector3 to Havok format with floating origin offset subtracted.
+     * Use this for world-space positions being sent to Havok.
+     * @param v - The vector to convert
+     * @param offset - Optional offset to use. If not provided, no offset is applied.
+     * @returns The converted vector
+     */
+    _bVecToV3WithOffset(v, offset) {
+        if (offset) {
+            return [v._x - offset._x, v._y - offset._y, v._z - offset._z];
+        }
+        return [v._x, v._y, v._z];
+    }
     _bQuatToV4(q) {
         return [q._x, q._y, q._z, q._w];
     }