From d08e239541e98217512101b074849f5cb4a2b3a0 Mon Sep 17 00:00:00 2001 From: ousttrue Date: Thu, 17 Jul 2025 22:16:36 +0900 Subject: [PATCH 1/7] add angle limit data BlittableJointMutable --- .../Runtime/SpringBoneJobs/Anglelimit.meta | 8 +++++ .../Anglelimit/AnglelimitCone.cs | 33 +++++++++++++++++ .../Anglelimit/AnglelimitCone.cs.meta | 11 ++++++ .../Anglelimit/AnglelimitHinge.cs | 32 +++++++++++++++++ .../Anglelimit/AnglelimitHinge.cs.meta | 11 ++++++ .../Anglelimit/AnglelimitSpace.cs | 36 +++++++++++++++++++ .../Anglelimit/AnglelimitSpace.cs.meta | 11 ++++++ .../Anglelimit/AnglelimitSpherical.cs | 36 +++++++++++++++++++ .../Anglelimit/AnglelimitSpherical.cs.meta | 11 ++++++ .../Anglelimit/AnglelimitTypes.cs | 11 ++++++ .../Anglelimit/AnglelimitTypes.cs.meta | 11 ++++++ .../Blittables/BlittableJointMutable.cs | 35 +++++++++++++++--- 12 files changed, 241 insertions(+), 5 deletions(-) create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit.meta create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs.meta create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitHinge.cs.meta create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs.meta create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs.meta create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitTypes.cs create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitTypes.cs.meta 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/AnglelimitCone.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs new file mode 100644 index 000000000..d335044e3 --- /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 src; + } + } +} \ 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/AnglelimitSpace.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs new file mode 100644 index 000000000..1d9e06008 --- /dev/null +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs @@ -0,0 +1,36 @@ +using Unity.Mathematics; + +namespace UniGLTF.SpringBoneJobs +{ + public static class AnglelimitSpace + { + public static float3 ToTailDir(in quaternion src) + { + // // Y+方向からjointのheadからtailに向かうベクトルへの最小回転 + // let axisRotation = fromToQuaternion(vec3(0, 1, 0), boneAxis); + + // // limitのローカル空間をワールド空間に写像する回転 + // let rotation = + // joint.parent.worldRotation * + // joint.localSpaceInitialRotation * + // axisRotation * + // joint.limit.rotation; + + // // tailの向きをlimitのローカル空間に写像する + // tailDir = tailDir.applyQuaternion(rotation.inverse); + + // // limitを適用する + // joint.limit.apply(tailDir); + + // // tailの向きをワールド空間に戻す + // tailDir = tailDir.applyQuaternion(rotation); + + throw new System.NotImplementedException(); + } + + public static quaternion FromTailDir(in float3 src) + { + throw new System.NotImplementedException(); + } + } +} \ No newline at end of file diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs.meta b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs.meta new file mode 100644 index 000000000..99e27a255 --- /dev/null +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs.meta @@ -0,0 +1,11 @@ +fileFormatVersion: 2 +guid: e38e2998df511664cb0d6e582e6a05a3 +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..e6d07be63 --- /dev/null +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs @@ -0,0 +1,36 @@ +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を用いて再計算する + return new float3(math.sin(theta), math.cos(theta) * math.cos(phi), math.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 From 011664097d40e430e7b514863a9c7a2c9a33a411 Mon Sep 17 00:00:00 2001 From: ousttrue Date: Thu, 17 Jul 2025 22:52:08 +0900 Subject: [PATCH 2/7] impl Anglelimit --- .../SpringBoneJobs/Anglelimit/Anglelimit.cs | 84 +++++++++++++++++++ ...elimitSpace.cs.meta => Anglelimit.cs.meta} | 2 +- .../Anglelimit/AnglelimitSpace.cs | 36 -------- .../SpringBoneJobs/UpdateFastSpringBoneJob.cs | 4 + 4 files changed, 89 insertions(+), 37 deletions(-) create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs rename Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/{AnglelimitSpace.cs.meta => Anglelimit.cs.meta} (83%) delete mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs new file mode 100644 index 000000000..353a8547b --- /dev/null +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs @@ -0,0 +1,84 @@ +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) + { + return quaternion.AxisAngle( + angle: math.acos( + math.clamp( + math.dot( + math.normalize(from), + math.normalize(to) + ), + -1f, + 1f) + ), + axis: math.normalize(math.cross(from, to)) + ); + } + + } +} \ No newline at end of file diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs.meta b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs.meta similarity index 83% rename from Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs.meta rename to Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs.meta index 99e27a255..f50304933 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs.meta +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs.meta @@ -1,5 +1,5 @@ fileFormatVersion: 2 -guid: e38e2998df511664cb0d6e582e6a05a3 +guid: 3c2f98858052fbe4c89b445bad4c979b MonoImporter: externalObjects: {} serializedVersion: 2 diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs deleted file mode 100644 index 1d9e06008..000000000 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpace.cs +++ /dev/null @@ -1,36 +0,0 @@ -using Unity.Mathematics; - -namespace UniGLTF.SpringBoneJobs -{ - public static class AnglelimitSpace - { - public static float3 ToTailDir(in quaternion src) - { - // // Y+方向からjointのheadからtailに向かうベクトルへの最小回転 - // let axisRotation = fromToQuaternion(vec3(0, 1, 0), boneAxis); - - // // limitのローカル空間をワールド空間に写像する回転 - // let rotation = - // joint.parent.worldRotation * - // joint.localSpaceInitialRotation * - // axisRotation * - // joint.limit.rotation; - - // // tailの向きをlimitのローカル空間に写像する - // tailDir = tailDir.applyQuaternion(rotation.inverse); - - // // limitを適用する - // joint.limit.apply(tailDir); - - // // tailの向きをワールド空間に戻す - // tailDir = tailDir.applyQuaternion(rotation); - - throw new System.NotImplementedException(); - } - - public static quaternion FromTailDir(in float3 src) - { - throw new System.NotImplementedException(); - } - } -} \ No newline at end of file diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/UpdateFastSpringBoneJob.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/UpdateFastSpringBoneJob.cs index 3235b8964..139009d6c 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) { @@ -131,6 +133,8 @@ namespace UniGLTF.SpringBoneJobs } } + nextTail = Anglelimit.Apply(logic, joint, parentRotation, head: headTransform.position, nextTail: nextTail); + NextTail[logicIndex] = centerTransform.HasValue ? MathHelper.MultiplyPoint3x4(centerTransform.Value.worldToLocalMatrix, nextTail) : nextTail; From 6b71582ea631425623d4301ac40feeb449d17897 Mon Sep 17 00:00:00 2001 From: ousttrue Date: Fri, 18 Jul 2025 05:00:14 +0900 Subject: [PATCH 3/7] =?UTF-8?q?=E3=81=99=E3=81=B9=E3=81=A6=E3=81=AE?= =?UTF-8?q?=E8=A1=9D=E7=AA=81=E5=BE=8C=E3=81=A7=E3=81=AF=E3=81=AA=E3=81=8F?= =?UTF-8?q?=E3=80=81=E8=A1=9D=E7=AA=81=E6=AF=8E=E3=81=AB=E8=A7=92=E5=BA=A6?= =?UTF-8?q?=E5=88=B6=E9=99=90=E3=81=99=E3=82=8B=E5=BF=85=E8=A6=81=E3=81=82?= =?UTF-8?q?=E3=82=8A?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../SpringBoneJobs/SpringBoneCollision.cs | 206 ++++++++++++++++++ .../SpringBoneCollision.cs.meta | 11 + .../SpringBoneJobs/UpdateFastSpringBoneJob.cs | 180 +-------------- 3 files changed, 225 insertions(+), 172 deletions(-) create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs create mode 100644 Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs.meta 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 From 08f22b917c9fb603b3e0c8aef1de48fdc5c5728a Mon Sep 17 00:00:00 2001 From: ousttrue Date: Fri, 18 Jul 2025 17:17:30 +0900 Subject: [PATCH 4/7] return tailDir --- .../UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs index d335044e3..d8c159f86 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitCone.cs @@ -27,7 +27,7 @@ namespace UniGLTF.SpringBoneJobs tailDir.y = cosAngle; } - return src; + return tailDir; } } } \ No newline at end of file From df49cc5e1eec60cb62148e9f9653eb153c860bff Mon Sep 17 00:00:00 2001 From: ousttrue Date: Fri, 18 Jul 2025 17:27:48 +0900 Subject: [PATCH 5/7] Update Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- .../SpringBoneJobs/Anglelimit/Anglelimit.cs | 27 ++++++++++++------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs index 353a8547b..b91e3bd77 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/Anglelimit.cs @@ -66,17 +66,24 @@ namespace UniGLTF.SpringBoneJobs // 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( - math.dot( - math.normalize(from), - math.normalize(to) - ), - -1f, - 1f) - ), - axis: math.normalize(math.cross(from, to)) + angle: math.acos(math.clamp(dot, -1f, 1f)), + axis: math.normalize(math.cross(fromNorm, toNorm)) ); } From 8020b1ccadcb5a2c32a374204b4d83d6670d9d86 Mon Sep 17 00:00:00 2001 From: ousttrue Date: Fri, 18 Jul 2025 17:28:06 +0900 Subject: [PATCH 6/7] Update Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs index a8eeed670..a8f5fc3b4 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/SpringBoneCollision.cs @@ -100,7 +100,7 @@ namespace UniGLTF.SpringBoneJobs /// If collide update nextTail. /// /// joint - /// collier + /// collider /// colliderTransform.localToWorldMatrix.MultiplyPoint3x4(collider.offset); /// result of verlet integration private static bool TryResolvePlaneCollision( From 9dddb6517bdcce28c8a13cc6002a463b4d4589cb Mon Sep 17 00:00:00 2001 From: ousttrue Date: Fri, 18 Jul 2025 17:32:21 +0900 Subject: [PATCH 7/7] cos_theta --- .../Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs index e6d07be63..b15ce10fa 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Anglelimit/AnglelimitSpherical.cs @@ -30,7 +30,8 @@ namespace UniGLTF.SpringBoneJobs } // tailDirをphi・thetaを用いて再計算する - return new float3(math.sin(theta), math.cos(theta) * math.cos(phi), math.cos(theta) * math.sin(phi)); + 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