feat: support capsule collider

src/foundation/physics/CapsuleCollider.ts

@@ -5,4 +5,28 @@ export class CapsuleCollider {
   public position = Vector3.zero();
   public radius = 0;
   public tail = Vector3.zero();
+
+  collision(baseSceneGraph: SceneGraphComponent, bonePosition: Vector3, boneRadius: number) {
+    const spherePosWorld = baseSceneGraph.getWorldPositionOf(this.position);
+    let tailPosWorld = baseSceneGraph.getWorldPositionOf(this.tail);
+    tailPosWorld = Vector3.subtract(tailPosWorld, spherePosWorld);
+    const lengthSqCapsule = tailPosWorld.lengthSquared();
+    let direction = Vector3.subtract(bonePosition, spherePosWorld);
+    const dot = tailPosWorld.dot(direction);
+
+    if (dot <= 0.0) { // if bone is near from the head
+      // do nothing
+    } else if (lengthSqCapsule <= dot) { // if bone is near from the tail
+      direction = Vector3.subtract(direction, tailPosWorld);
+    } else { // if bone is between two ends
+      tailPosWorld = Vector3.multiply(tailPosWorld, (dot / lengthSqCapsule));
+      direction = Vector3.subtract(direction, tailPosWorld);
+    }
+
+    const radius = this.radius + boneRadius;
+    const distance = direction.length() - radius;
+    direction = Vector3.normalize(direction);
+
+    return {direction, distance};
+  }
 }

src/foundation/physics/VRMSpringBonePhysicsStrategy.ts

@@ -154,6 +154,18 @@ export class VRMSpringBonePhysicsStrategy implements PhysicsStrategy {
           return nextTail;
         }
       }
+      for (const collider of collisionGroup.capsuleColliders) {
+        const {direction, distance} = collider.collision(collisionGroup.baseSceneGraph!, nextTail, boneHitRadius);
+        if (distance < 0) {
+          // Hit
+          nextTail = Vector3.add(nextTail, Vector3.multiply(direction, -distance));
+
+          // normalize bone length
+          nextTail = this.normalizeBoneLength(nextTail, bone, head);
+
+          return nextTail;
+        }
+      }
     }
 
     return nextTail;