/// <summary>
/// Calculates the final position that the collider should have when moving from <paramref name="startPos"/> to <paramref name="endPos"/>.
/// For all movement purposes, <see cref="ResolveFast(Vector3, Vector3, float, bool)"/> should be used instead as it will handle moving at high speeds with more accurate collisions.
/// Note that the rotation of the collider is taken at the time of calling this method, so adjust collider rotation before calling this.
/// </summary>
/// <param name="startPos">The starting position of the collider in world space.</param>
/// <param name="endPos">The target end position of the collider in world space.</param>
/// <param name="maxTimeMs">The maximum allowed time to resolve the collisions, in milliseconds. After this time has elapsed then the method automatically quits which may result in incomplete or incorrect collision resolution.
/// If set to zero or less, infinite time is allowed, but this is dangerous because an infinite loop can be caused in certain situations.</param>
/// <param name="givePenetrationStats">If set to true, the collision result will include a list of colliders and their penetration depth that were encounted while resolving collisions. Useful for applying forces to them.</param>
/// <returns>The collision result. Includes final position, collision info and debug stats. Also may contain an error message, see <see cref="CustomCollisionResult.Error"/>.</returns>
public virtual CustomCollisionResult Resolve(Vector3 startPos, Vector3 endPos, float maxTimeMs = 2f, bool givePenetrationStats = false)
{
CustomCollisionResult res = new CustomCollisionResult();
if (Collider == null)
{
res.Error = "Collider is null. Assign a valid collider to CustomCollisionResolver.Collider.";
return(res);
}
if (!(Collider is CapsuleCollider) && !(Collider is SphereCollider) && !(Collider is BoxCollider))
{
res.Error = $"Collider must be a capsule, a sphere or a box. {Collider.GetType().Name} is not valid.";
return(res);
}
if (givePenetrationStats)
{
res.ColliderPenetration = new List <(Collider collider, float totalDepth)>();
penetrationDepths.Clear();
}
if (DrawDebug)
{
Debug.DrawLine(startPos, endPos, new Color(0, 1, 1, 0.7f));
Debug.DrawLine(startPos, startPos + (endPos - startPos) * 0.2f, Color.blue);
}
watch.Restart();
Vector3 workingPos = endPos;
int itterations = 0;
do
{
// Get all the overlaps at the current working position.
int count = GetAllOverlaps(workingPos, Overlaps);
// Need to filter out colliders that we don't want to be colliding with...
int newCount = count;
for (int i = 0; i < count; i++)
{
Collider c = Overlaps[i];
bool shouldCollide = ShouldCollideWith(c);
// Remove this collider if it shouldn't be collided with.
if (!shouldCollide)
{
Overlaps[i] = null;
newCount--;
}
}
// If at this working position nothing is overlaping, then we have found the final position.
if (newCount == 0)
{
if (itterations == 0)
{
res.NoCollisions = true;
}
break;
}
// Take the first collider...
Collider found = null;
for (int i = 0; i < count; i++)
{
Collider c = Overlaps[i];
if (c != null)
{
found = c;
break;
}
}
// Calculate penetration resolution.
bool doOverlap = Physics.ComputePenetration(Collider, workingPos, Collider.transform.rotation, found, found.transform.position, found.transform.rotation, out Vector3 resolveDir, out float resolveDst);
itterations++;
if (doOverlap)
{
// Update stats if necessary.
if (givePenetrationStats)
{
if (!penetrationDepths.ContainsKey(found))
{
penetrationDepths.Add(found, resolveDst);
}
else
{
penetrationDepths[found] += resolveDst;
}
}
// Move working position out of the way.
const float ADDITIONAL = 0.0001f;
Vector3 offset = resolveDir * (resolveDst + ADDITIONAL);
if (DrawDebug)
{
Debug.DrawLine(workingPos, workingPos + offset, new Color(1f, 0.92f, 0.016f, 0.7f));
Debug.DrawLine(workingPos, workingPos + offset * 0.2f, Color.red);
}
workingPos += offset;
}
else
{
//Debug.LogWarning($"Did not overlap with {found.name}!");
if (DrawDebug)
{
Debug.DrawLine(startPos, endPos, Color.red);
Debug.DrawLine(startPos, startPos + Vector3.up * 0.1f, Color.red);
}
break;
}
} while (maxTimeMs <= 0f || watch.Elapsed.TotalMilliseconds < maxTimeMs);
// Stop watch...
watch.Stop();
// Write results...
res.FinalPosition = workingPos;
res.ComputationTime = (float)watch.Elapsed.TotalSeconds;
res.ComputationItterations = itterations;
if (givePenetrationStats)
{
foreach (var pair in penetrationDepths)
{
res.ColliderPenetration.Add((pair.Key, pair.Value));
}
}
if (maxTimeMs > 0f && watch.Elapsed.TotalMilliseconds >= maxTimeMs)
{
res.Error = $"Computation time ({watch.Elapsed.TotalMilliseconds:F2} ms) exceeded the allowed limit ({maxTimeMs:F2} ms). Collisions may not have been resolved correctly!";
}
return(res);
}
/// <summary>
/// Calculates the final position that the collider should have when moving from <paramref name="startPos"/> to <paramref name="endPos"/>.
/// This version of the method, compared to <see cref="Resolve(Vector3, Vector3, float, bool)"/>, is optimized for large distances between the start and end position, allowing for fast movement.
/// Note that the rotation of the collider is taken at the time of calling this method, so adjust collider rotation before calling this.
/// </summary>
/// <param name="startPos">The starting position of the collider in world space.</param>
/// <param name="endPos">The target end position of the collider in world space.</param>
/// <param name="maxTimeMs">The maximum allowed time to resolve the collisions, in milliseconds. After this time has elapsed then the method automatically quits which may result in incomplete or incorrect collision resolution.
/// If set to zero or less, infinite time is allowed, but this is dangerous because an infinite loop can be caused in certain situations.</param>
/// <param name="givePenetrationStats">If set to true, the collision result will include a list of colliders and their penetration depth that were encounted while resolving collisions. Useful for applying forces to them.</param>
/// <returns>The collision result. Includes final position, collision info and debug stats. Also may contain an error message, see <see cref="CustomCollisionResult.Error"/>.</returns>
public CustomCollisionResult ResolveFast(Vector3 startPos, Vector3 endPos, float maxTimeMS = 2f, bool givePenetrationStats = false)
{
// Sweep along line to check for collision.
bool didHit = SweepCheck(startPos, endPos, out Vector3 finalPos);
if (!didHit)
{
return(new CustomCollisionResult()
{
FinalPosition = endPos, ComputationItterations = 0, NoCollisions = true
});
}
// Resolve from the initial point of contact, to just past it. This essentially gives the 'normal' position of the collider, where the collider will not be clipping with the other
// but this will also prevent the sliding along surfaces that is desired.
const float IDENTICAL_POS_TOLERANCE = 0.002f * 0.002f; // 2mm
Vector3 dir = (endPos - startPos).normalized;
// Note to self: if pen depth is greater than the overall distance that we want to travel (start to end), then due to the nature of the algorithm the resolved position will put the object further than (start to end) distance.
// Because of this pend depth must be limited to be at most as large as the total distance.
float depthToUse = _fastResolveDepth;
//float cap = (endPos - startPos).magnitude;
//if (depthToUse > cap)
// depthToUse = cap;
Vector3 offset = dir * depthToUse;
float targetDistance = (endPos - finalPos).magnitude * (1f);
CustomCollisionResult result = default;
float multiplier = 1f;
// To solve the no sliding issue, we slide a bunch of times in very small increments.
for (int i = 0; i < 1000; i++)
{
result = Resolve(finalPos, finalPos + offset, maxTimeMS, givePenetrationStats);
// If we are moving back to the same position, then we can't move any further.
if ((finalPos - result.FinalPosition).sqrMagnitude <= IDENTICAL_POS_TOLERANCE)
{
break;
}
if (i == 0)
{
// Work out normal of first surface.
Vector3 start = finalPos + offset;
Vector3 end = result.FinalPosition;
// Work out incoming vector.
Vector3 start2 = finalPos;
Vector3 end2 = finalPos + offset;
// Draw normals.
if (DrawDebug)
{
Debug.DrawLine(start, end - (end - start) * 0.7f, Color.green);
Debug.DrawLine(end2, end2 + (start2 - end2) * 0.3f, Color.green);
}
Vector3 normalized1 = (end - start).normalized;
Vector3 normalized2 = (start2 - end2).normalized;
float dot = Vector3.Dot(normalized1, normalized2);
multiplier = Mathf.Clamp01(1f - dot);
multiplier = SurfaceAngleDistanceCurve.Evaluate(multiplier);
targetDistance *= multiplier;
}
targetDistance -= (finalPos - result.FinalPosition).magnitude;
// Draw final path (magenta).
if (DrawDebug)
{
Debug.DrawLine(finalPos, result.FinalPosition, new Color(1, 0, 1, 0.7f));
}
// Rewind 'jittering' bug fix:
bool appliedJitterFix = false;
if (DistanceCheckMode == ExtraDistanceResolveMode.Rewind)
{
if (multiplier < 0.01f)
{
DistanceCheckMode = ExtraDistanceResolveMode.Truncate;
appliedJitterFix = true;
}
}
bool done = false;
switch (DistanceCheckMode)
{
case ExtraDistanceResolveMode.None:
if (targetDistance <= 0f)
{
done = true;
}
break;
case ExtraDistanceResolveMode.Truncate:
if (targetDistance <= 0f && i > 0) // Only applicable after the first move, because otherwise there is nothing to truncate!
{
// Set this frames final position to last frames final. Essentially rewinds the path by one segment.
result.FinalPosition = finalPos;
done = true;
}
break;
case ExtraDistanceResolveMode.Rewind:
if (targetDistance <= 0f)
{
// Rewind along the 'real' path (magenta).
float excess = -targetDistance;
Vector3 fixDir = (finalPos - result.FinalPosition);
if (excess < fixDir.magnitude)
{
Vector3 realFinal = result.FinalPosition + fixDir.normalized * excess;
result.FinalPosition = realFinal;
}
else
{
Vector3 realFinal = finalPos; // Last frame's one.
result.FinalPosition = realFinal;
}
done = true;
}
break;
}
if (appliedJitterFix)
{
DistanceCheckMode = ExtraDistanceResolveMode.Rewind;
}
if (done)
{
break;
}
finalPos = result.FinalPosition;
}
return(result);
}