Align collider position with collider shape offset

packages/three-vrm-springbone/src/VRMSpringBoneCollider.ts

@@ -10,9 +10,42 @@ export class VRMSpringBoneCollider extends THREE.Object3D {
    */
   public readonly shape: VRMSpringBoneColliderShape;
 
+  /**
+   * World space matrix for the collider shape used in collision calculations.
+   */
+  public readonly colliderMatrix = new THREE.Matrix4();
+
   public constructor(shape: VRMSpringBoneColliderShape) {
     super();
 
     this.shape = shape;
+    this.updateWorldMatrix(false, false);
+  }
+
+  public updateWorldMatrix(updateParents: boolean, updateChildren: boolean): void {
+    super.updateWorldMatrix(updateParents, updateChildren);
+
+    updateColliderMatrix(this.shape.offset, this.matrixWorld, this.colliderMatrix);
   }
 }
+
+/**
+ * Computes the colliderMatrix based on an offset and a world matrix.
+ * Equivalent to the following code when matrixWorld is an affine matrix:
+ * ```js
+ * out.makeTranslation(offset).premultiply(matrixWorld)
+ * ```
+ *
+ * @param offset The collider shape offset.
+ * @param matrixWorld The world matrix fo the collider object.
+ * @param out The target matrix to store the result in.
+ */
+function updateColliderMatrix(offset: THREE.Vector3, matrixWorld: THREE.Matrix4, out: THREE.Matrix4) {
+  const me = matrixWorld.elements;
+
+  out.copy(matrixWorld);
+
+  out.elements[12] = me[0] * offset.x + me[4] * offset.y + me[8] * offset.z + me[12];
+  out.elements[13] = me[1] * offset.x + me[5] * offset.y + me[9] * offset.z + me[13];
+  out.elements[14] = me[2] * offset.x + me[6] * offset.y + me[10] * offset.z + me[14];
+}

packages/three-vrm-springbone/src/VRMSpringBoneColliderShape.ts

@@ -9,6 +9,11 @@ export abstract class VRMSpringBoneColliderShape {
    */
   public abstract get type(): string;
 
+  /**
+   * The offset to the shape.
+   */
+  public abstract get offset(): THREE.Vector3;
+
   /**
    * Calculate a distance and a direction from the collider to a target object.
    * It's hit if the distance is negative.

packages/three-vrm-springbone/src/VRMSpringBoneColliderShapeCapsule.ts

@@ -44,8 +44,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
+    _v3A.setFromMatrixPosition(colliderMatrix); // transformed head
+    _v3B.subVectors(this.tail, this.offset).applyMatrix4(colliderMatrix); // transformed tail
     _v3B.sub(_v3A); // from head to tail
     const lengthSqCapsule = _v3B.lengthSq();
 
@@ -64,13 +64,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;

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

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;

packages/three-vrm-springbone/src/VRMSpringBoneJoint.ts

@@ -283,7 +283,7 @@ export class VRMSpringBoneJoint {
     for (let cg = 0; cg < this.colliderGroups.length; cg++) {
       for (let c = 0; c < this.colliderGroups[cg].colliders.length; c++) {
         const collider = this.colliderGroups[cg].colliders[c];
-        const dist = collider.shape.calculateCollision(collider.matrixWorld, tail, this.settings.hitRadius, _v3A);
+        const dist = collider.shape.calculateCollision(collider.colliderMatrix, tail, this.settings.hitRadius, _v3A);
 
         if (dist < 0.0) {
           // hit

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;
 

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;