Align collider position with collider shape offset

pixiv · Nov 26, 2024 · a7d1201 · a7d1201
1 parent c702462
commit a7d1201
Show file tree

Hide file tree

Showing 6 changed files with 40 additions and 25 deletions.
diff --git a/packages/three-vrm-springbone/src/VRMSpringBoneCollider.ts b/packages/three-vrm-springbone/src/VRMSpringBoneCollider.ts
@@ -14,5 +14,8 @@ export class VRMSpringBoneCollider extends THREE.Object3D {
     super();
 
     this.shape = shape;
+    if ('offset' in shape && (shape.offset as THREE.Vector3).isVector3) {
+      this.position.copy(shape.offset as THREE.Vector3);
+    }
   }
 }
diff --git a/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeCapsule.ts b/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeCapsule.ts
@@ -29,11 +29,17 @@ export class VRMSpringBoneColliderShapeCapsule extends VRMSpringBoneColliderShap
    */
   public inside: boolean;
 
+  /**
+   * Vector from head to tail in colliders local space.
+   */
+  private _headToTail: THREE.Vector3;
+
   public constructor(params?: { radius?: number; offset?: THREE.Vector3; tail?: THREE.Vector3; inside?: boolean }) {
     super();
 
     this.offset = params?.offset ?? new THREE.Vector3(0.0, 0.0, 0.0);
     this.tail = params?.tail ?? new THREE.Vector3(0.0, 0.0, 0.0);
+    this._headToTail = this.tail.clone().sub(this.offset);
     this.radius = params?.radius ?? 0.0;
     this.inside = params?.inside ?? false;
   }
@@ -44,9 +50,8 @@ export class VRMSpringBoneColliderShapeCapsule extends VRMSpringBoneColliderShap
     objectRadius: number,
     target: THREE.Vector3,
   ): number {
-    _v3A.copy(this.offset).applyMatrix4(colliderMatrix); // transformed head
-    _v3B.copy(this.tail).applyMatrix4(colliderMatrix); // transformed tail
-    _v3B.sub(_v3A); // from head to tail
+    _v3A.setFromMatrixPosition(colliderMatrix); // transformed head
+    _v3B.copy(this._headToTail).transformDirection(colliderMatrix).multiplyScalar(this._headToTail.length()); // transformed tail
     const lengthSqCapsule = _v3B.lengthSq();
 
     target.copy(objectPosition).sub(_v3A); // from head to object
@@ -64,13 +69,14 @@ export class VRMSpringBoneColliderShapeCapsule extends VRMSpringBoneColliderShap
       target.sub(_v3B); // from the shaft point to object
     }
 
-    const distance = this.inside
-      ? this.radius - objectRadius - target.length()
-      : target.length() - objectRadius - this.radius;
+    const length = target.length();
+    const distance = this.inside ? this.radius - objectRadius - length : length - objectRadius - this.radius;
 
-    target.normalize(); // convert the delta to the direction
-    if (this.inside) {
-      target.negate(); // if inside, reverse the direction
+    if (distance < 0) {
+      target.multiplyScalar(1 / length); // convert the delta to the direction
+      if (this.inside) {
+        target.negate(); // if inside, reverse the direction
+      }
     }
 
     return distance;

diff --git a/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapePlane.ts b/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapePlane.ts
@@ -32,7 +32,7 @@ export class VRMSpringBoneColliderShapePlane extends VRMSpringBoneColliderShape
     objectRadius: number,
     target: THREE.Vector3,
   ): number {
-    target.copy(this.offset).applyMatrix4(colliderMatrix); // transformed offset
+    target.setFromMatrixPosition(colliderMatrix); // transformed offset
     target.negate().add(objectPosition); // a vector from collider center to object position
 
     _mat3A.getNormalMatrix(colliderMatrix); // convert the collider matrix to the normal matrix

diff --git a/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeSphere.ts b/packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeSphere.ts
@@ -1,6 +1,8 @@
 import * as THREE from 'three';
 import { VRMSpringBoneColliderShape } from './VRMSpringBoneColliderShape';
 
+const _v3A = new THREE.Vector3();
+
 export class VRMSpringBoneColliderShapeSphere extends VRMSpringBoneColliderShape {
   public get type(): 'sphere' {
     return 'sphere';
@@ -35,16 +37,16 @@ export class VRMSpringBoneColliderShapeSphere extends VRMSpringBoneColliderShape
     objectRadius: number,
     target: THREE.Vector3,
   ): number {
-    target.copy(this.offset).applyMatrix4(colliderMatrix); // transformed offset
-    target.negate().add(objectPosition); // a vector from collider center to object position
+    target.subVectors(objectPosition, _v3A.setFromMatrixPosition(colliderMatrix));
 
-    const distance = this.inside
-      ? this.radius - objectRadius - target.length()
-      : target.length() - objectRadius - this.radius;
+    const length = target.length();
+    const distance = this.inside ? this.radius - objectRadius - length : length - objectRadius - this.radius;
 
-    target.normalize(); // convert the delta to the direction
-    if (this.inside) {
-      target.negate(); // if inside, reverse the direction
+    if (distance < 0) {
+      target.multiplyScalar(1 / length); // convert the delta to the direction
+      if (this.inside) {
+        target.negate(); // if inside, reverse the direction
+      }
     }
 
     return distance;

diff --git a/packages/three-vrm-springbone/src/tests/VRMSpringBoneColliderShapeCapsule.test.ts b/packages/three-vrm-springbone/src/tests/VRMSpringBoneColliderShapeCapsule.test.ts
@@ -75,7 +75,7 @@ describe('VRMSpringBoneColliderShapeCapsule', () => {
       });
 
       it('must calculate a collision properly, object is near the head', () => {
-        const colliderMatrix = new THREE.Matrix4();
+        const colliderMatrix = new THREE.Matrix4().makeTranslation(shape.offset);
         const objectPosition = new THREE.Vector3(-2.0, 0.0, 1.0);
         const objectRadius = 1.0;
 
@@ -87,19 +87,19 @@ describe('VRMSpringBoneColliderShapeCapsule', () => {
       });
 
       it('must calculate a collision properly, object is near the tail', () => {
-        const colliderMatrix = new THREE.Matrix4();
+        const colliderMatrix = new THREE.Matrix4().makeTranslation(shape.offset);
         const objectPosition = new THREE.Vector3(3.0, 0.0, 0.0);
-        const objectRadius = 1.0;
+        const objectRadius = 2.0;
 
         const dir = new THREE.Vector3();
         const distSq = shape.calculateCollision(colliderMatrix, objectPosition, objectRadius, dir);
 
-        expect(distSq).toBeCloseTo(0.44949); // sqrt(6) - 2
+        expect(distSq).toBeCloseTo(-0.55051); // sqrt(6) - 3
         expect(dir).toBeCloseToVector3(new THREE.Vector3(2.0, -1.0, -1.0).normalize());
       });
 
       it('must calculate a collision properly, object is between two ends', () => {
-        const colliderMatrix = new THREE.Matrix4();
+        const colliderMatrix = new THREE.Matrix4().makeTranslation(shape.offset);
         const objectPosition = new THREE.Vector3(0.0, 0.0, 0.0);
         const objectRadius = 1.0;
 

diff --git a/packages/three-vrm-springbone/src/tests/VRMSpringBoneColliderShapeSphere.test.ts b/packages/three-vrm-springbone/src/tests/VRMSpringBoneColliderShapeSphere.test.ts
@@ -64,7 +64,9 @@ describe('VRMSpringBoneColliderShapeSphere', () => {
         offset: new THREE.Vector3(0.0, 0.0, -1.0),
       });
 
-      const colliderMatrix = new THREE.Matrix4().makeTranslation(1.0, 0.0, 0.0);
+      const colliderMatrix = new THREE.Matrix4()
+        .makeTranslation(1.0, 0.0, 0.0)
+        .multiply(new THREE.Matrix4().makeTranslation(shape.offset));
       const objectPosition = new THREE.Vector3(2.0, 1.0, 0.0);
       const objectRadius = 1.0;
 
@@ -81,7 +83,9 @@ describe('VRMSpringBoneColliderShapeSphere', () => {
         offset: new THREE.Vector3(0.0, 1.0, 1.0),
       });
 
-      const colliderMatrix = new THREE.Matrix4().makeRotationX(-0.5 * Math.PI);
+      const colliderMatrix = new THREE.Matrix4()
+        .makeRotationX(-0.5 * Math.PI)
+        .multiply(new THREE.Matrix4().makeTranslation(shape.offset));
       const objectPosition = new THREE.Vector3(-1.0, 1.0, -1.0);
       const objectRadius = 1.0;