fix: mathematical precision

packages/core/src/Transform.ts

@@ -508,7 +508,7 @@ export class Transform extends Component {
     const zAxis = Transform._tempVec30;
     Vector3.subtract(this.worldPosition, targetPosition, zAxis);
     let axisLen = zAxis.length();
-    if (axisLen <= MathUtil.zeroTolerance) {
+    if (axisLen < MathUtil.epsilon) {
       // The current position and the target position are almost the same.
       return;
     }
@@ -520,7 +520,7 @@ export class Transform extends Component {
       xAxis.set(zAxis.z, 0, -zAxis.x);
     }
     axisLen = xAxis.length();
-    if (axisLen <= MathUtil.zeroTolerance) {
+    if (axisLen < MathUtil.epsilon) {
       // @todo:
       // 1.worldUp is（0,0,0）
       // 2.worldUp is parallel to zAxis

packages/math/src/CollisionUtil.ts

@@ -113,26 +113,23 @@ export class CollisionUtil {
    */
   static intersectsRayAndPlane(ray: Ray, plane: Plane): number {
     const { normal } = plane;
-    const { zeroTolerance } = MathUtil;
+    const { epsilon } = MathUtil;
 
     const dir = Vector3.dot(normal, ray.direction);
     // Parallel
-    if (Math.abs(dir) < zeroTolerance) {
+    if (Math.abs(dir) < epsilon) {
       return -1;
     }
 
     const position = Vector3.dot(normal, ray.origin);
     let distance = (-plane.distance - position) / dir;
-
-    if (distance < 0) {
-      if (distance < -zeroTolerance) {
-        return -1;
-      }
-
-      distance = 0;
+    if (distance >= 0) {
+      return distance;
+    } else if (distance > -epsilon) {
+      return 0;
+    } else {
+      return -1;
     }
-
-    return distance;
   }
 
   /**
@@ -142,7 +139,7 @@ export class CollisionUtil {
    * @returns The distance from ray to box if intersecting, -1 otherwise
    */
   static intersectsRayAndBox(ray: Ray, box: BoundingBox): number {
-    const { zeroTolerance } = MathUtil;
+    const { epsilon } = MathUtil;
     const { origin, direction } = ray;
     const { min, max } = box;
     const dirX = direction.x;
@@ -154,7 +151,7 @@ export class CollisionUtil {
     let distance = 0;
     let tmax = Number.MAX_VALUE;
 
-    if (Math.abs(dirX) < zeroTolerance) {
+    if (Math.abs(dirX) < epsilon) {
       if (oriX < min.x || oriX > max.x) {
         return -1;
       }
@@ -177,7 +174,7 @@ export class CollisionUtil {
       }
     }
 
-    if (Math.abs(dirY) < zeroTolerance) {
+    if (Math.abs(dirY) < epsilon) {
       if (oriY < min.y || oriY > max.y) {
         return -1;
       }
@@ -200,7 +197,7 @@ export class CollisionUtil {
       }
     }
 
-    if (Math.abs(dirZ) < zeroTolerance) {
+    if (Math.abs(dirZ) < epsilon) {
       if (oriZ < min.z || oriZ > max.z) {
         return -1;
       }

packages/math/src/MathUtil.ts

@@ -2,6 +2,8 @@
  * Common utility methods for math operations.
  */
 export class MathUtil {
+  /** Ths critical value as denominator. */
+  static readonly epsilon: number = 1e-15;
   /** The value for which all absolute numbers smaller than are considered equal to zero. */
   static readonly zeroTolerance: number = 1e-6;
   /** The conversion factor that radian to degree. */
@@ -28,7 +30,7 @@ export class MathUtil {
    * @returns True if a almost equal to b, false otherwise
    */
   static equals(a: number, b: number): boolean {
-    return Math.abs(a - b) <= MathUtil.zeroTolerance;
+    return Math.abs(a - b) < MathUtil.zeroTolerance;
   }
 
   /**

packages/math/src/Matrix.ts

@@ -219,7 +219,7 @@ export class Matrix implements IClone<Matrix>, ICopy<Matrix, Matrix> {
     let len = Math.sqrt(x * x + y * y + z * z);
     let s, c, t;
 
-    if (Math.abs(len) < MathUtil.zeroTolerance) {
+    if (Math.abs(len) < MathUtil.epsilon) {
       return;
     }
 
@@ -561,7 +561,7 @@ export class Matrix implements IClone<Matrix>, ICopy<Matrix, Matrix> {
     let { _x: x, _y: y, _z: z } = axis;
     let len = Math.sqrt(x * x + y * y + z * z);
 
-    if (Math.abs(len) < MathUtil.zeroTolerance) {
+    if (Math.abs(len) < MathUtil.epsilon) {
       return;
     }
 
@@ -978,9 +978,9 @@ export class Matrix implements IClone<Matrix>, ICopy<Matrix, Matrix> {
     scale.set(sx, sy, sz);
 
     if (
-      Math.abs(sx) < MathUtil.zeroTolerance ||
-      Math.abs(sy) < MathUtil.zeroTolerance ||
-      Math.abs(sz) < MathUtil.zeroTolerance
+      Math.abs(sx) < MathUtil.epsilon ||
+      Math.abs(sy) < MathUtil.epsilon ||
+      Math.abs(sz) < MathUtil.epsilon
     ) {
       rotation.identity();
       return false;
@@ -1012,7 +1012,7 @@ export class Matrix implements IClone<Matrix>, ICopy<Matrix, Matrix> {
     const e = this.elements;
     let trace = e[0] + e[5] + e[10];
 
-    if (trace > MathUtil.zeroTolerance) {
+    if (trace >= MathUtil.epsilon) {
       let s = Math.sqrt(trace + 1.0) * 2;
       out._w = 0.25 * s;
       out._x = (e[6] - e[9]) / s;

packages/math/src/Quaternion.ts

@@ -209,7 +209,7 @@ export class Quaternion implements IClone<Quaternion>, ICopy<QuaternionLike, Qua
   static invert(a: Quaternion, out: Quaternion): void {
     const { _x: x, _y: y, _z: z, _w: w } = a;
     const dot = x * x + y * y + z * z + w * w;
-    if (dot > MathUtil.zeroTolerance) {
+    if (dot >= MathUtil.epsilon) {
       const invDot = 1.0 / dot;
       out._x = -x * invDot;
       out._y = -y * invDot;
@@ -272,7 +272,7 @@ export class Quaternion implements IClone<Quaternion>, ICopy<QuaternionLike, Qua
       bw = -bw;
     }
     // calculate coefficients
-    if (1.0 - cosom > MathUtil.zeroTolerance) {
+    if (1.0 - cosom > MathUtil.epsilon) {
       // standard case (slerp)
       const omega = Math.acos(cosom);
       const sinom = Math.sin(omega);
@@ -300,13 +300,9 @@ export class Quaternion implements IClone<Quaternion>, ICopy<QuaternionLike, Qua
   static normalize(a: Quaternion, out: Quaternion): void {
     const { _x, _y, _z, _w } = a;
     let len = Math.sqrt(_x * _x + _y * _y + _z * _z + _w * _w);
-    if (len > MathUtil.zeroTolerance) {
+    if (len >= MathUtil.epsilon) {
       len = 1 / len;
-      out._x = _x * len;
-      out._y = _y * len;
-      out._z = _z * len;
-      out._w = _w * len;
-      out._onValueChanged && out._onValueChanged();
+      out.set(_x * len, _y * len, _z * len, _w * len);
     }
   }
 
@@ -485,7 +481,7 @@ export class Quaternion implements IClone<Quaternion>, ICopy<QuaternionLike, Qua
   public get normalized(): boolean {
     return (
       Math.abs(this._x * this._x + this._y * this._y + this._z * this._z + this._w * this._w - 1) <
-      MathUtil.zeroTolerance
+      MathUtil.epsilon
     );
   }
 
@@ -553,18 +549,18 @@ export class Quaternion implements IClone<Quaternion>, ICopy<QuaternionLike, Qua
     const { _x, _y, _z } = this;
     const length = _x * _x + _y * _y + _z * _z;
 
-    if (length < MathUtil.zeroTolerance) {
+    if (length < MathUtil.epsilon) {
       out._x = 1;
       out._y = 0;
       out._z = 0;
-
+      out._onValueChanged && out._onValueChanged();
       return 0;
     } else {
       const inv = 1.0 / length;
-      out._x = this._x * inv;
-      out._y = this._y * inv;
-      out._z = this._z * inv;
-
+      out._x = _x * inv;
+      out._y = _y * inv;
+      out._z = _z * inv;
+      out._onValueChanged && out._onValueChanged();
       return Math.acos(this._w) * 2.0;
     }
   }
@@ -789,7 +785,7 @@ export class Quaternion implements IClone<Quaternion>, ICopy<QuaternionLike, Qua
     const xw = _x * _w;
 
     out._y = Math.asin(2.0 * (xw - yz));
-    if (Math.cos(out.y) > MathUtil.zeroTolerance) {
+    if (Math.cos(out.y) >= MathUtil.epsilon) {
       out._z = Math.atan2(2.0 * (xy + zw), 1.0 - 2.0 * (zz + xx));
       out._x = Math.atan2(2.0 * (zx + yw), 1.0 - 2.0 * (yy + xx));
     } else {

packages/math/src/Vector2.ts

@@ -160,11 +160,9 @@ export class Vector2 implements IClone<Vector2>, ICopy<Vector2Like, Vector2> {
   static normalize(left: Vector2, out: Vector2): void {
     const { _x, _y } = left;
     let len = Math.sqrt(_x * _x + _y * _y);
-    if (len > MathUtil.zeroTolerance) {
+    if (len >= MathUtil.epsilon) {
       len = 1 / len;
-      out._x = _x * len;
-      out._y = _y * len;
-      out._onValueChanged && out._onValueChanged();
+      out.set(_x * len, _y * len);
     }
   }
 

packages/math/src/Vector3.ts

@@ -192,7 +192,7 @@ export class Vector3 implements IClone<Vector3>, ICopy<Vector3Like, Vector3> {
   static normalize(a: Vector3, out: Vector3): void {
     const { _x, _y, _z } = a;
     let len = Math.sqrt(_x * _x + _y * _y + _z * _z);
-    if (len > MathUtil.zeroTolerance) {
+    if (len >= MathUtil.epsilon) {
       len = 1 / len;
       out.set(_x * len, _y * len, _z * len);
     }

packages/math/src/Vector4.ts

@@ -187,13 +187,9 @@ export class Vector4 implements IClone<Vector4>, ICopy<Vector4Like, Vector4> {
   static normalize(a: Vector4, out: Vector4): void {
     const { _x, _y, _z, _w } = a;
     let len = Math.sqrt(_x * _x + _y * _y + _z * _z + _w * _w);
-    if (len > MathUtil.zeroTolerance) {
+    if (len >= MathUtil.epsilon) {
       len = 1 / len;
-      out._x = _x * len;
-      out._y = _y * len;
-      out._z = _z * len;
-      out._w = _w * len;
-      out._onValueChanged && out._onValueChanged();
+      out.set(_x * len, _y * len, _z * len, _w * len);
     }
   }
 

tests/src/math/Quaternion.test.ts

@@ -42,20 +42,32 @@ describe("Quaternion test", () => {
 
   it("static equals", () => {
     const a = new Quaternion(1, 2, 3, 4);
-    const b = new Quaternion(1 + MathUtil.zeroTolerance * 0.9, 2, 3, 4);
-
+    const b = new Quaternion(-1, -2, -3, -4);
+    expect(a.x === b.x).to.eq(false);
+    expect(a.y === b.y).to.eq(false);
+    expect(a.z === b.z).to.eq(false);
+    expect(a.w === b.w).to.eq(false);
+    expect(Quaternion.equals(a, b)).to.eq(false);
+    expect(Quaternion.equals(b, a)).to.eq(false);
+
+    b.copyFrom(a);
+    expect(a.x === b.x).to.eq(true);
+    expect(a.y === b.y).to.eq(true);
+    expect(a.z === b.z).to.eq(true);
+    expect(a.w === b.w).to.eq(true);
     expect(Quaternion.equals(a, b)).to.eq(true);
+    expect(Quaternion.equals(b, a)).to.eq(true);
   });
 
   it("static rotationAxisAngle", () => {
     const a = new Vector3(3, 7, 5);
     const b = new Vector3();
     const out = new Quaternion();
     Quaternion.rotationAxisAngle(a, Math.PI / 3, out);
-    const rad = out.getAxisAngle(b);
-
-    expect(MathUtil.equals(rad, Math.PI / 3)).to.eq(true);
-    expect(Vector3.equals(b.normalize(), a.normalize())).to.eq(true);
+    expect(MathUtil.equals(out.getAxisAngle(b), Math.PI / 3)).to.eq(true);
+    a.normalize();
+    b.normalize();
+    expect(Vector3.equals(a, b)).to.eq(true);
   });
 
   it("static rotationEuler | rotationYawPitchRoll", () => {
@@ -133,10 +145,16 @@ describe("Quaternion test", () => {
 
   it("static normalize", () => {
     const a = new Quaternion(3, 4, 0, 0);
-    const out = new Quaternion();
+    const b = new Quaternion();
+
+    expect(a.length() === 1).to.eq(false);
+    Quaternion.normalize(a, b);
+
+    expect(b.length() === 1).to.eq(true);
+    expect(Quaternion.equals(b, new Quaternion(0.6, 0.8, 0, 0))).to.eq(true);
 
-    Quaternion.normalize(a, out);
-    expect(Quaternion.equals(out, new Quaternion(0.6, 0.8, 0, 0))).to.eq(true);
+    b.set(0, 0, 0, 0).normalize();
+    expect(b.length() === 0).to.eq(true);
   });
 
   it("static rotation", () => {
@@ -241,7 +259,7 @@ describe("Quaternion test", () => {
 
   it("length", () => {
     const a = new Quaternion(3, 4, 5, 0);
-    expect(MathUtil.equals(Math.sqrt(50), a.length())).to.eq(true);
+    expect(a.length()).to.eq(Math.sqrt(50));
     expect(a.lengthSquared()).to.eq(50);
   });
 });

tests/src/math/Vector2.test.ts

@@ -65,9 +65,17 @@ describe("Vector2 test", () => {
 
   it("static equals", () => {
     const a = new Vector2(1, 2);
-    const b = new Vector2(1 + MathUtil.zeroTolerance * 0.9, 2);
-
+    const b = new Vector2(-1, -2);
+    expect(a.x === b.x).to.eq(false);
+    expect(a.y === b.y).to.eq(false);
+    expect(Vector2.equals(a, b)).to.eq(false);
+    expect(Vector2.equals(b, a)).to.eq(false);
+
+    a.copyFrom(b);
+    expect(a.x === b.x).to.eq(true);
+    expect(a.y === b.y).to.eq(true);
     expect(Vector2.equals(a, b)).to.eq(true);
+    expect(Vector2.equals(b, a)).to.eq(true);
   });
 
   it("static lerp", () => {
@@ -108,9 +116,15 @@ describe("Vector2 test", () => {
   it("static normalize", () => {
     const a = new Vector2(3, 4);
     const out = new Vector2();
-
+    expect(MathUtil.equals(a.length(), 1)).to.eq(false);
     Vector2.normalize(a, out);
-    expect(Vector2.equals(out, new Vector2(0.6, 0.8))).to.eq(true);
+    expect(MathUtil.equals(a.length(), 1)).to.eq(false);
+    expect(MathUtil.equals(out.length(), 1)).to.eq(true);
+
+    a.set(0, 0);
+    expect(MathUtil.equals(a.length(), 1)).to.eq(false);
+    a.normalize();
+    expect(MathUtil.equals(a.length(), 1)).to.eq(false);
   });
 
   it("static scale", () => {
@@ -194,8 +208,14 @@ describe("Vector2 test", () => {
 
   it("normalize", () => {
     const a = new Vector2(3, 4);
-    expect(toString(a.normalize())).to.eq(toString(a));
-    expect(Vector2.equals(a, new Vector2(0.6, 0.8))).to.eq(true);
+    expect(MathUtil.equals(a.length(), 1)).to.eq(false);
+    a.normalize();
+    expect(MathUtil.equals(a.length(), 1)).to.eq(true);
+
+    a.set(0, 0);
+    expect(MathUtil.equals(a.length(), 1)).to.eq(false);
+    a.normalize();
+    expect(MathUtil.equals(a.length(), 1)).to.eq(false);
   });
 
   it("scale", () => {