From 022e23b6d519b99b8add8f13902f46d9fe0e49e5 Mon Sep 17 00:00:00 2001 From: AoiKamishiro Date: Mon, 30 Jun 2025 02:07:41 +0900 Subject: [PATCH] =?UTF-8?q?BlittableTransform=E3=81=AE=E6=9C=80=E9=81=A9?= =?UTF-8?q?=E5=8C=96?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../Blittables/BlittableJointMutable.cs | 2 +- .../Blittables/BlittableModelLevel.cs | 2 +- .../Blittables/BlittableTransform.cs | 129 +++++++++++++- .../FastSpringBoneConbinedBuffer.cs | 27 +-- .../InputPorts/FastSpringBoneBuffer.cs | 27 +-- .../SpringBoneJobs/PullTransformJob.cs | 20 +-- .../SpringBoneJobs/UpdateFastSpringBoneJob.cs | 163 ++++++++++-------- 7 files changed, 249 insertions(+), 121 deletions(-) diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableJointMutable.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableJointMutable.cs index 02e728a14..7df912d18 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableJointMutable.cs +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableJointMutable.cs @@ -17,7 +17,7 @@ namespace UniGLTF.SpringBoneJobs.Blittables public float stiffnessForce => _data.c0.x; public float gravityPower => _data.c0.y; - public Vector3 gravityDir => _data.c1.xyz; + public float3 gravityDir => _data.c1.xyz; public float dragForce => _data.c0.z; public float radius => _data.c0.w; diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableModelLevel.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableModelLevel.cs index 9bb4582e4..769f72818 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableModelLevel.cs +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableModelLevel.cs @@ -10,7 +10,7 @@ namespace UniGLTF.SpringBoneJobs.Blittables /// /// World 座標系の追加の力。風など。 /// - public Vector3 ExternalForce => _data.xyz; + public float3 ExternalForce => _data.xyz; /// /// 処理結果の Transform への書き戻しを停止する。 diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableTransform.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableTransform.cs index 786aa0bb2..7e6fac0e2 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableTransform.cs +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/Blittables/BlittableTransform.cs @@ -1,5 +1,5 @@ using System; -using UnityEngine; +using Unity.Mathematics; namespace UniGLTF.SpringBoneJobs.Blittables { @@ -7,14 +7,125 @@ namespace UniGLTF.SpringBoneJobs.Blittables /// Transformの必要な機能だけを絞り、Blittableに対応させたクラス /// [Serializable] - public struct BlittableTransform + public readonly struct BlittableTransform { - public Vector3 position; - public Quaternion rotation; - public Vector3 localPosition; - public Quaternion localRotation; - public Vector3 localScale; - public Matrix4x4 localToWorldMatrix; - public Matrix4x4 worldToLocalMatrix; + public readonly float3 position; + public readonly quaternion rotation; + public readonly float3 localPosition; + public readonly quaternion localRotation; + public readonly float3 localScale; + public readonly float4x4 localToWorldMatrix; + public readonly float4x4 worldToLocalMatrix; + public float3 lossyScale + { + get + { + float4x4 tr = float4x4.TRS(position, rotation, new float3(1.0f, 1.0f, 1.0f)); + float4x4 m = math.mul(math.inverse(tr), localToWorldMatrix); + return new float3(m.c0.x, m.c1.y, m.c2.z); + } + } + + public BlittableTransform( + float3 position = default, + quaternion rotation = default, + float3 localPosition = default, + quaternion localRotation = default, + float3 localScale = default, + float4x4 localToWorldMatrix = default, + float4x4 worldToLocalMatrix = default) + { + this.position = position; + this.rotation = rotation; + this.localPosition = localPosition; + this.localRotation = localRotation; + this.localScale = localScale; + this.localToWorldMatrix = localToWorldMatrix; + this.worldToLocalMatrix = worldToLocalMatrix; + } + + public BlittableTransform SetPosition(float3 newPosition) + { + return new BlittableTransform( + position: newPosition, + rotation: rotation, + localPosition: localPosition, + localRotation: localRotation, + localScale: localScale, + localToWorldMatrix: localToWorldMatrix, + worldToLocalMatrix: worldToLocalMatrix); + } + + public BlittableTransform SetRotation(quaternion newRotation) + { + return new BlittableTransform( + position: position, + rotation: newRotation, + localPosition: localPosition, + localRotation: localRotation, + localScale: localScale, + localToWorldMatrix: localToWorldMatrix, + worldToLocalMatrix: worldToLocalMatrix); + } + + public BlittableTransform SetLocalPosition(float3 newLocalPosition) + { + return new BlittableTransform( + position: position, + rotation: rotation, + localPosition: newLocalPosition, + localRotation: localRotation, + localScale: localScale, + localToWorldMatrix: localToWorldMatrix, + worldToLocalMatrix: worldToLocalMatrix); + } + + public BlittableTransform SetLocalRotation(quaternion newLocalRotation) + { + return new BlittableTransform( + position: position, + rotation: rotation, + localPosition: localPosition, + localRotation: newLocalRotation, + localScale: localScale, + localToWorldMatrix: localToWorldMatrix, + worldToLocalMatrix: worldToLocalMatrix); + } + + public BlittableTransform SetLocalScale(float3 newLocalScale) + { + return new BlittableTransform( + position: position, + rotation: rotation, + localPosition: localPosition, + localRotation: localRotation, + localScale: newLocalScale, + localToWorldMatrix: localToWorldMatrix, + worldToLocalMatrix: worldToLocalMatrix); + } + + public BlittableTransform SetLocalToWorldMatrix(float4x4 newLocalToWorldMatrix) + { + return new BlittableTransform( + position: position, + rotation: rotation, + localPosition: localPosition, + localRotation: localRotation, + localScale: localScale, + localToWorldMatrix: newLocalToWorldMatrix, + worldToLocalMatrix: math.inverse(newLocalToWorldMatrix)); + } + + public BlittableTransform SetWorldToLocalMatrix(float4x4 newWorldToLocalMatrix) + { + return new BlittableTransform( + position: position, + rotation: rotation, + localPosition: localPosition, + localRotation: localRotation, + localScale: localScale, + localToWorldMatrix: math.inverse(newWorldToLocalMatrix), + worldToLocalMatrix: newWorldToLocalMatrix); + } } } \ No newline at end of file diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/FastSpringBoneConbinedBuffer.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/FastSpringBoneConbinedBuffer.cs index efc722c8a..9305d2662 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/FastSpringBoneConbinedBuffer.cs +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/FastSpringBoneConbinedBuffer.cs @@ -9,6 +9,7 @@ using UniGLTF.SpringBoneJobs.Blittables; using UniGLTF.SpringBoneJobs.InputPorts; using Unity.Collections; using Unity.Jobs; +using Unity.Mathematics; using UnityEngine; using UnityEngine.Jobs; using UnityEngine.Profiling; @@ -23,9 +24,9 @@ namespace UniGLTF.SpringBoneJobs // Joint Level private NativeArray _logics; private NativeArray _joints; - private NativeArray _prevTails; - private NativeArray _currentTails; - private NativeArray _nextTails; + private NativeArray _prevTails; + private NativeArray _currentTails; + private NativeArray _nextTails; // Spring Level private NativeArray _springs; // Moodel Level @@ -39,9 +40,9 @@ namespace UniGLTF.SpringBoneJobs // accessor: Joint Level public NativeArray Logics => _logics; public NativeArray Joints => _joints; - public NativeArray PrevTails => _prevTails; - public NativeArray CurrentTails => _currentTails; - public NativeArray NextTails => _nextTails; + public NativeArray PrevTails => _prevTails; + public NativeArray CurrentTails => _currentTails; + public NativeArray NextTails => _nextTails; // accessor: Spring Level public NativeArray Springs => _springs; // accessor: Model LEvel @@ -64,9 +65,9 @@ namespace UniGLTF.SpringBoneJobs // joint level _logics = new NativeArray(logicsCount, Allocator.Persistent); _joints = new NativeArray(logicsCount, Allocator.Persistent); - _prevTails = new NativeArray(logicsCount, Allocator.Persistent); - _currentTails = new NativeArray(logicsCount, Allocator.Persistent); - _nextTails = new NativeArray(logicsCount, Allocator.Persistent); + _prevTails = new NativeArray(logicsCount, Allocator.Persistent); + _currentTails = new NativeArray(logicsCount, Allocator.Persistent); + _nextTails = new NativeArray(logicsCount, Allocator.Persistent); // spring level _springs = new NativeArray(springsCount, Allocator.Persistent); // model level @@ -297,9 +298,9 @@ namespace UniGLTF.SpringBoneJobs [ReadOnly] public NativeArray Logics; [ReadOnly] public NativeArray Transforms; - [NativeDisableParallelForRestriction] public NativeSlice CurrentTails; - [NativeDisableParallelForRestriction] public NativeSlice PrevTails; - [NativeDisableParallelForRestriction] public NativeSlice NextTails; + [NativeDisableParallelForRestriction] public NativeSlice CurrentTails; + [NativeDisableParallelForRestriction] public NativeSlice PrevTails; + [NativeDisableParallelForRestriction] public NativeSlice NextTails; public void Execute(int springIndex) { @@ -324,7 +325,7 @@ namespace UniGLTF.SpringBoneJobs } var tail = Transforms[tailIndex]; - var tailPos = center.HasValue ? center.Value.worldToLocalMatrix.MultiplyPoint3x4(tail.position) : tail.position; + var tailPos = center.HasValue ? math.mul(center.Value.worldToLocalMatrix, new float4(tail.position, 1.0f)).xyz : tail.position; CurrentTails[jointIndex] = tailPos; PrevTails[jointIndex] = tailPos; NextTails[jointIndex] = tailPos; diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/InputPorts/FastSpringBoneBuffer.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/InputPorts/FastSpringBoneBuffer.cs index 617c993f0..b507f60d7 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/InputPorts/FastSpringBoneBuffer.cs +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/InputPorts/FastSpringBoneBuffer.cs @@ -2,9 +2,10 @@ using System; using System.Collections.Generic; using System.Linq; using Unity.Collections; -using UnityEngine; using UnityEngine.Profiling; using UniGLTF.SpringBoneJobs.Blittables; +using Unity.Mathematics; +using UnityEngine; namespace UniGLTF.SpringBoneJobs.InputPorts { @@ -22,8 +23,8 @@ namespace UniGLTF.SpringBoneJobs.InputPorts public NativeArray Joints { get; } public NativeArray Colliders { get; } public NativeArray Logics { get; } - private NativeArray _currentTailsBackup; - private NativeArray _nextTailsBackup; + private NativeArray _currentTailsBackup; + private NativeArray _nextTailsBackup; public Transform[] Transforms { get; } /// @@ -113,7 +114,7 @@ namespace UniGLTF.SpringBoneJobs.InputPorts var localPosition = tailJoint.Transform.localPosition; var scale = tailJoint.Transform.lossyScale; - var localChildPosition = new Vector3( + var localChildPosition = new float3( localPosition.x * scale.x, localPosition.y * scale.y, localPosition.z * scale.z @@ -124,12 +125,12 @@ namespace UniGLTF.SpringBoneJobs.InputPorts parentTransformIndex: Array.IndexOf(Transforms, joint.Transform.parent), tailTransformIndex: Array.IndexOf(Transforms, tailJoint.Transform), localRotation: joint.DefaultLocalRotation, - boneAxis: localChildPosition.normalized, - length: localChildPosition.magnitude); + boneAxis: math.normalize(localChildPosition), + length: math.length(localChildPosition)); } } - public void BackupCurrentTails(NativeArray currentTails, NativeArray nextTails, int offset) + public void BackupCurrentTails(NativeArray currentTails, NativeArray nextTails, int offset) { if (!Logics.IsCreated || Logics.Length == 0) { @@ -143,16 +144,16 @@ namespace UniGLTF.SpringBoneJobs.InputPorts { _nextTailsBackup = new(Logics.Length, Allocator.Persistent); } - NativeArray.Copy(currentTails, offset, _currentTailsBackup, 0, Logics.Length); - NativeArray.Copy(nextTails, offset, _nextTailsBackup, 0, Logics.Length); + NativeArray.Copy(currentTails, offset, _currentTailsBackup, 0, Logics.Length); + NativeArray.Copy(nextTails, offset, _nextTailsBackup, 0, Logics.Length); } - public void RestoreCurrentTails(NativeArray currentTails, NativeArray nextTails, int offset) + public void RestoreCurrentTails(NativeArray currentTails, NativeArray nextTails, int offset) { if (_currentTailsBackup.IsCreated) { - NativeArray.Copy(_currentTailsBackup, 0, currentTails, offset, Logics.Length); - NativeArray.Copy(_nextTailsBackup, 0, nextTails, offset, Logics.Length); + NativeArray.Copy(_currentTailsBackup, 0, currentTails, offset, Logics.Length); + NativeArray.Copy(_nextTailsBackup, 0, nextTails, offset, Logics.Length); } else { @@ -161,7 +162,7 @@ namespace UniGLTF.SpringBoneJobs.InputPorts { // mark velocity zero #if UNITY_2022_2_OR_NEWER - currentTails.GetSubArray(offset, Logics.Length).AsSpan().Fill(new Vector3(float.NaN, float.NaN, float.NaN)); + currentTails.GetSubArray(offset, Logics.Length).AsSpan().Fill(new float3(float.NaN, float.NaN, float.NaN)); #else var subArray = currentTails.GetSubArray(offset, Logics.Length); var value = new Vector3(float.NaN, float.NaN, float.NaN); diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/PullTransformJob.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/PullTransformJob.cs index f9074868c..006c3512c 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/PullTransformJob.cs +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/PullTransformJob.cs @@ -17,16 +17,16 @@ namespace UniGLTF.SpringBoneJobs public void Execute(int index, TransformAccess transform) { - Transforms[index] = new BlittableTransform - { - position = transform.position, - rotation = transform.rotation, - localPosition = transform.localPosition, - localRotation = transform.localRotation, - localScale = transform.localScale, - localToWorldMatrix = transform.localToWorldMatrix, - worldToLocalMatrix = transform.worldToLocalMatrix - }; + Transforms[index] = new BlittableTransform( + position: transform.position, + rotation: transform.rotation, + localPosition: transform.localPosition, + + localRotation: transform.localRotation, + localScale: transform.localScale, + localToWorldMatrix: transform.localToWorldMatrix, + worldToLocalMatrix: transform.worldToLocalMatrix + ); } } } diff --git a/Assets/UniGLTF/Runtime/SpringBoneJobs/UpdateFastSpringBoneJob.cs b/Assets/UniGLTF/Runtime/SpringBoneJobs/UpdateFastSpringBoneJob.cs index fc1a8983a..a05ffca59 100644 --- a/Assets/UniGLTF/Runtime/SpringBoneJobs/UpdateFastSpringBoneJob.cs +++ b/Assets/UniGLTF/Runtime/SpringBoneJobs/UpdateFastSpringBoneJob.cs @@ -1,8 +1,8 @@ using System; using Unity.Collections; using Unity.Jobs; -using UnityEngine; using UniGLTF.SpringBoneJobs.Blittables; +using Unity.Mathematics; #if ENABLE_SPRINGBONE_BURST using Unity.Burst; #endif @@ -26,10 +26,10 @@ namespace UniGLTF.SpringBoneJobs // Joints, Logics, PrevTail, CurrentTail, NextTail は同じ index [ReadOnly] public NativeArray Joints; [ReadOnly] public NativeArray Logics; - [ReadOnly] public NativeArray PrevTail; - [ReadOnly] public NativeArray CurrentTail; + [ReadOnly] public NativeArray PrevTail; + [ReadOnly] public NativeArray CurrentTail; // 処理後の tail 位置(ランダムアクセス) - [NativeDisableParallelForRestriction] public NativeArray NextTail; + [NativeDisableParallelForRestriction] public NativeArray NextTail; // Spring Level [ReadOnly] public NativeArray Springs; // Model Level @@ -67,43 +67,54 @@ namespace UniGLTF.SpringBoneJobs // 親があったら、親に依存するTransformを再計算 if (parentTransform.HasValue) { - headTransform.position = - parentTransform.Value.localToWorldMatrix.MultiplyPoint3x4(headTransform.localPosition); - headTransform.rotation = parentTransform.Value.rotation * headTransform.localRotation; + var newPosition = MultiplyPoint3x4(parentTransform.Value.localToWorldMatrix, headTransform.localPosition); + var newRotation = math.mul(parentTransform.Value.rotation, headTransform.localRotation); + + headTransform = new BlittableTransform( + position: newPosition, + rotation: newRotation, + localPosition: headTransform.localPosition, + localRotation: headTransform.localRotation, + localScale: headTransform.localScale, + localToWorldMatrix: headTransform.localToWorldMatrix, + worldToLocalMatrix: headTransform.worldToLocalMatrix); + + headTransform = headTransform.SetPosition(MultiplyPoint3x4(parentTransform.Value.localToWorldMatrix, headTransform.localPosition)); + headTransform = headTransform.SetRotation(math.mul(parentTransform.Value.rotation, headTransform.localRotation)); } var currentTail = centerTransform.HasValue - ? centerTransform.Value.localToWorldMatrix.MultiplyPoint3x4(CurrentTail[logicIndex]) + ? MultiplyPoint3x4(centerTransform.Value.localToWorldMatrix, CurrentTail[logicIndex]) : CurrentTail[logicIndex]; var prevTail = centerTransform.HasValue - ? centerTransform.Value.localToWorldMatrix.MultiplyPoint3x4(PrevTail[logicIndex]) + ? MultiplyPoint3x4(centerTransform.Value.localToWorldMatrix, PrevTail[logicIndex]) : PrevTail[logicIndex]; - var parentRotation = parentTransform?.rotation ?? Quaternion.identity; + var parentRotation = parentTransform?.rotation ?? quaternion.identity; // scaling 対応 - var scalingFactor = model.SupportsScalingAtRuntime ? TransformExtensions.AbsoluteMaxValue(headTransform.localToWorldMatrix.lossyScale) : 1.0f; + var scalingFactor = model.SupportsScalingAtRuntime ? TransformExtensions.AbsoluteMaxValue(headTransform.lossyScale) : 1.0f; // verlet積分で次の位置を計算 var external = (joint.gravityDir * joint.gravityPower + model.ExternalForce) * DeltaTime; var nextTail = currentTail + (currentTail - prevTail) * (1.0f - joint.dragForce) // 前フレームの移動を継続する(減衰もあるよ) - + parentRotation * logic.localRotation * logic.boneAxis * + + math.mul(math.mul(parentRotation, logic.localRotation), logic.boneAxis) * joint.stiffnessForce * DeltaTime * scalingFactor // 親の回転による子ボーンの移動目標 + external * scalingFactor; // 外力による移動量 // 長さをboneLengthに強制 - nextTail = headTransform.position + (nextTail - headTransform.position).normalized * logic.length; + nextTail = headTransform.position + math.normalize(nextTail - headTransform.position) * logic.length; // Collisionで移動 for (var colliderIndex = colliderSpan.startIndex; colliderIndex < colliderSpan.startIndex + colliderSpan.count; ++colliderIndex) { var collider = Colliders[colliderIndex]; var colliderTransform = Transforms[collider.transformIndex + transformIndexOffset]; - var colliderScale = colliderTransform.localToWorldMatrix.lossyScale; - var maxColliderScale = Mathf.Max(Mathf.Max(Mathf.Abs(colliderScale.x), Mathf.Abs(colliderScale.y)), Mathf.Abs(colliderScale.z)); - var worldPosition = colliderTransform.localToWorldMatrix.MultiplyPoint3x4(collider.offset); - var worldTail = colliderTransform.localToWorldMatrix.MultiplyPoint3x4(collider.tailOrNormal); + var colliderScale = colliderTransform.lossyScale; + var maxColliderScale = math.max(math.max(math.abs(colliderScale.x), math.abs(colliderScale.y)), math.abs(colliderScale.z)); + var worldPosition = MultiplyPoint3x4(colliderTransform.localToWorldMatrix, collider.offset); + var worldTail = MultiplyPoint3x4(colliderTransform.localToWorldMatrix, collider.tailOrNormal); switch (collider.colliderType) { @@ -133,38 +144,35 @@ namespace UniGLTF.SpringBoneJobs } NextTail[logicIndex] = centerTransform.HasValue - ? centerTransform.Value.worldToLocalMatrix.MultiplyPoint3x4(nextTail) + ? MultiplyPoint3x4(centerTransform.Value.worldToLocalMatrix, nextTail) : nextTail; //回転を適用 - var rotation = parentRotation * logic.localRotation; - headTransform.rotation = Quaternion.FromToRotation(rotation * logic.boneAxis, - nextTail - headTransform.position) * rotation; + var rotation = math.mul(parentRotation, logic.localRotation); + headTransform = headTransform.SetRotation(UnityEngine.Quaternion.FromToRotation(math.mul(rotation, logic.boneAxis), + nextTail - headTransform.position) * rotation); // Transformを更新 if (parentTransform.HasValue) { var parentLocalToWorldMatrix = parentTransform.Value.localToWorldMatrix; - headTransform.localRotation = Normalize(Quaternion.Inverse(parentTransform.Value.rotation) * headTransform.rotation); - headTransform.localToWorldMatrix = - parentLocalToWorldMatrix * - Matrix4x4.TRS( - headTransform.localPosition, - headTransform.localRotation, - headTransform.localScale - ); - headTransform.worldToLocalMatrix = headTransform.localToWorldMatrix.inverse; + headTransform = headTransform.SetLocalRotation(math.normalize(math.mul(math.inverse(parentTransform.Value.rotation), headTransform.rotation))); + headTransform = headTransform.SetLocalToWorldMatrix(math.mul( + parentLocalToWorldMatrix, + float4x4.TRS( + headTransform.localPosition, + headTransform.localRotation, + headTransform.localScale + ))); } else { - headTransform.localToWorldMatrix = - Matrix4x4.TRS( - headTransform.position, - headTransform.rotation, - headTransform.localScale - ); - headTransform.worldToLocalMatrix = headTransform.localToWorldMatrix.inverse; - headTransform.localRotation = headTransform.rotation; + headTransform = headTransform.SetLocalToWorldMatrix(float4x4.TRS( + headTransform.position, + headTransform.rotation, + headTransform.localScale + )); + headTransform = headTransform.SetLocalRotation(headTransform.rotation); } if (!model.StopSpringBoneWriteback) @@ -180,42 +188,33 @@ namespace UniGLTF.SpringBoneJobs } } - /// - /// BurstではMathfがエラーを吐くため、内部でMathfを呼ばないNormalizeを自前実装 - /// - private static Quaternion Normalize(Quaternion q) - { - var num = (float)Math.Sqrt(Quaternion.Dot(q, q)); - return num < float.Epsilon ? Quaternion.identity : new Quaternion(q.x / num, q.y / num, q.z / num, q.w / num); - } - private static void ResolveCapsuleCollision( - Vector3 worldTail, - Vector3 worldPosition, + float3 worldTail, + float3 worldPosition, BlittableTransform headTransform, BlittableJointMutable joint, BlittableCollider collider, float maxColliderScale, BlittableJointImmutable logic, - ref Vector3 nextTail) + ref float3 nextTail) { var direction = worldTail - worldPosition; - if (direction.sqrMagnitude == 0) + if (math.lengthsq(direction) == 0) { // head側半球の球判定 ResolveSphereCollision(joint, collider, worldPosition, headTransform, maxColliderScale, logic, ref nextTail); return; } - var P = direction.normalized; + var P = math.normalize(direction); var Q = headTransform.position - worldPosition; - var dot = Vector3.Dot(P, Q); + var dot = math.dot(P, Q); if (dot <= 0) { // head側半球の球判定 ResolveSphereCollision(joint, collider, worldPosition, headTransform, maxColliderScale, logic, ref nextTail); return; } - if (dot >= direction.magnitude) + if (dot >= math.length(direction)) { // tail側半球の球判定 ResolveSphereCollision(joint, collider, worldTail, headTransform, maxColliderScale, logic, ref nextTail); @@ -230,20 +229,20 @@ namespace UniGLTF.SpringBoneJobs private static void ResolveSphereCollision( BlittableJointMutable joint, BlittableCollider collider, - Vector3 worldPosition, + float3 worldPosition, BlittableTransform headTransform, float maxColliderScale, BlittableJointImmutable logic, - ref Vector3 nextTail) + ref float3 nextTail) { var r = joint.radius + collider.radius * maxColliderScale; - if (Vector3.SqrMagnitude(nextTail - worldPosition) <= (r * r)) + if (math.lengthsq(nextTail - worldPosition) <= (r * r)) { // ヒット。Colliderの半径方向に押し出す - var normal = (nextTail - worldPosition).normalized; + var normal = math.normalize(nextTail - worldPosition); var posFromCollider = worldPosition + normal * r; // 長さをboneLengthに強制 - nextTail = headTransform.position + (posFromCollider - headTransform.position).normalized * logic.length; + nextTail = headTransform.position + math.normalize(posFromCollider - headTransform.position) * logic.length; } } @@ -251,18 +250,18 @@ namespace UniGLTF.SpringBoneJobs BlittableJointMutable joint, BlittableCollider collider, BlittableTransform colliderTransform, - ref Vector3 nextTail) + ref float3 nextTail) { - var transformedOffset = colliderTransform.localToWorldMatrix.MultiplyPoint(collider.offset); + var transformedOffset = MultiplyPoint(colliderTransform.localToWorldMatrix, collider.offset); var delta = nextTail - transformedOffset; // ジョイントとコライダーの距離。負の値は衝突していることを示す - var distance = collider.radius - joint.radius - delta.magnitude; + var distance = collider.radius - joint.radius - math.length(delta); // ジョイントとコライダーの距離の方向。衝突している場合、この方向にジョイントを押し出す if (distance < 0) { - var direction = -delta.normalized; + var direction = -1 * math.normalize(delta); nextTail -= direction * distance; } } @@ -271,15 +270,15 @@ namespace UniGLTF.SpringBoneJobs BlittableJointMutable joint, BlittableCollider collider, BlittableTransform colliderTransform, - ref Vector3 nextTail) + ref float3 nextTail) { - var transformedOffset = colliderTransform.localToWorldMatrix.MultiplyPoint(collider.offset); - var transformedTail = colliderTransform.localToWorldMatrix.MultiplyPoint(collider.tailOrNormal); + var transformedOffset = MultiplyPoint(colliderTransform.localToWorldMatrix, collider.offset); + var transformedTail = MultiplyPoint(colliderTransform.localToWorldMatrix, collider.tailOrNormal); var offsetToTail = transformedTail - transformedOffset; - var lengthSqCapsule = offsetToTail.sqrMagnitude; + var lengthSqCapsule = math.lengthsq(offsetToTail); var delta = nextTail - transformedOffset; - var dot = Vector3.Dot(offsetToTail, delta); + var dot = math.dot(offsetToTail, delta); if (dot < 0.0) { @@ -298,12 +297,12 @@ namespace UniGLTF.SpringBoneJobs } // ジョイントとコライダーの距離。負の値は衝突していることを示す - var distance = collider.radius - joint.radius - delta.magnitude; + var distance = collider.radius - joint.radius - math.length(delta); // ジョイントとコライダーの距離の方向。衝突している場合、この方向にジョイントを押し出す if (distance < 0) { - var direction = -delta.normalized; + var direction = -1 * math.normalize(delta); nextTail -= direction * distance; } } @@ -320,14 +319,14 @@ namespace UniGLTF.SpringBoneJobs BlittableJointMutable joint, BlittableCollider collider, BlittableTransform colliderTransform, - ref Vector3 nextTail) + ref float3 nextTail) { - var transformedOffset = colliderTransform.localToWorldMatrix.MultiplyPoint(collider.offset); - var transformedNormal = colliderTransform.localToWorldMatrix.MultiplyVector(collider.tailOrNormal).normalized; + var transformedOffset = MultiplyPoint(colliderTransform.localToWorldMatrix, collider.offset); + var transformedNormal = math.normalize(MultiplyVector(colliderTransform.localToWorldMatrix, collider.tailOrNormal)); var delta = nextTail - transformedOffset; // ジョイントとコライダーの距離。負の値は衝突していることを示す - var distance = Vector3.Dot(delta, transformedNormal) - joint.radius; + var distance = math.dot(delta, transformedNormal) - joint.radius; if (distance < 0) { @@ -336,5 +335,21 @@ namespace UniGLTF.SpringBoneJobs nextTail -= direction * distance; } } + + private static float3 MultiplyPoint3x4(float4x4 matrix, float3 point) + { + return math.mul(matrix, new float4(point, 1)).xyz; + } + + private static float3 MultiplyPoint(float4x4 matrix, float3 point) + { + var v = math.mul(matrix, new float4(point, 1.0f)); + return v.xyz / v.w; + } + + private static float3 MultiplyVector(float4x4 matrix, float3 vector) + { + return math.mul(matrix, new float4(vector, 0)).xyz; + } } } \ No newline at end of file