Integration of decomposition and matrix interpolation functions

Babylon/Animations/babylon.animation.js

@@ -82,6 +82,32 @@
             return BABYLON.Color3.Lerp(startValue, endValue, gradient);
         };
 
+        Animation.prototype.matrixInterpolateFunction = function (startValue, endValue, gradient) {
+            var startScale = new BABYLON.Vector3(0, 0, 0);
+            var startRotation = new BABYLON.Quaternion();
+            var startTranslation = new BABYLON.Vector3(0, 0, 0);
+            startValue.decompose(startScale, startRotation, startTranslation);
+
+            var endScale = new BABYLON.Vector3(0, 0, 0);
+            var endRotation = new BABYLON.Quaternion();
+            var endTranslation = new BABYLON.Vector3(0, 0, 0);
+            endValue.decompose(endScale, endRotation, endTranslation);
+
+            var resultScale = this.vector3InterpolateFunction(startScale, endScale, gradient);
+            var resultRotation = this.quaternionInterpolateFunction(startRotation, endRotation, gradient);
+            var resultTranslation = this.vector3InterpolateFunction(startTranslation, endTranslation, gradient);
+
+            var m = BABYLON.Matrix.FromValues(resultScale.x, 0, 0, 0, 0, resultScale.y, 0, 0, 0, 0, resultScale.z, 0, 0, 0, 0, 1);
+
+            var rotationMatrix = BABYLON.Matrix.Identity();
+            resultRotation.toRotationMatrix(rotationMatrix);
+            m = m.multiply(rotationMatrix);
+
+            m.setTranslation(resultTranslation);
+
+            return m;
+        };
+
         Animation.prototype.clone = function () {
             var clone = new Animation(this.name, this.targetPropertyPath.join("."), this.framePerSecond, this.dataType, this.loopMode);
 
@@ -181,6 +207,7 @@
                             switch (loopMode) {
                                 case Animation.ANIMATIONLOOPMODE_CYCLE:
                                 case Animation.ANIMATIONLOOPMODE_CONSTANT:
+                                    return this.matrixInterpolateFunction(startValue, endValue, gradient);
                                 case Animation.ANIMATIONLOOPMODE_RELATIVE:
                                     return startValue;
                             }

Babylon/Animations/babylon.animation.ts

@@ -88,6 +88,35 @@
             return Color3.Lerp(startValue, endValue, gradient);
         }
 
+        public matrixInterpolateFunction(startValue: Matrix, endValue: Matrix, gradient: number): Matrix {
+            var startScale = new Vector3(0, 0, 0);
+            var startRotation = new Quaternion();
+            var startTranslation = new Vector3(0, 0, 0);
+            startValue.decompose(startScale, startRotation, startTranslation);
+
+            var endScale = new Vector3(0, 0, 0);
+            var endRotation = new Quaternion();
+            var endTranslation = new Vector3(0, 0, 0);
+            endValue.decompose(endScale, endRotation, endTranslation);
+
+            var resultScale = this.vector3InterpolateFunction(startScale, endScale, gradient);
+            var resultRotation = this.quaternionInterpolateFunction(startRotation, endRotation, gradient);
+            var resultTranslation = this.vector3InterpolateFunction(startTranslation, endTranslation, gradient);
+
+            var m = Matrix.FromValues(resultScale.x, 0, 0, 0,
+                0, resultScale.y, 0, 0,
+                0, 0, resultScale.z, 0,
+                0, 0, 0, 1);
+
+            var rotationMatrix = Matrix.Identity();
+            resultRotation.toRotationMatrix(rotationMatrix);
+            m = m.multiply(rotationMatrix);
+
+            m.setTranslation(resultTranslation);
+
+            return m;
+        }
+
         public clone(): Animation {
             var clone = new Animation(this.name, this.targetPropertyPath.join("."), this.framePerSecond, this.dataType, this.loopMode);
 
@@ -189,6 +218,7 @@
                             switch (loopMode) {
                                 case Animation.ANIMATIONLOOPMODE_CYCLE:
                                 case Animation.ANIMATIONLOOPMODE_CONSTANT:
+                                    return this.matrixInterpolateFunction(startValue, endValue, gradient);
                                 case Animation.ANIMATIONLOOPMODE_RELATIVE:
                                     return startValue;
                             }

Babylon/Math/babylon.math.js

@@ -1410,6 +1410,10 @@
             return new Quaternion(-q.x, -q.y, -q.z, q.w);
         };
 
+        Quaternion.Identity = function () {
+            return new Quaternion(0, 0, 0, 1);
+        };
+
         Quaternion.RotationAxis = function (axis, angle) {
             var result = new Quaternion();
             var sin = Math.sin(angle / 2);
@@ -1677,6 +1681,36 @@
             return Matrix.FromValues(this.m[0], this.m[1], this.m[2], this.m[3], this.m[4], this.m[5], this.m[6], this.m[7], this.m[8], this.m[9], this.m[10], this.m[11], this.m[12], this.m[13], this.m[14], this.m[15]);
         };
 
+        Matrix.prototype.decompose = function (scale, rotation, translation) {
+            translation.x = this.m[12];
+            translation.y = this.m[13];
+            translation.z = this.m[14];
+
+            var xs = BABYLON.Tools.Sign(this.m[0] * this.m[1] * this.m[2] * this.m[3]) < 0 ? -1 : 1;
+            var ys = BABYLON.Tools.Sign(this.m[4] * this.m[5] * this.m[6] * this.m[7]) < 0 ? -1 : 1;
+            var zs = BABYLON.Tools.Sign(this.m[8] * this.m[9] * this.m[10] * this.m[11]) < 0 ? -1 : 1;
+
+            debugger;
+
+            scale.x = xs * Math.sqrt(this.m[0] * this.m[0] + this.m[1] * this.m[1] + this.m[2] * this.m[2]);
+            scale.y = ys * Math.sqrt(this.m[4] * this.m[4] + this.m[5] * this.m[5] + this.m[6] * this.m[6]);
+            scale.z = zs * Math.sqrt(this.m[8] * this.m[8] + this.m[9] * this.m[9] + this.m[10] * this.m[10]);
+
+            if (scale.x == 0 || scale.y == 0 || scale.z == 0) {
+                rotation.x = 0;
+                rotation.y = 0;
+                rotation.z = 0;
+                rotation.w = 1;
+                return false;
+            }
+
+            var rotationMatrix = BABYLON.Matrix.FromValues(this.m[0] / scale.x, this.m[1] / scale.x, this.m[2] / scale.x, 0, this.m[4] / scale.y, this.m[5] / scale.y, this.m[6] / scale.y, 0, this.m[8] / scale.z, this.m[9] / scale.z, this.m[10] / scale.z, 0, 0, 0, 0, 1);
+
+            rotation.fromRotationMatrix(rotationMatrix);
+
+            return true;
+        };
+
         // Statics
         Matrix.FromArray = function (array, offset) {
             var result = new Matrix();

Babylon/Math/babylon.math.ts

@@ -1371,6 +1371,10 @@
             return new Quaternion(-q.x, -q.y, -q.z, q.w);
         }
 
+        public static Identity(): Quaternion {
+            return new Quaternion(0, 0, 0, 1);
+        }
+
         public static RotationAxis(axis: Vector3, angle: number): Quaternion {
             var result = new Quaternion();
             var sin = Math.sin(angle / 2);
@@ -1653,6 +1657,39 @@
                 this.m[12], this.m[13], this.m[14], this.m[15]);
         }
 
+        public decompose(scale: Vector3, rotation: Quaternion, translation: Vector3) {
+            translation.x = this.m[12];
+            translation.y = this.m[13];
+            translation.z = this.m[14];
+
+            var xs = Tools.Sign(this.m[0] * this.m[1] * this.m[2] * this.m[3]) < 0 ? -1 : 1;
+            var ys = Tools.Sign(this.m[4] * this.m[5] * this.m[6] * this.m[7]) < 0 ? -1 : 1;
+            var zs = Tools.Sign(this.m[8] * this.m[9] * this.m[10] * this.m[11]) < 0 ? -1 : 1;
+
+            debugger;
+
+            scale.x = xs * Math.sqrt(this.m[0] * this.m[0] + this.m[1] * this.m[1] + this.m[2] * this.m[2]);
+            scale.y = ys * Math.sqrt(this.m[4] * this.m[4] + this.m[5] * this.m[5] + this.m[6] * this.m[6]);
+            scale.z = zs * Math.sqrt(this.m[8] * this.m[8] + this.m[9] * this.m[9] + this.m[10] * this.m[10]);
+
+            if (scale.x == 0 || scale.y == 0 || scale.z == 0) {
+                rotation.x = 0;
+                rotation.y = 0;
+                rotation.z = 0;
+                rotation.w = 1;
+                return false;
+            }
+
+            var rotationMatrix = BABYLON.Matrix.FromValues(this.m[0] / scale.x, this.m[1] / scale.x, this.m[2] / scale.x, 0,
+                this.m[4] / scale.y, this.m[5] / scale.y, this.m[6] / scale.y, 0,
+                this.m[8] / scale.z, this.m[9] / scale.z, this.m[10] / scale.z, 0,
+                0, 0, 0, 1);
+
+            rotation.fromRotationMatrix(rotationMatrix);
+
+            return true;
+        }
+
         // Statics
         public static FromArray(array: number[], offset?: number): Matrix {
             var result = new Matrix();
@@ -2612,7 +2649,7 @@
     }
 
     export class PathCursor {
-        private _onchange = new Array <(cursor: PathCursor) => void>();
+        private _onchange = new Array<(cursor: PathCursor) => void>();
 
         value: number = 0;
         animations = new Array<Animation>();
@@ -2620,14 +2657,14 @@
         constructor(private path: Path2) {
         }
 
-        getPoint() : Vector3 {
+        getPoint(): Vector3 {
             var point = this.path.getPointAtLengthPosition(this.value);
             return new Vector3(point.x, 0, point.y);
         }
 
         moveAhead(step: number = 0.002) {
             this.move(step);
-            
+
         }
 
         moveBack(step: number = 0.002) {
@@ -2635,7 +2672,7 @@
         }
 
         move(step: number) {
-            
+
             if (Math.abs(step) > 1) {
                 throw "step size should be less than 1.";
             }
@@ -2648,7 +2685,7 @@
         private ensureLimits() {
             while (this.value > 1) {
                 this.value -= 1;
-            } 
+            }
             while (this.value < 0) {
                 this.value += 1;
             }

Babylon/Tools/babylon.tools.js

@@ -280,6 +280,17 @@
             return Math.min(max, Math.max(min, value));
         };
 
+        // Returns -1 when value is a negative number and
+        // +1 when value is a positive number.
+        Tools.Sign = function (value) {
+            value = +value; // convert to a number
+
+            if (value === 0 || isNaN(value))
+                return value;
+
+            return value > 0 ? 1 : -1;
+        };
+
         Tools.Format = function (value, decimals) {
             if (typeof decimals === "undefined") { decimals = 2; }
             return value.toFixed(decimals);

Babylon/Tools/babylon.tools.ts

@@ -311,6 +311,17 @@
             return Math.min(max, Math.max(min, value));
         }
 
+        // Returns -1 when value is a negative number and
+        // +1 when value is a positive number. 
+        public static Sign(value: number): number {
+            value = +value; // convert to a number
+
+            if (value === 0 || isNaN(value))
+                return value;
+
+            return value > 0 ? 1 : -1
+        }
+
         public static Format(value: number, decimals: number = 2): string {
             return value.toFixed(decimals);
         }