Merge pull request #2696 from ousttrue/fix/VRMC_springBone_limit

[VRMC_springBone_limit] job 側のロジック実装

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit.meta

@@ -0,0 +1,8 @@
+fileFormatVersion: 2
+guid: b09ae318bccbfce4993fafb418e1e8c7
+folderAsset: yes
+DefaultImporter:
+  externalObjects: {}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs

@@ -0,0 +1,91 @@
+using UniGLTF.SpringBoneJobs.Blittables;
+using Unity.Mathematics;
+
+namespace UniGLTF.SpringBoneJobs
+{
+    public static class Anglelimit
+    {
+        public static float3 Apply(
+            in BlittableJointImmutable logic, in BlittableJointMutable joint,
+            in quaternion parentRotation, in float3 head, in float3 nextTail)
+        {
+            switch (joint.anglelimitType)
+            {
+                case AnglelimitTypes.None:
+                    // do nothing
+                    return nextTail;
+
+                case AnglelimitTypes.Cone:
+                    {
+                        var angleSpaceToWorld = anglelimitSpaceToWorld(logic, joint, parentRotation);
+                        var tailDir = math.mul(math.inverse(angleSpaceToWorld), nextTail - head);
+                        tailDir = AnglelimitCone.Apply(tailDir, joint.anglelimit1);
+                        return math.mul(angleSpaceToWorld, tailDir);
+                    }
+
+                case AnglelimitTypes.Hinge:
+                    {
+                        var angleSpaceToWorld = anglelimitSpaceToWorld(logic, joint, parentRotation);
+                        var tailDir = math.mul(math.inverse(angleSpaceToWorld), nextTail - head);
+                        tailDir = AnglelimitHinge.Apply(tailDir, joint.anglelimit1);
+                        return math.mul(angleSpaceToWorld, tailDir);
+                    }
+
+
+                case AnglelimitTypes.Spherical:
+                    {
+                        var angleSpaceToWorld = anglelimitSpaceToWorld(logic, joint, parentRotation);
+                        var tailDir = math.mul(math.inverse(angleSpaceToWorld), nextTail - head);
+                        tailDir = AnglelimitSpherical.Apply(tailDir, joint.anglelimit1, joint.anglelimit2);
+                        return math.mul(angleSpaceToWorld, tailDir);
+                    }
+
+                default:
+                    throw new System.ArgumentException($"unknown joint.anglelimitType: {joint.anglelimitType}");
+            }
+        }
+
+        /// <param name="nextTail">nextTail(position vector in world space)</param>
+        /// <returns>tailDir(directionay vector in angle space)</returns>
+        /// <exception cref="System.NotImplementedException"></exception>
+        public static quaternion anglelimitSpaceToWorld(in BlittableJointImmutable logic, in BlittableJointMutable joint,
+        in quaternion parentRotation)
+        {
+            // Y+方向からjointのheadからtailに向かうベクトルへの最小回転
+            var axisRotation = fromToQuaternion(new float3(0, 1, 0), logic.boneAxis);
+
+            // limitのローカル空間をワールド空間に写像する回転
+            return
+                math.mul(parentRotation,
+                math.mul(logic.localRotation,
+                math.mul(axisRotation,
+                joint.anglelimitOffset)))
+            ;
+        }
+
+        // https://discussions.unity.com/t/unity-mathematics-equivalent-to-quaternion-fromtorotation/237459
+        public static quaternion fromToQuaternion(in float3 from, in float3 to)
+        {
+            var fromNorm = math.normalize(from);
+            var toNorm = math.normalize(to);
+            var dot = math.dot(fromNorm, toNorm);
+
+            // Handle the case where from and to are parallel but opposite
+            if (math.abs(dot + 1f) < 1e-6f) // dot is approximately -1
+            {
+                // Find a perpendicular axis
+                var perpAxis = math.abs(fromNorm.x) > math.abs(fromNorm.z)
+                    ? new float3(-fromNorm.y, fromNorm.x, 0f)
+                    : new float3(0f, -fromNorm.z, fromNorm.y);
+                return quaternion.AxisAngle(math.normalize(perpAxis), math.PI);
+            }
+
+            // General case
+            return quaternion.AxisAngle(
+                  angle: math.acos(math.clamp(dot, -1f, 1f)),
+                  axis: math.normalize(math.cross(fromNorm, toNorm))
+                );
+        }
+
+    }
+}

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 3c2f98858052fbe4c89b445bad4c979b
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs

@@ -0,0 +1,33 @@
+using Unity.Mathematics;
+
+namespace UniGLTF.SpringBoneJobs
+{
+    public static class AnglelimitCone
+    {
+        /// <param name="src">AngleLimit空間の方向ベクトル</param>
+        /// <param name="angleLimit">radius</param>
+        /// <returns>AngleLimit空間の方向ベクトル</returns>
+        public static float3 Apply(in float3 src, float angleLimit)
+        {
+            // tailDirのy要素をjointに設定されたangleの余弦と比較する
+            var cosAngle = math.cos(angleLimit);
+            if (src.y >= cosAngle)
+            {
+                return src;
+            }
+
+            var tailDir = src;
+            {
+                // x・z要素を、tailDirの正弦とjointに設定されたangleの正弦の比を用いてスケールする
+                var ratio = math.sqrt((1.0f - cosAngle * cosAngle) / (1.0f - tailDir.y * tailDir.y));
+                tailDir.x *= ratio;
+                tailDir.z *= ratio;
+
+                // y要素を、jointに設定されたangleの余弦とする
+                tailDir.y = cosAngle;
+            }
+
+            return tailDir;
+        }
+    }
+}

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 780b79f298faea74ba4a123039a3faeb
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs

@@ -0,0 +1,32 @@
+using Unity.Mathematics;
+
+namespace UniGLTF.SpringBoneJobs
+{
+    public static class AnglelimitHinge
+    {
+        /// <param name="src">AngleLimit空間の方向ベクトル</param>
+        /// <param name="angleLimit">radius</param>
+        /// <returns>AngleLimit空間の方向ベクトル</returns>
+        public static float3 Apply(in float3 src, float limitAngle)
+        {
+            // x要素を0にし、正規化する
+            float3 tailDir = src;
+            tailDir.x = 0.0f;
+            tailDir = math.normalize(tailDir);
+
+            // tailDirのy要素をjointに設定されたangleの余弦と比較する
+            var cosAngle = math.cos(limitAngle);
+            if (tailDir.y < cosAngle)
+            {
+                // z要素を、tailDirの正弦とjointに設定されたangleの正弦の比を用いてスケールする
+                var ratio = math.sqrt((1.0f - cosAngle * cosAngle) / (1.0f - tailDir.y * tailDir.y));
+                tailDir.z *= ratio;
+
+                // y要素を、jointに設定されたangleの余弦とする
+                tailDir.y = cosAngle;
+            }
+
+            return tailDir;
+        }
+    }
+}

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: bb1e1bbfc1a339c40b5df4d846f09da4
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs

@@ -0,0 +1,37 @@
+using Unity.Mathematics;
+
+namespace UniGLTF.SpringBoneJobs
+{
+    public static class AnglelimitSpherical
+    {
+        /// <param name="tailDir">AngleLimit空間の方向ベクトル</param>
+        /// <param name="limitAnglePhi">radius</param>
+        /// <param name="limitAngleTheta">radius</param>
+        /// <returns>AngleLimit空間の方向ベクトル</returns>
+        public static float3 Apply(in float3 tailDir, float limitAnglePhi, float limitAngleTheta)
+        {
+            // tailDirのphi・thetaを計算する
+            var phi = math.atan2(tailDir.z, tailDir.y);
+            var theta = math.asin(tailDir.x);
+
+            // phi・thetaをjointに設定されたphi・thetaを用いて制限する
+            // var isLimited = false;
+            if (math.abs(phi) > limitAnglePhi)
+            {
+                // isLimited = true;
+                phi = limitAnglePhi * math.sign(phi);
+            }
+
+            // thetaをjointに設定されたthetaを用いて制限する
+            if (math.abs(theta) > limitAngleTheta)
+            {
+                //   isLimited = true;
+                theta = limitAngleTheta * math.sign(theta);
+            }
+
+            // tailDirをphi・thetaを用いて再計算する
+            var cos_theta = math.cos(theta);
+            return new float3(math.sin(theta), cos_theta * math.cos(phi), cos_theta * math.sin(phi));
+        }
+    }
+}

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: cc767b94391a0c44ebd21390c57a869e
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitTypes.cs

@@ -0,0 +1,11 @@
+
+namespace UniGLTF.SpringBoneJobs
+{
+    public enum AnglelimitTypes
+    {
+        None,
+        Cone,
+        Hinge,
+        Spherical,
+    }
+}

Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitTypes.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 56836f5a87c46a641a0ffc048c10655f
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableJointMutable.cs

@@ -12,23 +12,48 @@ namespace UniGLTF.SpringBoneJobs.Blittables
     [Serializable]
     public readonly struct BlittableJointMutable
     {
-        private readonly float4x2 _data;
-        
+        /// <summary>
+        /// |x       |y       |z       |w       |
+        /// |--------|--------|--------|--------|
+        /// |stiff   |g-power |drag    |radius  |
+        /// |g-dir.x |g-dir.y |g-dir.z |        |
+        /// |al.type |limit1  |limit2  |  
+        /// |offset.x|offset.y|offset.z|offset.w|
+        /// 
+        /// g-power = gravityPower
+        /// g-dir = gravityDir
+        /// al = anglelimit
+        /// offset = anglelimitOffset
+        /// </summary>
+        private readonly float4x4 _data;
+
         public float stiffnessForce => _data.c0.x;
         public float gravityPower => _data.c0.y;
         public float3 gravityDir => _data.c1.xyz;
         public float dragForce => _data.c0.z;
         public float radius => _data.c0.w;
-        
+
+        public AnglelimitTypes anglelimitType => (AnglelimitTypes)_data.c2.x;
+        public float anglelimit1 => _data.c2.y;
+        public float anglelimit2 => _data.c2.z;
+        public quaternion anglelimitOffset => _data.c3.xyzw;
+
         public BlittableJointMutable(float stiffnessForce = 0,
             float gravityPower = 0,
             float3 gravityDir = default,
             float dragForce = 0,
-            float radius = 0)
+            float radius = 0,
+            // anglelimit
+            float alType = 0,
+            float limit1 = 0,
+            float limit2 = 0,
+            quaternion offset = default)
         {
             var c0 = new float4(stiffnessForce, gravityPower, dragForce, radius);
             var c1 = new float4(gravityDir, 0);
-            _data = new float4x2(c0, c1);
+            var c2 = new float4(alType, limit1, limit2, 0);
+            var c3 = offset.value;
+            _data = new float4x4(c0, c1, c2, c3);
         }
     }
 }

Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs

@@ -0,0 +1,206 @@
+using UniGLTF.Runtime.Utils;
+using UniGLTF.SpringBoneJobs.Blittables;
+using Unity.Mathematics;
+
+namespace UniGLTF.SpringBoneJobs
+{
+    public static class SpringBoneCollision
+    {
+        public static bool TryCollide(
+            in BlittableJointImmutable logic, in BlittableJointMutable joint, in BlittableTransform headTransform,
+            in BlittableCollider collider, in BlittableTransform colliderTransform, float maxColliderScale,
+            in float3 colliderWorldTail, in float3 colliderWorldPosition,
+            in float3 nextTail, out float3 newNextTail)
+        {
+            switch (collider.colliderType)
+            {
+                case BlittableColliderType.Sphere:
+                    return TryResolveSphereCollision(joint, collider, colliderWorldPosition, headTransform, maxColliderScale, logic, nextTail, out newNextTail);
+
+                case BlittableColliderType.Capsule:
+                    return TryResolveCapsuleCollision(colliderWorldTail, colliderWorldPosition, headTransform, joint, collider, maxColliderScale, logic, nextTail, out newNextTail);
+
+                case BlittableColliderType.Plane:
+                    return TryResolvePlaneCollision(joint, collider, colliderTransform, nextTail, out newNextTail);
+
+                case BlittableColliderType.SphereInside:
+                    return TryResolveSphereCollisionInside(joint, collider, colliderTransform, nextTail, out newNextTail);
+
+                case BlittableColliderType.CapsuleInside:
+                    return TryResolveCapsuleCollisionInside(joint, collider, colliderTransform, nextTail, out newNextTail);
+
+                default:
+                    throw new System.NotImplementedException();
+            }
+        }
+
+        private static bool TryResolveSphereCollision(
+            in BlittableJointMutable joint,
+            in BlittableCollider collider,
+            in float3 worldPosition,
+            in BlittableTransform headTransform,
+            in float maxColliderScale,
+            in BlittableJointImmutable logic,
+            in float3 nextTail, out float3 newNextTail)
+        {
+            var r = joint.radius + collider.radius * maxColliderScale;
+            if (math.lengthsq(nextTail - worldPosition) <= (r * r))
+            {
+                // ヒット。Colliderの半径方向に押し出す
+                var normal = math.normalize(nextTail - worldPosition);
+                var posFromCollider = worldPosition + normal * r;
+                // 長さをboneLengthに強制
+                newNextTail = headTransform.position + math.normalize(posFromCollider - headTransform.position) * logic.length;
+                return true;
+            }
+            else
+            {
+                newNextTail = default;
+                return false;
+            }
+        }
+
+        private static bool TryResolveCapsuleCollision(
+            float3 worldTail,
+            float3 worldPosition,
+            BlittableTransform headTransform,
+            BlittableJointMutable joint,
+            BlittableCollider collider,
+            float maxColliderScale,
+            BlittableJointImmutable logic,
+            in float3 nextTail, out float3 newNextTail)
+        {
+            var direction = worldTail - worldPosition;
+            if (math.lengthsq(direction) == 0)
+            {
+                // head側半球の球判定
+                return TryResolveSphereCollision(joint, collider, worldPosition, headTransform, maxColliderScale, logic, nextTail, out newNextTail);
+            }
+            var P = math.normalize(direction);
+            var Q = headTransform.position - worldPosition;
+            var dot = math.dot(P, Q);
+            if (dot <= 0)
+            {
+                // head側半球の球判定
+                return TryResolveSphereCollision(joint, collider, worldPosition, headTransform, maxColliderScale, logic, nextTail, out newNextTail);
+            }
+            if (dot >= math.length(direction))
+            {
+                // tail側半球の球判定
+                return TryResolveSphereCollision(joint, collider, worldTail, headTransform, maxColliderScale, logic, nextTail, out newNextTail);
+            }
+
+            // head-tail上の m_transform.position との最近点
+            var p = worldPosition + P * dot;
+            return TryResolveSphereCollision(joint, collider, p, headTransform, maxColliderScale, logic, nextTail, out newNextTail);
+        }
+
+        /// <summary>
+        /// Collision with SpringJoint and PlaneCollider.
+        /// If collide update nextTail.
+        /// </summary>
+        /// <param name="joint">joint</param>
+        /// <param name="collider">collider</param>
+        /// <param name="colliderTransform">colliderTransform.localToWorldMatrix.MultiplyPoint3x4(collider.offset);</param>
+        /// <param name="nextTail">result of verlet integration</param>
+        private static bool TryResolvePlaneCollision(
+            in BlittableJointMutable joint,
+            in BlittableCollider collider,
+            in BlittableTransform colliderTransform,
+            in float3 nextTail, out float3 newNextTail)
+        {
+            var transformedOffset = MathHelper.MultiplyPoint(colliderTransform.localToWorldMatrix, collider.offset);
+            var transformedNormal = math.normalize(MathHelper.MultiplyVector(colliderTransform.localToWorldMatrix, collider.tailOrNormal));
+            var delta = nextTail - transformedOffset;
+
+            // ジョイントとコライダーの距離。負の値は衝突していることを示す
+            var distance = math.dot(delta, transformedNormal) - joint.radius;
+
+            if (distance < 0)
+            {
+                // ジョイントとコライダーの距離の方向。衝突している場合、この方向にジョイントを押し出す
+                var direction = transformedNormal;
+                newNextTail = nextTail - direction * distance;
+                return true;
+            }
+            else
+            {
+                newNextTail = default;
+                return false;
+            }
+        }
+
+        private static bool TryResolveSphereCollisionInside(
+            in BlittableJointMutable joint,
+            in BlittableCollider collider,
+            in BlittableTransform colliderTransform,
+            in float3 nextTail, out float3 newNextTail)
+        {
+            var transformedOffset = MathHelper.MultiplyPoint(colliderTransform.localToWorldMatrix, collider.offset);
+            var delta = nextTail - transformedOffset;
+
+            // ジョイントとコライダーの距離。負の値は衝突していることを示す
+            var distance = collider.radius - joint.radius - math.length(delta);
+
+            // ジョイントとコライダーの距離の方向。衝突している場合、この方向にジョイントを押し出す
+            if (distance < 0)
+            {
+                var direction = -1 * math.normalize(delta);
+                newNextTail = nextTail - direction * distance;
+                return true;
+            }
+            else
+            {
+                newNextTail = default;
+                return false;
+            }
+        }
+
+        private static bool TryResolveCapsuleCollisionInside(
+            in BlittableJointMutable joint,
+            in BlittableCollider collider,
+            in BlittableTransform colliderTransform,
+            in float3 nextTail, out float3 newNextTail)
+        {
+            var transformedOffset = MathHelper.MultiplyPoint(colliderTransform.localToWorldMatrix, collider.offset);
+            var transformedTail = MathHelper.MultiplyPoint(colliderTransform.localToWorldMatrix, collider.tailOrNormal);
+            var offsetToTail = transformedTail - transformedOffset;
+            var lengthSqCapsule = math.lengthsq(offsetToTail);
+
+            var delta = nextTail - transformedOffset;
+            var dot = math.dot(offsetToTail, delta);
+
+            if (dot < 0.0)
+            {
+                // ジョイントがカプセルの始点側にある場合
+                // なにもしない
+            }
+            else if (dot > lengthSqCapsule)
+            {
+                // ジョイントがカプセルの終点側にある場合
+                delta -= offsetToTail;
+            }
+            else
+            {
+                // ジョイントがカプセルの始点と終点の間にある場合
+                delta -= offsetToTail * (dot / lengthSqCapsule);
+            }
+
+            // ジョイントとコライダーの距離。負の値は衝突していることを示す
+            var distance = collider.radius - joint.radius - math.length(delta);
+
+            // ジョイントとコライダーの距離の方向。衝突している場合、この方向にジョイントを押し出す
+            if (distance < 0)
+            {
+                var direction = -1 * math.normalize(delta);
+                newNextTail = nextTail - direction * distance;
+                return true;
+            }
+            else
+            {
+                newNextTail = default;
+                return false;
+            }
+        }
+    }
+}

Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs.meta

@@ -0,0 +1,11 @@
+fileFormatVersion: 2
+guid: 400f2bc818ed9ed44ac8b852cbd9dcc5
+MonoImporter:
+  externalObjects: {}
+  serializedVersion: 2
+  defaultReferences: []
+  executionOrder: 0
+  icon: {instanceID: 0}
+  userData: 
+  assetBundleName: 
+  assetBundleVariant: 

Assets/UniGLTF/Runtime/SpringBoneJobs/UpdateFastSpringBoneJob.cs

@@ -94,6 +94,8 @@ namespace UniGLTF.SpringBoneJobs
                 // 長さをboneLengthに強制
                 nextTail = headTransform.position + math.normalize(nextTail - headTransform.position) * logic.length;
 
+                nextTail = Anglelimit.Apply(logic, joint, parentRotation, head: headTransform.position, nextTail: nextTail);
+
                 // Collisionで移動
                 for (var colliderIndex = colliderSpan.startIndex; colliderIndex < colliderSpan.startIndex + colliderSpan.count; ++colliderIndex)
                 {
@@ -104,30 +106,15 @@ namespace UniGLTF.SpringBoneJobs
                     var worldPosition = MathHelper.MultiplyPoint3x4(colliderTransform.localToWorldMatrix, collider.offset);
                     var worldTail = MathHelper.MultiplyPoint3x4(colliderTransform.localToWorldMatrix, collider.tailOrNormal);
 
-                    switch (collider.colliderType)
+                    if (SpringBoneCollision.TryCollide(logic, joint,
+                        headTransform,
+                        collider, colliderTransform, maxColliderScale,
+                        colliderWorldTail: worldTail, colliderWorldPosition: worldPosition,
+                        nextTail: nextTail,
+                        out var newNextTail))
                     {
-                        case BlittableColliderType.Sphere:
-                            ResolveSphereCollision(joint, collider, worldPosition, headTransform, maxColliderScale, logic, ref nextTail);
-                            break;
-
-                        case BlittableColliderType.Capsule:
-                            ResolveCapsuleCollision(worldTail, worldPosition, headTransform, joint, collider, maxColliderScale, logic, ref nextTail);
-                            break;
-
-                        case BlittableColliderType.Plane:
-                            ResolvePlaneCollision(joint, collider, colliderTransform, ref nextTail);
-                            break;
-
-                        case BlittableColliderType.SphereInside:
-                            ResolveSphereCollisionInside(joint, collider, colliderTransform, ref nextTail);
-                            break;
-
-                        case BlittableColliderType.CapsuleInside:
-                            ResolveCapsuleCollisionInside(joint, collider, colliderTransform, ref nextTail);
-                            break;
-
-                        default:
-                            throw new NotImplementedException();
+                        // 衝突毎に nextTail を更新する。
+                        nextTail = Anglelimit.Apply(logic, joint, parentRotation, head: headTransform.position, nextTail: newNextTail);
                     }
                 }
 
@@ -154,152 +141,5 @@ namespace UniGLTF.SpringBoneJobs
             }
         }
 
-        private static void ResolveCapsuleCollision(
-            float3 worldTail,
-            float3 worldPosition,
-            BlittableTransform headTransform,
-            BlittableJointMutable joint,
-            BlittableCollider collider,
-            float maxColliderScale,
-            BlittableJointImmutable logic,
-            ref float3 nextTail)
-        {
-            var direction = worldTail - worldPosition;
-            if (math.lengthsq(direction) == 0)
-            {
-                // head側半球の球判定
-                ResolveSphereCollision(joint, collider, worldPosition, headTransform, maxColliderScale, logic, ref nextTail);
-                return;
-            }
-            var P = math.normalize(direction);
-            var Q = headTransform.position - worldPosition;
-            var dot = math.dot(P, Q);
-            if (dot <= 0)
-            {
-                // head側半球の球判定
-                ResolveSphereCollision(joint, collider, worldPosition, headTransform, maxColliderScale, logic, ref nextTail);
-                return;
-            }
-            if (dot >= math.length(direction))
-            {
-                // tail側半球の球判定
-                ResolveSphereCollision(joint, collider, worldTail, headTransform, maxColliderScale, logic, ref nextTail);
-                return;
-            }
-
-            // head-tail上の m_transform.position との最近点
-            var p = worldPosition + P * dot;
-            ResolveSphereCollision(joint, collider, p, headTransform, maxColliderScale, logic, ref nextTail);
-        }
-
-        private static void ResolveSphereCollision(
-            in BlittableJointMutable joint,
-            in BlittableCollider collider,
-            in float3 worldPosition,
-            in BlittableTransform headTransform,
-            in float maxColliderScale,
-            in BlittableJointImmutable logic,
-            ref float3 nextTail)
-        {
-            var r = joint.radius + collider.radius * maxColliderScale;
-            if (math.lengthsq(nextTail - worldPosition) <= (r * r))
-            {
-                // ヒット。Colliderの半径方向に押し出す
-                var normal = math.normalize(nextTail - worldPosition);
-                var posFromCollider = worldPosition + normal * r;
-                // 長さをboneLengthに強制
-                nextTail = headTransform.position + math.normalize(posFromCollider - headTransform.position) * logic.length;
-            }
-        }
-
-        private static void ResolveSphereCollisionInside(
-            in BlittableJointMutable joint,
-            in BlittableCollider collider,
-            in BlittableTransform colliderTransform,
-            ref float3 nextTail)
-        {
-            var transformedOffset = MathHelper.MultiplyPoint(colliderTransform.localToWorldMatrix, collider.offset);
-            var delta = nextTail - transformedOffset;
-
-            // ジョイントとコライダーの距離。負の値は衝突していることを示す
-            var distance = collider.radius - joint.radius - math.length(delta);
-
-            // ジョイントとコライダーの距離の方向。衝突している場合、この方向にジョイントを押し出す
-            if (distance < 0)
-            {
-                var direction = -1 * math.normalize(delta);
-                nextTail -= direction * distance;
-            }
-        }
-
-        private static void ResolveCapsuleCollisionInside(
-            in BlittableJointMutable joint,
-            in BlittableCollider collider,
-            in BlittableTransform colliderTransform,
-            ref float3 nextTail)
-        {
-            var transformedOffset = MathHelper.MultiplyPoint(colliderTransform.localToWorldMatrix, collider.offset);
-            var transformedTail = MathHelper.MultiplyPoint(colliderTransform.localToWorldMatrix, collider.tailOrNormal);
-            var offsetToTail = transformedTail - transformedOffset;
-            var lengthSqCapsule = math.lengthsq(offsetToTail);
-
-            var delta = nextTail - transformedOffset;
-            var dot = math.dot(offsetToTail, delta);
-
-            if (dot < 0.0)
-            {
-                // ジョイントがカプセルの始点側にある場合
-                // なにもしない
-            }
-            else if (dot > lengthSqCapsule)
-            {
-                // ジョイントがカプセルの終点側にある場合
-                delta -= offsetToTail;
-            }
-            else
-            {
-                // ジョイントがカプセルの始点と終点の間にある場合
-                delta -= offsetToTail * (dot / lengthSqCapsule);
-            }
-
-            // ジョイントとコライダーの距離。負の値は衝突していることを示す
-            var distance = collider.radius - joint.radius - math.length(delta);
-
-            // ジョイントとコライダーの距離の方向。衝突している場合、この方向にジョイントを押し出す
-            if (distance < 0)
-            {
-                var direction = -1 * math.normalize(delta);
-                nextTail -= direction * distance;
-            }
-        }
-
-        /// <summary>
-        /// Collision with SpringJoint and PlaneCollider.
-        /// If collide update nextTail.
-        /// </summary>
-        /// <param name="joint">joint</param>
-        /// <param name="collider">collier</param>
-        /// <param name="colliderTransform">colliderTransform.localToWorldMatrix.MultiplyPoint3x4(collider.offset);</param>
-        /// <param name="nextTail">result of verlet integration</param>
-        private static void ResolvePlaneCollision(
-            in BlittableJointMutable joint,
-            in BlittableCollider collider,
-            in BlittableTransform colliderTransform,
-            ref float3 nextTail)
-        {
-            var transformedOffset = MathHelper.MultiplyPoint(colliderTransform.localToWorldMatrix, collider.offset);
-            var transformedNormal = math.normalize(MathHelper.MultiplyVector(colliderTransform.localToWorldMatrix, collider.tailOrNormal));
-            var delta = nextTail - transformedOffset;
-
-            // ジョイントとコライダーの距離。負の値は衝突していることを示す
-            var distance = math.dot(delta, transformedNormal) - joint.radius;
-
-            if (distance < 0)
-            {
-                // ジョイントとコライダーの距離の方向。衝突している場合、この方向にジョイントを押し出す
-                var direction = transformedNormal;
-                nextTail -= direction * distance;
-            }
-        }
     }
 }