float findOptimalSplitPos(Node p_node, List <object> p_objects, AxisMark p_axis, Vector3 p_currentSize, float p_currentArea, Vector3 p_currentPos, ref float outLeftCost, ref float outRightCost, ref float outCost)
{
float bestpos = 0.0f;
float bestcost = 9999999.0f;
float leftC = outLeftCost, rightC = outRightCost;
//outLeftCost = leftC;
//outRightCost = rightC;
Vector3 axis = p_axis.getVec();
foreach (object obj in p_objects)
{
GameObject gobj = obj as GameObject;
float left_extreme = getLeftExtreme(gobj, axis, p_currentPos);
float right_extreme = getRightExtreme(gobj, axis, p_currentPos);
float cost = calculatecost(p_node, p_objects, left_extreme, axis, p_currentSize, p_currentArea, p_currentPos,
out leftC, out rightC);
if (cost < bestcost)
{
bestcost = cost; bestpos = left_extreme;
outLeftCost = leftC;
outRightCost = rightC;
}
//if (cost >= 1000000) Debug.Log("L!!! " + cost);
cost = calculatecost(p_node, p_objects, right_extreme, axis, p_currentSize, p_currentArea, p_currentPos,
out leftC, out rightC);
if (cost < bestcost)
{
bestcost = cost; bestpos = right_extreme;
outLeftCost = leftC;
outRightCost = rightC;
}
//if (cost >= 1000000) Debug.Log("R!!! " + cost);
}
outCost = bestcost;
return(bestpos);
}
void buildTree(Node p_node, List<object> p_objects, int p_dimsz, int p_dim, int p_idx, Vector3 pos, Vector3 parentSize, float p_cost)
{
p_node.pos = pos;
p_node.size = parentSize;
if (p_dimsz > 1 || p_objects.Count < 3 || p_idx/*<<1*/>16000/*m_tree.Length-2*/)
{
if (p_dimsz > 30) Debug.DrawLine(pos, pos + Vector3.up * 10.0f, Color.green, 100000.0f);
if (p_objects.Count < 3) Debug.DrawLine(pos, pos + Vector3.up * 10.0f, Color.white, 100000.0f);
if (p_idx>8000) Debug.DrawLine(pos, pos + Vector3.up * 10.0f, Color.black, 100000.0f);
p_node.m_objects = p_objects; // in c++ do deep copy here
p_node.m_isLeaf = true;
return;
}
if (p_dim > 2) p_dim = 0;
AxisMark splitPlane=new AxisMark();
splitPlane.setVec(p_dim);
float costLeft = p_cost;
float costRight = p_cost;
float cost = float.MaxValue;
float currentArea = calculateArea(parentSize);
float splitpos = findOptimalSplitPos(p_node, p_objects, splitPlane, parentSize, currentArea, pos, ref costLeft, ref costRight, ref cost);
// extra break condition if too expensive
Debug.Log(costRight + costLeft + " " + p_cost);
if (splitpos != 0.0f && cost > m_intersectionCost*p_objects.Count)
{
p_node.m_objects = p_objects; // in c++ do deep copy here
p_node.m_isLeaf = true;
Debug.DrawLine(pos, pos + Vector3.up * 10.0f, Color.red, 100000.0f);
return;
}
Vector3 split = splitPlane.getVec();
Vector3 offset = split * splitpos;
Vector3 currentOrigo = pos + offset;
Vector3 lsize;
Vector3 rsize;
getChildVoxelsMeasurement(splitpos, split, parentSize,
out lsize, out rsize);
Vector3 leftBoxPos = currentOrigo + 0.5f * entrywiseMul(lsize, split);
Vector3 rightBoxPos = currentOrigo - 0.5f * entrywiseMul(rsize, split);
Vector3 leftBox = lsize;
Vector3 rightBox = rsize;
Node leftnode = new Node();
Node rightnode = new Node();
m_tree.Add(leftnode);
int leftChildId = m_tree.Count - 1;
m_tree.Add(rightnode);
p_node.m_leftChildIdx = leftChildId;
//p_idx << 1;
p_node.m_split = splitPlane;
p_node.m_position = splitpos;
Vector3 splitoffset = entrywiseMul(parentSize*0.5f, split);
float splitposoffset = splitoffset.x + splitoffset.y + splitoffset.z;
p_node.m_fposition = splitpos + splitposoffset;
//
// m_tree[p_node.m_leftChildIdx] = leftnode;
// m_tree[p_node.m_leftChildIdx+1] = rightnode;
//
List<object> leftObjects = new List<object>();
List<object> rightObjects = new List<object>();
//m_tempObjectsStack.Push(rightObjects);
//m_tempObjectsStack.Push(leftObjects);
//
foreach (object obj in p_objects)
{
if (objIntersectNode(/*true, splitpos, splitPlane, */obj,leftBoxPos, leftBox))
{
//p_node.m_objects.Remove(obj);
leftObjects.Add(obj);
}
if (objIntersectNode(/*false, splitpos, splitPlane, */obj, rightBoxPos, rightBox))
{
//p_node.m_objects.Remove(obj);
rightObjects.Add(obj);
}
}
//Debug.Log("stack: "+m_tempObjectsStack.Count);
buildTree(leftnode, leftObjects, p_dimsz + 1, p_dim + 1, p_node.m_leftChildIdx, leftBoxPos, leftBox, costLeft); // power of two structure
//m_tempObjectsStack.Pop();
buildTree(rightnode, rightObjects, p_dimsz + 1, p_dim + 1, p_node.m_leftChildIdx + 1, rightBoxPos, rightBox, costRight);
//m_tempObjectsStack.Pop();
}
float findOptimalSplitPos(Node p_node, List<object> p_objects, AxisMark p_axis, Vector3 p_currentSize, float p_currentArea, Vector3 p_currentPos, ref float outLeftCost, ref float outRightCost, ref float outCost)
{
float bestpos = 0.0f;
float bestcost = 9999999.0f;
float leftC = outLeftCost, rightC = outRightCost;
//outLeftCost = leftC;
//outRightCost = rightC;
Vector3 axis = p_axis.getVec();
foreach (object obj in p_objects)
{
GameObject gobj = obj as GameObject;
float left_extreme = getLeftExtreme(gobj, axis, p_currentPos);
float right_extreme = getRightExtreme(gobj, axis, p_currentPos);
float cost = calculatecost(p_node, p_objects, left_extreme, axis, p_currentSize, p_currentArea, p_currentPos,
out leftC, out rightC);
if (cost < bestcost)
{
bestcost = cost; bestpos = left_extreme;
outLeftCost = leftC;
outRightCost = rightC;
}
//if (cost >= 1000000) Debug.Log("L!!! " + cost);
cost = calculatecost(p_node, p_objects, right_extreme, axis, p_currentSize, p_currentArea, p_currentPos,
out leftC, out rightC);
if (cost < bestcost)
{
bestcost = cost; bestpos = right_extreme;
outLeftCost = leftC;
outRightCost = rightC;
}
//if (cost >= 1000000) Debug.Log("R!!! " + cost);
}
outCost = bestcost;
return bestpos;
}
void buildTree(Node p_node, List <object> p_objects, int p_dimsz, int p_dim, int p_idx, Vector3 pos, Vector3 parentSize, float p_cost)
{
p_node.pos = pos;
p_node.size = parentSize;
if (p_dimsz > 1 || p_objects.Count < 3 || p_idx /*<<1*/ > 16000 /*m_tree.Length-2*/)
{
if (p_dimsz > 30)
{
Debug.DrawLine(pos, pos + Vector3.up * 10.0f, Color.green, 100000.0f);
}
if (p_objects.Count < 3)
{
Debug.DrawLine(pos, pos + Vector3.up * 10.0f, Color.white, 100000.0f);
}
if (p_idx > 8000)
{
Debug.DrawLine(pos, pos + Vector3.up * 10.0f, Color.black, 100000.0f);
}
p_node.m_objects = p_objects; // in c++ do deep copy here
p_node.m_isLeaf = true;
return;
}
if (p_dim > 2)
{
p_dim = 0;
}
AxisMark splitPlane = new AxisMark();
splitPlane.setVec(p_dim);
float costLeft = p_cost;
float costRight = p_cost;
float cost = float.MaxValue;
float currentArea = calculateArea(parentSize);
float splitpos = findOptimalSplitPos(p_node, p_objects, splitPlane, parentSize, currentArea, pos, ref costLeft, ref costRight, ref cost);
// extra break condition if too expensive
Debug.Log(costRight + costLeft + " " + p_cost);
if (splitpos != 0.0f && cost > m_intersectionCost * p_objects.Count)
{
p_node.m_objects = p_objects; // in c++ do deep copy here
p_node.m_isLeaf = true;
Debug.DrawLine(pos, pos + Vector3.up * 10.0f, Color.red, 100000.0f);
return;
}
Vector3 split = splitPlane.getVec();
Vector3 offset = split * splitpos;
Vector3 currentOrigo = pos + offset;
Vector3 lsize;
Vector3 rsize;
getChildVoxelsMeasurement(splitpos, split, parentSize,
out lsize, out rsize);
Vector3 leftBoxPos = currentOrigo + 0.5f * entrywiseMul(lsize, split);
Vector3 rightBoxPos = currentOrigo - 0.5f * entrywiseMul(rsize, split);
Vector3 leftBox = lsize;
Vector3 rightBox = rsize;
Node leftnode = new Node();
Node rightnode = new Node();
m_tree.Add(leftnode);
int leftChildId = m_tree.Count - 1;
m_tree.Add(rightnode);
p_node.m_leftChildIdx = leftChildId;
//p_idx << 1;
p_node.m_split = splitPlane;
p_node.m_position = splitpos;
Vector3 splitoffset = entrywiseMul(parentSize * 0.5f, split);
float splitposoffset = splitoffset.x + splitoffset.y + splitoffset.z;
p_node.m_fposition = splitpos + splitposoffset;
//
// m_tree[p_node.m_leftChildIdx] = leftnode;
// m_tree[p_node.m_leftChildIdx+1] = rightnode;
//
List <object> leftObjects = new List <object>();
List <object> rightObjects = new List <object>();
//m_tempObjectsStack.Push(rightObjects);
//m_tempObjectsStack.Push(leftObjects);
//
foreach (object obj in p_objects)
{
if (objIntersectNode(/*true, splitpos, splitPlane, */ obj, leftBoxPos, leftBox))
{
//p_node.m_objects.Remove(obj);
leftObjects.Add(obj);
}
if (objIntersectNode(/*false, splitpos, splitPlane, */ obj, rightBoxPos, rightBox))
{
//p_node.m_objects.Remove(obj);
rightObjects.Add(obj);
}
}
//Debug.Log("stack: "+m_tempObjectsStack.Count);
buildTree(leftnode, leftObjects, p_dimsz + 1, p_dim + 1, p_node.m_leftChildIdx, leftBoxPos, leftBox, costLeft); // power of two structure
//m_tempObjectsStack.Pop();
buildTree(rightnode, rightObjects, p_dimsz + 1, p_dim + 1, p_node.m_leftChildIdx + 1, rightBoxPos, rightBox, costRight);
//m_tempObjectsStack.Pop();
}