diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit.meta b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit.meta new file mode 100644 index 000000000..7185a9b0e --- /dev/null +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit.meta @@ -0,0 +1,8 @@ +fileFormatVersion: 2 +guid: b09ae318bccbfce4993fafb418e1e8c7 +folderAsset: yes +DefaultImporter: + externalObjects: {} + userData: + assetBundleName: + assetBundleVariant: diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs new file mode 100644 index 000000000..b91e3bd77 --- /dev/null +++ b/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}"); + } + } + + /// nextTail(position vector in world space) + /// tailDir(directionay vector in angle space) + /// + 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)) + ); + } + + } +} \ No newline at end of file diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs.meta b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs.meta new file mode 100644 index 000000000..f50304933 --- /dev/null +++ b/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: diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs new file mode 100644 index 000000000..d8c159f86 --- /dev/null +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs @@ -0,0 +1,33 @@ +using Unity.Mathematics; + +namespace UniGLTF.SpringBoneJobs +{ + public static class AnglelimitCone + { + /// AngleLimit空間の方向ベクトル + /// radius + /// AngleLimit空間の方向ベクトル + 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; + } + } +} \ No newline at end of file diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs.meta b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs.meta new file mode 100644 index 000000000..ce8cc27a8 --- /dev/null +++ b/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: diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs new file mode 100644 index 000000000..571984689 --- /dev/null +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs @@ -0,0 +1,32 @@ +using Unity.Mathematics; + +namespace UniGLTF.SpringBoneJobs +{ + public static class AnglelimitHinge + { + /// AngleLimit空間の方向ベクトル + /// radius + /// AngleLimit空間の方向ベクトル + 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; + } + } +} \ No newline at end of file diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs.meta b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs.meta new file mode 100644 index 000000000..b385eae67 --- /dev/null +++ b/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: diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs new file mode 100644 index 000000000..b15ce10fa --- /dev/null +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs @@ -0,0 +1,37 @@ +using Unity.Mathematics; + +namespace UniGLTF.SpringBoneJobs +{ + public static class AnglelimitSpherical + { + /// AngleLimit空間の方向ベクトル + /// radius + /// radius + /// AngleLimit空間の方向ベクトル + 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)); + } + } +} \ No newline at end of file diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs.meta b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs.meta new file mode 100644 index 000000000..c0439fd3e --- /dev/null +++ b/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: diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitTypes.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitTypes.cs new file mode 100644 index 000000000..656ef327d --- /dev/null +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitTypes.cs @@ -0,0 +1,11 @@ + +namespace UniGLTF.SpringBoneJobs +{ + public enum AnglelimitTypes + { + None, + Cone, + Hinge, + Spherical, + } +} \ No newline at end of file diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitTypes.cs.meta b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitTypes.cs.meta new file mode 100644 index 000000000..839768b66 --- /dev/null +++ b/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: diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableJointMutable.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableJointMutable.cs index 96b48fa3a..d6566a659 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableJointMutable.cs +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableJointMutable.cs @@ -12,23 +12,48 @@ namespace UniGLTF.SpringBoneJobs.Blittables [Serializable] public readonly struct BlittableJointMutable { - private readonly float4x2 _data; - + /// + /// |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 + /// + 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); } } } \ No newline at end of file diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs new file mode 100644 index 000000000..a8f5fc3b4 --- /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 + /// collider + /// 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 3235b8964..2cec59241 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/UpdateFastSpringBoneJob.cs +++ b/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; - } - } - - /// - /// 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