public virtual CollisionContent MakeCollisionContent(Gather g)
{
// todo: I really should find the major modes, and align the AABB to
// those by rotating. Then counter-rotate when collision testing.
// Oh, well.
CollisionContent cc = new CollisionContent();
cc.Bounds = g.Bounds;
Vector3 hd = cc.Bounds.HalfDim;
Vector3 hc = cc.Bounds.Center;
hd.X = Math.Max(hd.X, Math.Max(hd.Y, hd.Z));
hd.Y = hd.X;
hd.Z = hd.X;
cc.Bounds.Lo = hc - hd;
cc.Bounds.Hi = hc + hd;
cc.Triangles = g.Triangles.ToArray();
cc.Vertices = g.Vertices.ToArray();
List<TreeNode> TreeNodes = new List<TreeNode>();
int[] ixs = new int[cc.Triangles.Length];
for (int i = 0; i < ixs.Length; ++i)
ixs[i] = i;
BuildNodes(cc, ixs, 0, ixs.Length, TreeNodes, cc.Bounds,
g.BoundingSpheres.ToArray());
cc.Nodes = TreeNodes.ToArray();
for (int i = 0; i < ixs.Length; ++i)
{
cc.Triangles[i] = g.Triangles[ixs[i]];
cc.Triangles[i].CalcColl(cc);
}
context.Logger.LogImportantMessage("Built CollisionContent with {0} triangles, {1} vertices, {2} cells",
cc.Triangles.Length, cc.Vertices.Length, cc.Nodes.Length);
return cc;
}
// This partition algorithm is inspired by QuickSort; sorting on the
// split axis of the node in question.
private void PartitionSmallerTriangles(CollisionContent cc, int[] tris, int lo, ref int mid, int hi, BoundingSphere[] tbs, Vector3 d, float r)
{
int ilo = lo;
int ihi = hi;
while (lo < hi && lo < ihi && hi > ilo)
{
// if "lo" and "hi - 1" alias, then one of the first ifs will succeed, and
// I'll increment/decrement and break out of the loop!
if (Vector3.Dot(tbs[tris[lo]].Center, d) < r)
{
++lo;
}
else if (!(Vector3.Dot(tbs[tris[hi - 1]].Center, d) < r))
{
--hi;
}
else
{
// I know that "lo" points at a larger tri, and "hi-1" points at a lower tri; so swap them
int j = tris[lo];
tris[lo] = tris[hi - 1];
tris[hi - 1] = j;
// because of the failure of both of the first two tests, I know that
// lo must be lower than hi-1.
Debug.Assert(lo < hi - 1);
++lo;
--hi;
}
}
// "lo" points at the first "high" value; "hi" points at the last "lo" value
Debug.Assert(lo == hi);
Debug.Assert(lo == ihi || !(Vector3.Dot(tbs[tris[lo]].Center, d) < r));
mid = lo;
}
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;
}
void GatherLargeTriangles(ref TreeNode tn, CollisionContent cc, int[] tris, ref int lo, int hi, float r, BoundingSphere[] tbs)
{
for (int i = lo; i < hi; ++i)
{
if (tbs[tris[i]].Radius >= r)
{
int j = tris[i];
tris[i] = tris[lo];
tris[lo] = j;
++lo;
}
}
}
internal bool Intersects(CollisionContent cc, ref OBB box)
{
Vector3 bc = box.Pos;
Vector3 va = cc.Vertices[VertexA] - bc;
Vector3 vb = cc.Vertices[VertexB] - bc;
Vector3 vc = cc.Vertices[VertexC] - bc;
Vector3.TransformNormal(ref va, ref box.InvOriMatrix, out va);
Vector3.TransformNormal(ref vb, ref box.InvOriMatrix, out vb);
Vector3.TransformNormal(ref vc, ref box.InvOriMatrix, out vc);
tempAABB.Set(-box.HalfDim, box.HalfDim);
return AABBIntersects(ref va, ref vb, ref vc, ref tempAABB);
}
public bool Intersects(ref Triangle t, CollisionContent cc)
{
return t.Intersects(cc, ref sphere);
}
internal bool Intersects(CollisionContent cc, ref BoundingSphere sphere)
{
// Test triangle plane against sphere
float sd;
Vector3.Dot(ref sphere.Center, ref Normal, out sd);
if (sd < Distance - sphere.Radius || sd > Distance + sphere.Radius)
return false;
Vector3 pc;
Vector3.Multiply(ref Normal, Distance - sd, out pc);
Vector3.Add(ref pc, ref sphere.Center, out pc);
// Find the closest point on each edge of the triangle
// Set up the triangle vertices
Vector3 a = cc.Vertices[this.VertexA];
Vector3 b = cc.Vertices[this.VertexB];
Vector3 c = cc.Vertices[this.VertexC];
// Calculate the edges (non-normalized)
Vector3 ba, cb, ac;
Vector3.Subtract(ref b, ref a, out ba);
Vector3.Subtract(ref c, ref b, out cb);
Vector3.Subtract(ref a, ref c, out ac);
// Find the square of the length of the edges
float lba, lcb, lac;
Vector3.Dot(ref ba, ref ba, out lba);
Vector3.Dot(ref cb, ref cb, out lcb);
Vector3.Dot(ref ac, ref ac, out lac);
// Calculate vertex-relative position of sphere center
Vector3 pca, pcb, pcc;
Vector3.Subtract(ref pc, ref a, out pca);
Vector3.Subtract(ref pc, ref b, out pcb);
Vector3.Subtract(ref pc, ref c, out pcc);
// Caluclate length-scaled distance along each edge
float dab, dbc, dca;
Vector3.Dot(ref ba, ref pca, out dab);
Vector3.Dot(ref cb, ref pcb, out dbc);
Vector3.Dot(ref ac, ref pcc, out dca);
// calculate 0..1 barycentric coordinate for each edge
float vba = dab / lba;
float vcb = dbc / lcb;
float vac = dca / lac;
// Test barycentric coordinates: if (s >= 0) and (t >= 0) and (s + t <= 1) we're inside.
// Because I've wound each edge, I have to negate one of them (and I only need 2)
if (vba >= 0 && (1 - vac) >= 0 && vba + (1 - vac) <= 1)
{
return true;
}
float r2 = sphere.Radius * sphere.Radius;
float d;
// find actual points, clipped to triangle, and test distance to sphere center
// B-A
if (vba < 0)
ba = a;
else if (vba > 1)
ba = b;
else
{
Vector3.Multiply(ref ba, vba, out ba);
Vector3.Add(ref ba, ref a, out pca);
}
Vector3.Subtract(ref pca, ref sphere.Center, out pca);
Vector3.Dot(ref pca, ref pca, out d);
if (d <= r2)
{
return true;
}
// C-B
if (vcb < 0)
cb = b;
else if (vcb > 1)
cb = c;
else
{
Vector3.Multiply(ref cb, vcb, out cb);
Vector3.Add(ref cb, ref b, out pcb);
}
Vector3.Subtract(ref pcb, ref sphere.Center, out pcb);
Vector3.Dot(ref pcb, ref pcb, out d);
if (d <= r2)
{
return true;
}
// A-C
if (vac < 0)
ac = c;
else if (vac > 1)
ac = a;
else
{
Vector3.Multiply(ref ac, vac, out ac);
Vector3.Add(ref ac, ref c, out pcc);
}
Vector3.Subtract(ref pcc, ref sphere.Center, out pcc);
Vector3.Dot(ref pcc, ref pcc, out d);
if (d <= r2)
{
return true;
}
// nothing fit
return false;
}
public bool Intersects(ref Triangle t, CollisionContent cc)
{
float dd = collRayD;
return t.Intersects(cc, ref collRay, ref dd);
}
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);
}
public void CalcColl(CollisionContent cc)
{
U = cc.Vertices[VertexB] - cc.Vertices[VertexA];
V = cc.Vertices[VertexC] - cc.Vertices[VertexA];
uu = U.LengthSquared();
vv = V.LengthSquared();
uv = Vector3.Dot(U, V);
float d = uv * uv - uu * vv;
if (Math.Abs(d) < 1e-10f)
{
throw new InvalidOperationException(String.Format(
"Degenerate triangle {1} in mesh. Verts: {2} {3} {4}", this,
cc.Vertices[VertexA], cc.Vertices[VertexB], cc.Vertices[VertexC]));
}
di = 1.0f / d;
}
public bool Intersects(CollisionContent cc, ref Ray collRay, ref float dd)
{
Vector3 P = cc.Vertices[VertexA];
Vector3 w0 = collRay.Position - P; // ray position in triangle space
float b = Vector3.Dot(Normal, collRay.Direction);
if (-b < 1e-10) // ray is in plane, or pointing at backside of tri
return false;
float a = Vector3.Dot(Normal, w0);
if (a < 0)
return false; // ray starts below triangle
float r = -a / b;
if (r > dd)
return false; // triangle too far away
Vector3 I = collRay.Position + collRay.Direction * r;
Vector3 W = I - P;
float uw = Vector3.Dot(U, W);
float vw = Vector3.Dot(V, W);
float s = (uv * vw - vv * uw) * di;
if (s < CollMin || s > CollMax) // outside in "s" space
return false;
float t = (uv * uw - uu * vw) * di;
if (t < CollMin || (s + t) > CollMax) // outside in "s-t" space
return false;
// found an intersection
dd = r;
return true;
}
