private static void CollideInnerLeaf(
ref Context context, AlignedBox3TreeNode node1, AlignedBox3TreeNode node2
)
{
Box3 box1, box2;
node1.BoundingBox.CreateBox3(ref context.worldTransform1, out box1);
node2.BoundingBox.CreateBox3(ref context.worldTransform2, out box2);
if (Intersection.IntersectBox3Box3(ref box1, ref box2))
{
// we know already that node2 is a leaf. this makes things simpler...
if (node1.Left.HasChildren)
{
CollideInnerLeaf(ref context, node1.Left, node2);
}
else
{
CollideLeafLeaf(ref context, node1.Left, node2);
}
if (context.needAllContacts || context.contact.Count == 0)
{
if (node1.Right.HasChildren)
{
CollideInnerLeaf(ref context, node1.Right, node2);
}
else
{
CollideLeafLeaf(ref context, node1.Right, node2);
}
}
}
}
private static void CollideInnerLeaf(
Entity entity1, AlignedBox3TreeNode node1, Vector3[] positions1, ref Matrix worldTransform1,
Entity entity2, AlignedBox3TreeNode node2, Vector3[] positions2, ref Matrix worldTransform2,
ref Contact contact, bool reverse
)
{
Sphere3 sphere1, sphere2;
node1.BoundingBox.CreateSphere3(ref worldTransform1, out sphere1);
node2.BoundingBox.CreateSphere3(ref worldTransform2, out sphere2);
if ((sphere1.Center - sphere2.Center).LengthSquared() > (sphere1.Radius + sphere2.Radius) * (sphere1.Radius + sphere2.Radius))
{
return;
}
// we know already that node2 is a leaf. this makes things simpler...
if (node1.Left.HasChildren)
{
CollideInnerLeaf(
entity1, node1.Left, positions1, ref worldTransform1,
entity2, node2, positions2, ref worldTransform2,
ref contact, reverse
);
}
else
{
CollideLeafLeaf(
entity1, node1.Left, positions1, ref worldTransform1,
entity2, node2, positions2, ref worldTransform2,
ref contact, reverse
);
}
if (node1.Right.HasChildren)
{
CollideInnerLeaf(
entity1, node1.Right, positions1, ref worldTransform1,
entity2, node2, positions2, ref worldTransform2,
ref contact, reverse
);
}
else
{
CollideLeafLeaf(
entity1, node1.Right, positions1, ref worldTransform1,
entity2, node2, positions2, ref worldTransform2,
ref contact, reverse
);
}
}
private static void CollideLeafLeaf(
Entity entity1, AlignedBox3TreeNode node1, Vector3[] positions1, ref Matrix worldTransform1,
Entity entity2, AlignedBox3TreeNode node2, Vector3[] positions2, ref Matrix worldTransform2,
ref Contact contact, bool reverse
)
{
Sphere3 sphere1, sphere2;
node1.BoundingBox.CreateSphere3(ref worldTransform1, out sphere1);
node2.BoundingBox.CreateSphere3(ref worldTransform2, out sphere2);
if ((sphere1.Center - sphere2.Center).LengthSquared() > (sphere1.Radius + sphere2.Radius) * (sphere1.Radius + sphere2.Radius))
{
return;
}
// "simple" tri<->tri test...
for (int i = 0; i < node1.NumTriangles; ++i)
{
Triangle3 tri1 = node1.GetTriangle(positions1, i);
tri1.Vertex0 = Vector3.Transform(tri1.Vertex0, worldTransform1);
tri1.Vertex1 = Vector3.Transform(tri1.Vertex1, worldTransform1);
tri1.Vertex2 = Vector3.Transform(tri1.Vertex2, worldTransform1);
for (int j = 0; j < node2.NumTriangles; ++j)
{
Triangle3 tri2 = node2.GetTriangle(positions2, j);
tri2.Vertex0 = Vector3.Transform(tri2.Vertex0, worldTransform2);
tri2.Vertex1 = Vector3.Transform(tri2.Vertex1, worldTransform2);
tri2.Vertex2 = Vector3.Transform(tri2.Vertex2, worldTransform2);
//System.Console.WriteLine("performing triangle<->triangle test!");
bool coplanar;
Vector3 isectpt1, isectpt2;
if (Intersection.IntersectTriangle3Triangle3(ref tri1, ref tri2, out coplanar, out isectpt1, out isectpt2))
{
Vector3 point = (isectpt1 + isectpt2) / 2.0f;
if (!contact.ContainsPoint(ref point))
{
Debug.Assert((reverse && contact.EntityA == entity2) || contact.EntityA == entity1);
Vector3 normal = reverse ? tri2.Normal : tri1.Normal;
contact.AddContactPoint(ref point, ref normal);
}
}
}
}
}
private bool GetIntersectionPoint(
ref Ray3 ray,
AlignedBox3TreeNode node,
Vector3[] positions,
out float t
)
{
AlignedBox3 boundingBox = node.BoundingBox;
if (!Intersection.IntersectRay3AlignedBox3(ref ray, ref boundingBox))
{
t = 0.0f;
return(false);
}
if (node.HasChildren)
{
float t1, t2;
bool isect1 = GetIntersectionPoint(ref ray, node.Left, positions, out t1);
bool isect2 = GetIntersectionPoint(ref ray, node.Right, positions, out t2);
if (isect1 && isect2)
{
t = t1 < t2 ? t1 : t2;
return(true);
}
else if (isect1)
{
t = t1;
return(true);
}
else if (isect2)
{
t = t2;
return(true);
}
else
{
t = 0.0f;
return(false);
}
}
else
{
bool intersection = false;
float smallestT = float.MaxValue;
for (int i = 0; i < node.NumTriangles; ++i)
{
float currentT = 0.0f;
Vector3 isectPt = Vector3.Zero;
Triangle3 tri = node.GetTriangle(positions, i);
if (Intersection.IntersectRay3Triangle3(ref ray, ref tri, out currentT, out isectPt))
{
if (currentT < smallestT)
{
smallestT = currentT;
}
if (!intersection)
{
intersection = true;
}
}
}
if (intersection)
{
t = smallestT;
return(true);
}
else
{
t = 0.0f;
return(false);
}
}
}
private static void CollideInnerInner(
Entity entity1, AlignedBox3TreeNode node1, Vector3[] positions1, ref Matrix worldTransform1,
Entity entity2, AlignedBox3TreeNode node2, Vector3[] positions2, ref Matrix worldTransform2,
ref Contact contact, bool reverse
)
{
Sphere3 sphere1, sphere2;
node1.BoundingBox.CreateSphere3(ref worldTransform1, out sphere1);
node2.BoundingBox.CreateSphere3(ref worldTransform2, out sphere2);
if ((sphere1.Center - sphere2.Center).LengthSquared() > (sphere1.Radius + sphere2.Radius) * (sphere1.Radius + sphere2.Radius))
{
return;
}
if (node1.Left.HasChildren)
{
if (node2.Left.HasChildren)
{
// node1.Left is innernode, node2.Left is innernode
CollideInnerInner(
entity1, node1.Left, positions1, ref worldTransform1,
entity2, node2.Left, positions2, ref worldTransform2,
ref contact, reverse
);
}
else
{
// node1.Left is innernode, node2.Left is leafnode
CollideInnerLeaf(
entity1, node1.Left, positions1, ref worldTransform1,
entity2, node2.Left, positions2, ref worldTransform2,
ref contact, reverse
);
}
if (node2.Right.HasChildren)
{
// node1.Left is innernode, node2.Right is innernode
CollideInnerInner(
entity1, node1.Left, positions1, ref worldTransform1,
entity2, node2.Right, positions2, ref worldTransform2,
ref contact, reverse
);
}
else
{
// node1.Left is innernode, node2.Right is leafnode
CollideInnerLeaf(
entity1, node1.Left, positions1, ref worldTransform1,
entity2, node2.Right, positions2, ref worldTransform2,
ref contact, reverse
);
}
}
else
{
if (node2.Left.HasChildren)
{
// node1.Left is leafnode, node2.Left is innernode
CollideInnerLeaf(
entity2, node2.Left, positions2, ref worldTransform2,
entity1, node1.Left, positions1, ref worldTransform1,
ref contact, !reverse
);
}
else
{
// node1.Left is leafnode, node2.Left is leafnode
CollideLeafLeaf(
entity1, node1.Left, positions1, ref worldTransform1,
entity2, node2.Left, positions2, ref worldTransform2,
ref contact, reverse
);
}
if (node2.Right.HasChildren)
{
// node1.Left is leafnode, node2.Right is innernode
CollideInnerLeaf(
entity2, node2.Right, positions2, ref worldTransform2,
entity1, node1.Left, positions1, ref worldTransform1,
ref contact, !reverse
);
}
else
{
// node1.Left is leafnode, node2.Right is leafnode
CollideLeafLeaf(
entity1, node1.Left, positions1, ref worldTransform1,
entity2, node2.Right, positions2, ref worldTransform2,
ref contact, reverse
);
}
}
if (node1.Right.HasChildren)
{
if (node2.Left.HasChildren)
{
// node1.Right is innernode, node2.Left is innernode
CollideInnerInner(
entity1, node1.Right, positions1, ref worldTransform1,
entity2, node2.Left, positions2, ref worldTransform2,
ref contact, reverse
);
}
else
{
// node1.Right is innernode, node2.Left is leafnode
CollideInnerLeaf(
entity1, node1.Right, positions1, ref worldTransform1,
entity2, node2.Left, positions2, ref worldTransform2,
ref contact, reverse
);
}
if (node2.Right.HasChildren)
{
// node1.Right is innernode, node2.Right is innernode
CollideInnerInner(
entity1, node1.Right, positions1, ref worldTransform1,
entity2, node2.Right, positions2, ref worldTransform2,
ref contact, reverse
);
}
else
{
// node1.Right is innernode, node2.Right is leafnode
CollideInnerLeaf(
entity1, node1.Right, positions1, ref worldTransform1,
entity2, node2.Right, positions2, ref worldTransform2,
ref contact, reverse
);
}
}
else
{
if (node2.Left.HasChildren)
{
// node1.Right is leafnode, node2.Left is innernode
CollideInnerLeaf(
entity2, node2.Left, positions2, ref worldTransform2,
entity1, node1.Right, positions1, ref worldTransform1,
ref contact, !reverse
);
}
else
{
// node1.Right is leafnode, node2.Left is leafnode
CollideLeafLeaf(
entity1, node1.Right, positions1, ref worldTransform1,
entity2, node2.Left, positions2, ref worldTransform2,
ref contact, reverse
);
}
if (node2.Right.HasChildren)
{
// node1.Right is leafnode, node2.Right is innernode
CollideInnerLeaf(
entity2, node2.Right, positions2, ref worldTransform2,
entity1, node1.Right, positions1, ref worldTransform1,
ref contact, !reverse
);
}
else
{
// node1.Right is leafnode, node2.Right is leafnode
CollideLeafLeaf(
entity1, node1.Right, positions1, ref worldTransform1,
entity2, node2.Right, positions2, ref worldTransform2,
ref contact, reverse
);
}
}
}
private static void CollideLeafLeaf(
ref Context context, AlignedBox3TreeNode node1, AlignedBox3TreeNode node2
)
{
Box3 box1, box2;
node1.BoundingBox.CreateBox3(ref context.worldTransform1, out box1);
node2.BoundingBox.CreateBox3(ref context.worldTransform2, out box2);
if (Intersection.IntersectBox3Box3(ref box1, ref box2))
{
// "simple" tri<->tri test...
for (int i = 0; i < node1.NumTriangles; ++i)
{
Triangle3 tri1 = node1.GetTriangle(context.positions1, i);
tri1.Vertex0 = Vector3.Transform(tri1.Vertex0, context.worldTransform1);
tri1.Vertex1 = Vector3.Transform(tri1.Vertex1, context.worldTransform1);
tri1.Vertex2 = Vector3.Transform(tri1.Vertex2, context.worldTransform1);
for (int j = 0; j < node2.NumTriangles; ++j)
{
Triangle3 tri2 = node2.GetTriangle(context.positions2, j);
tri2.Vertex0 = Vector3.Transform(tri2.Vertex0, context.worldTransform2);
tri2.Vertex1 = Vector3.Transform(tri2.Vertex1, context.worldTransform2);
tri2.Vertex2 = Vector3.Transform(tri2.Vertex2, context.worldTransform2);
//System.Console.WriteLine("performing triangle<->triangle test!");
bool coplanar;
Vector3 isectpt1, isectpt2;
if (Intersection.IntersectTriangle3Triangle3(ref tri1, ref tri2, out coplanar, out isectpt1, out isectpt2))
{
Vector3 point = (isectpt1 + isectpt2) / 2.0f;
if (!context.contact.ContainsPoint(ref point))
{
Debug.Assert(context.contact.EntityA == context.entity1 && context.contact.EntityB == context.entity2);
Vector3 normal = tri1.Normal;
context.contact.AddContactPoint(ref point, ref normal);
}
if (!context.needAllContacts)
{
break;
}
}
}
if (!context.needAllContacts && context.contact.Count > 0)
{
break;
}
}
/*
* // Contact c = new Contact(context.entity1, context.entity2);
*
* Vector3 dir = Vector3.Normalize(box2.Center - box1.Center);
* Vector3 p = box1.Center+box2.Center/2;
* /*
* Vector3 p1 = context.entity1.GetVector3(CommonNames.Position);
* Vector3 p2 = context.entity2.GetVector3(CommonNames.Position);
* Vector3 dir = Vector3.Normalize(p2-p1);
* Vector3 p = p2+p1/2;
* /
*
* context.contact.AddContactPoint(ref p, ref dir);*/
// return c;
}
}
private static void CollideInnerInner(
ref Context context, AlignedBox3TreeNode node1, AlignedBox3TreeNode node2
)
{
Box3 box1, box2;
node1.BoundingBox.CreateBox3(ref context.worldTransform1, out box1);
node2.BoundingBox.CreateBox3(ref context.worldTransform2, out box2);
if (Intersection.IntersectBox3Box3(ref box1, ref box2))
{
if (node1.Left.HasChildren)
{
if (node2.Left.HasChildren)
{
// node1.Left is innernode, node2.Left is innernode
CollideInnerInner(ref context, node1.Left, node2.Left);
}
else
{
// node1.Left is innernode, node2.Left is leafnode
CollideInnerLeaf(ref context, node1.Left, node2.Left);
}
if (context.needAllContacts || context.contact.Count == 0)
{
if (node2.Right.HasChildren)
{
// node1.Left is innernode, node2.Right is innernode
CollideInnerInner(ref context, node1.Left, node2.Right);
}
else
{
// node1.Left is innernode, node2.Right is leafnode
CollideInnerLeaf(ref context, node1.Left, node2.Right);
}
}
}
else
{
if (node2.Left.HasChildren)
{
// node1.Left is leafnode, node2.Left is innernode
CollideLeafInner(ref context, node1.Left, node2.Left);
}
else
{
// node1.Left is leafnode, node2.Left is leafnode
CollideLeafLeaf(ref context, node1.Left, node2.Left);
}
if (context.needAllContacts || context.contact.Count == 0)
{
if (node2.Right.HasChildren)
{
// node1.Left is leafnode, node2.Right is innernode
CollideLeafInner(ref context, node1.Left, node2.Right);
}
else
{
// node1.Left is leafnode, node2.Right is leafnode
CollideLeafLeaf(ref context, node1.Left, node2.Right);
}
}
}
if (node1.Right.HasChildren)
{
if (node2.Left.HasChildren)
{
// node1.Right is innernode, node2.Left is innernode
CollideInnerInner(ref context, node1.Right, node2.Left);
}
else
{
// node1.Right is innernode, node2.Left is leafnode
CollideInnerLeaf(ref context, node1.Right, node2.Left);
}
if (context.needAllContacts || context.contact.Count == 0)
{
if (node2.Right.HasChildren)
{
// node1.Right is innernode, node2.Right is innernode
CollideInnerInner(ref context, node1.Right, node2.Right);
}
else
{
// node1.Right is innernode, node2.Right is leafnode
CollideInnerLeaf(ref context, node1.Right, node2.Right);
}
}
}
else
{
if (node2.Left.HasChildren)
{
// node1.Right is leafnode, node2.Left is innernode
CollideLeafInner(ref context, node1.Right, node2.Left);
}
else
{
// node1.Right is leafnode, node2.Left is leafnode
CollideLeafLeaf(ref context, node1.Right, node2.Left);
}
if (context.needAllContacts || context.contact.Count == 0)
{
if (node2.Right.HasChildren)
{
// node1.Right is leafnode, node2.Right is innernode
CollideLeafInner(ref context, node1.Right, node2.Right);
}
else
{
// node1.Right is leafnode, node2.Right is leafnode
CollideLeafLeaf(ref context, node1.Right, node2.Right);
}
}
}
}
}
private static void Test(
Entity entity1, AlignedBox3TreeNode node1, Vector3[] positions1, ref Matrix worldTransform1, ref Vector3 translation1, ref Quaternion rotation1, ref Vector3 scale1,
Entity entity2, ref Sphere3 sphere2, ref Matrix worldTransform2, ref Vector3 translation2, ref Quaternion rotation2, ref Vector3 scale2,
bool needAllContacts, ref Contact contact
)
{
/*Sphere3 sphere1;
* node1.BoundingBox.CreateSphere3(ref worldTransform1, out sphere1);
* Vector3 diff = sphere1.Center - sphere2.Center;
* if (diff.LengthSquared() > (sphere1.Radius + sphere2.Radius) * (sphere1.Radius + sphere2.Radius))
* {
* return;
* }
*/
Box3 box1, box2;
Matrix identityMatrix = Matrix.Identity;
node1.BoundingBox.CreateBox3(ref worldTransform1, out box1);
sphere2.CreateBox3(ref identityMatrix, out box2);
if (!Intersection.IntersectBox3Box3(ref box1, ref box2))
{
return;
}
// see if this node has some primitives => has no children
if (!node1.HasChildren)
{
// test here for all triangles...
int tricount = node1.NumTriangles;
for (int i = 0; i < tricount; ++i)
{
//size_t contacts_size = contacts.size();
Triangle3 tri = node1.GetTriangle(positions1, i);
tri.Vertex0 = Vector3.Transform(tri.Vertex0, worldTransform1);
tri.Vertex1 = Vector3.Transform(tri.Vertex1, worldTransform1);
tri.Vertex2 = Vector3.Transform(tri.Vertex2, worldTransform1);
Vector3 sphereCenter = sphere2.Center;
Vector3 closestPoint = Vector3.Zero;
float squaredDistance = SquaredDistance.Vector3Triangle3(ref sphereCenter, ref tri, out closestPoint);
if (squaredDistance < sphere2.Radius * sphere2.Radius)
{
if (!contact.ContainsPoint(ref closestPoint))
{
Vector3 normal = tri.Normal;
contact.AddContactPoint(ref closestPoint, ref normal);
}
if (!needAllContacts)
{
return;
}
}
}
}
else
{
Test(
entity1, node1.Left, positions1, ref worldTransform1, ref translation1, ref rotation1, ref scale1,
entity2, ref sphere2, ref worldTransform2, ref translation2, ref rotation2, ref scale2,
needAllContacts, ref contact
);
if (!needAllContacts && contact.Count > 0)
{
return;
}
Test(
entity1, node1.Right, positions1, ref worldTransform1, ref translation1, ref rotation1, ref scale1,
entity2, ref sphere2, ref worldTransform2, ref translation2, ref rotation2, ref scale2,
needAllContacts, ref contact
);
}
}
