private void CreateChildBox(ref AlignedBox box, int sector, out AlignedBox output)
{
Vector3 size;
Vector3.Subtract(ref box.Maximum, ref box.Minimum, out size);
Vector3.Multiply(ref size, 0.5f, out size);
Vector3 p;
switch (sector % 4)
{
case 0: p = new Vector3(0f, 0f, 0f); break;
case 1: p = new Vector3(0f, 1f, 0f); break;
case 2: p = new Vector3(1f, 1f, 0f); break;
case 3: p = new Vector3(1f, 0f, 0f); break;
default: throw new Exception("Internal error: invalid bounding box section.");
}
if (sector > 3)
{
p.Z = 1f;
}
Vector3.Multiply(ref p, ref size, out output.Minimum);
Vector3.Add(ref output.Minimum, ref box.Minimum, out output.Minimum);
Vector3.Add(ref output.Minimum, ref size, out output.Maximum);
}
internal void ProcessTriangles(TriangleFunctor tf)
{
Triangle tri;
int stackIdx = 0, stackSize = tf.Stack.Length;
int bufIdx = 0, bufSize = tf.Buffer.Length;
var box = tf.BoundingBox;
if (AlignedBox.Intersect(ref box, ref _nodes[0].BoundingBox) == BoxIntersectType.None)
{
return;
}
tf.Stack[0] = 0;
while (stackIdx >= 0)
{
int nodeIdx = tf.Stack[stackIdx];
if (AlignedBox.Intersect(ref box, ref _nodes[nodeIdx].BoundingBox) != BoxIntersectType.None)
{
for (int i = 0; i < _nodes[nodeIdx].Triangles.Length; i++)
{
if (AlignedBox.Intersect(ref box, ref _triangleBoxes[_nodes[nodeIdx].Triangles[i]]) != BoxIntersectType.None)
{
int[] vertices = _triangles[_nodes[nodeIdx].Triangles[i]];
tri.V1 = _body[vertices[0]];
tri.V2 = _body[vertices[1]];
tri.V3 = _body[vertices[2]];
tri.Normal = _normals[_nodes[nodeIdx].Triangles[i]];
tf.Buffer[bufIdx++] = tri;
if (bufIdx == bufSize)
{
tf.Process(bufSize);
bufIdx = 0;
}
}
}
}
stackIdx--;
for (int i = 0; i < _nodes[nodeIdx].Children.Length; i++)
{
if (stackIdx == stackSize - 2)
{
stackSize = tf.GrowStack();
}
tf.Stack[++stackIdx] = _nodes[nodeIdx].Children[i];
}
}
if (bufIdx > 0)
{
tf.Process(bufIdx);
}
}
public void Initialize(CollisionFunctor cf, PolyhedronPart a, MeshPart b, Vector3 delta)
{
_cf = cf;
_a = a;
_b = b;
_delta = delta;
_useSweptTest = _delta != Vector3.Zero;
Depth = float.MaxValue;
a.Center(out _center);
// transform bounding box to body space
b.BoundingBox(out BoundingBox);
AlignedBox.Transform(ref BoundingBox, ref b.TransformInverse, out BoundingBox);
}
public void Initialize(CollisionFunctor cf, CapsulePart a, MeshPart b, Vector3 offset)
{
_cf = cf;
_b = b;
_radius = a.World.Radius * b.TransformInverse.Scale;
_radiusSquared = _radius * _radius;
_offset = offset;
_hasCollision = false;
Vector3.Add(ref a.World.P1, ref _offset, out _cap.P1);
Vector3.Add(ref a.World.P2, ref _offset, out _cap.P2);
// calculate points and bounding box in body space
var radius = new Vector3(_radius);
Vector3.Transform(ref _cap.P1, ref b.TransformInverse.Combined, out _cap.P1);
Vector3.Transform(ref _cap.P2, ref b.TransformInverse.Combined, out _cap.P2);
AlignedBox.Fit(ref _cap.P1, ref _cap.P2, out BoundingBox);
Vector3.Subtract(ref BoundingBox.Minimum, ref radius, out BoundingBox.Minimum);
Vector3.Add(ref BoundingBox.Maximum, ref radius, out BoundingBox.Maximum);
}
public void Initialize(CollisionFunctor cf, SpherePart a, MeshPart b, Vector3 delta)
{
_cf = cf;
_a = a;
_b = b;
_radius = a.World.Radius * b.TransformInverse.Scale;
_radiusSquared = _radius * _radius;
Vector3.Transform(ref a.World.Center, ref b.TransformInverse.Combined, out _path.P1);
Vector3.Transform(ref delta, ref b.TransformInverse.Orientation, out delta);
Vector3.Multiply(ref delta, b.TransformInverse.Scale, out delta);
Vector3.Add(ref _path.P1, ref delta, out _path.P2);
AlignedBox.Fit(ref _path.P1, ref _path.P2, out BoundingBox);
var radius = new Vector3(_radius);
Vector3.Subtract(ref BoundingBox.Minimum, ref radius, out BoundingBox.Minimum);
Vector3.Add(ref BoundingBox.Maximum, ref radius, out BoundingBox.Maximum);
}
private void BuildOctree()
{
var tempNodes = new List <TempNode>();
var root = new TempNode();
tempNodes.Add(root);
// create bounding boxes for all triangles
_triangleBoxes = new AlignedBox[_triangles.Length];
for (int i = 0; i < _triangles.Length; i++)
{
AlignedBox.Fit(ref _body[_triangles[i][0]], ref _body[_triangles[i][1]], ref _body[_triangles[i][2]], out _triangleBoxes[i]);
AlignedBox.Merge(ref root.BoundingBox, ref _triangleBoxes[i], out root.BoundingBox);
}
var subBoxes = new AlignedBox[8];
// add each triangle to the bounding box
for (int i = 0; i < _triangles.Length; i++)
{
int idx = 0;
var box = root.BoundingBox;
while (AlignedBox.Intersect(ref tempNodes[idx].BoundingBox, ref _triangleBoxes[i]) == BoxIntersectType.AContainsB)
{
int sector = -1;
for (int j = 0; j < 8; j++)
{
CreateChildBox(ref tempNodes[idx].BoundingBox, j, out subBoxes[j]);
if (AlignedBox.Intersect(ref subBoxes[j], ref _triangleBoxes[i]) == BoxIntersectType.AContainsB)
{
sector = j;
break;
}
}
if (sector == -1)
{
tempNodes[idx].Triangles.Add(i);
break;
}
else
{
if (tempNodes[idx].Children[sector] > 0)
{
idx = tempNodes[idx].Children[sector];
}
else
{
var child = new TempNode();
child.BoundingBox = subBoxes[sector];
tempNodes.Add(child);
idx = tempNodes[idx].Children[sector] = tempNodes.Count - 1;
}
}
}
}
_nodes = (from node in tempNodes
select new Node()
{
Children = (from i in node.Children where i > 0 select(ushort) i).ToArray(),
Triangles = (from i in node.Triangles select(ushort) i).ToArray(),
BoundingBox = node.BoundingBox
}).ToArray();
}