private bool IsContactUnique(ref ContactData contactCandidate, ref QuickList <ContactData> candidatesToAdd)
{
contactCandidate.Validate();
float distanceSquared;
RigidTransform meshTransform = MeshTransform;
for (int i = 0; i < contacts.Count; i++)
{
Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
{
//This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that
//a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent.
Vector3.Dot(ref contacts.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared);
if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum)
{
//Update the existing 'redundant' contact with the new information.
//This works out because the new contact is the deepest contact according to the previous collision detection iteration.
contacts.Elements[i].Normal = contactCandidate.Normal;
contacts.Elements[i].Position = contactCandidate.Position;
contacts.Elements[i].PenetrationDepth = contactCandidate.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = contactCandidate.PenetrationDepth;
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref convex.worldTransform, out supplementData.Elements[i].LocalOffsetA);
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref meshTransform, out supplementData.Elements[i].LocalOffsetB);
return(false);
}
}
}
for (int i = 0; i < candidatesToAdd.Count; i++)
{
Vector3.DistanceSquared(ref candidatesToAdd.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
{
//This is a nonconvex manifold. There will be times where a an object will be shoved into a corner such that
//a single position will have two reasonable normals. If the normals aren't mostly aligned, they should NOT be considered equivalent.
Vector3.Dot(ref candidatesToAdd.Elements[i].Normal, ref contactCandidate.Normal, out distanceSquared);
if (Math.Abs(distanceSquared) >= CollisionDetectionSettings.nonconvexNormalDotMinimum)
{
return(false);
}
}
}
//for (int i = 0; i < edgeContacts.count; i++)
//{
// Vector3.DistanceSquared(ref edgeContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared);
// if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
// {
// return false;
// }
//}
//for (int i = 0; i < vertexContacts.count; i++)
//{
// Vector3.DistanceSquared(ref vertexContacts.Elements[i].ContactData.Position, ref contactCandidate.Position, out distanceSquared);
// if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
// {
// return false;
// }
//}
return(true);
}
protected override void Add(ref ContactData contactCandidate)
{
ContactSupplementData supplement;
supplement.BasePenetrationDepth = contactCandidate.PenetrationDepth;
//The closest point method computes the local space versions before transforming to world... consider cutting out the middle man
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref collidableA.worldTransform, out supplement.LocalOffsetA);
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref collidableB.worldTransform, out supplement.LocalOffsetB);
supplementData.Add(ref supplement);
base.Add(ref contactCandidate);
}
float GetSubmergedHeight(EntityCollidable collidable, float maxLength, float boundingBoxHeight, ref Vector3 rayOrigin, ref Vector3 xSpacing, ref Vector3 zSpacing, int i, int j, out Vector3 volumeCenter)
{
Ray ray;
Vector3.Multiply(ref xSpacing, i, out ray.Position);
Vector3.Multiply(ref zSpacing, j, out ray.Direction);
Vector3.Add(ref ray.Position, ref ray.Direction, out ray.Position);
Vector3.Add(ref ray.Position, ref rayOrigin, out ray.Position);
ray.Direction = upVector;
//do a bottom-up raycast.
RayHit rayHit;
//Only go up to maxLength. If it's further away than maxLength, then it's above the water and it doesn't contribute anything.
if (collidable.RayCast(ray, maxLength, out rayHit))
{
//Position the ray to point from the other side.
Vector3.Multiply(ref ray.Direction, boundingBoxHeight, out ray.Direction);
Vector3.Add(ref ray.Position, ref ray.Direction, out ray.Position);
Vector3.Negate(ref upVector, out ray.Direction);
//Transform the hit into local space.
RigidTransform.TransformByInverse(ref rayHit.Location, ref surfaceTransform, out rayHit.Location);
float bottomY = rayHit.Location.Y;
float bottom = rayHit.T;
Vector3 bottomPosition = rayHit.Location;
if (collidable.RayCast(ray, boundingBoxHeight - rayHit.T, out rayHit))
{
//Transform the hit into local space.
RigidTransform.TransformByInverse(ref rayHit.Location, ref surfaceTransform, out rayHit.Location);
Vector3.Add(ref rayHit.Location, ref bottomPosition, out volumeCenter);
Vector3.Multiply(ref volumeCenter, .5f, out volumeCenter);
return(Math.Min(-bottomY, boundingBoxHeight - rayHit.T - bottom));
}
//This inner raycast should always hit, but just in case it doesn't due to some numerical problem, give it a graceful way out.
volumeCenter = Vector3.Zero;
return(0);
}
volumeCenter = Vector3.Zero;
return(0);
}
private bool IsContactUnique(ref ContactData contactCandidate)
{
contactCandidate.Validate();
for (int i = 0; i < contacts.Count; i++)
{
float distanceSquared;
Vector3.DistanceSquared(ref contacts.Elements[i].Position, ref contactCandidate.Position, out distanceSquared);
if (distanceSquared < CollisionDetectionSettings.ContactMinimumSeparationDistanceSquared)
{
//Update the existing 'redundant' contact with the new information.
//This works out because the new contact is the deepest contact according to the previous collision detection iteration.
contacts.Elements[i].Normal = contactCandidate.Normal;
contacts.Elements[i].Position = contactCandidate.Position;
contacts.Elements[i].PenetrationDepth = contactCandidate.PenetrationDepth;
supplementData.Elements[i].BasePenetrationDepth = contactCandidate.PenetrationDepth;
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref collidableA.worldTransform, out supplementData.Elements[i].LocalOffsetA);
RigidTransform.TransformByInverse(ref contactCandidate.Position, ref collidableB.worldTransform, out supplementData.Elements[i].LocalOffsetB);
return(false);
}
}
return(true);
}
///<summary>
/// Tests if a box and sphere are colliding.
///</summary>
///<param name="box">Box to test.</param>
///<param name="sphere">Sphere to test.</param>
///<param name="boxTransform">Transform to apply to the box.</param>
///<param name="spherePosition">Transform to apply to the sphere.</param>
///<param name="contact">Contact point between the shapes, if any.</param>
///<returns>Whether or not the shapes were colliding.</returns>
public static bool AreShapesColliding(BoxShape box, SphereShape sphere, ref RigidTransform boxTransform, ref Vector3 spherePosition, out ContactData contact)
{
contact = new ContactData();
Vector3 localPosition;
RigidTransform.TransformByInverse(ref spherePosition, ref boxTransform, out localPosition);
#if !WINDOWS
Vector3 localClosestPoint = new Vector3();
#else
Vector3 localClosestPoint;
#endif
localClosestPoint.X = MathHelper.Clamp(localPosition.X, -box.halfWidth, box.halfWidth);
localClosestPoint.Y = MathHelper.Clamp(localPosition.Y, -box.halfHeight, box.halfHeight);
localClosestPoint.Z = MathHelper.Clamp(localPosition.Z, -box.halfLength, box.halfLength);
RigidTransform.Transform(ref localClosestPoint, ref boxTransform, out contact.Position);
Vector3 offset;
Vector3.Subtract(ref spherePosition, ref contact.Position, out offset);
float offsetLength = offset.LengthSquared();
if (offsetLength > (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance) * (sphere.collisionMargin + CollisionDetectionSettings.maximumContactDistance))
{
return(false);
}
//Colliding.
if (offsetLength > Toolbox.Epsilon)
{
offsetLength = (float)Math.Sqrt(offsetLength);
//Outside of the box.
Vector3.Divide(ref offset, offsetLength, out contact.Normal);
contact.PenetrationDepth = sphere.collisionMargin - offsetLength;
}
else
{
//Inside of the box.
Vector3 penetrationDepths;
penetrationDepths.X = localClosestPoint.X < 0 ? localClosestPoint.X + box.halfWidth : box.halfWidth - localClosestPoint.X;
penetrationDepths.Y = localClosestPoint.Y < 0 ? localClosestPoint.Y + box.halfHeight : box.halfHeight - localClosestPoint.Y;
penetrationDepths.Z = localClosestPoint.Z < 0 ? localClosestPoint.Z + box.halfLength : box.halfLength - localClosestPoint.Z;
if (penetrationDepths.X < penetrationDepths.Y && penetrationDepths.X < penetrationDepths.Z)
{
contact.Normal = localClosestPoint.X > 0 ? Toolbox.RightVector : Toolbox.LeftVector;
contact.PenetrationDepth = penetrationDepths.X;
}
else if (penetrationDepths.Y < penetrationDepths.Z)
{
contact.Normal = localClosestPoint.Y > 0 ? Toolbox.UpVector : Toolbox.DownVector;
contact.PenetrationDepth = penetrationDepths.Y;
}
else
{
contact.Normal = localClosestPoint.Z > 0 ? Toolbox.BackVector : Toolbox.ForwardVector;
contact.PenetrationDepth = penetrationDepths.Z;
}
contact.PenetrationDepth += sphere.collisionMargin;
Quaternion.Transform(ref contact.Normal, ref boxTransform.Orientation, out contact.Normal);
}
return(true);
}
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();
}