private void LineCheckTriangles(KDopCollisionCheck check, KDopTree kDopTree)
{
for (int triIndex = data.startIndex; triIndex < data.startIndex + data.numTriangles; ++triIndex)
{
KDopTriangle tri = kDopTree.triangles[triIndex];
Vector3 trip0 = tri.v0;
Vector3 trip1 = tri.v1;
Vector3 trip2 = tri.v2;
Vector3 linep0 = check.LocalStart;
Vector3 linep1 = check.LocalEnd;
// Line is on the same side of triangle-plane
Vector3 triNormal = Vector3.Cross(trip1 - trip0, trip2 - trip0);
triNormal.Normalize();
float triPlaneD = Vector3.Dot(trip0, triNormal);
Vector4 triPlane = new Vector4(triNormal.x, triNormal.y, triNormal.z, triPlaneD);
float line0D = Vector3.Dot(linep0, triNormal) - triPlaneD;
float line1D = Vector3.Dot(linep1, triNormal) - triPlaneD;
if (line0D * line1D > 0)
{
continue;
}
// Figure out the hit point(intersection)
float hitTime = line0D / (line0D - line1D);
Vector3 lineDir = linep1 - linep0;
Vector3 hitP = linep0 + lineDir * hitTime;
// Check if the point point is inside the triangle
s_trianglePoints[0] = trip0;
s_trianglePoints[1] = trip1;
s_trianglePoints[2] = trip2;
for (int sideIndex = 0; sideIndex < 3; ++sideIndex)
{
Vector3 edge = s_trianglePoints[(sideIndex + 1) % 3] - s_trianglePoints[sideIndex];
Vector3 sideDir = Vector3.Cross(triNormal, edge);
Vector3 hitDir = hitP - s_trianglePoints[sideIndex];
float side = Vector3.Dot(hitDir, sideDir);
if (side < 0)
{
// Hit point is outside the triangle.
hitTime = float.MaxValue;
break;
}
}
if (hitTime < check.HitResult.hitTime)
{
check.HitResult.hitTime = hitTime;
check.HitResult.isHit = true;
check.HitResult.hitTriangle = triIndex;
}
}
}
private void Start()
{
colors = new Color[100];
for (int i = 0; i < colors.Length; ++i)
{
colors[i] = new Color(Random.value, Random.value, Random.value);
}
kDopTree = new KDopTree();
System.Diagnostics.Stopwatch stopWatch = new System.Diagnostics.Stopwatch();
stopWatch.Start();
kDopTree.Build(mf.mesh);
stopWatch.Stop();
UnityEngine.Debug.LogError("Elapsed Milliseconds for Building KDopTree:" + stopWatch.ElapsedMilliseconds);
}
public void LineCheck(KDopCollisionCheck check, KDopTree kDopTree)
{
if (isLeaf)
{
if (LineCheckBounds(check, kDopTree))
{
LineCheckTriangles(check, kDopTree);
}
}
else
{
if (LineCheckBounds(check, kDopTree))
{
kDopTree.nodes[data.leftNode].LineCheck(check, kDopTree);
kDopTree.nodes[data.rightNode].LineCheck(check, kDopTree);
}
}
}
private bool LineCheckBounds(KDopCollisionCheck check, KDopTree kDopTree)
{
Vector3 min = boundingVolumes.min;
Vector3 max = boundingVolumes.max;
Vector3 startP = check.LocalStart;
Vector3 endP = check.LocalEnd;
Vector3 dir = check.LocalEnd - check.LocalStart;
Vector3 oneOverDir = check.LocalOneOverDir;
// Slabs
float _minSlabX = (min.x - startP.x) * oneOverDir.x;
float _minSlabY = (min.y - startP.y) * oneOverDir.y;
float _minSlabZ = (min.z - startP.z) * oneOverDir.z;
float _maxSlabX = (max.x - startP.x) * oneOverDir.x;
float _maxSlabY = (max.y - startP.y) * oneOverDir.y;
float _maxSlabZ = (max.z - startP.z) * oneOverDir.z;
// Min/Max Slabs
float minSlabX = Mathf.Min(_minSlabX, _maxSlabX);
float minSlabY = Mathf.Min(_minSlabY, _maxSlabY);
float minSlabZ = Mathf.Min(_minSlabZ, _maxSlabZ);
float maxSlabX = Mathf.Max(_minSlabX, _maxSlabX);
float maxSlabY = Mathf.Max(_minSlabY, _maxSlabY);
float maxSlabZ = Mathf.Max(_minSlabZ, _maxSlabZ);
float minSlab = Mathf.Max(Mathf.Max(minSlabX, minSlabY), minSlabZ);
float maxSlab = Mathf.Min(Mathf.Min(maxSlabX, maxSlabY), maxSlabZ);
bool bHit = maxSlab >= 0.0f && maxSlab >= minSlab && minSlab <= 1.0f;
if (bHit)
{
int hitSurface = 0;
if (minSlab >= 0 && minSlab <= 1)
{
// Draw first hit point
++hitSurface;
}
if (maxSlab >= 0 && maxSlab <= 1)
{
// Draw second hit point
++hitSurface;
}
if (hitSurface == 0)
{
// line segment inside bounds
return(true);
}
else
{
// line segment hit surface
return(true);
}
}
else
{
// line segment hit nothing
return(false);
}
}
public KDopBounds SplitTriangleList(int startIndex, int numTriangles, KDopTree kDopTree)
{
if (numTriangles > 4)
{
isLeaf = false;
int BestPlane = -1;
float BestMean = 0.0f;
float BestVariance = 0.0f;
for (int nPlane = 0; nPlane < KDopTree.NUM_PLANES; nPlane++)
{
float Mean = 0.0f;
float Variance = 0.0f;
for (int nTriangle = startIndex; nTriangle < startIndex + numTriangles; nTriangle++)
{
Mean += kDopTree.triangles[nTriangle].GetCentroid()[nPlane];
}
Mean /= (float)numTriangles;
for (int nTriangle = startIndex; nTriangle < startIndex + numTriangles; nTriangle++)
{
float Dot = kDopTree.triangles[nTriangle].GetCentroid()[nPlane];
Variance += (Dot - Mean) * (Dot - Mean);
}
Variance /= (float)numTriangles;
if (Variance >= BestVariance)
{
BestPlane = nPlane;
BestVariance = Variance;
BestMean = Mean;
}
}
int Left = startIndex - 1;
int Right = startIndex + numTriangles;
while (Left < Right)
{
float Dot;
do
{
Dot = kDopTree.triangles[++Left].GetCentroid()[BestPlane];
}while (Dot < BestMean && Left < Right);
do
{
Dot = kDopTree.triangles[--Right].GetCentroid()[BestPlane];
}while (Dot >= BestMean && Right > 0 && Left < Right);
if (Left < Right)
{
KDopTriangle Temp = kDopTree.triangles[Left];
kDopTree.triangles[Left] = kDopTree.triangles[Right];
kDopTree.triangles[Right] = Temp;
}
}
if (Left == startIndex + numTriangles || Right == startIndex)
{
Left = startIndex + (numTriangles / 2);
}
KDopNode leftNode = new KDopNode();
int leftNodeIndex = kDopTree.nodes.Count;
kDopTree.nodes.Add(leftNode);
data.leftNode = leftNodeIndex;
KDopNode rightNode = new KDopNode();
int rightNodeIndex = kDopTree.nodes.Count;
kDopTree.nodes.Add(rightNode);
data.rightNode = rightNodeIndex;
KDopBounds leftBoundingVolume = leftNode.SplitTriangleList(startIndex, Left - startIndex, kDopTree);
KDopBounds rightBoundingVolume = rightNode.SplitTriangleList(Left, startIndex + numTriangles - Left, kDopTree);
boundingVolumes.Encapsulate(leftBoundingVolume);
boundingVolumes.Encapsulate(rightBoundingVolume);
kDopTree.nodes[leftNodeIndex] = leftNode;
kDopTree.nodes[rightNodeIndex] = rightNode;
}
else
{
isLeaf = true;
data.startIndex = startIndex;
data.numTriangles = numTriangles;
for (int triIndex = data.startIndex; triIndex < data.startIndex + numTriangles; ++triIndex)
{
KDopTriangle tri = kDopTree.triangles[triIndex];
boundingVolumes.Encapsulate(tri.v0);
boundingVolumes.Encapsulate(tri.v1);
boundingVolumes.Encapsulate(tri.v2);
}
}
return(boundingVolumes);
}
