Merge pull request #1264 from actnwit/feat/vrm-capsule-collider

feat: vrm capsule collider
actnwit · Jun 11, 2023 · 21c3d1f · 21c3d1f
2 parents 873f099 + a81e68a
commit 21c3d1f
Show file tree

Hide file tree

Showing 6 changed files with 82 additions and 25 deletions.
diff --git a/src/foundation/importer/Vrm0xImporter.ts b/src/foundation/importer/Vrm0xImporter.ts
@@ -208,7 +208,7 @@ export class Vrm0xImporter {
         sphereCollider.radius = collider.radius;
         colliders.push(sphereCollider);
       }
-      vrmColliderGroup.colliders = colliders;
+      vrmColliderGroup.sphereColliders = colliders;
       const baseSg = gltfModel.asset.extras!.rnEntities![colliderGroup.node].getSceneGraph();
       vrmColliderGroup.baseSceneGraph = baseSg;
     }

diff --git a/src/foundation/importer/VrmImporter.ts b/src/foundation/importer/VrmImporter.ts
@@ -19,6 +19,7 @@ import { Gltf2Importer } from './Gltf2Importer';
 import { Sampler } from '../textures/Sampler';
 import { VrmComponent, VrmExpression, VrmExpressionMorphBind } from '../components';
 import { VRMSpringBone } from '../physics/VRMSpringBone';
+import { CapsuleCollider } from '../physics/CapsuleCollider';
 
 export class VrmImporter {
   private constructor() {}
@@ -116,25 +117,36 @@ export class VrmImporter {
 
       const vrmColliderGroup = new VRMColliderGroup();
       colliderGroups.push(vrmColliderGroup);
-      const colliders: SphereCollider[] = [];
       for (const colliderIdx of colliderGroup.colliders) {
         const collider = gltfModel.extensions.VRMC_springBone.colliders[colliderIdx];
 
-        if (collider.shape.sphere) {
+        const baseSg = gltfModel.asset.extras!.rnEntities![collider.node].getSceneGraph();
+        vrmColliderGroup.baseSceneGraph = baseSg;
+        if (Is.exist(collider.shape.sphere)) {
           const sphereCollider = new SphereCollider();
           sphereCollider.position = Vector3.fromCopyArray([
             collider.shape.sphere.offset[0],
             collider.shape.sphere.offset[1],
             collider.shape.sphere.offset[2],
           ]);
           sphereCollider.radius = collider.shape.sphere.radius;
-          colliders.push(sphereCollider);
-          const baseSg = gltfModel.asset.extras!.rnEntities![collider.node].getSceneGraph();
-          sphereCollider.baseSceneGraph = baseSg;
-          vrmColliderGroup.baseSceneGraph = baseSg;
+          vrmColliderGroup.sphereColliders.push(sphereCollider);
+        } else if (Is.exist(collider.shape.capsule)) {
+          const capsuleCollider = new CapsuleCollider();
+          capsuleCollider.position = Vector3.fromCopyArray([
+            collider.shape.capsule.offset[0],
+            collider.shape.capsule.offset[1],
+            collider.shape.capsule.offset[2],
+          ]);
+          capsuleCollider.radius = collider.shape.capsule.radius;
+          capsuleCollider.tail = Vector3.fromCopyArray([
+            collider.shape.capsule.tail[0],
+            collider.shape.capsule.tail[1],
+            collider.shape.capsule.tail[2],
+          ]);
+          vrmColliderGroup.capsuleColliders.push(capsuleCollider);
         }
       }
-      vrmColliderGroup.colliders = colliders;
     }
 
 

diff --git a/src/foundation/physics/CapsuleCollider.ts b/src/foundation/physics/CapsuleCollider.ts
@@ -0,0 +1,32 @@
+import { SceneGraphComponent } from "../components/SceneGraph/SceneGraphComponent";
+import { Vector3 } from "../math/Vector3";
+
+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};
+  }
+}
diff --git a/src/foundation/physics/SphereCollider.ts b/src/foundation/physics/SphereCollider.ts
@@ -1,8 +1,17 @@
-import { SceneGraphComponent } from '../components';
+import { SceneGraphComponent } from '../components/SceneGraph/SceneGraphComponent';
 import { Vector3 } from '../math/Vector3';
 
 export class SphereCollider {
   public position = Vector3.zero();
   public radius = 0;
-  baseSceneGraph?: SceneGraphComponent;
+
+  collision(baseSceneGraph: SceneGraphComponent, bonePosition: Vector3, boneRadius: number) {
+    const spherePosWorld = baseSceneGraph.getWorldPositionOf(this.position);
+    const delta = Vector3.subtract(bonePosition, spherePosWorld);
+    const direction = Vector3.normalize(delta);
+    const radius = this.radius + boneRadius;
+    const distance = delta.length() - radius;
+
+    return {direction, distance};
+  }
 }
diff --git a/src/foundation/physics/VRMColliderGroup.ts b/src/foundation/physics/VRMColliderGroup.ts
@@ -1,7 +1,9 @@
 import { SphereCollider } from './SphereCollider';
 import { SceneGraphComponent } from '../components/SceneGraph/SceneGraphComponent';
+import { CapsuleCollider } from './CapsuleCollider';
 
 export class VRMColliderGroup {
-  colliders: SphereCollider[] = [];
+  sphereColliders: SphereCollider[] = [];
+  capsuleColliders: CapsuleCollider[] = [];
   baseSceneGraph?: SceneGraphComponent;
 }
diff --git a/src/foundation/physics/VRMSpringBonePhysicsStrategy.ts b/src/foundation/physics/VRMSpringBonePhysicsStrategy.ts
@@ -142,21 +142,23 @@ export class VRMSpringBonePhysicsStrategy implements PhysicsStrategy {
 
   collision(collisionGroups: VRMColliderGroup[], nextTail: Vector3, boneHitRadius: number, head: SceneGraphComponent, bone: VRMSpringBone) {
     for (const collisionGroup of collisionGroups) {
-      for (const collider of collisionGroup.colliders) {
-        const worldColliderPos = collisionGroup.baseSceneGraph!.getWorldPositionOf(
-          collider.position
-        );
-        const r = boneHitRadius + collider.radius;
-        const delta = Vector3.subtract(nextTail, worldColliderPos);
-        const deltaScalar = r - delta.length();
-        if (deltaScalar > 0) {
-          const normal = Vector3.normalize(delta);
-          const resilienceVec = Vector3.multiply(
-            normal,
-            deltaScalar
-          );
+      for (const collider of collisionGroup.sphereColliders) {
+        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;
+        }
+      }
+      for (const collider of collisionGroup.capsuleColliders) {
+        const {direction, distance} = collider.collision(collisionGroup.baseSceneGraph!, nextTail, boneHitRadius);
+        if (distance < 0) {
           // Hit
-          nextTail = Vector3.add(nextTail, resilienceVec);
+          nextTail = Vector3.add(nextTail, Vector3.multiply(direction, -distance));
 
           // normalize bone length
           nextTail = this.normalizeBoneLength(nextTail, bone, head);