diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs
new file mode 100644
index 000000000..a8eeed670
--- /dev/null
+++ b/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);
+ }
+
+ ///
+ /// Collision with SpringJoint and PlaneCollider.
+ /// If collide update nextTail.
+ ///
+ /// joint
+ /// collier
+ /// colliderTransform.localToWorldMatrix.MultiplyPoint3x4(collider.offset);
+ /// result of verlet integration
+ 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;
+ }
+ }
+ }
+}
\ No newline at end of file
diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs.meta b/Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs.meta
new file mode 100644
index 000000000..e52fa675b
--- /dev/null
+++ b/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:
diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/UpdateFastSpringBoneJob.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/UpdateFastSpringBoneJob.cs
index 139009d6c..2cec59241 100644
--- a/Assets/UniGLTF/Runtime/SpringBoneJobs/UpdateFastSpringBoneJob.cs
+++ b/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;
- }
- }
-
- ///
- /// Collision with SpringJoint and PlaneCollider.
- /// If collide update nextTail.
- ///
- /// joint
- /// collier
- /// colliderTransform.localToWorldMatrix.MultiplyPoint3x4(collider.offset);
- /// result of verlet integration
- 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;
- }
- }
}
}
\ No newline at end of file