Merge pull request #1520 from actnwit/fix/optimize-math

fix: optimize math

src/foundation/components/SceneGraph/SceneGraphComponent.ts

@@ -24,10 +24,14 @@ import { RaycastResultEx2 } from '../../geometry/types/GeometryTypes';
 import { TranslationGizmo } from '../../gizmos/TranslationGizmo';
 import { ScaleGizmo } from '../../gizmos/ScaleGizmo';
 import { IMatrix44 } from '../../math/IMatrix';
-import { IQuaternion, IVector3, MutableScalar, Quaternion } from '../../math';
 import { OimoPhysicsStrategy } from '../../physics/Oimo/OimoPhysicsStrategy';
 import { TransformComponent } from '../Transform/TransformComponent';
 import { flattenHierarchy } from './SceneGraphOps';
+import { MutableScalar } from '../../math/MutableScalar';
+import { MutableQuaternion } from '../../math/MutableQuaternion';
+import { IQuaternion } from '../../math/IQuaternion';
+import { Quaternion } from '../../math/Quaternion';
+import { IVector3 } from '../../math/IVector';
 
 export class SceneGraphComponent extends Component {
   private __parent?: SceneGraphComponent;
@@ -66,6 +70,8 @@ export class SceneGraphComponent extends Component {
   private static __tmp_mat4 = MutableMatrix44.identity();
   private static __tmp_mat4_2 = MutableMatrix44.identity();
   private static __tmp_mat4_3 = MutableMatrix44.identity();
+  private static __tmp_quat_0 = MutableQuaternion.identity();
+  private static __tmp_quat_1 = MutableQuaternion.identity();
 
   private static __updateCount = -1;
 
@@ -789,10 +795,18 @@ export class SceneGraphComponent extends Component {
     return this.matrixInner.getTranslate();
   }
 
+  getPositionTo(outVec: MutableVector3): MutableVector3 {
+    return this.matrixInner.getTranslateTo(outVec);
+  }
+
   get positionRest(): MutableVector3 {
     return this.matrixRestInner.getTranslate();
   }
 
+  getPositionRestTo(outVec: MutableVector3): MutableVector3 {
+    return this.matrixRestInner.getTranslateTo(outVec);
+  }
+
   set eulerAngles(vec: IVector3) {
     if (Is.not.exist(this.__parent)) {
       this.entity.getTransform().localEulerAngles = vec;
@@ -855,6 +869,20 @@ export class SceneGraphComponent extends Component {
     return this.entity.getTransform().localRotationInner;
   }
 
+  getRotationTo(outQuat: MutableQuaternion): MutableQuaternion {
+    const parent = this.parent;
+    if (parent != null) {
+      return Quaternion.multiplyTo(
+        parent.getRotationTo(SceneGraphComponent.__tmp_quat_0),
+        this.entity.getTransform().localRotationInner,
+        outQuat
+      ) as MutableQuaternion;
+    }
+    const quat = this.entity.getTransform().localRotationInner;
+    outQuat.setComponents(quat._v[0], quat._v[1], quat._v[2], quat._v[3]);
+    return outQuat;
+  }
+
   get rotationRest(): Quaternion {
     const parent = this.parent;
     if (parent != null) {

src/foundation/geometry/Frustum.ts

@@ -19,6 +19,31 @@ export class Frustum {
   private __vp = MutableMatrix44.zero();
   private __invProjMat = MutableMatrix44.zero();
   private __invViewMat = MutableMatrix44.zero();
+  private __tmp_vec4_0 = MutableVector4.zero();
+  private __tmp_vec4_1 = MutableVector4.zero();
+  private __tmp_vec4_array = [
+    MutableVector4.zero(),
+    MutableVector4.zero(),
+    MutableVector4.zero(),
+    MutableVector4.zero(),
+    MutableVector4.zero(),
+    MutableVector4.zero(),
+    MutableVector4.zero(),
+    MutableVector4.zero(),
+  ];
+  private __hCorners = [
+    // near
+    Vector4.fromCopy4(-1, 1, 1, 1),
+    Vector4.fromCopy4(1, 1, 1, 1),
+    Vector4.fromCopy4(1, -1, 1, 1),
+    Vector4.fromCopy4(-1, -1, 1, 1),
+    // far
+    Vector4.fromCopy4(-1, 1, -1, 1),
+    Vector4.fromCopy4(1, 1, -1, 1),
+    Vector4.fromCopy4(1, -1, -1, 1),
+    Vector4.fromCopy4(-1, -1, -1, 1),
+  ];
+
   public corners: Vector4[] = [];
 
   constructor() {}
@@ -69,31 +94,16 @@ export class Frustum {
     // this.right.normalize3();
 
     // Calculate the corners of the view frustum.
-    const hCorners = [
-      // near
-      Vector4.fromCopy4(-1, 1, 1, 1),
-      Vector4.fromCopy4(1, 1, 1, 1),
-      Vector4.fromCopy4(1, -1, 1, 1),
-      Vector4.fromCopy4(-1, -1, 1, 1),
-      // far
-      Vector4.fromCopy4(-1, 1, -1, 1),
-      Vector4.fromCopy4(1, 1, -1, 1),
-      Vector4.fromCopy4(1, -1, -1, 1),
-      Vector4.fromCopy4(-1, -1, -1, 1),
-    ];
-
     Matrix44.invertTo(projectionMatrix, this.__invProjMat);
     Matrix44.invertTo(viewMatrix, this.__invViewMat);
     for (let i = 0; i < 8; i++) {
-      hCorners[i] = this.__invProjMat.multiplyVector(hCorners[i]);
-      hCorners[i] = Vector4.fromCopy4(
-        hCorners[i].x / hCorners[i].w,
-        hCorners[i].y / hCorners[i].w,
-        hCorners[i].z / hCorners[i].w,
-        1
-      );
-
-      this.corners[i] = this.__invViewMat.multiplyVector(hCorners[i]);
+      this.__invProjMat.multiplyVectorTo(this.__hCorners[i], this.__tmp_vec4_0);
+      this.__tmp_vec4_1.x = this.__tmp_vec4_0.x / this.__tmp_vec4_0.w;
+      this.__tmp_vec4_1.y = this.__tmp_vec4_0.y / this.__tmp_vec4_0.w;
+      this.__tmp_vec4_1.z = this.__tmp_vec4_0.z / this.__tmp_vec4_0.w;
+      this.__tmp_vec4_1.w = 1;
+      this.__invViewMat.multiplyVectorTo(this.__tmp_vec4_1, this.__tmp_vec4_array[i]);
+      this.corners[i] = this.__tmp_vec4_array[i];
     }
   }
 

src/foundation/math/MutableMatrix44.ts

@@ -273,6 +273,11 @@ export class MutableMatrix44 extends Matrix44 implements IMutableMatrix, IMutabl
     return rotateMat;
   }
 
+  getTranslateTo(outVec: MutableVector3) {
+    const rotateMat = super.getTranslateTo(outVec) as MutableVector3;
+    return rotateMat;
+  }
+
   getScale() {
     const rotateMat = super.getScale() as MutableVector3;
     return rotateMat;

src/foundation/math/Quaternion.ts

@@ -20,6 +20,7 @@ export class Quaternion extends AbstractQuaternion implements IQuaternion {
   private static __tmp_vec3_2: any = MutableVector3.zero();
   private static __tmp_vec3_3: any = MutableVector3.zero();
   private static __tmp_vec3_4: any = MutableVector3.zero();
+  private static __tmp_vec3_5: any = MutableVector3.zero();
 
   constructor(x: Float32Array) {
     super();
@@ -770,7 +771,8 @@ export class Quaternion extends AbstractQuaternion implements IQuaternion {
   }
 
   transformVector3(v: IVector3) {
-    const u = Vector3.fromCopy3(this._v[0], this._v[1], this._v[2]);
+    const u = Quaternion.__tmp_vec3_5;
+    u.setComponents(this._v[0], this._v[1], this._v[2]);
     const uv = Vector3.cross(u, v);
     const uuv = Vector3.cross(u, uv);
     const uvw = Vector3.multiply(uv, this._v[3]);
@@ -780,7 +782,8 @@ export class Quaternion extends AbstractQuaternion implements IQuaternion {
   }
 
   transformVector3To(v: IVector3, out: IMutableVector3) {
-    const u = Vector3.fromCopy3(this._v[0], this._v[1], this._v[2]);
+    const u = Quaternion.__tmp_vec3_5;
+    u.setComponents(this._v[0], this._v[1], this._v[2]);
     const uv = Vector3.crossTo(u, v, Quaternion.__tmp_vec3_0);
     const uuv = Vector3.crossTo(u, uv, Quaternion.__tmp_vec3_1);
     const uvw = Vector3.multiplyTo(uv, this._v[3], Quaternion.__tmp_vec3_2);

src/foundation/physics/VRMSpring/VRMSpringBone.ts

@@ -21,6 +21,8 @@ export class VRMSpringBone extends RnObject {
   initialized = false;
 
   private static __tmp_vec3_0 = MutableVector3.zero();
+  private static __tmp_vec3_1 = MutableVector3.zero();
+  private static __tmp_vec3_2_zero = Vector3.zero();
 
   constructor(node: ISceneGraphEntity) {
     super();
@@ -62,11 +64,11 @@ export class VRMSpringBone extends RnObject {
   }
 
   _calcWorldSpaceBoneLength(): void {
-    const v3A = this.node.getSceneGraph().matrixInner.getTranslate();
-    let v3B = Vector3.zero();
+    const v3A = this.node.getSceneGraph().matrixInner.getTranslateTo(VRMSpringBone.__tmp_vec3_0);
+    let v3B = VRMSpringBone.__tmp_vec3_2_zero;
     const children = this.node.getSceneGraph().children;
     if (children.length > 0) {
-      v3B = children[0].matrixInner.getTranslate();
+      v3B = children[0].matrixInner.getTranslateTo(VRMSpringBone.__tmp_vec3_1);
     } else {
       // v3B = this.node.getSceneGraph().matrixInner.multiplyVector3(this.initialLocalChildPosition);
       v3B = Vector3.multiplyMatrix4(

src/foundation/physics/VRMSpring/VRMSpringBonePhysicsStrategy.ts

@@ -32,12 +32,19 @@ export class VRMSpringBonePhysicsStrategy implements PhysicsStrategy {
   private static __tmp_normalizeBoneLength_vec3_1 = MutableVector3.zero();
   private static __tmp_normalizeBoneLength_vec3_2 = MutableVector3.zero();
   private static __tmp_normalizeBoneLength_vec3_3 = MutableVector3.zero();
+  private static __tmp_normalizeBoneLength_vec3_4 = MutableVector3.zero();
+  private static __tmp_normalizeBoneLength_vec3_5 = MutableVector3.zero();
   private static __tmp_applyRotation_vec3_0 = MutableVector3.zero();
   private static __tmp_applyRotation_vec3_1 = MutableVector3.zero();
+  private static __tmp_applyRotation_vec3_2 = MutableVector3.zero();
+  private static __tmp_applyRotation_vec3_3 = MutableVector3.zero();
   private static __tmp_applyRotation_quat_0 = MutableQuaternion.identity();
   private static __tmp_applyRotation_quat_1 = MutableQuaternion.identity();
   private static __tmp_applyRotation_quat_2 = MutableQuaternion.identity();
   private static __tmp_applyRotation_quat_3 = MutableQuaternion.identity();
+  private static __tmp_applyRotation_quat_4 = MutableQuaternion.identity();
+  private static __tmp_getParentRotation_quat_0 = MutableQuaternion.identity();
+  private static __tmp_getParentRotation_quat_1_identity = MutableQuaternion.identity();
   private static __tmp_collision_vec3_0 = MutableVector3.zero();
   private static __tmp_collision_vec3_1 = MutableVector3.zero();
   private static __tmp_collision_vec3_2 = MutableVector3.zero();
@@ -48,7 +55,9 @@ export class VRMSpringBonePhysicsStrategy implements PhysicsStrategy {
   constructor() {}
 
   getParentRotation(head: SceneGraphComponent) {
-    return head.parent != null ? head.parent.rotation : Quaternion.identity();
+    return head.parent != null
+      ? head.parent.getRotationTo(VRMSpringBonePhysicsStrategy.__tmp_getParentRotation_quat_0)
+      : VRMSpringBonePhysicsStrategy.__tmp_getParentRotation_quat_1_identity;
   }
 
   update() {
@@ -168,13 +177,17 @@ export class VRMSpringBonePhysicsStrategy implements PhysicsStrategy {
     const sub = Vector3.normalizeTo(
       Vector3.subtractTo(
         nextTail,
-        bone.node.position,
+        bone.node
+          .getSceneGraph()
+          .getPositionTo(VRMSpringBonePhysicsStrategy.__tmp_normalizeBoneLength_vec3_4),
         VRMSpringBonePhysicsStrategy.__tmp_normalizeBoneLength_vec3_0
       ),
       VRMSpringBonePhysicsStrategy.__tmp_normalizeBoneLength_vec3_1
     );
     return Vector3.addTo(
-      bone.node.position,
+      bone.node
+        .getSceneGraph()
+        .getPositionTo(VRMSpringBonePhysicsStrategy.__tmp_normalizeBoneLength_vec3_5),
       Vector3.multiplyTo(
         sub,
         bone.boneLength,
@@ -187,20 +200,22 @@ export class VRMSpringBonePhysicsStrategy implements PhysicsStrategy {
   applyRotation(nextTail: Vector3, bone: VRMSpringBone) {
     const sub = Vector3.subtractTo(
       nextTail,
-      bone.node.position,
+      bone.node
+        .getSceneGraph()
+        .getPositionTo(VRMSpringBonePhysicsStrategy.__tmp_applyRotation_vec3_3),
       VRMSpringBonePhysicsStrategy.__tmp_applyRotation_vec3_0
     );
     const to = Quaternion.invertTo(
       Quaternion.multiplyTo(
-        bone.node.parent!.rotation,
+        bone.node.parent!.getRotationTo(VRMSpringBonePhysicsStrategy.__tmp_applyRotation_quat_4),
         bone.node.localRotationRestInner,
         VRMSpringBonePhysicsStrategy.__tmp_applyRotation_quat_0
       ),
       VRMSpringBonePhysicsStrategy.__tmp_applyRotation_quat_1
     ).transformVector3To(sub, VRMSpringBonePhysicsStrategy.__tmp_applyRotation_vec3_1);
     const rot = Quaternion.fromToRotationTo(
       bone.boneAxis,
-      Vector3.normalizeTo(to, VRMSpringBonePhysicsStrategy.__tmp_applyRotation_vec3_1),
+      Vector3.normalizeTo(to, VRMSpringBonePhysicsStrategy.__tmp_applyRotation_vec3_2),
       VRMSpringBonePhysicsStrategy.__tmp_applyRotation_quat_2
     );
     const result = Quaternion.multiplyTo(