すべての衝突後ではなく、衝突毎に角度制限する必要あり

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">collier</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

@@ -106,35 +106,18 @@ 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);
                     }
                 }
 
-                nextTail = Anglelimit.Apply(logic, joint, parentRotation, head: headTransform.position, nextTail: nextTail);
-
                 NextTail[logicIndex] = centerTransform.HasValue
                     ? MathHelper.MultiplyPoint3x4(centerTransform.Value.worldToLocalMatrix, nextTail)
                     : nextTail;
@@ -158,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;
-            }
-        }
     }
 }