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);
}
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);
}
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();
}
