EdenAutoMorpherScript 디컴파일 소스 추가
This commit is contained in:
767
Assets/@Eden_Tools/Eden_AutoMorpher/Script/BvhTriangleMesh.cs
Normal file
767
Assets/@Eden_Tools/Eden_AutoMorpher/Script/BvhTriangleMesh.cs
Normal file
@@ -0,0 +1,767 @@
|
||||
// Warning: Some assembly references could not be resolved automatically. This might lead to incorrect decompilation of some parts,
|
||||
// for ex. property getter/setter access. To get optimal decompilation results, please manually add the missing references to the list of loaded assemblies.
|
||||
// EdenAutoMorpherScript, Version=0.0.0.0, Culture=neutral, PublicKeyToken=null
|
||||
// Eden.AutoMorpher.BvhTriangleMesh
|
||||
using System.Collections.Generic;
|
||||
using Eden.AutoMorpher;
|
||||
using UnityEngine;
|
||||
|
||||
public class BvhTriangleMesh
|
||||
{
|
||||
public struct ClosestHit
|
||||
{
|
||||
public int triangleIndex;
|
||||
|
||||
public Vector3 closestPoint;
|
||||
|
||||
public Vector3 normal;
|
||||
|
||||
public float sqrDistance;
|
||||
|
||||
public HumanBodyBones mainHumanBone;
|
||||
}
|
||||
|
||||
private TriangleUtil triangleUtil;
|
||||
|
||||
private HumanBodyBones[] humanBones = new HumanBodyBones[34]
|
||||
{
|
||||
HumanBodyBones.Hips,
|
||||
HumanBodyBones.Spine,
|
||||
HumanBodyBones.Chest,
|
||||
HumanBodyBones.UpperChest,
|
||||
HumanBodyBones.Neck,
|
||||
HumanBodyBones.LeftShoulder,
|
||||
HumanBodyBones.LeftUpperArm,
|
||||
HumanBodyBones.LeftLowerArm,
|
||||
HumanBodyBones.LeftHand,
|
||||
HumanBodyBones.LeftThumbProximal,
|
||||
HumanBodyBones.LeftIndexProximal,
|
||||
HumanBodyBones.LeftMiddleProximal,
|
||||
HumanBodyBones.LeftRingProximal,
|
||||
HumanBodyBones.LeftLittleProximal,
|
||||
HumanBodyBones.RightShoulder,
|
||||
HumanBodyBones.RightUpperArm,
|
||||
HumanBodyBones.RightLowerArm,
|
||||
HumanBodyBones.RightHand,
|
||||
HumanBodyBones.RightThumbProximal,
|
||||
HumanBodyBones.RightIndexProximal,
|
||||
HumanBodyBones.RightMiddleProximal,
|
||||
HumanBodyBones.RightRingProximal,
|
||||
HumanBodyBones.RightLittleProximal,
|
||||
HumanBodyBones.LeftUpperLeg,
|
||||
HumanBodyBones.LeftLowerLeg,
|
||||
HumanBodyBones.LeftFoot,
|
||||
HumanBodyBones.LeftToes,
|
||||
HumanBodyBones.RightUpperLeg,
|
||||
HumanBodyBones.RightLowerLeg,
|
||||
HumanBodyBones.RightFoot,
|
||||
HumanBodyBones.RightToes,
|
||||
HumanBodyBones.Head,
|
||||
HumanBodyBones.LeftEye,
|
||||
HumanBodyBones.RightEye
|
||||
};
|
||||
|
||||
public BvhTriangle[] triangles;
|
||||
|
||||
public BvhNode[] nodes;
|
||||
|
||||
public int[] triIndices;
|
||||
|
||||
private const int LeafMaxTriangles = 4;
|
||||
|
||||
public BvhTriangleMesh()
|
||||
{
|
||||
triangleUtil = new TriangleUtil();
|
||||
}
|
||||
|
||||
private HumanBodyBones[] BuildVertexMainHumanBone(SkinnedMeshRenderer smr, Animator animator, HumanBodyBones[] bodyBones)
|
||||
{
|
||||
Mesh sharedMesh = smr.sharedMesh;
|
||||
BoneWeight[] boneWeights = sharedMesh.boneWeights;
|
||||
int[] boneToBodyIndex = BuildBoneToBodyIndexMap(smr, animator, bodyBones);
|
||||
HumanBodyBones[] array = new HumanBodyBones[sharedMesh.vertexCount];
|
||||
for (int i = 0; i < sharedMesh.vertexCount; i++)
|
||||
{
|
||||
BoneWeight boneWeight = boneWeights[i];
|
||||
int bestBodyIdx = -1;
|
||||
float bestWeight = 0f;
|
||||
Try(boneWeight.boneIndex0, boneWeight.weight0);
|
||||
Try(boneWeight.boneIndex1, boneWeight.weight1);
|
||||
Try(boneWeight.boneIndex2, boneWeight.weight2);
|
||||
Try(boneWeight.boneIndex3, boneWeight.weight3);
|
||||
array[i] = ((bestBodyIdx >= 0) ? bodyBones[bestBodyIdx] : HumanBodyBones.LastBone);
|
||||
void Try(int boneIdx, float w)
|
||||
{
|
||||
if (!(w <= 0f) && boneIdx >= 0 && boneIdx < boneToBodyIndex.Length)
|
||||
{
|
||||
int num = boneToBodyIndex[boneIdx];
|
||||
if (num >= 0 && w > bestWeight)
|
||||
{
|
||||
bestWeight = w;
|
||||
bestBodyIdx = num;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
return array;
|
||||
}
|
||||
|
||||
public BvhTriangleMesh BuildFromSkinnedMeshes(IReadOnlyList<SkinnedMeshRenderer> renderers, Animator animator)
|
||||
{
|
||||
if (renderers == null || renderers.Count == 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
BvhTriangleMesh bvhTriangleMesh = new BvhTriangleMesh();
|
||||
int num = 0;
|
||||
foreach (SkinnedMeshRenderer renderer in renderers)
|
||||
{
|
||||
if (!(renderer == null) && !(renderer.sharedMesh == null))
|
||||
{
|
||||
num += renderer.sharedMesh.triangles.Length / 3;
|
||||
}
|
||||
}
|
||||
if (num == 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
bvhTriangleMesh.triangles = new BvhTriangle[num];
|
||||
int num2 = 0;
|
||||
Mesh mesh = new Mesh();
|
||||
foreach (SkinnedMeshRenderer renderer2 in renderers)
|
||||
{
|
||||
if (renderer2 == null || renderer2.sharedMesh == null)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
mesh.Clear();
|
||||
renderer2.BakeMesh(mesh);
|
||||
Vector3[] vertices = mesh.vertices;
|
||||
int[] array = renderer2.sharedMesh.triangles;
|
||||
BoneWeight[] boneWeights = renderer2.sharedMesh.boneWeights;
|
||||
int[] boneToBodyIndex = BuildBoneToBodyIndexMap(renderer2, animator, humanBones);
|
||||
int num3 = array.Length / 3;
|
||||
Transform transform = renderer2.transform;
|
||||
Vector3 lossyScale = transform.lossyScale;
|
||||
Vector3 vector = new Vector3(1f / Mathf.Max(lossyScale.x, 1E-08f), 1f / Mathf.Max(lossyScale.y, 1E-08f), 1f / Mathf.Max(lossyScale.z, 1E-08f));
|
||||
Matrix4x4 matrix4x = transform.localToWorldMatrix * Matrix4x4.Scale(vector);
|
||||
for (int i = 0; i < num3; i++)
|
||||
{
|
||||
int num4 = array[i * 3];
|
||||
int num5 = array[i * 3 + 1];
|
||||
int num6 = array[i * 3 + 2];
|
||||
Vector3 vector2 = matrix4x.MultiplyPoint3x4(vertices[num4]);
|
||||
Vector3 vector3 = matrix4x.MultiplyPoint3x4(vertices[num5]);
|
||||
Vector3 vector4 = matrix4x.MultiplyPoint3x4(vertices[num6]);
|
||||
Vector3 normal = Vector3.Cross(vector3 - vector2, vector4 - vector2);
|
||||
float magnitude = normal.magnitude;
|
||||
if (magnitude > 1E-08f)
|
||||
{
|
||||
normal /= magnitude;
|
||||
}
|
||||
else
|
||||
{
|
||||
normal = Vector3.up;
|
||||
}
|
||||
int num7 = ComputeTriangleMainHumanBoneIndex(num4, num5, num6, boneWeights, boneToBodyIndex, humanBones.Length);
|
||||
HumanBodyBones mainHumanBone = ((num7 >= 0) ? humanBones[num7] : HumanBodyBones.LastBone);
|
||||
bvhTriangleMesh.triangles[num2++] = new BvhTriangle
|
||||
{
|
||||
a = vector2,
|
||||
b = vector3,
|
||||
c = vector4,
|
||||
normal = normal,
|
||||
mainHumanBone = mainHumanBone
|
||||
};
|
||||
mesh.Clear();
|
||||
}
|
||||
}
|
||||
int num8 = num;
|
||||
int[] array2 = new int[num8];
|
||||
for (int j = 0; j < num8; j++)
|
||||
{
|
||||
array2[j] = j;
|
||||
}
|
||||
bvhTriangleMesh.triIndices = array2;
|
||||
List<BvhNode> list = new List<BvhNode>();
|
||||
BuildRecursive(bvhTriangleMesh.triangles, array2, 0, num8, list);
|
||||
bvhTriangleMesh.nodes = list.ToArray();
|
||||
return bvhTriangleMesh;
|
||||
}
|
||||
|
||||
private int[] BuildBoneToBodyIndexMap(SkinnedMeshRenderer smr, Animator animator, HumanBodyBones[] bodyBones)
|
||||
{
|
||||
Transform[] bones = smr.bones;
|
||||
int[] array = new int[bones.Length];
|
||||
for (int i = 0; i < array.Length; i++)
|
||||
{
|
||||
array[i] = -1;
|
||||
}
|
||||
if (animator == null || bodyBones == null || bones == null)
|
||||
{
|
||||
return array;
|
||||
}
|
||||
Dictionary<Transform, int> dictionary = new Dictionary<Transform, int>();
|
||||
for (int j = 0; j < bones.Length; j++)
|
||||
{
|
||||
if (!(bones[j] == null) && !dictionary.ContainsKey(bones[j]))
|
||||
{
|
||||
dictionary.Add(bones[j], j);
|
||||
}
|
||||
}
|
||||
Dictionary<HumanBodyBones, HashSet<Transform>> dictionary2 = new MeshClassifier().MeshHumanoidBoneMatcher(animator, new SkinnedMeshRenderer[1] { smr });
|
||||
for (int k = 0; k < bodyBones.Length; k++)
|
||||
{
|
||||
HumanBodyBones key = bodyBones[k];
|
||||
if (!dictionary2.TryGetValue(key, out var value) || value == null)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
foreach (Transform item in value)
|
||||
{
|
||||
if (!(item == null) && dictionary.TryGetValue(item, out var value2))
|
||||
{
|
||||
array[value2] = k;
|
||||
}
|
||||
}
|
||||
}
|
||||
for (int l = 0; l < bones.Length; l++)
|
||||
{
|
||||
if (array[l] != -1)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
Transform transform = bones[l];
|
||||
if (transform == null)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
Transform parent = transform.parent;
|
||||
if (!(parent == null) && dictionary.TryGetValue(parent, out var value3))
|
||||
{
|
||||
int num = array[value3];
|
||||
if (num != -1)
|
||||
{
|
||||
array[l] = num;
|
||||
}
|
||||
}
|
||||
}
|
||||
return array;
|
||||
}
|
||||
|
||||
private int ComputeTriangleMainHumanBoneIndex(int vi0, int vi1, int vi2, BoneWeight[] weights, int[] boneToBodyIndex, int bodyBonesCount)
|
||||
{
|
||||
if (weights == null || weights.Length == 0 || boneToBodyIndex == null || boneToBodyIndex.Length == 0)
|
||||
{
|
||||
return -1;
|
||||
}
|
||||
float[] scores = new float[bodyBonesCount];
|
||||
Accumulate(vi0);
|
||||
Accumulate(vi1);
|
||||
Accumulate(vi2);
|
||||
int result = -1;
|
||||
float num = 0f;
|
||||
for (int i = 0; i < scores.Length; i++)
|
||||
{
|
||||
if (scores[i] > num)
|
||||
{
|
||||
num = scores[i];
|
||||
result = i;
|
||||
}
|
||||
}
|
||||
return result;
|
||||
void Accumulate(int v)
|
||||
{
|
||||
if (v >= 0 && v < weights.Length)
|
||||
{
|
||||
BoneWeight boneWeight = weights[v];
|
||||
Add(boneWeight.boneIndex0, boneWeight.weight0);
|
||||
Add(boneWeight.boneIndex1, boneWeight.weight1);
|
||||
Add(boneWeight.boneIndex2, boneWeight.weight2);
|
||||
Add(boneWeight.boneIndex3, boneWeight.weight3);
|
||||
}
|
||||
}
|
||||
void Add(int boneIdx, float w)
|
||||
{
|
||||
if (!(w <= 0f) && boneIdx >= 0 && boneIdx < boneToBodyIndex.Length)
|
||||
{
|
||||
int num2 = boneToBodyIndex[boneIdx];
|
||||
if (num2 >= 0)
|
||||
{
|
||||
scores[num2] += w;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
public BvhTriangleMesh BuildFromSkinnedMeshes(IList<SkinnedMeshRenderer> renderers)
|
||||
{
|
||||
if (renderers == null || renderers.Count == 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
BvhTriangleMesh bvhTriangleMesh = new BvhTriangleMesh();
|
||||
int num = 0;
|
||||
foreach (SkinnedMeshRenderer renderer in renderers)
|
||||
{
|
||||
if (!(renderer == null) && !(renderer.sharedMesh == null))
|
||||
{
|
||||
num += renderer.sharedMesh.triangles.Length / 3;
|
||||
}
|
||||
}
|
||||
if (num == 0)
|
||||
{
|
||||
return null;
|
||||
}
|
||||
bvhTriangleMesh.triangles = new BvhTriangle[num];
|
||||
int num2 = 0;
|
||||
foreach (SkinnedMeshRenderer renderer2 in renderers)
|
||||
{
|
||||
if (renderer2 == null || renderer2.sharedMesh == null)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
Mesh sharedMesh = renderer2.sharedMesh;
|
||||
Vector3[] vertices = sharedMesh.vertices;
|
||||
int[] array = sharedMesh.triangles;
|
||||
int num3 = array.Length / 3;
|
||||
for (int i = 0; i < num3; i++)
|
||||
{
|
||||
int num4 = array[i * 3];
|
||||
int num5 = array[i * 3 + 1];
|
||||
int num6 = array[i * 3 + 2];
|
||||
Vector3 vector = renderer2.transform.TransformPoint(vertices[num4]);
|
||||
Vector3 vector2 = renderer2.transform.TransformPoint(vertices[num5]);
|
||||
Vector3 vector3 = renderer2.transform.TransformPoint(vertices[num6]);
|
||||
Vector3 normal = Vector3.Cross(vector2 - vector, vector3 - vector);
|
||||
float magnitude = normal.magnitude;
|
||||
if (magnitude > 1E-08f)
|
||||
{
|
||||
normal /= magnitude;
|
||||
}
|
||||
else
|
||||
{
|
||||
normal = Vector3.up;
|
||||
}
|
||||
bvhTriangleMesh.triangles[num2++] = new BvhTriangle
|
||||
{
|
||||
a = vector,
|
||||
b = vector2,
|
||||
c = vector3,
|
||||
normal = normal
|
||||
};
|
||||
}
|
||||
}
|
||||
int num7 = num;
|
||||
int[] array2 = new int[num7];
|
||||
for (int j = 0; j < num7; j++)
|
||||
{
|
||||
array2[j] = j;
|
||||
}
|
||||
bvhTriangleMesh.triIndices = array2;
|
||||
List<BvhNode> list = new List<BvhNode>();
|
||||
BuildRecursive(bvhTriangleMesh.triangles, array2, 0, num7, list);
|
||||
bvhTriangleMesh.nodes = list.ToArray();
|
||||
return bvhTriangleMesh;
|
||||
}
|
||||
|
||||
public BvhTriangleMesh BuildFromMesh(Mesh mesh, Transform transform)
|
||||
{
|
||||
BvhTriangleMesh bvhTriangleMesh = new BvhTriangleMesh();
|
||||
Vector3[] vertices = mesh.vertices;
|
||||
int[] array = mesh.triangles;
|
||||
int num = array.Length / 3;
|
||||
bvhTriangleMesh.triangles = new BvhTriangle[num];
|
||||
for (int i = 0; i < num; i++)
|
||||
{
|
||||
int num2 = array[i * 3];
|
||||
int num3 = array[i * 3 + 1];
|
||||
int num4 = array[i * 3 + 2];
|
||||
Vector3 vector = transform.TransformPoint(vertices[num2]);
|
||||
Vector3 vector2 = transform.TransformPoint(vertices[num3]);
|
||||
Vector3 vector3 = transform.TransformPoint(vertices[num4]);
|
||||
Vector3 normal = Vector3.Cross(vector2 - vector, vector3 - vector);
|
||||
float magnitude = normal.magnitude;
|
||||
if (magnitude > 1E-08f)
|
||||
{
|
||||
normal /= magnitude;
|
||||
}
|
||||
else
|
||||
{
|
||||
normal = Vector3.up;
|
||||
}
|
||||
bvhTriangleMesh.triangles[i] = new BvhTriangle
|
||||
{
|
||||
a = vector,
|
||||
b = vector2,
|
||||
c = vector3,
|
||||
normal = normal
|
||||
};
|
||||
}
|
||||
int[] array2 = new int[num];
|
||||
for (int j = 0; j < num; j++)
|
||||
{
|
||||
array2[j] = j;
|
||||
}
|
||||
bvhTriangleMesh.triIndices = array2;
|
||||
List<BvhNode> list = new List<BvhNode>();
|
||||
BuildRecursive(bvhTriangleMesh.triangles, array2, 0, num, list);
|
||||
bvhTriangleMesh.nodes = list.ToArray();
|
||||
return bvhTriangleMesh;
|
||||
}
|
||||
|
||||
private int BuildRecursive(BvhTriangle[] tris, int[] triIndices, int start, int count, List<BvhNode> outNodes)
|
||||
{
|
||||
int count2 = outNodes.Count;
|
||||
BvhNode bvhNode = default(BvhNode);
|
||||
Bounds bounds = default(Bounds);
|
||||
bool flag = true;
|
||||
for (int i = start; i < start + count; i++)
|
||||
{
|
||||
BvhTriangle bvhTriangle = tris[triIndices[i]];
|
||||
if (flag)
|
||||
{
|
||||
bounds = new Bounds(bvhTriangle.a, Vector3.zero);
|
||||
bounds.Encapsulate(bvhTriangle.b);
|
||||
bounds.Encapsulate(bvhTriangle.c);
|
||||
flag = false;
|
||||
}
|
||||
else
|
||||
{
|
||||
bounds.Encapsulate(bvhTriangle.a);
|
||||
bounds.Encapsulate(bvhTriangle.b);
|
||||
bounds.Encapsulate(bvhTriangle.c);
|
||||
}
|
||||
}
|
||||
bvhNode.bounds = bounds;
|
||||
if (count <= 4)
|
||||
{
|
||||
bvhNode.isLeaf = true;
|
||||
bvhNode.start = start;
|
||||
bvhNode.count = count;
|
||||
bvhNode.leftChild = -1;
|
||||
bvhNode.rightChild = -1;
|
||||
outNodes.Add(bvhNode);
|
||||
return count2;
|
||||
}
|
||||
Vector3 size = bounds.size;
|
||||
int num = 0;
|
||||
if (size.y > size.x && size.y > size.z)
|
||||
{
|
||||
num = 1;
|
||||
}
|
||||
else if (size.z > size.x && size.z > size.y)
|
||||
{
|
||||
num = 2;
|
||||
}
|
||||
float num2 = 0f;
|
||||
for (int j = start; j < start + count; j++)
|
||||
{
|
||||
BvhTriangle bvhTriangle2 = tris[triIndices[j]];
|
||||
num2 += ((bvhTriangle2.a + bvhTriangle2.b + bvhTriangle2.c) / 3f)[num];
|
||||
}
|
||||
num2 /= (float)count;
|
||||
int num3 = Partition(tris, triIndices, start, count, num, num2);
|
||||
if (num3 == start || num3 == start + count)
|
||||
{
|
||||
num3 = start + count / 2;
|
||||
}
|
||||
bvhNode.isLeaf = false;
|
||||
bvhNode.start = -1;
|
||||
bvhNode.count = 0;
|
||||
outNodes.Add(bvhNode);
|
||||
int leftChild = BuildRecursive(tris, triIndices, start, num3 - start, outNodes);
|
||||
int rightChild = BuildRecursive(tris, triIndices, num3, start + count - num3, outNodes);
|
||||
bvhNode.leftChild = leftChild;
|
||||
bvhNode.rightChild = rightChild;
|
||||
outNodes[count2] = bvhNode;
|
||||
return count2;
|
||||
}
|
||||
|
||||
private int Partition(BvhTriangle[] tris, int[] triIndices, int start, int count, int axis, float splitPos)
|
||||
{
|
||||
int num = start;
|
||||
int num2 = start + count - 1;
|
||||
while (num <= num2)
|
||||
{
|
||||
BvhTriangle bvhTriangle = tris[triIndices[num]];
|
||||
BvhTriangle bvhTriangle2 = tris[triIndices[num2]];
|
||||
float num3 = (bvhTriangle.a[axis] + bvhTriangle.b[axis] + bvhTriangle.c[axis]) / 3f;
|
||||
_ = (bvhTriangle2.a[axis] + bvhTriangle2.b[axis] + bvhTriangle2.c[axis]) / 3f;
|
||||
if (num3 < splitPos)
|
||||
{
|
||||
num++;
|
||||
continue;
|
||||
}
|
||||
int num4 = triIndices[num];
|
||||
triIndices[num] = triIndices[num2];
|
||||
triIndices[num2] = num4;
|
||||
num2--;
|
||||
}
|
||||
return num;
|
||||
}
|
||||
|
||||
public ClosestHit QueryClosest(Vector3 point)
|
||||
{
|
||||
ClosestHit best = new ClosestHit
|
||||
{
|
||||
triangleIndex = -1,
|
||||
sqrDistance = float.MaxValue
|
||||
};
|
||||
if (nodes == null || nodes.Length == 0)
|
||||
{
|
||||
return best;
|
||||
}
|
||||
QueryClosestRecursive(0, point, ref best);
|
||||
return best;
|
||||
}
|
||||
|
||||
public ClosestHit QueryClosest(Vector3 point, HashSet<HumanBodyBones> allowedBones)
|
||||
{
|
||||
ClosestHit best = new ClosestHit
|
||||
{
|
||||
triangleIndex = -1,
|
||||
sqrDistance = float.MaxValue
|
||||
};
|
||||
if (nodes == null || nodes.Length == 0)
|
||||
{
|
||||
return best;
|
||||
}
|
||||
if (allowedBones == null || allowedBones.Count == 0)
|
||||
{
|
||||
QueryClosestRecursive(0, point, ref best);
|
||||
}
|
||||
else
|
||||
{
|
||||
QueryClosestRecursiveFiltered(0, point, ref best, allowedBones);
|
||||
}
|
||||
return best;
|
||||
}
|
||||
|
||||
private void QueryClosestRecursiveFiltered(int nodeIndex, Vector3 p, ref ClosestHit best, HashSet<HumanBodyBones> allowedBones)
|
||||
{
|
||||
BvhNode bvhNode = nodes[nodeIndex];
|
||||
if (DistanceSqPointAABB(p, bvhNode.bounds) > best.sqrDistance)
|
||||
{
|
||||
return;
|
||||
}
|
||||
if (bvhNode.isLeaf)
|
||||
{
|
||||
int num = bvhNode.start + bvhNode.count;
|
||||
for (int i = bvhNode.start; i < num; i++)
|
||||
{
|
||||
int num2 = triIndices[i];
|
||||
BvhTriangle bvhTriangle = triangles[num2];
|
||||
if (allowedBones.Contains(bvhTriangle.mainHumanBone))
|
||||
{
|
||||
Vector3 vector = triangleUtil.ClosestPointOnTriangle(p, bvhTriangle.a, bvhTriangle.b, bvhTriangle.c);
|
||||
float sqrMagnitude = (p - vector).sqrMagnitude;
|
||||
if (sqrMagnitude < best.sqrDistance)
|
||||
{
|
||||
best.sqrDistance = sqrMagnitude;
|
||||
best.triangleIndex = num2;
|
||||
best.closestPoint = vector;
|
||||
best.normal = bvhTriangle.normal;
|
||||
best.mainHumanBone = bvhTriangle.mainHumanBone;
|
||||
}
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
int leftChild = bvhNode.leftChild;
|
||||
int rightChild = bvhNode.rightChild;
|
||||
float num3 = DistanceSqPointAABB(p, nodes[leftChild].bounds);
|
||||
float num4 = DistanceSqPointAABB(p, nodes[rightChild].bounds);
|
||||
if (num3 < num4)
|
||||
{
|
||||
QueryClosestRecursiveFiltered(leftChild, p, ref best, allowedBones);
|
||||
QueryClosestRecursiveFiltered(rightChild, p, ref best, allowedBones);
|
||||
}
|
||||
else
|
||||
{
|
||||
QueryClosestRecursiveFiltered(rightChild, p, ref best, allowedBones);
|
||||
QueryClosestRecursiveFiltered(leftChild, p, ref best, allowedBones);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private void QueryClosestRecursive(int nodeIndex, Vector3 p, ref ClosestHit best)
|
||||
{
|
||||
BvhNode bvhNode = nodes[nodeIndex];
|
||||
if (DistanceSqPointAABB(p, bvhNode.bounds) > best.sqrDistance)
|
||||
{
|
||||
return;
|
||||
}
|
||||
if (bvhNode.isLeaf)
|
||||
{
|
||||
int num = bvhNode.start + bvhNode.count;
|
||||
for (int i = bvhNode.start; i < num; i++)
|
||||
{
|
||||
int num2 = triIndices[i];
|
||||
BvhTriangle bvhTriangle = triangles[num2];
|
||||
Vector3 vector = triangleUtil.ClosestPointOnTriangle(p, bvhTriangle.a, bvhTriangle.b, bvhTriangle.c);
|
||||
float sqrMagnitude = (p - vector).sqrMagnitude;
|
||||
if (sqrMagnitude < best.sqrDistance)
|
||||
{
|
||||
best.sqrDistance = sqrMagnitude;
|
||||
best.triangleIndex = num2;
|
||||
best.closestPoint = vector;
|
||||
best.normal = bvhTriangle.normal;
|
||||
best.mainHumanBone = bvhTriangle.mainHumanBone;
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
int leftChild = bvhNode.leftChild;
|
||||
int rightChild = bvhNode.rightChild;
|
||||
float num3 = DistanceSqPointAABB(p, nodes[leftChild].bounds);
|
||||
float num4 = DistanceSqPointAABB(p, nodes[rightChild].bounds);
|
||||
if (num3 < num4)
|
||||
{
|
||||
QueryClosestRecursive(leftChild, p, ref best);
|
||||
QueryClosestRecursive(rightChild, p, ref best);
|
||||
}
|
||||
else
|
||||
{
|
||||
QueryClosestRecursive(rightChild, p, ref best);
|
||||
QueryClosestRecursive(leftChild, p, ref best);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private float DistanceSqPointAABB(Vector3 p, Bounds b)
|
||||
{
|
||||
float num = Mathf.Max(b.min.x - p.x, 0f, p.x - b.max.x);
|
||||
float num2 = Mathf.Max(b.min.y - p.y, 0f, p.y - b.max.y);
|
||||
float num3 = Mathf.Max(b.min.z - p.z, 0f, p.z - b.max.z);
|
||||
return num * num + num2 * num2 + num3 * num3;
|
||||
}
|
||||
|
||||
public int QueryClosestN(Vector3 point, int maxCount, float maxDistance, List<ClosestHit> results)
|
||||
{
|
||||
results.Clear();
|
||||
if (nodes == null || nodes.Length == 0 || maxCount <= 0)
|
||||
{
|
||||
return 0;
|
||||
}
|
||||
float num = maxDistance * maxDistance;
|
||||
float currentMaxSq = num;
|
||||
QueryClosestNRecursive(0, point, maxCount, num, results, ref currentMaxSq);
|
||||
return results.Count;
|
||||
}
|
||||
|
||||
private void QueryClosestNRecursive(int nodeIndex, Vector3 p, int maxCount, float maxDistanceSq, List<ClosestHit> bestHits, ref float currentMaxSq)
|
||||
{
|
||||
BvhNode bvhNode = nodes[nodeIndex];
|
||||
if (DistanceSqPointAABB(p, bvhNode.bounds) > currentMaxSq)
|
||||
{
|
||||
return;
|
||||
}
|
||||
if (bvhNode.isLeaf)
|
||||
{
|
||||
int num = bvhNode.start + bvhNode.count;
|
||||
for (int i = bvhNode.start; i < num; i++)
|
||||
{
|
||||
int num2 = triIndices[i];
|
||||
BvhTriangle bvhTriangle = triangles[num2];
|
||||
Vector3 vector = triangleUtil.ClosestPointOnTriangle(p, bvhTriangle.a, bvhTriangle.b, bvhTriangle.c);
|
||||
float sqrMagnitude = (p - vector).sqrMagnitude;
|
||||
if (sqrMagnitude > maxDistanceSq)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
if (bestHits.Count < maxCount)
|
||||
{
|
||||
bestHits.Add(new ClosestHit
|
||||
{
|
||||
triangleIndex = num2,
|
||||
closestPoint = vector,
|
||||
normal = bvhTriangle.normal,
|
||||
sqrDistance = sqrMagnitude,
|
||||
mainHumanBone = bvhTriangle.mainHumanBone
|
||||
});
|
||||
if (bestHits.Count == maxCount)
|
||||
{
|
||||
currentMaxSq = GetMaxSqrDistance(bestHits, maxDistanceSq);
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
if (sqrMagnitude >= currentMaxSq)
|
||||
{
|
||||
continue;
|
||||
}
|
||||
int index = 0;
|
||||
float sqrDistance = bestHits[0].sqrDistance;
|
||||
for (int j = 1; j < bestHits.Count; j++)
|
||||
{
|
||||
if (bestHits[j].sqrDistance > sqrDistance)
|
||||
{
|
||||
sqrDistance = bestHits[j].sqrDistance;
|
||||
index = j;
|
||||
}
|
||||
}
|
||||
bestHits[index] = new ClosestHit
|
||||
{
|
||||
triangleIndex = num2,
|
||||
closestPoint = vector,
|
||||
normal = bvhTriangle.normal,
|
||||
sqrDistance = sqrMagnitude
|
||||
};
|
||||
currentMaxSq = GetMaxSqrDistance(bestHits, maxDistanceSq);
|
||||
}
|
||||
}
|
||||
}
|
||||
else
|
||||
{
|
||||
int leftChild = bvhNode.leftChild;
|
||||
int rightChild = bvhNode.rightChild;
|
||||
float num3 = DistanceSqPointAABB(p, nodes[leftChild].bounds);
|
||||
float num4 = DistanceSqPointAABB(p, nodes[rightChild].bounds);
|
||||
if (num3 < num4)
|
||||
{
|
||||
QueryClosestNRecursive(leftChild, p, maxCount, maxDistanceSq, bestHits, ref currentMaxSq);
|
||||
QueryClosestNRecursive(rightChild, p, maxCount, maxDistanceSq, bestHits, ref currentMaxSq);
|
||||
}
|
||||
else
|
||||
{
|
||||
QueryClosestNRecursive(rightChild, p, maxCount, maxDistanceSq, bestHits, ref currentMaxSq);
|
||||
QueryClosestNRecursive(leftChild, p, maxCount, maxDistanceSq, bestHits, ref currentMaxSq);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
private float GetMaxSqrDistance(List<ClosestHit> bestHits, float maxDistanceSq)
|
||||
{
|
||||
if (bestHits.Count == 0)
|
||||
{
|
||||
return maxDistanceSq;
|
||||
}
|
||||
float sqrDistance = bestHits[0].sqrDistance;
|
||||
for (int i = 1; i < bestHits.Count; i++)
|
||||
{
|
||||
if (bestHits[i].sqrDistance > sqrDistance)
|
||||
{
|
||||
sqrDistance = bestHits[i].sqrDistance;
|
||||
}
|
||||
}
|
||||
return Mathf.Min(sqrDistance, maxDistanceSq);
|
||||
}
|
||||
|
||||
public Vector3 ComputeMoveVectorToSurface(Vector3 p, float targetGap = 0f)
|
||||
{
|
||||
ClosestHit closestHit = QueryClosest(p);
|
||||
if (closestHit.triangleIndex < 0)
|
||||
{
|
||||
return Vector3.zero;
|
||||
}
|
||||
Vector3 result = closestHit.closestPoint - p;
|
||||
if (targetGap > 0f)
|
||||
{
|
||||
result += closestHit.normal.normalized * targetGap;
|
||||
}
|
||||
return result;
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user