public override void UpdateTimeOfImpact(Collidable requester, double dt)
{
//Notice that we don't test for convex entity null explicitly. The convex.IsActive property does that for us.
if (convex.IsActive && convex.Entity.PositionUpdateMode == PositionUpdateMode.Continuous)
{
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
Vector3 velocity = convex.Entity.LinearVelocity * dt;
double velocitySquared = velocity.LengthSquared();
double minimumRadius = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadius * minimumRadius < velocitySquared)
{
for (int i = 0; i < contactManifold.ActivePairs.Count; i++)
{
GeneralConvexPairTester pair = contactManifold.ActivePairs.Values[i];
ReusableGenericCollidable <ConvexShape> boxCollidable = (ReusableGenericCollidable <ConvexShape>)pair.CollidableB;
RayHit rayHit;
RigidTransform worldTransform = boxCollidable.WorldTransform;
if (GJKToolbox.CCDSphereCast(new Ray(convex.WorldTransform.Position, velocity), minimumRadius, boxCollidable.Shape, ref worldTransform, timeOfImpact, out rayHit) &&
rayHit.T > Toolbox.BigEpsilon)
{
timeOfImpact = rayHit.T;
}
}
}
}
}
///<summary>
/// Casts a fat (sphere expanded) ray against the shape. If the raycast appears to be stuck in the shape, the cast will be attempted
/// with a smaller ray (scaled by the MotionSettings.CoreShapeScaling each time).
///</summary>
///<param name="ray">Ray to test against the shape.</param>
///<param name="radius">Radius of the ray.</param>
///<param name="target">Shape to test against.</param>
///<param name="shapeTransform">Transform to apply to the shape for the test.</param>
///<param name="maximumLength">Maximum length of the ray in units of the ray direction's length.</param>
///<param name="hit">Hit data of the sphere cast, if any.</param>
///<returns>Whether or not the sphere cast hit the shape.</returns>
public static bool CCDSphereCast(Ray ray, float radius, ConvexShape target, ref RigidTransform shapeTransform, float maximumLength,
out RayHit hit)
{
int iterations = 0;
while (true)
{
if (GJKToolbox.SphereCast(ray, radius, target, ref shapeTransform, maximumLength, out hit) &&
hit.T > 0)
{
//The ray cast isn't embedded in the shape, and it's less than maximum length away!
return(true);
}
if (hit.T > maximumLength || hit.T < 0)
{
return(false); //Failure showed it was too far, or behind.
}
radius *= MotionSettings.CoreShapeScaling;
iterations++;
if (iterations > 3) //Limit could be configurable.
{
//It's iterated too much, let's just do a last ditch attempt using a raycast and hope that can help.
return(GJKToolbox.RayCast(ray, target, ref shapeTransform, maximumLength, out hit) && hit.T > 0);
}
}
}
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
//Notice that we don't test for convex entity null explicitly. The convex.IsActive property does that for us.
if (convex.IsActive && convex.entity.PositionUpdateMode == PositionUpdateMode.Continuous)
{
//TODO: This system could be made more robust by using a similar region-based rejection of edges.
//CCD events are awfully rare under normal circumstances, so this isn't usually an issue.
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
System.Numerics.Vector3 velocity;
Vector3Ex.Multiply(ref convex.entity.linearVelocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
var minimumRadius = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadius * minimumRadius < velocitySquared)
{
var triangle = PhysicsThreadResources.GetTriangle();
triangle.collisionMargin = 0;
//Spherecast against all triangles to find the earliest time.
for (int i = 0; i < MeshManifold.overlappedTriangles.Count; i++)
{
mesh.Shape.TriangleMeshData.GetTriangle(MeshManifold.overlappedTriangles.Elements[i], out triangle.vA, out triangle.vB, out triangle.vC);
//Put the triangle into 'localish' space of the convex.
Vector3Ex.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA);
Vector3Ex.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB);
Vector3Ex.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC);
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) &&
rayHit.T > Toolbox.BigEpsilon)
{
if (mesh.sidedness != TriangleSidedness.DoubleSided)
{
System.Numerics.Vector3 AB, AC;
Vector3Ex.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3Ex.Subtract(ref triangle.vC, ref triangle.vA, out AC);
System.Numerics.Vector3 normal;
Vector3Ex.Cross(ref AB, ref AC, out normal);
float dot;
Vector3Ex.Dot(ref normal, ref rayHit.Normal, out dot);
//Only perform sweep if the object is in danger of hitting the object.
//Triangles can be one sided, so check the impact normal against the triangle normal.
if (mesh.sidedness == TriangleSidedness.Counterclockwise && dot < 0 ||
mesh.sidedness == TriangleSidedness.Clockwise && dot > 0)
{
timeOfImpact = rayHit.T;
}
}
else
{
timeOfImpact = rayHit.T;
}
}
}
PhysicsThreadResources.GiveBack(triangle);
}
}
}
/// <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 virtual bool RayTest(ref Ray ray, ref RigidTransform transform, float maximumLength, out RayHit hit)
{
//TODO:
//RayHit newHit;
//bool newBool = GJKToolbox.RayCast(ray, this, ref transform, maximumLength, out newHit);
//bool oldBool = OldGJKVerifier.RayCastGJK(ray.Position, ray.Direction, maximumLength, this, transform, out hit.Location, out hit.Normal, out hit.T);
//if (newBool != oldBool || ((newBool && oldBool) && Vector3.DistanceSquared(newHit.Location, hit.Location) > .01f))
// Debug.WriteLine("break.");
//return oldBool;
return(GJKToolbox.RayCast(ray, this, ref transform, maximumLength, out hit));
}
private bool DoExternalSeparated(TriangleShape triangle, out TinyStructList <ContactData> contactList)
{
if (GJKToolbox.AreShapesIntersecting(convex, triangle, ref Toolbox.RigidIdentity, ref Toolbox.RigidIdentity, ref localSeparatingAxis))
{
state = CollisionState.ExternalNear;
return(DoExternalNear(triangle, out contactList));
}
TryToEscape();
contactList = new TinyStructList <ContactData>();
return(false);
}
///<summary>
/// Generates a contact between the objects, if possible.
///</summary>
///<param name="contact">Contact created between the pair, if possible.</param>
///<returns>Whether or not the objects were colliding.</returns>
public bool GenerateContactCandidate(out ContactData contact)
{
//Generate contacts. This will just find one closest point using general supportmapping based systems like MPR and GJK.
//The collision system moves through a state machine depending on the latest collision generation result.
//At first, assume that the pair is completely separating. This is almost always the correct guess for new pairs.
//An extremely fast, warm-startable boolean GJK test can be performed. If it returns with nonintersection, we can quit and do nothing.
//If the initial boolean GJK test finds intersection, move onto a shallow contact test.
//The shallow contact test is a different kind of GJK test that finds the closest points between the shape pair. It's not as speedy as the boolean version.
//The algorithm is run between the marginless versions of the shapes, so that the closest points will form a contact somewhere in the space separating the cores.
//If the closest point system finds no intersection and returns the closest points, the state is changed to ShallowContact.
//If the closest point system finds intersection of the core shapes, then the state is changed to DeepContact, and MPR is run to determine contact information.
//The system tries to escape from deep contact to shallow contact, and from shallow contact to separated whenever possible.
//Here's the state flow:
//On Separated: BooleanGJK
// -Intersecting -> Go to ShallowContact.
// -Nonintersecting -> Do nothing.
//On ShallowContact: ClosestPointsGJK
// -Intersecting -> Go to DeepContact.
// -Nonintersecting: Go to Separated (without test) if squared distance > margin squared, otherwise use closest points to make contact.
//On DeepContact: MPR
// -Intersecting -> Go to ShallowContact if penetration depth < margin
// -Nonintersecting -> This case is rare, but not impossible. Go to Separated (without test).
previousState = state;
switch (state)
{
case CollisionState.Separated:
if (GJKToolbox.AreShapesIntersecting(collidableA.Shape, collidableB.Shape,
ref collidableA.worldTransform, ref collidableB.worldTransform, ref localSeparatingAxis))
{
state = CollisionState.ShallowContact;
return(DoShallowContact(out contact));
}
contact = new ContactData();
return(false);
case CollisionState.ShallowContact:
return(DoShallowContact(out contact));
case CollisionState.DeepContact:
return(DoDeepContact(out contact));
}
contact = new ContactData();
return(false);
}
/// <summary>
/// Performs a convex cast to determine if a step of a given height is valid.
/// This means that stepping up onto the new support wouldn't shove the character's head into a ceiling or other obstacle.
/// </summary>
/// <param name="hitDistance">Vertical distance from the convex cast start to the hit location.</param>
/// <returns>Whether or not the step is safe.</returns>
private bool IsStepSafe(float hitDistance)
{
float stepHeight = maximumStepHeight - hitDistance;
var sweep = new Vector3(0, stepHeight + Body.CollisionInformation.Shape.CollisionMargin, 0);
Vector3 startingLocation = headBlockageFinderOffset + Body.Position;
foreach (Entity candidate in headCollisionPairCollector.CollisionInformation.OverlappedEntities)
{
//foreach (CollisionPair pair in headCollisionPairCollector.CollisionPairs)
//{
// //Determine which member of the collision pair is the possible blockage.
// //The comparisons are all kept on a "parent" as opposed to "collider" level so that interaction with compound shapes is simpler.
// Entity candidate = pair.ParentA == headCollisionPairCollector ? pair.ParentB : pair.ParentA;
//Ensure that the candidate is a valid blocking entity.
if (candidate.CollisionInformation.CollisionRules.Personal > CollisionRule.Normal)
{
continue; //It is invalid!
}
//Fire a convex cast at the candidate and determine some details!
ConvexShape targetShape = candidate.CollisionInformation.Shape as ConvexShape;
if (targetShape != null)
{
RigidTransform sweepTransform = new RigidTransform(startingLocation);
RigidTransform targetTransform = new RigidTransform(candidate.Position, candidate.Orientation);
RayHit rayHit;
if (GJKToolbox.ConvexCast(castingShape, targetShape, ref sweep,
ref sweepTransform, ref targetTransform,
out rayHit))
{
return(false);
}
}
}
return(true);
}
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
BroadPhaseOverlap overlap = BroadPhaseOverlap;
PositionUpdateMode triangleMode = triangle.entity == null
? PositionUpdateMode.Discrete
: triangle.entity.PositionUpdateMode;
PositionUpdateMode convexMode =
convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode;
if (
(overlap.entryA.IsActive || overlap.entryB.IsActive) && //At least one has to be active.
(
convexMode == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A.
triangleMode == PositionUpdateMode.Continuous &&
overlap.entryA == requester ||
(convexMode == PositionUpdateMode.Continuous) ^ //If only one is continuous, then we must do it.
(triangleMode == PositionUpdateMode.Continuous)
)
)
{
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
Vector3 velocity;
if (convexMode == PositionUpdateMode.Discrete)
//Triangle is static for the purposes of this continuous test.
{
velocity = triangle.entity.linearVelocity;
}
else if (triangleMode == PositionUpdateMode.Discrete)
//Convex is static for the purposes of this continuous test.
{
Vector3.Negate(ref convex.entity.linearVelocity, out velocity);
}
else
//Both objects are moving.
{
Vector3.Subtract(ref triangle.entity.linearVelocity, ref convex.entity.linearVelocity,
out velocity);
}
Vector3.Multiply(ref velocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
float minimumRadiusA = convex.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadiusA * minimumRadiusA < velocitySquared)
{
//Spherecast A against B.
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(convex.worldTransform.Position, -velocity), minimumRadiusA,
triangle.Shape, ref triangle.worldTransform, timeOfImpact, out rayHit))
{
if (triangle.Shape.sidedness != TriangleSidedness.DoubleSided)
{
//Only perform sweep if the object is in danger of hitting the object.
//Triangles can be one sided, so check the impact normal against the triangle normal.
Vector3 AB, AC;
Vector3.Subtract(ref triangle.Shape.vB, ref triangle.Shape.vA, out AB);
Vector3.Subtract(ref triangle.Shape.vC, ref triangle.Shape.vA, out AC);
Vector3 normal;
Vector3.Cross(ref AB, ref AC, out normal);
float dot;
Vector3.Dot(ref rayHit.Normal, ref normal, out dot);
if (triangle.Shape.sidedness == TriangleSidedness.Counterclockwise && dot < 0 ||
triangle.Shape.sidedness == TriangleSidedness.Clockwise && dot > 0)
{
timeOfImpact = rayHit.T;
}
}
else
{
timeOfImpact = rayHit.T;
}
}
}
//TECHNICALLY, the triangle should be casted too. But, given the way triangles are usually used and their tiny minimum radius, ignoring it is usually just fine.
//var minimumRadiusB = triangle.minimumRadius * MotionSettings.CoreShapeScaling;
//if (minimumRadiusB * minimumRadiusB < velocitySquared)
//{
// //Spherecast B against A.
// RayHit rayHit;
// if (GJKToolbox.SphereCast(new Ray(triangle.entity.position, velocity), minimumRadiusB, convex.Shape, ref convex.worldTransform, 1, out rayHit) &&
// rayHit.T < timeOfImpact)
// {
// if (triangle.Shape.sidedness != TriangleSidedness.DoubleSided)
// {
// float dot;
// Vector3.Dot(ref rayHit.Normal, ref normal, out dot);
// if (dot > 0)
// {
// timeOfImpact = rayHit.T;
// }
// }
// else
// {
// timeOfImpact = rayHit.T;
// }
// }
//}
//If it's intersecting, throw our hands into the air and give up.
//This is generally a perfectly acceptable thing to do, since it's either sitting
//inside another object (no ccd makes sense) or we're still in an intersecting case
//from a previous frame where CCD took place and a contact should have been created
//to deal with interpenetrating velocity. Sometimes that contact isn't sufficient,
//but it's good enough.
if (timeOfImpact == 0)
{
timeOfImpact = 1;
}
}
}
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
//TODO: This conditional early outing stuff could be pulled up into a common system, along with most of the pair handler.
if (convex.IsActive && convex.entity.PositionUpdateMode == PositionUpdateMode.Continuous)
{
//TODO: This system could be made more robust by using a similar region-based rejection of edges.
//CCD events are awfully rare under normal circumstances, so this isn't usually an issue.
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
Vector3 velocity;
Vector3.Multiply(ref convex.entity.linearVelocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
var minimumRadius = convex.Shape.minimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadius * minimumRadius < velocitySquared)
{
var triangle = Resources.GetTriangle();
triangle.collisionMargin = 0;
Vector3 terrainUp = new Vector3(terrain.worldTransform.LinearTransform.M21, terrain.worldTransform.LinearTransform.M22, terrain.worldTransform.LinearTransform.M23);
//Spherecast against all triangles to find the earliest time.
for (int i = 0; i < TerrainManifold.overlappedTriangles.count; i++)
{
terrain.Shape.GetTriangle(ref TerrainManifold.overlappedTriangles.Elements[i], ref terrain.worldTransform, out triangle.vA, out triangle.vB, out triangle.vC);
//Put the triangle into 'localish' space of the convex.
Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA);
Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB);
Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC);
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) &&
rayHit.T > Toolbox.BigEpsilon)
{
Vector3 AB, AC;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC);
Vector3 normal;
Vector3.Cross(ref AC, ref AB, out normal);
float dot;
Vector3.Dot(ref normal, ref terrainUp, out dot);
if (dot < 0)
{
Vector3.Dot(ref normal, ref rayHit.Normal, out dot);
}
else
{
Vector3.Dot(ref normal, ref rayHit.Normal, out dot);
dot = -dot;
}
//Only perform sweep if the object is in danger of hitting the object.
//Triangles can be one sided, so check the impact normal against the triangle normal.
if (dot < 0)
{
timeOfImpact = rayHit.T;
}
}
}
Resources.GiveBack(triangle);
}
}
}
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
var collidableA = CollidableA as ConvexCollidable;
var collidableB = CollidableB as ConvexCollidable;
var modeA = collidableA.entity == null ? PositionUpdateMode.Discrete : collidableA.entity.PositionUpdateMode;
var modeB = collidableB.entity == null ? PositionUpdateMode.Discrete : collidableB.entity.PositionUpdateMode;
var overlap = BroadPhaseOverlap;
if (
(overlap.entryA.IsActive || overlap.entryB.IsActive) && //At least one has to be active.
(
(
modeA == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A.
modeB == PositionUpdateMode.Continuous &&
overlap.entryA == requester
) ||
(
modeA == PositionUpdateMode.Continuous ^ //If only one is continuous, then we must do it.
modeB == PositionUpdateMode.Continuous
)
)
)
{
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
//Discrete objects have already had their linear motion integrated, so don't use their velocity.
Vector3 velocity;
if (modeA == PositionUpdateMode.Discrete)
{
//CollidableA is static for the purposes of this continuous test.
velocity = collidableB.entity.linearVelocity;
}
else if (modeB == PositionUpdateMode.Discrete)
{
//CollidableB is static for the purposes of this continuous test.
Vector3.Negate(ref collidableA.entity.linearVelocity, out velocity);
}
else
{
//Both objects are moving.
Vector3.Subtract(ref collidableB.entity.linearVelocity, ref collidableA.entity.linearVelocity, out velocity);
}
Vector3.Multiply(ref velocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
var minimumRadiusA = collidableA.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadiusA * minimumRadiusA < velocitySquared)
{
//Spherecast A against B.
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(collidableA.worldTransform.Position, -velocity), minimumRadiusA, collidableB.Shape, ref collidableB.worldTransform, timeOfImpact, out rayHit))
{
timeOfImpact = rayHit.T;
}
}
var minimumRadiusB = collidableB.Shape.MinimumRadius * MotionSettings.CoreShapeScaling;
if (minimumRadiusB * minimumRadiusB < velocitySquared)
{
//Spherecast B against A.
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(collidableB.worldTransform.Position, velocity), minimumRadiusB, collidableA.Shape, ref collidableA.worldTransform, timeOfImpact, out rayHit))
{
timeOfImpact = rayHit.T;
}
}
//If it's intersecting, throw our hands into the air and give up.
//This is generally a perfectly acceptable thing to do, since it's either sitting
//inside another object (no ccd makes sense) or we're still in an intersecting case
//from a previous frame where CCD took place and a contact should have been created
//to deal with interpenetrating velocity. Sometimes that contact isn't sufficient,
//but it's good enough.
if (timeOfImpact == 0)
{
timeOfImpact = 1;
}
}
}
///<summary>
/// Updates the time of impact for the pair.
///</summary>
///<param name="requester">Collidable requesting the update.</param>
///<param name="dt">Timestep duration.</param>
public override void UpdateTimeOfImpact(Collidable requester, float dt)
{
//TODO: This conditional early outing stuff could be pulled up into a common system, along with most of the pair handler.
var overlap = BroadPhaseOverlap;
var convexMode = convex.entity == null ? PositionUpdateMode.Discrete : convex.entity.PositionUpdateMode;
if (
(mobileMesh.IsActive || (convex.entity == null ? false : convex.entity.activityInformation.IsActive)) && //At least one has to be active.
(
(
convexMode == PositionUpdateMode.Continuous && //If both are continuous, only do the process for A.
mobileMesh.entity.PositionUpdateMode == PositionUpdateMode.Continuous &&
overlap.entryA == requester
) ||
(
convexMode == PositionUpdateMode.Continuous ^ //If only one is continuous, then we must do it.
mobileMesh.entity.PositionUpdateMode == PositionUpdateMode.Continuous
)
)
)
{
//TODO: This system could be made more robust by using a similar region-based rejection of edges.
//CCD events are awfully rare under normal circumstances, so this isn't usually an issue.
//Only perform the test if the minimum radii are small enough relative to the size of the velocity.
Vector3 velocity;
if (convex.entity != null)
{
Vector3.Subtract(ref convex.entity.linearVelocity, ref mobileMesh.entity.linearVelocity, out velocity);
}
else
{
Vector3.Negate(ref mobileMesh.entity.linearVelocity, out velocity);
}
Vector3.Multiply(ref velocity, dt, out velocity);
float velocitySquared = velocity.LengthSquared();
var minimumRadius = convex.Shape.minimumRadius * MotionSettings.CoreShapeScaling;
timeOfImpact = 1;
if (minimumRadius * minimumRadius < velocitySquared)
{
TriangleSidedness sidedness = mobileMesh.Shape.Sidedness;
Matrix3X3 orientation;
Matrix3X3.CreateFromQuaternion(ref mobileMesh.worldTransform.Orientation, out orientation);
var triangle = Resources.GetTriangle();
triangle.collisionMargin = 0;
//Spherecast against all triangles to find the earliest time.
for (int i = 0; i < MeshManifold.overlappedTriangles.count; i++)
{
MeshBoundingBoxTreeData data = mobileMesh.Shape.TriangleMesh.Data;
int triangleIndex = MeshManifold.overlappedTriangles.Elements[i];
data.GetTriangle(triangleIndex, out triangle.vA, out triangle.vB, out triangle.vC);
Matrix3X3.Transform(ref triangle.vA, ref orientation, out triangle.vA);
Matrix3X3.Transform(ref triangle.vB, ref orientation, out triangle.vB);
Matrix3X3.Transform(ref triangle.vC, ref orientation, out triangle.vC);
Vector3.Add(ref triangle.vA, ref mobileMesh.worldTransform.Position, out triangle.vA);
Vector3.Add(ref triangle.vB, ref mobileMesh.worldTransform.Position, out triangle.vB);
Vector3.Add(ref triangle.vC, ref mobileMesh.worldTransform.Position, out triangle.vC);
//Put the triangle into 'localish' space of the convex.
Vector3.Subtract(ref triangle.vA, ref convex.worldTransform.Position, out triangle.vA);
Vector3.Subtract(ref triangle.vB, ref convex.worldTransform.Position, out triangle.vB);
Vector3.Subtract(ref triangle.vC, ref convex.worldTransform.Position, out triangle.vC);
RayHit rayHit;
if (GJKToolbox.CCDSphereCast(new Ray(Toolbox.ZeroVector, velocity), minimumRadius, triangle, ref Toolbox.RigidIdentity, timeOfImpact, out rayHit) &&
rayHit.T > Toolbox.BigEpsilon)
{
if (sidedness != TriangleSidedness.DoubleSided)
{
Vector3 AB, AC;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out AB);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out AC);
Vector3 normal;
Vector3.Cross(ref AB, ref AC, out normal);
float dot;
Vector3.Dot(ref normal, ref rayHit.Normal, out dot);
//Only perform sweep if the object is in danger of hitting the object.
//Triangles can be one sided, so check the impact normal against the triangle normal.
if (sidedness == TriangleSidedness.Counterclockwise && dot < 0 ||
sidedness == TriangleSidedness.Clockwise && dot > 0)
{
timeOfImpact = rayHit.T;
}
}
else
{
timeOfImpact = rayHit.T;
}
}
}
Resources.GiveBack(triangle);
}
}
}
private bool DoExternalNear(TriangleShape triangle, out TinyStructList <ContactData> contactList)
{
Vector3 closestA, closestB;
//Don't bother trying to do any clever caching. The continually transforming simplex makes it very rarely useful.
//TODO: Initialize the simplex of the GJK method using the 'true' center of the triangle.
//If left unmodified, the simplex that is used in GJK will just be a point at 0,0,0, which of course is at the origin.
//This causes an instant-out, always. Not good!
//By giving the contributing simplex the average centroid, it has a better guess.
Vector3 triangleCentroid;
Vector3.Add(ref triangle.vA, ref triangle.vB, out triangleCentroid);
Vector3.Add(ref triangleCentroid, ref triangle.vC, out triangleCentroid);
Vector3.Multiply(ref triangleCentroid, .33333333f, out triangleCentroid);
CachedSimplex initialSimplex = new CachedSimplex
{
State = SimplexState.Point, LocalSimplexB = { A = triangleCentroid }
};
if (GJKToolbox.GetClosestPoints(convex, triangle, ref Toolbox.RigidIdentity, ref Toolbox.RigidIdentity,
ref initialSimplex, out closestA, out closestB))
{
state = CollisionState.Deep;
return(DoDeepContact(triangle, out contactList));
}
Vector3 displacement;
Vector3.Subtract(ref closestB, ref closestA, out displacement);
float distanceSquared = displacement.LengthSquared();
float margin = convex.collisionMargin + triangle.collisionMargin;
contactList = new TinyStructList <ContactData>();
if (distanceSquared < margin * margin)
{
//Try to generate a contact.
ContactData contact = new ContactData();
//Determine if the normal points in the appropriate direction given the sidedness of the triangle.
if (triangle.sidedness != TriangleSidedness.DoubleSided)
{
Vector3 triangleNormal, ab, ac;
Vector3.Subtract(ref triangle.vB, ref triangle.vA, out ab);
Vector3.Subtract(ref triangle.vC, ref triangle.vA, out ac);
Vector3.Cross(ref ab, ref ac, out triangleNormal);
float dot;
Vector3.Dot(ref triangleNormal, ref displacement, out dot);
if (triangle.sidedness == TriangleSidedness.Clockwise && dot > 0)
{
return(false);
}
if (triangle.sidedness == TriangleSidedness.Counterclockwise && dot < 0)
{
return(false);
}
}
//Displacement is from A to B. point = A + t * AB, where t = marginA / margin.
if (margin > Toolbox.Epsilon) //This can be zero! It would cause a NaN if unprotected.
{
Vector3.Multiply(ref displacement, convex.collisionMargin / margin, out contact.Position); //t * AB
}
else
{
contact.Position = new Vector3();
}
Vector3.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB.
contact.Normal = displacement;
float distance = (float)Math.Sqrt(distanceSquared);
Vector3.Divide(ref contact.Normal, distance, out contact.Normal);
contact.PenetrationDepth = margin - distance;
contactList.Add(ref contact);
TryToEscape(triangle, ref contact.Position);
return(true);
}
//Too far to make a contact- move back to separation.
state = CollisionState.ExternalSeparated;
return(false);
}
private bool DoShallowContact(out ContactData contact)
{
Vector3 closestA, closestB;
//RigidTransform transform = RigidTransform.Identity;
//Vector3 closestAnew, closestBnew;
//CachedSimplex cachedTest = cachedSimplex;
//bool intersecting = GJKToolbox.GetClosestPoints(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, ref cachedTest, out closestAnew, out closestBnew);
////bool otherIntersecting = OldGJKVerifier.GetClosestPointsBetweenObjects(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, 0, 0, out closestA, out closestB);
//bool otherIntersecting = GJKToolbox.GetClosestPoints(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, out closestA, out closestB);
//Vector3 closestAold, closestBold;
//bool oldIntersecting = OldGJKVerifier.GetClosestPointsBetweenObjects(informationA.Shape, informationB.Shape, ref informationA.worldTransform, ref informationB.worldTransform, 0, 0, out closestAold, out closestBold);
//if (otherIntersecting != intersecting || (!otherIntersecting && !intersecting &&
// Vector3.DistanceSquared(closestAnew, closestBnew) - Vector3.DistanceSquared(closestA, closestB) > .0001f &&
// (Vector3.DistanceSquared(closestA, closestAnew) > .0001f ||
// Vector3.DistanceSquared(closestB, closestBnew) > .0001f)))// ||
// //Math.Abs(Vector3.Dot(closestB - closestA, closestBnew - closestAnew) - Vector3.Dot(closestB - closestA, closestB - closestA)) > Toolbox.Epsilon)))
// Debug.WriteLine("Break.");
//Vector3 sub;
//Vector3.Subtract(ref closestA, ref closestB, out sub);
//if (sub.LengthSquared() < Toolbox.Epsilon)
if (UseSimplexCaching)
{
GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref cachedSimplex, out closestA, out closestB);
}
else
{
//The initialization of the pair creates a pretty decent simplex to start from.
//Just don't try to update it.
CachedSimplex preInitializedSimplex = cachedSimplex;
GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform, ref collidableB.worldTransform, ref preInitializedSimplex, out closestA, out closestB);
}
Vector3 displacement;
Vector3.Subtract(ref closestB, ref closestA, out displacement);
float distanceSquared = displacement.LengthSquared();
if (distanceSquared < Toolbox.Epsilon)
{
state = CollisionState.DeepContact;
return(DoDeepContact(out contact));
}
localDirection = displacement; //Use this as the direction for future deep contacts.
float margin = collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin;
if (distanceSquared < margin * margin)
{
//Generate a contact.
contact = new ContactData();
//Displacement is from A to B. point = A + t * AB, where t = marginA / margin.
if (margin > Toolbox.Epsilon) //Avoid a NaN!
{
Vector3.Multiply(ref displacement, collidableA.Shape.collisionMargin / margin, out contact.Position); //t * AB
}
else
{
contact.Position = new Vector3();
}
Vector3.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB.
contact.Normal = displacement;
float distance = (float)Math.Sqrt(distanceSquared);
Vector3.Divide(ref contact.Normal, distance, out contact.Normal);
contact.PenetrationDepth = margin - distance;
return(true);
}
//Too shallow to make a contact- move back to separation.
state = CollisionState.Separated;
contact = new ContactData();
return(false);
}
/// <summary>
/// Constructs a new demo.
/// </summary>
/// <param name="game">Game owning this demo.</param>
public BooleanConvexTestDemo(DemosGame game)
: base(game)
{
var random = new Random();
int numberOfConfigurations = 1000;
int numberOfTestsPerConfiguration = 10000;
float size = 2;
var aPositionBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
var bPositionBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
size = 1;
var aShapeBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
var bShapeBounds = new BoundingBox(new Vector3(-size, -size, -size), new Vector3(size, size, size));
int pointsInA = 10;
int pointsInB = 10;
RawList <Vector3> points = new RawList <Vector3>();
long accumulatedMPR = 0;
long accumulatedGJK = 0;
long accumulatedGJKSeparatingAxis = 0;
for (int i = 0; i < numberOfConfigurations; i++)
{
//Create two convex hull shapes.
for (int j = 0; j < pointsInA; j++)
{
Vector3 point;
GetRandomPointInBoundingBox(random, ref aShapeBounds, out point);
points.Add(point);
}
var a = new ConvexHullShape(points);
points.Clear();
for (int j = 0; j < pointsInB; j++)
{
Vector3 point;
GetRandomPointInBoundingBox(random, ref bShapeBounds, out point);
points.Add(point);
}
var b = new ConvexHullShape(points);
points.Clear();
//Generate some random tranforms for the shapes.
RigidTransform aTransform;
var axis = Vector3.Normalize(new Vector3((float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2)));
var angle = (float)random.NextDouble() * MathHelper.TwoPi;
Quaternion.CreateFromAxisAngle(ref axis, angle, out aTransform.Orientation);
GetRandomPointInBoundingBox(random, ref aPositionBounds, out aTransform.Position);
RigidTransform bTransform;
axis = Vector3.Normalize(new Vector3((float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2), (float)((random.NextDouble() - .5f) * 2)));
angle = (float)random.NextDouble() * MathHelper.TwoPi;
Quaternion.CreateFromAxisAngle(ref axis, angle, out bTransform.Orientation);
GetRandomPointInBoundingBox(random, ref bPositionBounds, out bTransform.Position);
//Perform MPR tests.
//Warm up the cache a bit.
MPRToolbox.AreShapesOverlapping(a, b, ref aTransform, ref bTransform);
long start = Stopwatch.GetTimestamp();
for (int j = 0; j < numberOfTestsPerConfiguration; j++)
{
if (MPRToolbox.AreShapesOverlapping(a, b, ref aTransform, ref bTransform))
{
overlapsMPR++;
}
}
long end = Stopwatch.GetTimestamp();
accumulatedMPR += end - start;
//Perform GJK tests.
//Warm up the cache a bit.
GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform);
start = Stopwatch.GetTimestamp();
for (int j = 0; j < numberOfTestsPerConfiguration; j++)
{
if (GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform))
{
overlapsGJK++;
}
}
end = Stopwatch.GetTimestamp();
accumulatedGJK += end - start;
//Perform GJK Separating Axis tests.
//Warm up the cache a bit.
Vector3 localSeparatingAxis = Vector3.Up;
GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform, ref localSeparatingAxis);
start = Stopwatch.GetTimestamp();
for (int j = 0; j < numberOfTestsPerConfiguration; j++)
{
if (GJKToolbox.AreShapesIntersecting(a, b, ref aTransform, ref bTransform, ref localSeparatingAxis))
{
overlapsGJKSeparatingAxis++;
}
}
end = Stopwatch.GetTimestamp();
accumulatedGJKSeparatingAxis += end - start;
}
//Compute the actual time per test.
long denominator = Stopwatch.Frequency * numberOfConfigurations * numberOfTestsPerConfiguration;
timeMPR = (double)accumulatedMPR / denominator;
timeGJK = (double)accumulatedGJK / denominator;
timeGJKSeparatingAxis = (double)accumulatedGJKSeparatingAxis / denominator;
}
private bool DoShallowContact(out ContactData contact)
{
Vector3 closestA, closestB;
if (UseSimplexCaching)
{
GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform,
ref collidableB.worldTransform, ref cachedSimplex, out closestA, out closestB);
}
else
{
//The initialization of the pair creates a pretty decent simplex to start from.
//Just don't try to update it.
CachedSimplex preInitializedSimplex = cachedSimplex;
GJKToolbox.GetClosestPoints(collidableA.Shape, collidableB.Shape, ref collidableA.worldTransform,
ref collidableB.worldTransform, ref preInitializedSimplex, out closestA, out closestB);
}
Vector3 displacement;
Vector3.Subtract(ref closestB, ref closestA, out displacement);
float distanceSquared = displacement.LengthSquared();
if (distanceSquared < Toolbox.Epsilon)
{
state = CollisionState.DeepContact;
return(DoDeepContact(out contact));
}
localDirection = displacement; //Use this as the direction for future deep contacts.
float margin = collidableA.Shape.collisionMargin + collidableB.Shape.collisionMargin;
if (distanceSquared < margin * margin)
{
//Generate a contact.
contact = new ContactData();
//Displacement is from A to B. point = A + t * AB, where t = marginA / margin.
if (margin > Toolbox.Epsilon) //Avoid a NaN!
{
Vector3.Multiply(ref displacement, collidableA.Shape.collisionMargin / margin,
out contact.Position); //t * AB
}
else
{
contact.Position = new Vector3();
}
Vector3.Add(ref closestA, ref contact.Position, out contact.Position); //A + t * AB.
contact.Normal = displacement;
float distance = (float)Math.Sqrt(distanceSquared);
Vector3.Divide(ref contact.Normal, distance, out contact.Normal);
contact.PenetrationDepth = margin - distance;
return(true);
}
//Too shallow to make a contact- move back to separation.
state = CollisionState.Separated;
contact = new ContactData();
return(false);
}
