@babylonjs/core 6.6.0 → 6.7.0

package/Physics/v2/Plugins/havokPlugin.js

@@ -19,13 +19,6 @@ class MeshAccumulator {
     constructor(mesh, collectIndices, scene) {
         this._vertices = []; /// Vertices in body space
         this._indices = [];
-        const worldFromRoot = mesh.computeWorldMatrix(true);
-        const rootScale = new Vector3();
-        const rootOrientation = new Quaternion();
-        const rootTranslation = new Vector3();
-        worldFromRoot.decompose(rootScale, rootOrientation, rootTranslation);
-        this._bodyFromWorld = Matrix.Compose(Vector3.One(), mesh.rotationQuaternion ? mesh.rotationQuaternion : Quaternion.Identity(), mesh.position);
-        this._bodyFromWorld = this._bodyFromWorld.invert();
         this._isRightHanded = scene.useRightHandedSystem;
         this._collectIndices = collectIndices;
     }
@@ -44,8 +37,10 @@ class MeshAccumulator {
      */
     addMesh(mesh, includeChildren) {
         const indexOffset = this._vertices.length;
-        const worldFromShape = mesh.computeWorldMatrix(true);
-        const shapeFromBody = worldFromShape.multiply(this._bodyFromWorld);
+        // Force absoluteScaling to be computed
+        mesh.computeWorldMatrix(true);
+        const shapeFromBody = TmpVectors.Matrix[0];
+        Matrix.ScalingToRef(mesh.absoluteScaling.x, mesh.absoluteScaling.y, mesh.absoluteScaling.z, shapeFromBody);
         const vertexData = mesh.getVerticesData(VertexBuffer.PositionKind) || [];
         const numVerts = vertexData.length / 3;
         for (let v = 0; v < numVerts; v++) {
@@ -422,6 +417,7 @@ export class HavokPlugin {
      * physical behavior of the body.
      */
     syncTransform(body, transformNode) {
+        var _a;
         if (body._pluginDataInstances.length) {
             // instances
             const m = transformNode;
@@ -453,23 +449,28 @@ export class HavokPlugin {
                 quat.set(bodyOrientation[0], bodyOrientation[1], bodyOrientation[2], bodyOrientation[3]);
                 const parent = transformNode.parent;
                 // transform position/orientation in parent space
-                if (parent && parent.absoluteRotationQuaternion) {
-                    TmpVectors.Vector3[2].set(bodyTranslation[0], bodyTranslation[1], bodyTranslation[2]);
-                    parent.absoluteRotationQuaternion.conjugateToRef(TmpVectors.Quaternion[1]);
-                    quat.multiplyInPlace(TmpVectors.Quaternion[1]);
-                    TmpVectors.Vector3[2].subtractToRef(parent.absolutePosition, TmpVectors.Vector3[0]);
-                    const localPosition = TmpVectors.Vector3[1];
-                    TmpVectors.Vector3[0].rotateByQuaternionToRef(TmpVectors.Quaternion[1], localPosition);
-                    transformNode.position.set(localPosition.x, localPosition.y, localPosition.z); // use set so position._isDirty is true
+                if (parent && !parent.getWorldMatrix().isIdentity()) {
+                    parent.computeWorldMatrix(true);
+                    quat.normalize();
+                    const finalTransform = TmpVectors.Matrix[0];
+                    const finalTranslation = TmpVectors.Vector3[0];
+                    finalTranslation.copyFromFloats(bodyTranslation[0], bodyTranslation[1], bodyTranslation[2]);
+                    Matrix.ComposeToRef(transformNode.absoluteScaling, quat, finalTranslation, finalTransform);
+                    const parentInverseTransform = TmpVectors.Matrix[1];
+                    parent.getWorldMatrix().invertToRef(parentInverseTransform);
+                    const localTransform = TmpVectors.Matrix[2];
+                    finalTransform.multiplyToRef(parentInverseTransform, localTransform);
+                    localTransform.decomposeToTransformNode(transformNode);
+                    (_a = transformNode.rotationQuaternion) === null || _a === void 0 ? void 0 : _a.normalize();
                 }
                 else {
                     transformNode.position.set(bodyTranslation[0], bodyTranslation[1], bodyTranslation[2]);
-                }
-                if (transformNode.rotationQuaternion) {
-                    transformNode.rotationQuaternion.copyFrom(quat);
-                }
-                else {
-                    quat.toEulerAnglesToRef(transformNode.rotation);
+                    if (transformNode.rotationQuaternion) {
+                        transformNode.rotationQuaternion.copyFrom(quat);
+                    }
+                    else {
+                        quat.toEulerAnglesToRef(transformNode.rotation);
+                    }
                 }
             }
             catch (e) {
@@ -1032,7 +1033,7 @@ export class HavokPlugin {
      */
     _getTransformInfos(node) {
         if (node.parent) {
-            node.computeWorldMatrix();
+            node.computeWorldMatrix(true);
             return [this._bVecToV3(node.absolutePosition), this._bQuatToV4(node.absoluteRotationQuaternion)];
         }
         let orientation = TmpVectors.Quaternion[0];