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