static void DrawTreeRecursive(internalNode root, Vector3 origin, float scale, ref internalNode[] nodeData, ref internalNode[] leafData)
{
internalNode ChildA;
internalNode ChildB;
GetNodeChildren(root, out ChildA, out ChildB, ref leafData, ref nodeData);
Gizmos.color = Color.blue;
Vector3 nScale = new Vector3(root.maxPos.x - root.minPos.x, root.maxPos.y - root.minPos.y, root.maxPos.z - root.minPos.z);
Gizmos.DrawWireCube(origin, nScale);
Color semiTransparent = new Color(1, 1, 1, 0.5f);
Debug.DrawLine(origin, origin + new Vector3(-2 * scale, -1, 2), semiTransparent);
Debug.DrawLine(origin, origin + new Vector3(2 * scale, -1, -2), semiTransparent);
DrawNode(ChildA, origin + new Vector3(-2 * scale, -1, 2), ChildA.maxPos - ChildA.minPos);
DrawNode(ChildB, origin + new Vector3(2 * scale, -1, -2), ChildB.maxPos - ChildB.minPos);
if (!isLeaf(ChildA))
{
DrawTreeRecursive(ChildA, origin + new Vector3(-2 * scale, -1, 2), scale * 0.55f, ref nodeData, ref leafData);
}
if (!isLeaf(ChildB))
{
DrawTreeRecursive(ChildB, origin + new Vector3(2 * scale, -1, -2), scale * 0.55f, ref nodeData, ref leafData);
}
}
public static void GetNodeChildren(internalNode node, out internalNode childA, out internalNode childB, ref internalNode[] leafData, ref internalNode[] nodeData)
{
int2 leaves = node.leaves;
int2 intNodes = node.intNodes;
childA = new internalNode();
childB = new internalNode();
childA.nodeId = -1;
childB.nodeId = -1;
if (leaves.x != -1)
{
childA = leafData[leaves.x];
}
if (leaves.y != -1)
{
childB = leafData[leaves.y];
}
if (intNodes.x != -1)
{
childA = nodeData[intNodes.x];
}
if (intNodes.y != -1)
{
childB = nodeData[intNodes.y];
}
}
public static void VisualizeBVHTree(ref internalNode[] nodeData, ref internalNode[] leafData, float treeScale, internalNode sampleNode)
{
internalNode root = nodeData[0];
DrawNode(root, Vector3.zero, root.maxPos - root.minPos);
DrawTreeRecursive(root, Vector3.zero, 2f * treeScale, ref nodeData, ref leafData, sampleNode);
}
static void DrawBoundingRecursive(internalNode node, ref internalNode[] nodeData, ref internalNode[] leafData)
{
sortedNodeData.Add(node);
internalNode ChildA;
internalNode ChildB;
//Vector3 center = (node.minPos + node.maxPos) / 2;
//Vector3 scale = new Vector3(node.maxPos.x - node.minPos.x, node.maxPos.y - node.minPos.y, node.maxPos.z - node.minPos.z);
//Gizmos.DrawWireCube(center * GizmoPosScale, scale * GizmoScale);
GetNodeChildren(node, out ChildA, out ChildB, ref leafData, ref nodeData);
//Debug.DrawLine(origin, origin + new Vector3(-2 * scale, -1, 2));
//Debug.DrawLine(origin, origin + new Vector3(2 * scale, -1, -2));
//DrawNode(ChildA, origin + new Vector3(-2 * scale, -1, 2));
//DrawNode(ChildB, origin + new Vector3(2 * scale, -1, -2));
if (!isLeaf(ChildA))
{
DrawBoundingRecursive(ChildA, ref nodeData, ref leafData);
}
if (!isLeaf(ChildB))
{
DrawBoundingRecursive(ChildB, ref nodeData, ref leafData);
}
}
public static void DrawCube(internalNode node)
{
Vector3 center = (node.minPos + node.maxPos) / 2;
Vector3 scale = new Vector3(node.maxPos.x - node.minPos.x, node.maxPos.y - node.minPos.y, node.maxPos.z - node.minPos.z);
Gizmos.DrawWireCube(center * GizmoPosScale, scale * GizmoPosScale);
}
public static bool isLeaf(internalNode node)
{
if (node.objectId != -1)
{
return(true);
}
return(false);
}
static void DrawNode(internalNode node, Vector3 pos, Vector3 AABBVector)
{
Gizmos.color = Color.red * AABBVector.magnitude;// new Color(AABBVector.normalized.x, AABBVector.normalized.y, AABBVector.normalized.z) * 2;// / node.visited;
if (isLeaf(node))
{
Gizmos.color = Color.green;// / node.visited;
}
Gizmos.DrawSphere(pos, 0.5f);
}
public static void VisualizeBoundingBoxes(ref internalNode[] nodeData, ref internalNode[] leafData, int boundingBoxMin)
{
//CreateBoundingBoxes(ref nodeData,ref leafData);
sortedNodeData = new List <internalNode>();
DrawBoundingRecursive(nodeData[0], ref nodeData, ref leafData);
Vector3 offset = Vector3.zero;
Vector3 scale = Vector3.zero;
for (int i = boundingBoxMin; i < sortedNodeData.Count; i++)
{
internalNode node = sortedNodeData[i];
//offset += new Vector3(scale.x, 0, 0);
//Gizmos.color = Color.white * (node.overlap);
Vector3 center = (node.minPos + node.maxPos) / 2 + offset;
scale = new Vector3(node.maxPos.x - node.minPos.x, node.maxPos.y - node.minPos.y, node.maxPos.z - node.minPos.z);
Gizmos.DrawWireCube(center * GizmoPosScale, scale * GizmoScale);
}
}
public static void CalculateAABB(internalNode node, out Vector3 minPoint, out Vector3 maxPoint, ref internalNode[] nodeData, ref internalNode[] leafData)
{
internalNode childA;
internalNode childB;
GetNodeChildren(node, out childA, out childB, ref leafData, ref nodeData);
Vector3 posAA = childA.minPos;
Vector3 posBA = childB.minPos;
Vector3 posAB = childA.maxPos;
Vector3 posBB = childB.maxPos;
float xmin = Mathf.Min(posAA.x, posBA.x); //Mathf.Min(Mathf.Min(posBA.x, posBB.x), Mathf.Min(posAA.x, posAB.x));
float ymin = Mathf.Min(posAA.y, posBA.y); //Mathf.Min(Mathf.Min(posBA.y, posBB.y), Mathf.Min(posAA.y, posAB.y));
float zmin = Mathf.Min(posAA.z, posBA.z); //Mathf.Min(Mathf.Min(posBA.z, posBB.z), Mathf.Min(posAA.z, posAB.z));
float xmax = Mathf.Max(posAB.x, posBB.x); //Mathf.Max(Mathf.Max(posBA.x, posBB.x), Mathf.Max(posAA.x, posAB.x));
float ymax = Mathf.Max(posAB.y, posBB.y); //Mathf.Max(Mathf.Max(posBA.y, posBB.y), Mathf.Max(posAA.y, posAB.y));
float zmax = Mathf.Max(posAB.z, posBB.z); //Mathf.Max(Mathf.Max(posBA.z, posBB.z), Mathf.Max(posAA.z, posAB.z));
minPoint = new Vector3(xmin, ymin, zmin);
maxPoint = new Vector3(xmax, ymax, zmax);
}
void CreateBoundingBox(internalNode node, ref internalNode[] internalNodes, ref internalNode[] leafNodes)
{
internalNode ChildA;
internalNode ChildB;
PhysicsDebugger.GetNodeChildren(node, out ChildA, out ChildB, ref leafNodes, ref internalNodes);
if (!PhysicsDebugger.isLeaf(ChildA))
{
CreateBoundingBox(ChildA, ref internalNodes, ref leafNodes);
}
if (!PhysicsDebugger.isLeaf(ChildB))
{
CreateBoundingBox(ChildB, ref internalNodes, ref leafNodes);
}
Vector3 minPoint;
Vector3 maxPoint;
PhysicsDebugger.CalculateAABB(node, out minPoint, out maxPoint, ref internalNodes, ref leafNodes);
internalNodes[node.nodeId].minPos = minPoint;
internalNodes[node.nodeId].maxPos = maxPoint;
}
static void DrawNode(internalNode node, Vector3 pos)
{
Gizmos.color = Color.white;// / node.visited;
Gizmos.DrawSphere(pos, 0.5f);
}
static void TraverseBVHIterative(internalNode leaf, float radius, out int[] collisionList, ref internalNode[] internalNodeData, ref internalNode[] leafData)
{
internalNode node = internalNodeData[0];
int[] stack = new int[64];
collisionList = new int[64];
for (uint i = 0; i < 64; i++)
{
stack[i] = -2;
collisionList[i] = -1;
}
int traversalCount = 0;
int collisionCount = 0;
int maxLoop = 0;
Gizmos.color = Color.green;
Vector3 AABBRadius = new Vector3(radius, radius, radius) * 1;
do
{
internalNode childA;
internalNode childB;
GetNodeChildren(node, out childA, out childB, ref leafData, ref internalNodeData);
AABBRadius = new Vector3(radius, radius, radius) * 0;
bool overlapA = AABBOverlap(leaf.minPos - AABBRadius, leaf.maxPos + AABBRadius, childA.minPos - AABBRadius, childA.maxPos + AABBRadius);
bool overlapB = AABBOverlap(leaf.minPos - AABBRadius, leaf.maxPos + AABBRadius, childB.minPos - AABBRadius, childB.maxPos + AABBRadius);
if (overlapB)
{
Gizmos.color = Color.green;
DrawCube(childB);
}
if (overlapA)
{
Gizmos.color = Color.red;
DrawCube(childA);
}
if (overlapA && isLeaf(childA))
{
collisionList[collisionCount] = childA.objectId;
collisionCount++;
}
if (overlapB && isLeaf(childB))
{
collisionList[collisionCount] = childB.objectId;
collisionCount++;
}
bool traverseA = (overlapA && !isLeaf(childA));
bool traverseB = (overlapB && !isLeaf(childB));
if (!traverseA && !traverseB)
{
stack[traversalCount] = -1;
traversalCount--;
traversalCount = traversalCount <= 0 ? 0 : traversalCount;
//Debug.Log("Popping Stack: " + traversalCount);
if (stack[traversalCount] == -1)
{
return;
}
node = internalNodeData[stack[traversalCount]];
}
else
{
if (traverseA)
{
node = childA;
//Gizmos.color = Color.red;
//DrawCube(node);
}
else
{
node = childB;
//Gizmos.color = Color.green;
//DrawCube(node);
}
if (traverseA && traverseB)
{
stack[traversalCount] = childB.nodeId;
traversalCount++;
//Debug.Log("Pushing Stack");
}
}
maxLoop++;
} while (stack[traversalCount] != -1);//traversing && traversalCount < 64);
}
