@zephyr3d/scene 0.9.5 → 0.9.7

package/dist/animation/joint_dynamics/controller.js

@@ -1,6 +1,6 @@
 import { EPSILON } from './types.js';
 import { buildConstraints, buildSurfaceFaces } from './constraints.js';
-import { simulate, applyAngleLimits, applyResult } from './solver.js';
+import { simulate, applyAngleLimits, applyResult, extractLocalTwist } from './solver.js';
 import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@zephyr3d/base';
 
 // High-level orchestrator - SPCRJointDynamicsController API
@@ -46,6 +46,7 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
     _colliderHandleToIndex = new Map();
     _flatPlaneHandleToIndex = new Map();
     _grabberHandleToIndex = new Map();
+    _lastOutputLocalRotations = [];
     _baseSystemScale = 1;
     _currentSystemScale = 1;
     _basePointParentLengths = [];
@@ -84,8 +85,13 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
         this._baseConstraintLengths = this._constraints.map((constraint)=>constraint.length);
         // Flatten bone hierarchy to point list, build parent map
         const allPoints = [];
+        const visitedPoints = new Set();
         this._parentMap = new Map(); // child index -> parent index
         const walk = (node)=>{
+            if (visitedPoints.has(node.index)) {
+                return;
+            }
+            visitedPoints.add(node.index);
             allPoints.push(node);
             for (const c of node.children){
                 this._parentMap.set(c.index, node.index);
@@ -95,6 +101,7 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
         for (const r of rootPoints){
             walk(r);
         }
+        allPoints.sort((a, b)=>a.index - b.index);
         this._allPoints = allPoints;
         this._maxPointDepth = allPoints.length > 0 ? Math.max(...allPoints.map((p)=>p.depth)) : 0;
         // Compute boneAxis from transforms: local-space direction to first child
@@ -119,6 +126,7 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
         this._basePointParentLengths = this._pointsR.map((point)=>point.parentLength);
         this._pointsRW = allPoints.map(()=>this._createPointRW());
         this._positionsToTransform = new Array(allPoints.length).fill(Vector3.zero());
+        this._clearOutputTransformCache();
         // Initialize colliders
         this._collidersR = colliders.map((c)=>c.r);
         this._collidersRW = colliders.map((c)=>this._createColliderRW(c.transform));
@@ -180,9 +188,11 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
         }
         const blendRatio = this._computeBlendRatio();
         this._updateScaleDependentParameters();
+        const rootSlideLimit = this._config.rootSlideLimit < 0 ? -1 : this._config.rootSlideLimit * this._currentSystemScale;
         // Capture current transforms
         const rootPos = this._rootTransform.getWorldPosition();
         const rootRot = this._rootTransform.getWorldRotation();
+        const inputLocalRotations = this._getInputLocalRotations();
         for(let i = 0; i < this._pointsRW.length; i++){
             this._pointsRW[i].positionPreviousTransform = this._pointsRW[i].positionCurrentTransform.clone();
             this._pointsRW[i].positionCurrentTransform = this._pointTransforms[i].getWorldPosition();
@@ -206,7 +216,7 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
             subSteps: this._config.subSteps,
             rootPosition: rootPos,
             previousRootPosition: this._previousRootPosition,
-            rootSlideLimit: this._config.rootSlideLimit,
+            rootSlideLimit,
             rootRotation: rootRot,
             previousRootRotation: this._previousRootRotation,
             rootRotateLimit: this._config.rootRotateLimit,
@@ -232,12 +242,14 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
         }
         // Apply results
         const transformRots = this._pointTransforms.map((t)=>t.getWorldRotation());
-        const transformLocalRots = this._pointTransforms.map((t)=>t.getLocalRotation());
-        const outputs = applyResult(this._pointsR, this._pointsRW, this._positionsToTransform, blendRatio, transformRots, transformLocalRots, this._config.preserveTwist);
+        const transformLocalRots = inputLocalRotations;
+        const sceneParentRots = this._pointTransforms.map((t)=>this._getSceneParentWorldRotation(t));
+        const outputs = applyResult(this._pointsR, this._pointsRW, this._positionsToTransform, blendRatio, transformRots, transformLocalRots, this._config.preserveTwist, sceneParentRots);
         for(let i = 0; i < outputs.length; i++){
             this._pointTransforms[i].setWorldPosition(outputs[i].position);
             this._pointTransforms[i].setWorldRotation(outputs[i].rotation);
         }
+        this._lastOutputLocalRotations = this._pointTransforms.map((t)=>t.getLocalRotation());
         this._previousRootPosition = rootPos;
         this._previousRootRotation = rootRot;
     }
@@ -247,8 +259,9 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
    * output back to the transforms.
    */ getResults() {
         const transformRots = this._pointTransforms.map((t)=>t.getWorldRotation());
-        const transformLocalRots = this._pointTransforms.map((t)=>t.getLocalRotation());
-        const outputs = applyResult(this._pointsR, this._pointsRW, this._positionsToTransform, this._config.blendRatio, transformRots, transformLocalRots, this._config.preserveTwist);
+        const transformLocalRots = this._getInputLocalRotations();
+        const sceneParentRots = this._pointTransforms.map((t)=>this._getSceneParentWorldRotation(t));
+        const outputs = applyResult(this._pointsR, this._pointsRW, this._positionsToTransform, this._config.blendRatio, transformRots, transformLocalRots, this._config.preserveTwist, sceneParentRots);
         return outputs.map((o)=>({
                 position: o.position,
                 rotation: o.rotation
@@ -299,6 +312,7 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
    * @param config - Complete controller configuration to apply.
    */ setConfig(config) {
         this._config = this._sanitizeControllerConfig(this._cloneControllerConfig(config));
+        this._clearOutputTransformCache();
         this._refreshCachedSimulationConfig();
     }
     /**
@@ -362,6 +376,7 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
         }
         if (typeof config.preserveTwist === 'boolean') {
             this._config.preserveTwist = config.preserveTwist;
+            this._clearOutputTransformCache();
         }
         if (config.angleLimitConfig) {
             if (typeof config.angleLimitConfig.angleLimit === 'number') {
@@ -438,28 +453,62 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
         }
     }
     /**
-   * Compensates for teleportation by resetting the previous root transform to the current one.
-   * Call this after a character warp or teleport to avoid a large root-motion impulse on the
-   * next simulation step.
+   * Resets all physics state after teleportation.
+   * @deprecated Use {@link JointDynamicsSystemController.reset}. This method is kept as a compatibility alias.
    */ warp() {
-        if (!this._rootTransform) {
-            return;
-        }
-        this._previousRootPosition = this._rootTransform.getWorldPosition();
-        this._previousRootRotation = this._rootTransform.getWorldRotation();
+        this.reset();
     }
     /**
    * Resets all physics state.
-   * Each simulated point is snapped back to the current transform position and grab state
-   * is cleared.
+   * Simulated points, transform history, root-motion history, grab state and wave phase are
+   * snapped back to the current scene transforms. Runtime configuration and fixed/released
+   * point weights are preserved.
    */ reset() {
         for(let i = 0; i < this._pointsRW.length; i++){
             const pos = this._pointTransforms[i].getWorldPosition();
-            this._pointsRW[i].positionCurrent = pos.clone();
-            this._pointsRW[i].positionPrevious = pos.clone();
-            this._pointsRW[i].grabberIndex = -1;
+            const ptRW = this._pointsRW[i];
+            ptRW.positionCurrent = pos.clone();
+            ptRW.positionPrevious = pos.clone();
+            ptRW.positionCurrentTransform = pos.clone();
+            ptRW.positionPreviousTransform = pos.clone();
+            ptRW.positionToTransform = pos.clone();
+            this._positionsToTransform[i] = pos.clone();
+            ptRW.fakeWindDirection = Vector3.axisPZ();
+            ptRW.grabberIndex = -1;
+            ptRW.grabberDistance = 0;
+            ptRW.friction = 0;
+        }
+        for(let i = 0; i < this._pointsRW.length; i++){
+            const parent = this._pointsR[i]?.parent ?? -1;
+            if (parent !== -1) {
+                const direction = Vector3.sub(this._pointsRW[i].positionCurrent, this._pointsRW[parent].positionCurrent);
+                this._pointsRW[i].directionPrevious = direction.magnitudeSq > EPSILON ? direction : Vector3.axisPZ();
+            } else {
+                this._pointsRW[i].directionPrevious = Vector3.axisPZ();
+            }
+        }
+        for(let i = 0; i < this._collidersRW.length; i++){
+            const transform = this._colliderTransforms[i];
+            const pos = transform.getWorldPosition();
+            const rot = transform.getWorldRotation();
+            const scale = transform.getWorldScale();
+            const colRW = this._collidersRW[i];
+            colRW.positionCurrentTransform = pos.clone();
+            colRW.positionPreviousTransform = pos.clone();
+            colRW.directionCurrentTransform = rot.clone();
+            colRW.directionPreviousTransform = rot.clone();
+            colRW.worldScale = scale.clone();
+            colRW.worldToLocal = Matrix4x4.compose(scale, rot, pos).inplaceInvertAffine();
+        }
+        for(let i = 0; i < this._grabbersRW.length; i++){
+            this._grabbersRW[i].position = this._grabberTransforms[i].getWorldPosition();
+        }
+        if (this._rootTransform) {
+            this._previousRootPosition = this._rootTransform.getWorldPosition();
+            this._previousRootRotation = this._rootTransform.getWorldRotation();
         }
         this._fakeWaveCounter = 0;
+        this._clearOutputTransformCache();
     }
     /**
    * Releases a fixed point so it becomes dynamic, for example when cloth is detached.
@@ -862,6 +911,28 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
         const scale = this._rootTransform.getWorldScale();
         return Math.max((Math.abs(scale.x) + Math.abs(scale.y) + Math.abs(scale.z)) / 3, EPSILON);
     }
+    _getSceneParentWorldRotation(transform) {
+        const parent = transform.node?.parent;
+        if (!parent) {
+            return undefined;
+        }
+        const rotation = new Quaternion();
+        parent.worldMatrix.decompose(null, rotation, null);
+        return rotation;
+    }
+    _clearOutputTransformCache() {
+        this._lastOutputLocalRotations = [];
+    }
+    _getInputLocalRotations() {
+        return this._pointTransforms.map((transform, index)=>{
+            const localRotation = transform.getLocalRotation();
+            const lastOutput = this._lastOutputLocalRotations[index];
+            if (lastOutput && Quaternion.angleBetween(localRotation, lastOutput) <= EPSILON && this._pointsR[index]) {
+                return this._pointsR[index].initialLocalRotation.clone();
+            }
+            return localRotation;
+        });
+    }
     _updateScaleDependentParameters(force = false) {
         const systemScale = this._getSystemUniformScale();
         const scaleChanged = Math.abs(systemScale - this._currentSystemScale) > EPSILON;
@@ -878,14 +949,7 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
         }
         this._refreshPointParameters();
         if (scaleChanged) {
-            for(let i = 0; i < this._pointsRW.length; i++){
-                const pos = this._pointTransforms[i].getWorldPosition();
-                this._pointsRW[i].positionCurrent = pos.clone();
-                this._pointsRW[i].positionPrevious = pos.clone();
-                this._pointsRW[i].positionCurrentTransform = pos.clone();
-                this._pointsRW[i].positionPreviousTransform = pos.clone();
-            }
-            this.warp();
+            this.reset();
         }
     }
     _refreshPointParameters() {
@@ -945,11 +1009,7 @@ import { Vector3, Quaternion, Matrix4x4, clamp01, InterpolatorScalar } from '@ze
             boneAxis,
             initialLocalScale: initLocalScale,
             initialLocalRotation: initLocalRot,
-            initialLocalTwist: (()=>{
-                const tw = new Quaternion();
-                initLocalRot.decomposeSwingTwist(boneAxis, undefined, tw);
-                return tw;
-            })(),
+            initialLocalTwist: extractLocalTwist(initLocalRot, boneAxis),
             initialLocalPosition: initLocalPos
         };
         this._applyConfigToPoint(point, node);