/// <summary>
/// Casts a convex shape against the collidable.
/// </summary>
/// <param name="castShape">Shape to cast.</param>
/// <param name="startingTransform">Initial transform of the shape.</param>
/// <param name="sweep">Sweep to apply to the shape.</param>
/// <param name="hit">Hit data, if any.</param>
/// <returns>Whether or not the cast hit anything.</returns>
public override bool ConvexCast(ConvexShape castShape, ref RigidTransform startingTransform, ref Vector3 sweep, out RayHit hit)
{
hit = new RayHit();
BoundingBox localSpaceBoundingBox;
castShape.GetSweptLocalBoundingBox(ref startingTransform, ref worldTransform, ref sweep, out localSpaceBoundingBox);
var tri = PhysicsThreadResources.GetTriangle();
var hitElements = new QuickList <int>(BufferPools <int> .Thread);
if (Shape.GetOverlaps(localSpaceBoundingBox, ref hitElements))
{
hit.T = float.MaxValue;
for (int i = 0; i < hitElements.Count; i++)
{
Shape.GetTriangle(hitElements.Elements[i], ref worldTransform, out tri.vA, out tri.vB, out tri.vC);
Vector3 center;
Vector3.Add(ref tri.vA, ref tri.vB, out center);
Vector3.Add(ref center, ref tri.vC, out center);
Vector3.Multiply(ref center, 1f / 3f, out center);
Vector3.Subtract(ref tri.vA, ref center, out tri.vA);
Vector3.Subtract(ref tri.vB, ref center, out tri.vB);
Vector3.Subtract(ref tri.vC, ref center, out tri.vC);
tri.MaximumRadius = tri.vA.LengthSquared();
float radius = tri.vB.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
radius = tri.vC.LengthSquared();
if (tri.MaximumRadius < radius)
{
tri.MaximumRadius = radius;
}
tri.MaximumRadius = (float)Math.Sqrt(tri.MaximumRadius);
tri.collisionMargin = 0;
var triangleTransform = new RigidTransform {
Orientation = Quaternion.Identity, Position = center
};
RayHit tempHit;
if (MPRToolbox.Sweep(castShape, tri, ref sweep, ref Toolbox.ZeroVector, ref startingTransform, ref triangleTransform, out tempHit) && tempHit.T < hit.T)
{
hit = tempHit;
}
}
tri.MaximumRadius = 0;
PhysicsThreadResources.GiveBack(tri);
hitElements.Dispose();
return(hit.T != float.MaxValue);
}
PhysicsThreadResources.GiveBack(tri);
hitElements.Dispose();
return(false);
}
internal void Dispose()
{
BlockedEdgeRegions.Dispose();
BlockedVertexRegions.Dispose();
VertexContacts.Dispose();
EdgeContacts.Dispose();
}
protected override void UpdateContainedPairs(float dt)
{
var overlappedElements = new QuickList <int>(BufferPools <int> .Thread);
BoundingBox localBoundingBox;
Vector3 sweep;
Vector3.Multiply(ref mobileMesh.entity.linearVelocity, dt, out sweep);
mobileMesh.Shape.GetSweptLocalBoundingBox(ref mobileMesh.worldTransform, ref mesh.worldTransform, ref sweep, out localBoundingBox);
mesh.Shape.GetOverlaps(localBoundingBox, ref overlappedElements);
for (int i = 0; i < overlappedElements.Count; i++)
{
TryToAdd(overlappedElements.Elements[i]);
}
overlappedElements.Dispose();
}
/// <summary>
/// Checks if a transition from the current stance to the target stance is possible given the current environment.
/// </summary>
/// <param name="targetStance">Stance to check for transition safety.</param>
/// <param name="newHeight">If the transition is safe, the new height of the character. Zero otherwise.</param>
/// <param name="newPosition">If the transition is safe, the new location of the character body if the transition occurred. Zero vector otherwise.</param>
/// <returns>True if the target stance is different than the current stance and the transition is valid, false otherwise.</returns>
public bool CheckTransition(Stance targetStance, out float newHeight, out Vector3 newPosition)
{
var currentPosition = characterBody.position;
var down = characterBody.orientationMatrix.Down;
newPosition = new Vector3();
newHeight = 0;
if (CurrentStance != targetStance)
{
float currentHeight;
switch (CurrentStance)
{
case Stance.Prone:
currentHeight = proneHeight;
break;
case Stance.Crouching:
currentHeight = crouchingHeight;
break;
default:
currentHeight = standingHeight;
break;
}
float targetHeight;
switch (targetStance)
{
case Stance.Prone:
targetHeight = proneHeight;
break;
case Stance.Crouching:
targetHeight = crouchingHeight;
break;
default:
targetHeight = standingHeight;
break;
}
if (currentHeight >= targetHeight)
{
//The character is getting smaller, so no validation queries are required.
if (SupportFinder.HasSupport)
{
//On the ground, so need to move the position down.
newPosition = currentPosition + down * ((currentHeight - targetHeight) * 0.5f);
}
else
{
//We're in the air, so we don't have to change the position at all- just change the height.
newPosition = currentPosition;
}
newHeight = targetHeight;
return(true);
}
//The character is getting bigger, so validation is required.
ConvexCollidable <CylinderShape> queryObject;
switch (targetStance)
{
case Stance.Prone:
queryObject = proneQueryObject;
break;
case Stance.Crouching:
queryObject = crouchingQueryObject;
break;
default:
queryObject = standingQueryObject;
break;
}
var tractionContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
var supportContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
var sideContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
var headContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
try
{
if (SupportFinder.HasSupport)
{
//Increasing in size requires a query to verify that the new state is safe.
//TODO: State queries can be expensive if the character is crouching beneath something really detailed.
//There are some situations where you may want to do an upwards-pointing ray cast first. If it hits something,
//there's no need to do the full query.
newPosition = currentPosition - down * ((targetHeight - currentHeight) * .5f);
PrepareQueryObject(queryObject, ref newPosition);
QueryManager.QueryContacts(queryObject, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts, true);
if (IsObstructed(ref supportContacts, ref sideContacts, ref headContacts))
{
//Can't stand up if something is in the way!
return(false);
}
newHeight = targetHeight;
return(true);
}
else
{
//This is a complicated case. We must perform a semi-downstep query.
//It's different than a downstep because the head may be obstructed as well. Also, in downstepping, the character always goes *down*.
//In this, while the bottom of the character is extending downward, the character position actually either stays the same or goes up.
//(We arbitrarily ignore the case where the character could push off a ceiling.)
//The goal is to put the feet of the character on any support that can be found, and then verify that the rest of its body fits in that location.
float lowestBound = 0;
float originalHighestBound = (targetHeight - currentHeight) * -.5f;
float highestBound = originalHighestBound;
float currentOffset = 0;
int attempts = 0;
//Don't keep querying indefinitely. If we fail to reach it in a few informed steps, it's probably not worth continuing.
//The bound size check prevents the system from continuing to search a meaninglessly tiny interval.
var lastState = CharacterContactPositionState.Accepted;
while (attempts++ < 5 && lowestBound - highestBound > Toolbox.BigEpsilon)
{
Vector3 candidatePosition = currentPosition + currentOffset * down;
float hintOffset;
switch (lastState = TrySupportLocation(queryObject, ref candidatePosition, out hintOffset, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset <= 0 && currentOffset >= originalHighestBound)
{
newPosition = currentPosition + currentOffset * down;
newHeight = targetHeight;
return(true);
}
else
{
return(false);
}
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + highestBound) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + lowestBound) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
}
//If it terminated in a safe state, it can increase in size.
//TODO: You could handle this more efficiently by early terminating the above loop once obstruction is clearly unavoidable.
//(Not messing with it right now because this is likely going to change completely.)
if (lastState == CharacterContactPositionState.Accepted || lastState == CharacterContactPositionState.NoHit)
{
newPosition = currentPosition;
newHeight = targetHeight;
return(true);
}
return(false);
}
}
finally
{
tractionContacts.Dispose();
supportContacts.Dispose();
sideContacts.Dispose();
headContacts.Dispose();
}
}
return(false);
}
///<summary>
/// Updates the manifold.
///</summary>
///<param name="dt">Timestep duration.</param>
public override void Update(float dt)
{
//First, refresh all existing contacts. This is an incremental manifold.
var transform = MeshTransform;
ContactRefresher.ContactRefresh(contacts, supplementData, ref convex.worldTransform, ref transform, contactIndicesToRemove);
RemoveQueuedContacts();
CleanUpOverlappingTriangles();
//Get all the overlapped triangle indices.
//Note that the collection of triangles is left up to the child implementation.
//We're basically treating the child class like an indexable collection.
//A little gross to have it organized this way instead of an explicit collection to separate the logic up. Would be nice to improve someday!
int triangleCount = FindOverlappingTriangles(dt);
//Just use 32 elements for all the lists and sets in this system.
const int bufferPoolSizePower = 5;
BoundarySets boundarySets;
if (UseImprovedBoundaryHandling)
{
boundarySets = new BoundarySets(bufferPoolSizePower);
}
else
{
boundarySets = new BoundarySets();
}
var candidatesToAdd = new QuickList <ContactData>(BufferPools <ContactData> .Thread, bufferPoolSizePower);
//A single triangle shape will be reused for all operations. It's pulled from a thread local pool to avoid holding a TriangleShape around for every single contact manifold or pair tester.
var localTriangleShape = PhysicsThreadResources.GetTriangle();
//Precompute the transform to take triangles from their native local space to the convex's local space.
RigidTransform inverseConvexWorldTransform;
RigidTransform.Invert(ref convex.worldTransform, out inverseConvexWorldTransform);
AffineTransform convexInverseWorldTransform;
AffineTransform.CreateFromRigidTransform(ref inverseConvexWorldTransform, out convexInverseWorldTransform);
AffineTransform fromMeshLocalToConvexLocal;
PrecomputeTriangleTransform(ref convexInverseWorldTransform, out fromMeshLocalToConvexLocal);
//Grab the convex's local space bounding box up front. This will be used for a secondary pruning step.
BoundingBox convexLocalBoundingBox;
convex.Shape.GetBoundingBox(ref Toolbox.RigidIdentity, out convexLocalBoundingBox);
Matrix3x3 orientation;
Matrix3x3.CreateFromQuaternion(ref convex.worldTransform.Orientation, out orientation);
var guaranteedContacts = 0;
for (int i = 0; i < triangleCount; i++)
{
//Initialize the local triangle.
TriangleIndices indices;
if (ConfigureLocalTriangle(i, localTriangleShape, out indices))
{
//Put the triangle into the local space of the convex.
AffineTransform.Transform(ref localTriangleShape.vA, ref fromMeshLocalToConvexLocal, out localTriangleShape.vA);
AffineTransform.Transform(ref localTriangleShape.vB, ref fromMeshLocalToConvexLocal, out localTriangleShape.vB);
AffineTransform.Transform(ref localTriangleShape.vC, ref fromMeshLocalToConvexLocal, out localTriangleShape.vC);
//Do one last AABB test between the convex and triangle in the convex's local space.
//This can prune out a lot of triangles when dealing with larger objects, and it's pretty cheap to do.
BoundingBox triangleBoundingBox;
Toolbox.GetTriangleBoundingBox(ref localTriangleShape.vA, ref localTriangleShape.vB, ref localTriangleShape.vC, out triangleBoundingBox);
bool intersecting;
triangleBoundingBox.Intersects(ref convexLocalBoundingBox, out intersecting);
if (!intersecting)
{
continue;
}
//Find a pairtester for the triangle.
TrianglePairTester pairTester;
if (!activePairTesters.TryGetValue(indices, out pairTester))
{
pairTester = GetTester();
pairTester.Initialize(convex.Shape);
activePairTesters.Add(indices, pairTester);
}
pairTester.Updated = true;
//Now, generate a contact between the two shapes.
TinyStructList <ContactData> contactList;
if (pairTester.GenerateContactCandidates(localTriangleShape, out contactList))
{
for (int j = 0; j < contactList.Count; j++)
{
ContactData contact;
contactList.Get(j, out contact);
if (UseImprovedBoundaryHandling)
{
if (AnalyzeCandidate(ref indices, localTriangleShape, pairTester, ref contact, ref boundarySets))
{
//This is let through if there's a face contact. Face contacts cannot be blocked.
guaranteedContacts++;
AddLocalContact(ref contact, ref orientation, ref candidatesToAdd);
}
}
else
{
AddLocalContact(ref contact, ref orientation, ref candidatesToAdd);
}
}
}
//Get the voronoi region from the contact candidate generation. Possibly just recalculate, since most of the systems don't calculate it.
//Depending on which voronoi region it is in (Switch on enumeration), identify the indices composing that region. For face contacts, don't bother- just add it if unique.
//For AB, AC, or BC, add an Edge to the blockedEdgeRegions set with the corresponding indices.
//For A, B, or C, add the index of the vertex to the blockedVertexRegions set.
//If the edge/vertex is already present in the set, then DO NOT add the contact.
//When adding a contact, add ALL other voronoi regions to the blocked sets.
}
}
if (UseImprovedBoundaryHandling)
{
//If there were no face contacts that absolutely must be included, we may get into a very rare situation
//where absolutely no contacts get created. For example, a sphere falling directly on top of a vertex in a flat terrain.
//It will generally get locked out of usage by belonging only to restricted regions (numerical issues make it visible by both edges and vertices).
//In some cases, the contacts will be ignored instead of corrected (e.g. spheres).
//To prevent objects from just falling through the ground in such a situation, force-correct the contacts regardless of the pair tester's desires.
//Sure, it might not be necessary under normal circumstances, but it's a better option than having no contacts.
//TODO: There is another option: Changing restricted regions so that a vertex only restricts the other two vertices and the far edge,
//and an edge only restricts the far vertex and other two edges. This introduces an occasional bump though...
//It's possible, in very specific instances, for an object to wedge itself between two adjacent triangles.
//For this state to continue beyond a brief instant generally requires the object be orientation locked and slender.
//However, some characters fit this description, so it can't be ignored!
//Conceptually, this issue can occur at either a vertex junction or a shared edge (usually on extremely flat surfaces only).
//However, an object stuck between multiple triangles is not in a stable state. In the edge case, the object gets shoved to one side
//as one contact 'wins' the solver war. That's not enough to escape, unfortunately.
//The vertex case, on the other hand, is degenerate and decays into an edge case rapidly thanks to this lack of stability.
//So, we don't have to explicitly handle the somewhat more annoying and computationally expensive vertex unstucking case, because the edge case handles both! :)
//This isn't a completely free operation, but it's guarded behind pretty rare conditions.
//Essentially, we will check to see if there's just edge contacts fighting against each other.
//If they are, then we will correct any stuck-contributing normals to the triangle normal.
if (boundarySets.VertexContacts.Count == 0 && guaranteedContacts == 0 && boundarySets.EdgeContacts.Count > 1)
{
//There are only edge contacts, check to see if:
//all normals are coplanar, and
//at least one normal faces against the other normals (meaning it's probably stuck, as opposed to just colliding on a corner).
bool allNormalsInSamePlane = true;
bool atLeastOneNormalAgainst = false;
var firstNormal = boundarySets.EdgeContacts.Elements[0].ContactData.Normal;
boundarySets.EdgeContacts.Elements[0].CorrectedNormal.Normalize();
float dot;
Vector3.Dot(ref firstNormal, ref boundarySets.EdgeContacts.Elements[0].CorrectedNormal, out dot);
if (Math.Abs(dot) > .01f)
{
//Go ahead and test the first contact separately, since we're using its contact normal to determine coplanarity.
allNormalsInSamePlane = false;
}
else
{
//TODO: Note that we're only checking the new edge contacts, not the existing contacts.
//It's possible that some existing contacts could interfere and cause issues, but for the sake of simplicity and due to rarity
//we'll ignore that possibility for now.
for (int i = 1; i < boundarySets.EdgeContacts.Count; i++)
{
Vector3.Dot(ref boundarySets.EdgeContacts.Elements[i].ContactData.Normal, ref firstNormal, out dot);
if (dot < 0)
{
atLeastOneNormalAgainst = true;
}
//Check to see if the normal is outside the plane.
Vector3.Dot(ref boundarySets.EdgeContacts.Elements[i].ContactData.Normal, ref boundarySets.EdgeContacts.Elements[0].CorrectedNormal, out dot);
if (Math.Abs(dot) > .01f)
{
//We are not stuck!
allNormalsInSamePlane = false;
break;
}
}
}
if (allNormalsInSamePlane && atLeastOneNormalAgainst)
{
//Uh oh! all the normals are parallel... The object is probably in a weird situation.
//Let's correct the normals!
//Already normalized the first contact above.
//We don't need to perform the perpendicularity test here- we did that before! We know it's perpendicular already.
boundarySets.EdgeContacts.Elements[0].ContactData.Normal = boundarySets.EdgeContacts.Elements[0].CorrectedNormal;
boundarySets.EdgeContacts.Elements[0].ShouldCorrect = true;
for (int i = 1; i < boundarySets.EdgeContacts.Count; i++)
{
//Must normalize the corrected normal before using it.
boundarySets.EdgeContacts.Elements[i].CorrectedNormal.Normalize();
Vector3.Dot(ref boundarySets.EdgeContacts.Elements[i].CorrectedNormal, ref boundarySets.EdgeContacts.Elements[i].ContactData.Normal, out dot);
if (dot < .01)
{
//Only bother doing the correction if the normal appears to be pointing nearly horizontally- implying that it's a contributor to the stuckness!
//If it's blocked, the next section will use the corrected normal- if it's not blocked, the next section will use the direct normal.
//Make them the same thing :)
boundarySets.EdgeContacts.Elements[i].ContactData.Normal = boundarySets.EdgeContacts.Elements[i].CorrectedNormal;
boundarySets.EdgeContacts.Elements[i].ShouldCorrect = true;
//Note that the penetration depth is NOT corrected. The contact's depth no longer represents the true depth.
//However, we only need to have some penetration depth to get the object to escape the rut.
//Furthermore, the depth computed from the horizontal opposing contacts is known to be less than the depth in the perpendicular direction.
//If the current depth was NOT less than the true depth along the corrected normal, then the collision detection system
//would have picked a different depth, as it finds a reasonable approximation of the minimum penetration!
//As a consequence, this contact will not be active beyond the object's destuckification, because its contact depth will be negative (or very close to it).
}
}
}
}
for (int i = 0; i < boundarySets.EdgeContacts.Count; i++)
{
//Only correct if it's allowed AND it's blocked.
//If it's not blocked, the contact being created is necessary!
//The normal generated by the triangle-convex tester is already known not to
//violate the triangle sidedness.
if (!boundarySets.BlockedEdgeRegions.Contains(boundarySets.EdgeContacts.Elements[i].Edge))
{
//If it's not blocked, use the contact as-is without correcting it.
AddLocalContact(ref boundarySets.EdgeContacts.Elements[i].ContactData, ref orientation, ref candidatesToAdd);
}
else if (boundarySets.EdgeContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0)
{
//If it is blocked, we can still make use of the contact. But first, we need to change the contact normal to ensure that
//it will not interfere (and cause a bump or something).
float dot;
boundarySets.EdgeContacts.Elements[i].CorrectedNormal.Normalize();
Vector3.Dot(ref boundarySets.EdgeContacts.Elements[i].CorrectedNormal, ref boundarySets.EdgeContacts.Elements[i].ContactData.Normal, out dot);
boundarySets.EdgeContacts.Elements[i].ContactData.Normal = boundarySets.EdgeContacts.Elements[i].CorrectedNormal;
boundarySets.EdgeContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
AddLocalContact(ref boundarySets.EdgeContacts.Elements[i].ContactData, ref orientation, ref candidatesToAdd);
}
//If it's blocked AND it doesn't allow correction, ignore its existence.
}
for (int i = 0; i < boundarySets.VertexContacts.Count; i++)
{
if (!boundarySets.BlockedVertexRegions.Contains(boundarySets.VertexContacts.Elements[i].Vertex))
{
//If it's not blocked, use the contact as-is without correcting it.
AddLocalContact(ref boundarySets.VertexContacts.Elements[i].ContactData, ref orientation, ref candidatesToAdd);
}
else if (boundarySets.VertexContacts.Elements[i].ShouldCorrect || guaranteedContacts == 0)
{
//If it is blocked, we can still make use of the contact. But first, we need to change the contact normal to ensure that
//it will not interfere (and cause a bump or something).
float dot;
boundarySets.VertexContacts.Elements[i].CorrectedNormal.Normalize();
Vector3.Dot(ref boundarySets.VertexContacts.Elements[i].CorrectedNormal, ref boundarySets.VertexContacts.Elements[i].ContactData.Normal, out dot);
boundarySets.VertexContacts.Elements[i].ContactData.Normal = boundarySets.VertexContacts.Elements[i].CorrectedNormal;
boundarySets.VertexContacts.Elements[i].ContactData.PenetrationDepth *= MathHelper.Max(0, dot); //Never cause a negative penetration depth.
AddLocalContact(ref boundarySets.VertexContacts.Elements[i].ContactData, ref orientation, ref candidatesToAdd);
}
//If it's blocked AND it doesn't allow correction, ignore its existence.
}
boundarySets.Dispose();
}
//Remove stale pair testers.
for (int i = activePairTesters.Count - 1; i >= 0; --i)
{
var tester = activePairTesters.Values[i];
if (!tester.Updated)
{
tester.CleanUp();
GiveBackTester(tester);
activePairTesters.FastRemove(activePairTesters.Keys[i]);
}
else
{
tester.Updated = false;
}
}
//Some child types will want to do some extra post processing on the manifold.
ProcessCandidates(ref candidatesToAdd);
//Check if adding the new contacts would overflow the manifold.
if (contacts.Count + candidatesToAdd.Count > 4)
{
//Adding all the contacts would overflow the manifold. Reduce to the best subset.
var reducedCandidates = new QuickList <ContactData>(BufferPools <ContactData> .Thread, bufferPoolSizePower);
ContactReducer.ReduceContacts(contacts, ref candidatesToAdd, contactIndicesToRemove, ref reducedCandidates);
RemoveQueuedContacts();
for (int i = reducedCandidates.Count - 1; i >= 0; i--)
{
Add(ref reducedCandidates.Elements[i]);
reducedCandidates.RemoveAt(i);
}
reducedCandidates.Dispose();
}
else if (candidatesToAdd.Count > 0)
{
//Won't overflow the manifold, so just toss it in PROVIDED that it isn't too close to something else.
for (int i = 0; i < candidatesToAdd.Count; i++)
{
Add(ref candidatesToAdd.Elements[i]);
}
}
PhysicsThreadResources.GiveBack(localTriangleShape);
candidatesToAdd.Dispose();
}
bool TryToStepUsingContact(ref ContactData contact, ref Vector3 down, out Vector3 newPosition)
{
Vector3 position = characterBody.Position;
//The normal of the contact may not be facing perfectly out to the side.
//The detection process allows a bit of slop.
//Correct it by removing any component of the normal along the local up vector.
Vector3 normal = contact.Normal;
float dot;
Vector3.Dot(ref normal, ref down, out dot);
Vector3 error;
Vector3.Multiply(ref down, dot, out error);
Vector3.Subtract(ref normal, ref error, out normal);
normal.Normalize();
//Now we need to ray cast out from the center of the character in the direction of this normal to check for obstructions.
//Compute the ray origin location. Fire it out of the top of the character; if we're stepping, this must be a valid location.
//Putting it as high as possible helps to reject more invalid step geometry.
Ray ray;
float downRayLength = characterBody.Height;// MaximumStepHeight + upStepMargin;
Vector3.Multiply(ref down, characterBody.Height * .5f - downRayLength, out ray.Position);
Vector3.Add(ref ray.Position, ref position, out ray.Position);
ray.Direction = normal;
//Include a little margin in the length.
//Technically, the character only needs to teleport horizontally by the complicated commented expression.
//That puts it just far enough to have traction on the new surface.
//In practice, the current contact refreshing approach used for many pair types causes contacts to persist horizontally a bit,
//which can cause side effects for the character.
float horizontalOffsetAmount = characterBody.CollisionInformation.Shape.CollisionMargin; // (float)((1 - character.SupportFinder.sinMaximumSlope) * character.Body.CollisionInformation.Shape.CollisionMargin + 0);
float length = characterBody.Radius + horizontalOffsetAmount; // -contact.PenetrationDepth;
if (QueryManager.RayCastHitAnything(ray, length))
{
//The step is obstructed!
newPosition = new Vector3();
return(false);
}
//The down-cast ray origin has been verified by the previous ray cast.
//Let's look for a support!
Vector3 horizontalOffset;
Vector3.Multiply(ref normal, length, out horizontalOffset);
Vector3.Add(ref ray.Position, ref horizontalOffset, out ray.Position);
ray.Direction = down;
//Find the earliest hit, if any.
RayHit earliestHit;
if (!QueryManager.RayCast(ray, downRayLength, out earliestHit) || //Can't do anything if it didn't hit.
earliestHit.T <= 0 || //Can't do anything if the hit was invalid.
earliestHit.T - downRayLength > -minimumUpStepHeight || //Don't bother doing anything if the step is too small.
earliestHit.T - downRayLength < -maximumStepHeight - upStepMargin) //Can't do anything if the step is too tall.
{
//No valid hit was detected.
newPosition = new Vector3();
return(false);
}
//Ensure the candidate surface supports traction.
Vector3 supportNormal;
Vector3.Normalize(ref earliestHit.Normal, out supportNormal);
//Calibrate the normal to face in the same direction as the down vector for consistency.
Vector3.Dot(ref supportNormal, ref down, out dot);
if (dot < 0)
{
Vector3.Negate(ref supportNormal, out supportNormal);
dot = -dot;
}
//If the new surface does not have traction, do not attempt to step up.
if (dot < ContactCategorizer.TractionThreshold)
{
newPosition = new Vector3();
return(false);
}
//Since contact queries are frequently expensive compared to ray cast tests,
//do one more ray cast test. This time, starting from the same position, cast upwards.
//In order to step up, the previous down-ray hit must be at least a character height away from the result of the up-ray.
Vector3.Negate(ref down, out ray.Direction);
//Find the earliest hit, if any.
//RayHit earliestHitUp = new RayHit();
//earliestHitUp.T = float.MaxValue;
float upLength = characterBody.Height - earliestHit.T;
//If the sum of the up and down distances is less than the height, the character can't fit.
if (QueryManager.RayCastHitAnything(ray, upLength))
{
newPosition = new Vector3();
return(false);
}
//By now, a valid ray hit has been found. Now we need to validate it using contact queries.
//This process is very similar in concept to the down step verification, but it has some extra
//requirements.
//Predict a hit location based on the time of impact and the normal at the intersection.
//Take into account the radius of the character (don't forget the collision margin!)
RigidTransform transform = characterBody.CollisionInformation.WorldTransform;
//The transform must be modified to position the query body at the right location.
//The horizontal offset of the queries ensures that a tractionable part of the character will be put onto the new support.
Vector3.Multiply(ref normal, horizontalOffsetAmount, out horizontalOffset);
Vector3.Add(ref transform.Position, ref horizontalOffset, out transform.Position);
Vector3 verticalOffset;
Vector3.Multiply(ref down, -downRayLength, out verticalOffset);
Vector3.Add(ref transform.Position, ref verticalOffset, out transform.Position);
//We know that the closest point to the plane will be the extreme point in the plane's direction.
//Use it as the ray origin.
Ray downRay;
characterBody.CollisionInformation.Shape.GetExtremePoint(supportNormal, ref transform, out downRay.Position);
downRay.Direction = down;
//Intersect the ray against the plane defined by the support hit.
Vector3 intersection;
Vector3.Dot(ref earliestHit.Location, ref supportNormal, out dot);
Plane plane = new Plane(supportNormal, dot);
Vector3 candidatePosition;
//Define the interval bounds to be used later.
//The words 'highest' and 'lowest' here refer to the position relative to the character's body.
//The ray cast points downward relative to the character's body.
float highestBound = -maximumStepHeight;
float lowestBound = characterBody.CollisionInformation.Shape.CollisionMargin - downRayLength + earliestHit.T;
float currentOffset = lowestBound;
float hintOffset;
var tractionContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
var supportContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
var sideContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
var headContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
try
{
//This guess may either win immediately, or at least give us a better idea of where to search.
float hitT;
if (Toolbox.GetRayPlaneIntersection(ref downRay, ref plane, out hitT, out intersection))
{
hitT = -downRayLength + hitT + CollisionDetectionSettings.AllowedPenetration;
if (hitT < highestBound)
{
//Don't try a location known to be too high.
hitT = highestBound;
}
currentOffset = hitT;
if (currentOffset > lowestBound)
{
lowestBound = currentOffset;
}
candidatePosition = characterBody.Position + down * currentOffset + horizontalOffset;
switch (TryUpStepPosition(ref normal, ref candidatePosition, ref down,
ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts,
out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration)
{
//It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration.
//Just clamp the overall motion and let it penetrate a bit.
newPosition = characterBody.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset;
return(true);
}
else
{
newPosition = new Vector3();
return(false);
}
case CharacterContactPositionState.Rejected:
newPosition = new Vector3();
return(false);
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.HeadObstructed:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
} //TODO: If the ray cast doesn't hit, that could be used to early out... Then again, it pretty much can't happen.
//Our guesses failed.
//Begin the regular process. Start at the time of impact of the ray itself.
//How about trying the time of impact of the ray itself?
//Since we wouldn't be here unless there were no contacts at the body's current position,
//testing the ray cast location gives us the second bound we need to do an informed binary search.
int attempts = 0;
//Don't keep querying indefinitely. If we fail to reach it in a few informed steps, it's probably not worth continuing.
//The bound size check prevents the system from continuing to search a meaninglessly tiny interval.
while (attempts++ < 5 && lowestBound - highestBound > Toolbox.BigEpsilon)
{
candidatePosition = characterBody.Position + currentOffset * down + horizontalOffset;
switch (TryUpStepPosition(ref normal, ref candidatePosition, ref down,
ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts,
out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset < 0 && currentOffset > -maximumStepHeight - CollisionDetectionSettings.AllowedPenetration)
{
//It's possible that we let a just-barely-too-high step occur, limited by the allowed penetration.
//Just clamp the overall motion and let it penetrate a bit.
newPosition = characterBody.Position + Math.Max(-maximumStepHeight, currentOffset) * down + horizontalOffset;
return(true);
}
else
{
newPosition = new Vector3();
return(false);
}
case CharacterContactPositionState.Rejected:
newPosition = new Vector3();
return(false);
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + highestBound) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + lowestBound) * .5f;
break;
case CharacterContactPositionState.HeadObstructed:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
}
}
finally
{
tractionContacts.Dispose();
supportContacts.Dispose();
sideContacts.Dispose();
headContacts.Dispose();
}
//Couldn't find a candidate.
newPosition = new Vector3();
return(false);
}
/// <summary>
/// Determines if a down step is possible, and if so, computes the location to which the character should teleport.
/// </summary>
/// <param name="newPosition">New position the character should teleport to if the down step is accepted.</param>
/// <returns>Whether or not the character should attempt to step down.</returns>
public bool TryToStepDown(out Vector3 newPosition)
{
//Don't bother trying to step down if we already have a support contact or if the support ray doesn't have traction.
if (!(SupportFinder.Supports.Count == 0 && SupportFinder.SupportRayData != null && SupportFinder.SupportRayData.Value.HasTraction))
{
newPosition = new Vector3();
return(false);
}
if (!(SupportFinder.SupportRayData.Value.HitData.T - SupportFinder.BottomDistance > minimumDownStepHeight)) //Don't do expensive stuff if it's, at most, a super tiny step that gravity will take care of.
{
newPosition = new Vector3();
return(false);
}
//Predict a hit location based on the time of impact and the normal at the intersection.
//Take into account the radius of the character (don't forget the collision margin!)
Vector3 normal = SupportFinder.SupportRayData.Value.HitData.Normal;
Vector3 down = characterBody.orientationMatrix.Down;
RigidTransform transform = characterBody.CollisionInformation.WorldTransform;
//We know that the closest point to the plane will be the extreme point in the plane's direction.
//Use it as the ray origin.
Ray ray;
characterBody.CollisionInformation.Shape.GetExtremePoint(normal, ref transform, out ray.Position);
ray.Direction = down;
//Intersect the ray against the plane defined by the support hit.
Vector3 intersection;
Plane plane = new Plane(normal, Vector3.Dot(SupportFinder.SupportRayData.Value.HitData.Location, normal));
Vector3 candidatePosition;
//Define the interval bounds to be used later.
//The words 'highest' and 'lowest' here refer to the position relative to the character's body.
//The ray cast points downward relative to the character's body.
float highestBound = 0;
//The lowest possible distance is the ray distance plus the collision margin because the ray could theoretically be on the outskirts of the collision margin
//where the shape would actually have to move more than the bottom distance difference would imply.
//(Could compute the true lowest bound analytically based on the horizontal position of the ray...)
float lowestBound = characterBody.CollisionInformation.Shape.CollisionMargin + SupportFinder.SupportRayData.Value.HitData.T - SupportFinder.BottomDistance;
float currentOffset = lowestBound;
float hintOffset;
var tractionContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
var supportContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
var sideContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
var headContacts = new QuickList <CharacterContact>(BufferPools <CharacterContact> .Thread);
try
{
//This guess may either win immediately, or at least give us a better idea of where to search.
float hitT;
if (Toolbox.GetRayPlaneIntersection(ref ray, ref plane, out hitT, out intersection))
{
currentOffset = hitT + CollisionDetectionSettings.AllowedPenetration * 0.5f;
candidatePosition = characterBody.Position + down * currentOffset;
switch (TryDownStepPosition(ref candidatePosition, ref down,
ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts,
out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset > minimumDownStepHeight && currentOffset < maximumStepHeight)
{
newPosition = characterBody.Position + currentOffset * down;
return(true);
}
else
{
newPosition = new Vector3();
return(false);
}
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + currentOffset) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
}
//Our guesses failed.
//Begin the regular process. Start at the time of impact of the ray itself.
//How about trying the time of impact of the ray itself?
//Since we wouldn't be here unless there were no contacts at the body's current position,
//testing the ray cast location gives us the second bound we need to do an informed binary search.
int attempts = 0;
//Don't keep querying indefinitely. If we fail to reach it in a few informed steps, it's probably not worth continuing.
//The bound size check prevents the system from continuing to search a meaninglessly tiny interval.
while (attempts++ < 5 && lowestBound - highestBound > Toolbox.BigEpsilon)
{
candidatePosition = characterBody.Position + currentOffset * down;
switch (TryDownStepPosition(ref candidatePosition, ref down, ref tractionContacts, ref supportContacts, ref sideContacts, ref headContacts, out hintOffset))
{
case CharacterContactPositionState.Accepted:
currentOffset += hintOffset;
//Only use the new position location if the movement distance was the right size.
if (currentOffset > minimumDownStepHeight && currentOffset < maximumStepHeight)
{
newPosition = characterBody.Position + currentOffset * down;
return(true);
}
else
{
newPosition = new Vector3();
return(false);
}
case CharacterContactPositionState.NoHit:
highestBound = currentOffset + hintOffset;
currentOffset = (lowestBound + highestBound) * .5f;
break;
case CharacterContactPositionState.Obstructed:
lowestBound = currentOffset;
currentOffset = (highestBound + lowestBound) * .5f;
break;
case CharacterContactPositionState.TooDeep:
currentOffset += hintOffset;
lowestBound = currentOffset;
break;
}
}
//Couldn't find a candidate.
newPosition = new Vector3();
return(false);
}
finally
{
tractionContacts.Dispose();
supportContacts.Dispose();
sideContacts.Dispose();
headContacts.Dispose();
}
}