public override void UpdateCollision(float dt)
{
WasContaining = Containing;
WasTouching = Touching;
mobileTriangle.collisionMargin = mesh.Shape.MeshCollisionMargin;
//Scan the pairs in sequence, updating the state as we go.
//Touching can be set to true by a single touching subpair.
Touching = false;
//Containing can be set to false by a single noncontaining or nontouching subpair.
Containing = true;
var meshData = mesh.Shape.TriangleMesh.Data;
RigidTransform mobileTriangleTransform, detectorTriangleTransform;
mobileTriangleTransform.Orientation = Quaternion.Identity;
detectorTriangleTransform.Orientation = Quaternion.Identity;
for (int i = 0; i < meshData.Indices.Length; i += 3)
{
//Grab a triangle associated with the mobile mesh.
meshData.GetTriangle(i, out mobileTriangle.vA, out mobileTriangle.vB, out mobileTriangle.vC);
RigidTransform.Transform(ref mobileTriangle.vA, ref mesh.worldTransform, out mobileTriangle.vA);
RigidTransform.Transform(ref mobileTriangle.vB, ref mesh.worldTransform, out mobileTriangle.vB);
RigidTransform.Transform(ref mobileTriangle.vC, ref mesh.worldTransform, out mobileTriangle.vC);
Vector3.Add(ref mobileTriangle.vA, ref mobileTriangle.vB, out mobileTriangleTransform.Position);
Vector3.Add(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangleTransform.Position);
Vector3.Multiply(ref mobileTriangleTransform.Position, 1 / 3f, out mobileTriangleTransform.Position);
Vector3.Subtract(ref mobileTriangle.vA, ref mobileTriangleTransform.Position, out mobileTriangle.vA);
Vector3.Subtract(ref mobileTriangle.vB, ref mobileTriangleTransform.Position, out mobileTriangle.vB);
Vector3.Subtract(ref mobileTriangle.vC, ref mobileTriangleTransform.Position, out mobileTriangle.vC);
//Go through all the detector volume triangles which are near the mobile mesh triangle.
bool triangleTouching, triangleContaining;
BoundingBox mobileBoundingBox;
mobileTriangle.GetBoundingBox(ref mobileTriangleTransform, out mobileBoundingBox);
DetectorVolume.TriangleMesh.Tree.GetOverlaps(mobileBoundingBox, overlaps);
for (int j = 0; j < overlaps.Count; j++)
{
DetectorVolume.TriangleMesh.Data.GetTriangle(overlaps.Elements[j], out detectorTriangle.vA, out detectorTriangle.vB, out detectorTriangle.vC);
Vector3.Add(ref detectorTriangle.vA, ref detectorTriangle.vB, out detectorTriangleTransform.Position);
Vector3.Add(ref detectorTriangle.vC, ref detectorTriangleTransform.Position, out detectorTriangleTransform.Position);
Vector3.Multiply(ref detectorTriangleTransform.Position, 1 / 3f, out detectorTriangleTransform.Position);
Vector3.Subtract(ref detectorTriangle.vA, ref detectorTriangleTransform.Position, out detectorTriangle.vA);
Vector3.Subtract(ref detectorTriangle.vB, ref detectorTriangleTransform.Position, out detectorTriangle.vB);
Vector3.Subtract(ref detectorTriangle.vC, ref detectorTriangleTransform.Position, out detectorTriangle.vC);
//If this triangle collides with the convex, we can stop immediately since we know we're touching and not containing.)))
//[MPR is used here in lieu of GJK because the MPR implementation tends to finish quicker than GJK when objects are overlapping. The GJK implementation does better on separated objects.]
if (MPRToolbox.AreShapesOverlapping(detectorTriangle, mobileTriangle, ref detectorTriangleTransform, ref mobileTriangleTransform))
{
triangleTouching = true;
//The convex can't be fully contained if it's still touching the surface.
triangleContaining = false;
overlaps.Clear();
goto finishTriangleTest;
}
}
overlaps.Clear();
//If we get here, then there was no shell intersection.
//If the convex's center point is contained by the mesh, then the convex is fully contained.
//This test is only needed if containment hasn't yet been outlawed or a touching state hasn't been established.
if ((!Touching || Containing) && DetectorVolume.IsPointContained(ref mobileTriangleTransform.Position, overlaps))
{
triangleTouching = true;
triangleContaining = true;
goto finishTriangleTest;
}
//If we get here, then there was no surface intersection and the convex's center is not contained- the volume and convex are separate!
triangleTouching = false;
triangleContaining = false;
finishTriangleTest:
//Analyze the results of the triangle test.
if (triangleTouching)
{
Touching = true; //If one child is touching, then we are touching too.
}
else
{
Containing = false; //If one child isn't touching, then we aren't containing.
}
if (!triangleContaining) //If one child isn't containing, then we aren't containing.
{
Containing = false;
}
if (!Containing && Touching)
{
//If it's touching but not containing, no further pairs will change the state.
//Containment has been invalidated by something that either didn't touch or wasn't contained.
//Touching has been ensured by at least one object touching.
break;
}
}
//There is a possibility that the MobileMesh is solid and fully contains the DetectorVolume.
//In this case, we should be Touching, but currently we are not.
if (mesh.Shape.solidity == MobileMeshSolidity.Solid && !Containing && !Touching)
{
//To determine if the detector volume is fully contained, check if one of the detector mesh's vertices
//are in the mobile mesh.
//This *could* fail if the mobile mesh is actually multiple pieces, but that's not a common or really supported case for solids.
Vector3 vertex;
DetectorVolume.TriangleMesh.Data.GetVertexPosition(0, out vertex);
Ray ray;
ray.Direction = Vector3.Up;
RayHit hit;
RigidTransform.TransformByInverse(ref vertex, ref mesh.worldTransform, out ray.Position);
if (mesh.Shape.IsLocalRayOriginInMesh(ref ray, out hit))
{
Touching = true;
}
}
NotifyDetectorVolumeOfChanges();
}
/// <summary>
/// Gets the intersection between the convex shape and the ray.
/// </summary>
/// <param name="ray">Ray to test.</param>
/// <param name="transform">Transform of the convex shape.</param>
/// <param name="maximumLength">Maximum distance to travel in units of the ray direction's length.</param>
/// <param name="hit">Ray hit data, if any.</param>
/// <returns>Whether or not the ray hit the target.</returns>
public override bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit)
{
//Put the ray into local space.
Quaternion conjugate;
Quaternion.Conjugate(ref transform.Orientation, out conjugate);
Ray localRay;
Vector3.Subtract(ref ray.Position, ref transform.Position, out localRay.Position);
Quaternion.Transform(ref localRay.Position, ref conjugate, out localRay.Position);
Quaternion.Transform(ref ray.Direction, ref conjugate, out localRay.Direction);
//Check for containment.
if (localRay.Position.Y >= -halfHeight && localRay.Position.Y <= halfHeight && localRay.Position.X * localRay.Position.X + localRay.Position.Z * localRay.Position.Z <= radius * radius)
{
//It's inside!
hit.T = 0;
hit.Location = localRay.Position;
hit.Normal = new Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
{
Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
}
else
{
hit.Normal = new Vector3();
}
//Pull the hit into world space.
Quaternion.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return(true);
}
//Project the ray direction onto the plane where the cylinder is a circle.
//The projected ray is then tested against the circle to compute the time of impact.
//That time of impact is used to compute the 3d hit location.
Vector2 planeDirection = new Vector2(localRay.Direction.X, localRay.Direction.Z);
float planeDirectionLengthSquared = planeDirection.LengthSquared();
if (planeDirectionLengthSquared < Toolbox.Epsilon)
{
//The ray is nearly parallel with the axis.
//Skip the cylinder-sides test. We're either inside the cylinder and won't hit the sides, or we're outside
//and won't hit the sides.
if (localRay.Position.Y > halfHeight)
{
goto upperTest;
}
if (localRay.Position.Y < -halfHeight)
{
goto lowerTest;
}
hit = new RayHit();
return(false);
}
Vector2 planeOrigin = new Vector2(localRay.Position.X, localRay.Position.Z);
float dot;
Vector2.Dot(ref planeDirection, ref planeOrigin, out dot);
float closestToCenterT = -dot / planeDirectionLengthSquared;
Vector2 closestPoint;
Vector2.Multiply(ref planeDirection, closestToCenterT, out closestPoint);
Vector2.Add(ref planeOrigin, ref closestPoint, out closestPoint);
//How close does the ray come to the circle?
float squaredDistance = closestPoint.LengthSquared();
if (squaredDistance > radius * radius)
{
//It's too far! The ray cannot possibly hit the capsule.
hit = new RayHit();
return(false);
}
//With the squared distance, compute the distance backward along the ray from the closest point on the ray to the axis.
float backwardsDistance = radius * (float)Math.Sqrt(1 - squaredDistance / (radius * radius));
float tOffset = backwardsDistance / (float)Math.Sqrt(planeDirectionLengthSquared);
hit.T = closestToCenterT - tOffset;
//Compute the impact point on the infinite cylinder in 3d local space.
Vector3.Multiply(ref localRay.Direction, hit.T, out hit.Location);
Vector3.Add(ref hit.Location, ref localRay.Position, out hit.Location);
//Is it intersecting the cylindrical portion of the capsule?
if (hit.Location.Y <= halfHeight && hit.Location.Y >= -halfHeight && hit.T < maximumLength)
{
//Yup!
hit.Normal = new Vector3(hit.Location.X, 0, hit.Location.Z);
float normalLengthSquared = hit.Normal.LengthSquared();
if (normalLengthSquared > 1e-9f)
{
Vector3.Divide(ref hit.Normal, (float)Math.Sqrt(normalLengthSquared), out hit.Normal);
}
else
{
hit.Normal = new Vector3();
}
//Pull the hit into world space.
Quaternion.Transform(ref hit.Normal, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref hit.Location, ref transform, out hit.Location);
return(true);
}
if (hit.Location.Y < halfHeight)
{
goto lowerTest;
}
upperTest:
//Nope! It may be intersecting the ends of the cylinder though.
//We're above the cylinder, so cast a ray against the upper cap.
if (localRay.Direction.Y > -1e-9)
{
//Can't hit the upper cap if the ray isn't pointing down.
hit = new RayHit();
return(false);
}
float t = (halfHeight - localRay.Position.Y) / localRay.Direction.Y;
Vector3 planeIntersection;
Vector3.Multiply(ref localRay.Direction, t, out planeIntersection);
Vector3.Add(ref localRay.Position, ref planeIntersection, out planeIntersection);
if (planeIntersection.X * planeIntersection.X + planeIntersection.Z * planeIntersection.Z < radius * radius + 1e-9 && t < maximumLength)
{
//Pull the hit into world space.
Quaternion.Transform(ref Toolbox.UpVector, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref planeIntersection, ref transform, out hit.Location);
hit.T = t;
return(true);
}
//No intersection! We can't be hitting the other sphere, so it's over!
hit = new RayHit();
return(false);
lowerTest:
//Is it intersecting the bottom cap?
if (localRay.Direction.Y < 1e-9)
{
//Can't hit the bottom cap if the ray isn't pointing up.
hit = new RayHit();
return(false);
}
t = (-halfHeight - localRay.Position.Y) / localRay.Direction.Y;
Vector3.Multiply(ref localRay.Direction, t, out planeIntersection);
Vector3.Add(ref localRay.Position, ref planeIntersection, out planeIntersection);
if (planeIntersection.X * planeIntersection.X + planeIntersection.Z * planeIntersection.Z < radius * radius + 1e-9 && t < maximumLength)
{
//Pull the hit into world space.
Quaternion.Transform(ref Toolbox.DownVector, ref transform.Orientation, out hit.Normal);
RigidTransform.Transform(ref planeIntersection, ref transform, out hit.Location);
hit.T = t;
return(true);
}
//No intersection! We can't be hitting the other sphere, so it's over!
hit = new RayHit();
return(false);
}
