767 lines
27 KiB
C#
767 lines
27 KiB
C#
// 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 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()
|
|
{
|
|
this.triangleUtil = new TriangleUtil();
|
|
}
|
|
|
|
private HumanBodyBones[] BuildVertexMainHumanBone(SkinnedMeshRenderer smr, Animator animator, HumanBodyBones[] bodyBones)
|
|
{
|
|
Mesh sharedMesh = smr.sharedMesh;
|
|
BoneWeight[] boneWeights = sharedMesh.boneWeights;
|
|
int[] boneToBodyIndex = this.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 = this.BuildBoneToBodyIndexMap(renderer2, animator, this.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 = this.ComputeTriangleMainHumanBoneIndex(num4, num5, num6, boneWeights, boneToBodyIndex, this.humanBones.Length);
|
|
HumanBodyBones mainHumanBone = ((num7 >= 0) ? this.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>();
|
|
this.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>();
|
|
this.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>();
|
|
this.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 = this.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 = this.BuildRecursive(tris, triIndices, start, num3 - start, outNodes);
|
|
int rightChild = this.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 (this.nodes == null || this.nodes.Length == 0)
|
|
{
|
|
return best;
|
|
}
|
|
this.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 (this.nodes == null || this.nodes.Length == 0)
|
|
{
|
|
return best;
|
|
}
|
|
if (allowedBones == null || allowedBones.Count == 0)
|
|
{
|
|
this.QueryClosestRecursive(0, point, ref best);
|
|
}
|
|
else
|
|
{
|
|
this.QueryClosestRecursiveFiltered(0, point, ref best, allowedBones);
|
|
}
|
|
return best;
|
|
}
|
|
|
|
private void QueryClosestRecursiveFiltered(int nodeIndex, Vector3 p, ref ClosestHit best, HashSet<HumanBodyBones> allowedBones)
|
|
{
|
|
BvhNode bvhNode = this.nodes[nodeIndex];
|
|
if (this.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 = this.triIndices[i];
|
|
BvhTriangle bvhTriangle = this.triangles[num2];
|
|
if (allowedBones.Contains(bvhTriangle.mainHumanBone))
|
|
{
|
|
Vector3 vector = this.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 = this.DistanceSqPointAABB(p, this.nodes[leftChild].bounds);
|
|
float num4 = this.DistanceSqPointAABB(p, this.nodes[rightChild].bounds);
|
|
if (num3 < num4)
|
|
{
|
|
this.QueryClosestRecursiveFiltered(leftChild, p, ref best, allowedBones);
|
|
this.QueryClosestRecursiveFiltered(rightChild, p, ref best, allowedBones);
|
|
}
|
|
else
|
|
{
|
|
this.QueryClosestRecursiveFiltered(rightChild, p, ref best, allowedBones);
|
|
this.QueryClosestRecursiveFiltered(leftChild, p, ref best, allowedBones);
|
|
}
|
|
}
|
|
}
|
|
|
|
private void QueryClosestRecursive(int nodeIndex, Vector3 p, ref ClosestHit best)
|
|
{
|
|
BvhNode bvhNode = this.nodes[nodeIndex];
|
|
if (this.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 = this.triIndices[i];
|
|
BvhTriangle bvhTriangle = this.triangles[num2];
|
|
Vector3 vector = this.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 = this.DistanceSqPointAABB(p, this.nodes[leftChild].bounds);
|
|
float num4 = this.DistanceSqPointAABB(p, this.nodes[rightChild].bounds);
|
|
if (num3 < num4)
|
|
{
|
|
this.QueryClosestRecursive(leftChild, p, ref best);
|
|
this.QueryClosestRecursive(rightChild, p, ref best);
|
|
}
|
|
else
|
|
{
|
|
this.QueryClosestRecursive(rightChild, p, ref best);
|
|
this.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 (this.nodes == null || this.nodes.Length == 0 || maxCount <= 0)
|
|
{
|
|
return 0;
|
|
}
|
|
float num = maxDistance * maxDistance;
|
|
float currentMaxSq = num;
|
|
this.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 = this.nodes[nodeIndex];
|
|
if (this.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 = this.triIndices[i];
|
|
BvhTriangle bvhTriangle = this.triangles[num2];
|
|
Vector3 vector = this.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 = this.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 = this.GetMaxSqrDistance(bestHits, maxDistanceSq);
|
|
}
|
|
}
|
|
}
|
|
else
|
|
{
|
|
int leftChild = bvhNode.leftChild;
|
|
int rightChild = bvhNode.rightChild;
|
|
float num3 = this.DistanceSqPointAABB(p, this.nodes[leftChild].bounds);
|
|
float num4 = this.DistanceSqPointAABB(p, this.nodes[rightChild].bounds);
|
|
if (num3 < num4)
|
|
{
|
|
this.QueryClosestNRecursive(leftChild, p, maxCount, maxDistanceSq, bestHits, ref currentMaxSq);
|
|
this.QueryClosestNRecursive(rightChild, p, maxCount, maxDistanceSq, bestHits, ref currentMaxSq);
|
|
}
|
|
else
|
|
{
|
|
this.QueryClosestNRecursive(rightChild, p, maxCount, maxDistanceSq, bestHits, ref currentMaxSq);
|
|
this.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 = this.QueryClosest(p);
|
|
if (closestHit.triangleIndex < 0)
|
|
{
|
|
return Vector3.zero;
|
|
}
|
|
Vector3 result = closestHit.closestPoint - p;
|
|
if (targetGap > 0f)
|
|
{
|
|
result += closestHit.normal.normalized * targetGap;
|
|
}
|
|
return result;
|
|
}
|
|
}
|