internal bool Overlaps(ref AABB aabb, float expand)
{
// test OBB in AABB space
float dx = Math.Abs(PosOriMatrix.Right.X) * HalfDim.X +
Math.Abs(PosOriMatrix.Up.X) * HalfDim.Y +
Math.Abs(PosOriMatrix.Backward.X) * HalfDim.Z;
if (Pos.X + dx < aabb.Lo.X ||
Pos.X - dx > aabb.Hi.X)
return false;
float dy = Math.Abs(PosOriMatrix.Right.Y) * HalfDim.X +
Math.Abs(PosOriMatrix.Up.Y) * HalfDim.Y +
Math.Abs(PosOriMatrix.Backward.Y) * HalfDim.Z;
if (Pos.Y + dy < aabb.Lo.Y ||
Pos.Y - dy > aabb.Hi.Y)
return false;
float dz = Math.Abs(PosOriMatrix.Right.Z) * HalfDim.X +
Math.Abs(PosOriMatrix.Up.Z) * HalfDim.Y +
Math.Abs(PosOriMatrix.Backward.Z) * HalfDim.Z;
if (Pos.Z + dz < aabb.Lo.Z ||
Pos.Z - dz > aabb.Hi.Z)
return false;
// TODO: test AABB in OBB space
// TODO: test 9 cross axes
return true;
}
public void SetUnion(AABB other)
{
Lo.X = Math.Min(Lo.X, other.Lo.X);
Lo.Y = Math.Min(Lo.Y, other.Lo.Y);
Lo.Z = Math.Min(Lo.Z, other.Lo.Z);
Hi.X = Math.Max(Hi.X, other.Hi.X);
Hi.Y = Math.Max(Hi.Y, other.Hi.Y);
Hi.Z = Math.Max(Hi.Z, other.Hi.Z);
}
public bool SetIntersection(AABB other)
{
if (Empty)
{
Lo = other.Lo;
Hi = other.Hi;
}
else if (!other.Empty)
{
Lo.X = Math.Max(Lo.X, other.Lo.X);
Lo.Y = Math.Max(Lo.Y, other.Lo.Y);
Lo.Z = Math.Max(Lo.Z, other.Lo.Z);
Hi.X = Math.Min(Hi.X, other.Hi.X);
Hi.Y = Math.Min(Hi.Y, other.Hi.Y);
Hi.Z = Math.Min(Hi.Z, other.Hi.Z);
}
if (Hi.X > Lo.X && Hi.Y > Lo.Y && Hi.Z > Lo.Z)
return true;
Hi = Lo;
return false;
}
public bool Overlaps(ref AABB other, float expand)
{
if (other.Lo.X >= Hi.X + expand || other.Hi.X <= Lo.X - expand) return false;
if (other.Lo.Y >= Hi.Y + expand || other.Hi.Y <= Lo.Y - expand) return false;
if (other.Lo.Z >= Hi.Z + expand || other.Hi.Z <= Lo.Z - expand) return false;
return true;
}
public bool Overlaps(AABB other)
{
return Overlaps(ref other, 0);
}
public bool Overlaps(ref AABB aabb, float expand)
{
return aabb.Overlaps(ref sphere, expand);
}
void BuildNodes(CollisionContent cc, int[] tris, int lo, int hi, List<TreeNode> nodes, AABB bounds,
BoundingSphere[] tbs)
{
TreeNode tn = new TreeNode();
tn.TriStart = lo;
tn.Expansion = (bounds.Hi - bounds.Lo).Length() * ExpansionFactor;
GatherLargeTriangles(ref tn, cc, tris, ref lo, hi, tn.Expansion, tbs);
tn.TriEnd = lo;
int ix = nodes.Count;
nodes.Add(tn);
Vector3 lb = bounds.Lo;
Vector3 ub = bounds.Hi;
Vector3 cb = lb + (ub - lb) * 0.5f;
if (hi > lo + 4)
{
int midX = lo;
PartitionSmallerTriangles(cc, tris, lo, ref midX, hi, tbs, Vector3.Right, cb.X);
int midXlo = lo;
PartitionSmallerTriangles(cc, tris, lo, ref midXlo, midX, tbs, Vector3.Up, cb.Y);
int midXhi = midX;
PartitionSmallerTriangles(cc, tris, midX, ref midXhi, hi, tbs, Vector3.Up, cb.Y);
int midXloYlo = lo;
PartitionSmallerTriangles(cc, tris, lo, ref midXloYlo, midXlo, tbs, Vector3.Backward, cb.Z);
int midXloYhi = midXlo;
PartitionSmallerTriangles(cc, tris, midXlo, ref midXloYhi, midX, tbs, Vector3.Backward, cb.Z);
int midXhiYlo = midX;
PartitionSmallerTriangles(cc, tris, midX, ref midXhiYlo, midXhi, tbs, Vector3.Backward, cb.Z);
int midXhiYhi = midXhi;
PartitionSmallerTriangles(cc, tris, midXhi, ref midXhiYhi, hi, tbs, Vector3.Backward, cb.Z);
if (lo < midXloYlo)
{
tn.Child000 = nodes.Count;
BuildNodes(cc, tris, lo, midXloYlo, nodes, new AABB(lb, cb),
tbs);
}
if (midXloYlo < midXlo)
{
tn.Child001 = nodes.Count;
BuildNodes(cc, tris, midXloYlo, midXlo, nodes,
new AABB(new Vector3(lb.X, lb.Y, cb.Z), new Vector3(cb.X, cb.Y, ub.Z)),
tbs);
}
if (midXlo < midXloYhi)
{
tn.Child010 = nodes.Count;
BuildNodes(cc, tris, midXlo, midXloYhi, nodes,
new AABB(new Vector3(lb.X, cb.Y, lb.Z), new Vector3(cb.X, ub.Y, cb.Z)),
tbs);
}
if (midXloYhi < midX)
{
tn.Child011 = nodes.Count;
BuildNodes(cc, tris, midXloYhi, midX, nodes,
new AABB(new Vector3(lb.X, cb.Y, cb.Z), new Vector3(cb.X, ub.Y, ub.Z)),
tbs);
}
if (midX < midXhiYlo)
{
tn.Child100 = nodes.Count;
BuildNodes(cc, tris, midX, midXhiYlo, nodes,
new AABB(new Vector3(cb.X, lb.Y, lb.Z), new Vector3(ub.X, cb.Y, cb.Z)),
tbs);
}
if (midXhiYlo < midXhi)
{
tn.Child101 = nodes.Count;
BuildNodes(cc, tris, midXhiYlo, midXhi, nodes,
new AABB(new Vector3(cb.X, lb.Y, cb.Z), new Vector3(ub.X, cb.Y, ub.Z)),
tbs);
}
if (midXhi < midXhiYhi)
{
tn.Child110 = nodes.Count;
BuildNodes(cc, tris, midXhi, midXhiYhi, nodes,
new AABB(new Vector3(cb.X, cb.Y, lb.Z), new Vector3(ub.X, ub.Y, cb.Z)),
tbs);
}
if (midXhiYhi < hi)
{
tn.Child111 = nodes.Count;
BuildNodes(cc, tris, midXhiYhi, hi, nodes,
new AABB(new Vector3(cb.X, cb.Y, cb.Z), new Vector3(ub.X, ub.Y, ub.Z)),
tbs);
}
}
else
{
// include them all!
tn.TriEnd = hi;
}
nodes[ix] = tn;
}
bool AABBIntersects(ref Vector3 va, ref Vector3 vb, ref Vector3 vc, ref AABB box)
{
Vector3 hd = box.HalfDim;
if (va.X >= hd.X && vb.X >= hd.X && vc.X >= hd.X)
return false;
if (-va.X >= hd.X && -vb.X >= hd.X && -vc.X >= hd.X)
return false;
if (va.Y >= hd.Y && vb.Y >= hd.Y && vc.Y >= hd.Y)
return false;
if (-va.Y >= hd.Y && -vb.Y >= hd.Y && -vc.Y >= hd.Y)
return false;
if (va.Z >= hd.Z && vb.Z >= hd.Z && vc.Z >= hd.Z)
return false;
if (-va.Z >= hd.Z && -vb.Z >= hd.Z && -vc.Z >= hd.Z)
return false;
// box culled by triangle?
float d = Vector3.Dot(Normal, box.Vertex(0));
bool culled = true;
if (d < Distance)
{
for (int i = 1; i < 8; ++i)
{
if (Vector3.Dot(Normal, box.Vertex(i)) >= Distance)
{
culled = false;
break;
}
}
}
else
{
for (int i = 1; i < 8; ++i)
{
if (Vector3.Dot(Normal, box.Vertex(i)) < Distance)
{
culled = false;
break;
}
}
}
if (culled)
return false;
// triangle culled by cross product between triangle edge and box edge?
// (separating axis theorem)
if (AxisSeparates(ref va, ref vb, ref vc, Vector3.UnitX, ref hd))
return false;
if (AxisSeparates(ref vb, ref vc, ref va, Vector3.UnitX, ref hd))
return false;
if (AxisSeparates(ref vc, ref va, ref vb, Vector3.UnitX, ref hd))
return false;
if (AxisSeparates(ref va, ref vb, ref vc, Vector3.UnitY, ref hd))
return false;
if (AxisSeparates(ref vb, ref vc, ref va, Vector3.UnitY, ref hd))
return false;
if (AxisSeparates(ref vc, ref va, ref vb, Vector3.UnitY, ref hd))
return false;
if (AxisSeparates(ref va, ref vb, ref vc, Vector3.UnitZ, ref hd))
return false;
if (AxisSeparates(ref vb, ref vc, ref va, Vector3.UnitZ, ref hd))
return false;
if (AxisSeparates(ref vc, ref va, ref vb, Vector3.UnitZ, ref hd))
return false;
// ok, so they intersect
return true;
}
public bool Overlaps(ref AABB aabb, float expand)
{
return box.Overlaps(ref aabb, expand);
}
public bool Overlaps(ref AABB aabb, float expand)
{
float dd = collRayD;
return aabb.Intersects(ref collRay, ref dd, expand);
}
public bool Intersects(CollisionContent cc, ref AABB box)
{
// transform to box-relative coordinates
// triangle culled by box major axes?
Vector3 bc = box.Center;
Vector3 va = cc.Vertices[VertexA] - bc;
Vector3 vb = cc.Vertices[VertexB] - bc;
Vector3 vc = cc.Vertices[VertexC] - bc;
return AABBIntersects(ref va, ref vb, ref vc, ref box);
}
void GetNodeTris(int ix, ref AABB aabb, ITester tester)
public void TraverseNode(int ix, ref AABB bounds, ITester tester)
{
#if DEBUG
nodesTested.Add(bounds);
#endif
#if VALIDATE
GetNodeTris(ix, ref bounds, tester);
#else
GetNodeTris(ix, tester);
#endif
float expand = Nodes[ix].Expansion;
Vector3 c = bounds.Lo + (bounds.Hi - bounds.Lo) * 0.5f;
AABB aabb;
aabb.Lo = bounds.Lo;
aabb.Hi = c;
if (Nodes[ix].Child000 != 0 && tester.Overlaps(ref aabb, expand))
{
TraverseNode(Nodes[ix].Child000, ref aabb, tester);
}
aabb.Lo.Z = c.Z;
aabb.Hi.Z = bounds.Hi.Z;
if (Nodes[ix].Child001 != 0 && tester.Overlaps(ref aabb, expand))
{
TraverseNode(Nodes[ix].Child001, ref aabb, tester);
}
aabb.Lo.Z = bounds.Lo.Z;
aabb.Hi.Z = c.Z;
aabb.Lo.Y = c.Y;
aabb.Hi.Y = bounds.Hi.Y;
if (Nodes[ix].Child010 != 0 && tester.Overlaps(ref aabb, expand))
{
TraverseNode(Nodes[ix].Child010, ref aabb, tester);
}
aabb.Lo.Z = c.Z;
aabb.Hi.Z = bounds.Hi.Z;
if (Nodes[ix].Child011 != 0 && tester.Overlaps(ref aabb, expand))
{
TraverseNode(Nodes[ix].Child011, ref aabb, tester);
}
aabb.Lo.Z = bounds.Lo.Z;
aabb.Hi.Z = c.Z;
aabb.Lo.Y = bounds.Lo.Y;
aabb.Hi.Y = c.Y;
aabb.Lo.X = c.X;
aabb.Hi.X = bounds.Hi.X;
if (Nodes[ix].Child100 != 0 && tester.Overlaps(ref aabb, expand))
{
TraverseNode(Nodes[ix].Child100, ref aabb, tester);
}
aabb.Lo.Z = c.Z;
aabb.Hi.Z = bounds.Hi.Z;
if (Nodes[ix].Child101 != 0 && tester.Overlaps(ref aabb, expand))
{
TraverseNode(Nodes[ix].Child101, ref aabb, tester);
}
aabb.Lo.Z = bounds.Lo.Z;
aabb.Hi.Z = c.Z;
aabb.Lo.Y = c.Y;
aabb.Hi.Y = bounds.Hi.Y;
if (Nodes[ix].Child110 != 0 && tester.Overlaps(ref aabb, expand))
{
TraverseNode(Nodes[ix].Child110, ref aabb, tester);
}
aabb.Lo.Z = c.Z;
aabb.Hi.Z = bounds.Hi.Z;
if (Nodes[ix].Child111 != 0 && tester.Overlaps(ref aabb, expand))
{
TraverseNode(Nodes[ix].Child111, ref aabb, tester);
}
}
public List<int> CollectAABB(AABB bounds)
{
ReturnTriangles.Clear();
if (Bounds.Overlaps(ref bounds, 0))
{
AABBTester rt = new AABBTester();
rt.box = bounds;
TraverseNode(0, ref Bounds, rt);
}
return ReturnTriangles;
}
