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 OnDrawGizmos()
{
if (kDopTree != null)
{
Gizmos.matrix = mf.transform.localToWorldMatrix;
List <KDopNode> nodes = kDopTree.nodes;
int leafCount = 0;
for (int nodeIndex = 0; nodeIndex < nodes.Count; ++nodeIndex)
{
if (leafCount == depth)
{
break;
}
KDopNode node = kDopTree.nodes[nodeIndex];
if (node.isLeaf)
{
++leafCount;
KDopBounds bounds = node.boundingVolumes;
Gizmos.color = colors[nodeIndex % colors.Length];
Gizmos.DrawWireCube(bounds.Center, bounds.Size);
}
}
}
Gizmos.matrix = Matrix4x4.identity;
Gizmos.color = Color.white;
Gizmos.DrawLine(linep0.position, linep1.position);
if (kDopTree != null)
{
check.Init(linep0.position, linep1.position, mf.transform.worldToLocalMatrix);
kDopTree.LineCheck(check);
if (check.HitResult.isHit)
{
KDopTriangle tri = kDopTree.triangles[check.HitResult.hitTriangle];
Vector3 hitPoint = check.LocalStart + check.HitResult.hitTime * check.LocalDir;
Gizmos.matrix = mf.transform.localToWorldMatrix;
Gizmos.color = Color.red;
Gizmos.DrawLine(tri.v0, tri.v1);
Gizmos.DrawLine(tri.v1, tri.v2);
Gizmos.DrawLine(tri.v2, tri.v0);
Gizmos.DrawSphere(hitPoint, 0.01f);
Gizmos.DrawLine(hitPoint, hitPoint + tri.GetNormal() * 0.3f);
}
}
}
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);
}
