Merge pull request #1300 from actnwit/test/transform

fix: transform
actnwit · Dec 13, 2023 · f52177b · f52177b
2 parents 0f7edc5 + f2f339f
commit f52177b
Show file tree

Hide file tree

Showing 10 changed files with 352 additions and 89 deletions.
diff --git a/src/foundation/components/Skeletal/SkeletalComponent.ts b/src/foundation/components/Skeletal/SkeletalComponent.ts
@@ -236,9 +236,9 @@ export class SkeletalComponent extends Component {
 
       if (Config.boneDataType !== BoneDataType.Mat43x1) {
         const scaleVec = SkeletalComponent.__tmpVec3_0.setComponents(
-          Math.hypot(m.m00, m.m01, m.m02),
-          Math.hypot(m.m10, m.m11, m.m12),
-          Math.hypot(m.m20, m.m21, m.m22)
+          Math.hypot(m._v[0], m._v[1], m._v[2]),
+          Math.hypot(m._v[4], m._v[5], m._v[6]),
+          Math.hypot(m._v[8], m._v[9], m._v[10])
         );
 
         m.m00 /= scaleVec.x;

diff --git a/src/foundation/math/Matrix33.ts b/src/foundation/math/Matrix33.ts
@@ -447,16 +447,16 @@ export class Matrix33 extends AbstractMatrix implements IMatrix, IMatrix33 {
 
   getScale() {
     return Vector3.fromCopyArray([
-      Math.hypot(this._v[0], this._v[3], this._v[6]),
-      Math.hypot(this._v[1], this._v[4], this._v[7]),
-      Math.hypot(this._v[2], this._v[5], this._v[8]),
+      Math.hypot(this._v[0], this._v[1], this._v[2]),
+      Math.hypot(this._v[3], this._v[4], this._v[5]),
+      Math.hypot(this._v[6], this._v[7], this._v[8]),
     ]);
   }
 
   getScaleTo(outVec: MutableVector3) {
-    outVec._v[0] = Math.hypot(this._v[0], this._v[3], this._v[6]);
-    outVec._v[1] = Math.hypot(this._v[1], this._v[4], this._v[7]);
-    outVec._v[2] = Math.hypot(this._v[2], this._v[5], this._v[8]);
+    outVec._v[0] = Math.hypot(this._v[0], this._v[1], this._v[2]);
+    outVec._v[1] = Math.hypot(this._v[3], this._v[4], this._v[5]);
+    outVec._v[2] = Math.hypot(this._v[6], this._v[7], this._v[8]);
     return outVec;
   }
 

diff --git a/src/foundation/math/Matrix44.test.ts b/src/foundation/math/Matrix44.test.ts
@@ -1,6 +1,8 @@
+import { MathUtil } from './MathUtil';
 import { Matrix44 } from './Matrix44';
 import { MutableMatrix44 } from './MutableMatrix44';
 import { Vector3 } from './Vector3';
+import { Vector4 } from './Vector4';
 
 test('Test isEqual', () => {
   const a = Matrix44.fromCopy16RowMajor(1, 0, 0, 1, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
@@ -36,3 +38,137 @@ test('Make Matrix44 from MutableMatrix44 (4)', () => {
 
   expect(b.m03).toBe(0);
 });
+
+// prettier-ignore
+test("Matrix44 multiply", () => {
+  const a = Matrix44.fromCopy16RowMajor(
+    1, 2, 3, 4,
+    5, 6, 7, 8,
+    9, 10, 11, 12,
+    13, 14 ,15, 16
+  );
+  const b = Matrix44.fromCopy16RowMajor(
+    17, 18, 19, 20,
+    21, 22, 23, 24,
+    25, 26, 27, 28,
+    29, 30, 31, 32
+  );
+  const c = Matrix44.fromCopy16RowMajor(
+    250, 260, 270, 280,
+    618, 644, 670, 696,
+    986, 1028, 1070, 1112,
+    1354, 1412, 1470, 1528
+  );
+
+  expect((Matrix44.multiply(a, b) as Matrix44).isEqual(c)).toBe(true);
+});
+
+// prettier-ignore
+test('Matrix44 multiplyVector', () => {
+  const a = Matrix44.fromCopy16RowMajor(
+    1, 2, 3, 4,
+    5, 6, 7, 8,
+    9 ,10 ,11 ,12,
+    13, 14, 15, 16
+  );
+
+  const b = Vector4.fromCopy4(1, 2, 3, 4);
+
+  const c = Vector4.fromCopy4(30, 70, 110, 150);
+
+  expect(a.multiplyVector(b).isEqual(c)).toBe(true);
+});
+
+// prettier-ignore
+test('Matrix44 translate', () => {
+  const a = Matrix44.translate(Vector3.fromCopy3(1, 2, 3));
+  const b = Matrix44.fromCopy16RowMajor(
+    1, 0, 0, 1,
+    0, 1, 0, 2,
+    0, 0, 1, 3,
+    0, 0, 0, 1
+  );
+
+  expect(a.isEqual(b)).toBe(true);
+});
+
+// prettier-ignore
+test('Matrix44 rotateX', () => {
+  const a = Matrix44.rotateX(MathUtil.degreeToRadian(90));
+
+  const b = Matrix44.fromCopy16RowMajor(
+    1, 0, 0, 0,
+    0, 0, -1, 0,
+    0, 1, 0, 0,
+    0, 0, 0, 1
+  );
+
+  expect(a.isEqual(b)).toBe(true);
+});
+
+// prettier-ignore
+test('Matrix44 rotateY', () => {
+  const a = Matrix44.rotateY(MathUtil.degreeToRadian(90));
+
+  const b = Matrix44.fromCopy16RowMajor(
+    0, 0, 1, 0,
+    0, 1, 0, 0,
+    -1, 0, 0, 0,
+    0, 0, 0, 1
+  );
+
+  expect(a.isEqual(b)).toBe(true);
+});
+
+// prettier-ignore
+test('Matrix44 rotateZ', () => {
+  const a = Matrix44.rotateZ(MathUtil.degreeToRadian(90));
+
+  const b = Matrix44.fromCopy16RowMajor(
+    0, -1, 0, 0,
+    1, 0, 0, 0,
+    0, 0, 1, 0,
+    0 ,0 ,0 ,1
+  );
+
+  expect(a.isEqual(b)).toBe(true);
+});
+
+// prettier-ignore
+test('Matrix44 rotateXYZ', () => {
+  const a = Matrix44.rotateXYZ(
+    MathUtil.degreeToRadian(90),
+    MathUtil.degreeToRadian(90),
+    MathUtil.degreeToRadian(90)
+  );
+
+  const x = Matrix44.rotateX(MathUtil.degreeToRadian(90));
+  const y = Matrix44.rotateY(MathUtil.degreeToRadian(90));
+  const z = Matrix44.rotateZ(MathUtil.degreeToRadian(90));
+
+  const b = Matrix44.multiply(Matrix44.multiply(z, y), x);
+
+  const c = Matrix44.fromCopy16RowMajor(
+    0, 0, 1, 0,
+    0, 1, 0, 0,
+    -1, 0, 0, 0,
+    0 ,0 ,0 ,1
+  );
+
+  expect(a.isEqual(b)).toBe(true);
+  expect(a.isEqual(c)).toBe(true);
+});
+
+// prettier-ignore
+test('Matrix44 scale', () => {
+  const a = Matrix44.scale(Vector3.fromCopy3(1, 2, 3));
+
+  const b = Matrix44.fromCopy16RowMajor(
+    1, 0, 0, 0,
+    0, 2, 0, 0,
+    0, 0, 3, 0,
+    0 ,0 ,0 ,1
+  );
+
+  expect(a.isEqual(b)).toBe(true);
+});
diff --git a/src/foundation/math/Matrix44.ts b/src/foundation/math/Matrix44.ts
@@ -894,19 +894,19 @@ export class Matrix44 extends AbstractMatrix implements IMatrix, IMatrix44 {
 
   getScale() {
     return Vector3.fromCopyArray([
-      Math.hypot(this._v[0], this._v[4], this._v[8]),
-      Math.hypot(this._v[1], this._v[5], this._v[9]),
-      Math.hypot(this._v[2], this._v[6], this._v[10]),
+      Math.hypot(this._v[0], this._v[1], this._v[2]),
+      Math.hypot(this._v[4], this._v[5], this._v[6]),
+      Math.hypot(this._v[8], this._v[9], this._v[10]),
     ]);
   }
 
   /**
    * get scale vector from this matrix
    */
   getScaleTo(outVec: MutableVector3) {
-    outVec._v[0] = Math.hypot(this._v[0], this._v[4], this._v[8]);
-    outVec._v[1] = Math.hypot(this._v[1], this._v[5], this._v[9]);
-    outVec._v[2] = Math.hypot(this._v[2], this._v[6], this._v[10]);
+    outVec._v[0] = Math.hypot(this._v[0], this._v[1], this._v[2]);
+    outVec._v[1] = Math.hypot(this._v[4], this._v[5], this._v[6]);
+    outVec._v[2] = Math.hypot(this._v[8], this._v[9], this._v[10]);
     return outVec;
   }
 
@@ -974,9 +974,9 @@ export class Matrix44 extends AbstractMatrix implements IMatrix, IMatrix44 {
   getRotate() {
     // const quat = Quaternion.fromMatrix(this);
     // const rotateMat = (this.constructor as any).fromCopyQuaternion(quat) as Matrix44;
-    const scaleX = Math.hypot(this._v[0], this._v[4], this._v[8]);
-    const scaleY = Math.hypot(this._v[1], this._v[5], this._v[9]);
-    const scaleZ = Math.hypot(this._v[2], this._v[6], this._v[10]);
+    const scaleX = Math.hypot(this._v[0], this._v[1], this._v[2]);
+    const scaleY = Math.hypot(this._v[4], this._v[5], this._v[6]);
+    const scaleZ = Math.hypot(this._v[8], this._v[9], this._v[10]);
 
     const mat = Matrix44.fromCopy16RowMajor(
       this._v[0] / scaleX,

diff --git a/src/foundation/math/MutableMatrix44.ts b/src/foundation/math/MutableMatrix44.ts
@@ -982,7 +982,7 @@ export class MutableMatrix44 extends Matrix44 implements IMutableMatrix, IMutabl
     return new MutableMatrix44(v);
   }
 
-  static fromCopyMatrix44(mat: Matrix44) {
+  static fromCopyMatrix44(mat: IMatrix44) {
     const v = new Float32Array(16);
     v.set(mat._v);
     return new MutableMatrix44(v);

diff --git a/src/foundation/math/MutableQuaternion.ts b/src/foundation/math/MutableQuaternion.ts
@@ -3,6 +3,7 @@ import { IVector3, IVector4 } from './IVector';
 import { Array4, TypedArray } from '../../types/CommonTypes';
 import { IMutableQuaternion, ILogQuaternion, IQuaternion } from './IQuaternion';
 import { IMatrix44 } from './IMatrix';
+import { MutableMatrix44 } from './MutableMatrix44';
 
 export class MutableQuaternion extends Quaternion implements IMutableQuaternion {
   constructor(x: Float32Array) {
@@ -208,34 +209,60 @@ export class MutableQuaternion extends Quaternion implements IMutableQuaternion
   }
 
   fromMatrix(mat: IMatrix44) {
-    const tr = mat.m00 + mat.m11 + mat.m22;
+    let sx = Math.hypot(mat.m00, mat.m10, mat.m20);
+    const sy = Math.hypot(mat.m01, mat.m11, mat.m21);
+    const sz = Math.hypot(mat.m02, mat.m12, mat.m22);
 
-    if (tr > 0) {
-      const S = 0.5 / Math.sqrt(tr + 1.0);
-      this._v[0] = (mat.m21 - mat.m12) * S;
-      this._v[1] = (mat.m02 - mat.m20) * S;
-      this._v[2] = (mat.m10 - mat.m01) * S;
+    const det = mat.determinant();
+    if (det < 0) {
+      sx = -sx;
+    }
+
+    const m = MutableMatrix44.fromCopyMatrix44(mat);
+
+    const invSx = 1 / sx;
+    const invSy = 1 / sy;
+    const invSz = 1 / sz;
+
+    m.m00 *= invSx;
+    m.m10 *= invSx;
+    m.m20 *= invSx;
+
+    m.m01 *= invSy;
+    m.m11 *= invSy;
+    m.m21 *= invSy;
+
+    m.m02 *= invSz;
+    m.m12 *= invSz;
+    m.m22 *= invSz;
+
+    const trace = m.m00 + m.m11 + m.m22;
+
+    if (trace > 0) {
+      const S = 0.5 / Math.sqrt(trace + 1.0);
+      this._v[0] = (m.m21 - m.m12) * S;
+      this._v[1] = (m.m02 - m.m20) * S;
+      this._v[2] = (m.m10 - m.m01) * S;
       this._v[3] = 0.25 / S;
-    } else if (mat.m00 > mat.m11 && mat.m00 > mat.m22) {
-      const S = Math.sqrt(1.0 + mat.m00 - mat.m11 - mat.m22) * 2;
+    } else if (m.m00 > m.m11 && m.m00 > m.m22) {
+      const S = Math.sqrt(1.0 + m.m00 - m.m11 - m.m22) * 2;
       this._v[0] = 0.25 * S;
-      this._v[1] = (mat.m01 + mat.m10) / S;
-      this._v[2] = (mat.m02 + mat.m20) / S;
-      this._v[3] = (mat.m21 - mat.m12) / S;
-    } else if (mat.m11 > mat.m22) {
-      const S = Math.sqrt(1.0 + mat.m11 - mat.m00 - mat.m22) * 2;
-      this._v[0] = (mat.m01 + mat.m10) / S;
+      this._v[1] = (m.m01 + m.m10) / S;
+      this._v[2] = (m.m02 + m.m20) / S;
+      this._v[3] = (m.m21 - m.m12) / S;
+    } else if (m.m11 > m.m22) {
+      const S = Math.sqrt(1.0 + m.m11 - m.m00 - m.m22) * 2;
+      this._v[0] = (m.m01 + m.m10) / S;
       this._v[1] = 0.25 * S;
-      this._v[2] = (mat.m12 + mat.m21) / S;
-      this._v[3] = (mat.m02 - mat.m20) / S;
+      this._v[2] = (m.m12 + m.m21) / S;
+      this._v[3] = (m.m02 - m.m20) / S;
     } else {
-      const S = Math.sqrt(1.0 + mat.m22 - mat.m00 - mat.m11) * 2;
-      this._v[0] = (mat.m02 + mat.m20) / S;
-      this._v[1] = (mat.m12 + mat.m21) / S;
+      const S = Math.sqrt(1.0 + m.m22 - m.m00 - m.m11) * 2;
+      this._v[0] = (m.m02 + m.m20) / S;
+      this._v[1] = (m.m12 + m.m21) / S;
       this._v[2] = 0.25 * S;
-      this._v[3] = (mat.m10 - mat.m01) / S;
+      this._v[3] = (m.m10 - m.m01) / S;
     }
-
     return this;
   }
 

diff --git a/src/foundation/math/Quaternion.test.ts b/src/foundation/math/Quaternion.test.ts
@@ -84,3 +84,12 @@ test('Inverse Transform vector with quaternion', () => {
   console.log(result_vec);
   expect(result_vec.isEqual(Vector3.fromCopy3(-1, 0, 0), 0.0001)).toBe(true);
 });
+
+test('toEulerAngles', () => {
+  const q = Quaternion.fromCopy4(0.5, 0.5, 0.5, 0.5);
+  const e = q.toEulerAngles();
+
+  console.log(e);
+
+  expect(e.isEqual(Vector3.fromCopy3(Math.PI / 2, 0, Math.PI / 2), 0.0001)).toBe(true);
+});