StrandCollisionJob capsule

Assets/VRM10_Samples/ClothSample/RotateParticle/Runtime/Jobs/Collision.cs

@@ -40,29 +40,95 @@ namespace RotateParticle.Jobs
                 var pos = NextPositions[particleIndex];
                 for (int colliderIndex = 0; colliderIndex < Colliders.Length; ++colliderIndex)
                 {
-                    var collider = Colliders[colliderIndex];
-                    var col_pos = CurrentColliders[colliderIndex].MultiplyPoint(collider.offset);
-                    switch (collider.colliderType)
-                    {
-                        case BlittableColliderType.Sphere:
-                            {
-                                var d = Vector3.Distance(pos, col_pos);
-                                var min_d = info.Settings.radius + collider.radius;
-                                if (d < min_d)
-                                {
-                                    Vector3 normal = (pos - col_pos).normalized;
-                                    pos += normal * (min_d - d);
-                                }
-                                break;
-                            }
+                    var c = Colliders[colliderIndex];
+                    var m = CurrentColliders[colliderIndex];
 
-                        default:
-                            break;
+                    if (c.colliderType == BlittableColliderType.Capsule)
+                    {
+                        if (TryCollideCapsuleAndSphere(m.MultiplyPoint(c.offset), m.MultiplyPoint(c.tailOrNormal), c.radius,
+                            pos, info.Settings.radius, out var l))
+                        {
+                            pos += l.GetDelta(c.radius + info.Settings.radius);
+                        }
                     }
+                    else
+                    {
+                        if (TryCollideSphereAndSphere(m.MultiplyPoint(c.offset), c.radius,
+                            pos, info.Settings.radius, out var l))
+                        {
+                            pos += l.GetDelta(c.radius + info.Settings.radius);
+                        }
+                    }
+                    StrandCollision[particleIndex] = pos;
                 }
-                StrandCollision[particleIndex] = pos;
             }
         }
+
+        /// <summary>
+        /// collide sphere a and sphere b.
+        /// move sphere b to resolved if collide.
+        /// </summary>
+        /// <param name="from"></param>
+        /// <param name="ra"></param>
+        /// <param name="to"></param>
+        /// <param name="ba"></param>
+        /// <param name="resolved"></param>
+        /// <returns></returns>
+        static bool TryCollideSphereAndSphere(
+            in Vector3 from, float ra,
+            in Vector3 to, float rb,
+            out LineSegment resolved
+            )
+        {
+            var d = Vector3.Distance(from, to);
+            if (d > (ra + rb))
+            {
+                resolved = default;
+                return false;
+            }
+            Vector3 normal = (to - from).normalized;
+            resolved = new(from, from + normal * (d - rb));
+            return true;
+        }
+
+        /// <summary>
+        /// collide capsule and sphere b.
+        /// move sphere b to resolved if collide.
+        /// </summary>
+        /// <param name="capsuleHead"></param>
+        /// <param name="capsuleTail"></param>
+        /// <param name="capsuleRadius"></param>
+        /// <param name="b"></param>
+        /// <param name="rb"></param>
+        static bool TryCollideCapsuleAndSphere(
+            in Vector3 capsuleHead,
+            in Vector3 capsuleTail,
+            float capsuleRadius,
+            in Vector3 b,
+            float rb,
+            out LineSegment resolved
+            )
+        {
+            var P = (capsuleTail - capsuleHead).normalized;
+            var Q = b - capsuleHead;
+            var dot = Vector3.Dot(P, Q);
+            if (dot <= 0)
+            {
+                // head側半球の球判定
+                return TryCollideSphereAndSphere(capsuleHead, capsuleRadius, b, rb, out resolved);
+            }
+
+            var t = dot / P.magnitude;
+            if (t >= 1.0f)
+            {
+                // tail側半球の球判定
+                return TryCollideSphereAndSphere(capsuleTail, capsuleRadius, b, rb, out resolved);
+            }
+
+            // head-tail上の m_transform.position との最近点
+            var p = capsuleHead + P * t;
+            return TryCollideSphereAndSphere(p, capsuleRadius, b, rb, out resolved);
+        }
     }
 
     public struct ClothCollisionJob : IJobParallelFor