private static void CalculateSolidEarliestTimeOfImpact(SolidRigidbody solid)
{
Console.WriteLine("running on " + solid.Entity);
float earliest = 1f;
// Regional check against all statics
foreach (var staticbody in AllStatics)
{
foreach (var collider in solid.Colliders)
{
if (earliest == 0f)
{
break;
}
if (PhysicsMath.IntersectMoving(collider, staticbody.MainCollider, solid.ProcessingData.CalcVel, Vector2.Zero, out float first))
{
Console.WriteLine($"{solid.Entity} ({collider.LocalPosition}) hit {staticbody.Entity} at time {first}");
earliest = Math.Min(earliest, first);
}
}
}
// Regional check against all solids
foreach (var otherSolid in AllSolids)
{
if (earliest == 0f)
{
break;
}
// Skip self
if (ReferenceEquals(solid, otherSolid))
{
continue;
}
if (PhysicsMath.IntersectMoving(solid.MainCollider, otherSolid.MainCollider, solid.ProcessingData.CalcVel, otherSolid.ProcessingData.CalcVel, out float first))
{
Console.WriteLine($"{solid.Entity} hit {otherSolid.Entity} at time {first}");
earliest = Math.Min(earliest, first);
}
}
solid.ProcessingData.TimeOfImpact = earliest;
}
/// <summary>
/// Collides the given actor against all other rigidbodies and calculate the allowed velocity during this frame.
/// </summary>
/// <remarks>This function will immediately return if the calculated velocity becomes zero.</remarks>
private static void CalculateActorVelocity(ActorRigidbody actor)
{
actor.ProcessingData.CalcVel = actor.Velocity;
if (actor.Velocity == Vector2.Zero)
{
return;
}
foreach (var staticbody in AllStatics)
{
if (actor.ProcessingData.CalcVel == Vector2.Zero)
{
return;
}
if (PhysicsMath.IsOverlapping(actor.MainCollider, staticbody.MainCollider))
{
if (!PhysicsMath.IsSlidingCorner(actor.MainCollider, staticbody.MainCollider))
{
actor.ProcessingData.CalcVel = VelocityMath.IntoPlane(actor.ProcessingData.CalcVel, PhysicsMath.GetNormal(actor.MainCollider, staticbody.MainCollider));
}
}
}
foreach (var solid in AllSolids)
{
foreach (var solidCollider in solid.Colliders)
{
if (actor.ProcessingData.CalcVel == Vector2.Zero)
{
return;
}
if (PhysicsMath.IsOverlapping(actor.MainCollider, solidCollider))
{
if (!PhysicsMath.IsSlidingCorner(actor.MainCollider, solidCollider))
{
actor.ProcessingData.CalcVel = VelocityMath.IntoPlane(actor.ProcessingData.CalcVel, PhysicsMath.GetNormal(actor.MainCollider, solidCollider));
}
}
}
}
}
private static void ResolvingPhase()
{
foreach (var actor in AllActors)
{
foreach (var collider in AllColliders)
{
if (actor.Colliders.Contains(collider))
{
continue; // skip self
}
if (PhysicsMath.IsInsideExact(actor.MainCollider, collider))
{
var newPos = PhysicsMath.Depenetrate(actor.MainCollider, collider);
Console.WriteLine("Corrected by {0}", (actor.Entity.Position - newPos).ToStringFixed(8));
actor.Entity.Position = newPos;
}
}
}
}
private static void MoveSolid(SolidRigidbody solid)
{
List <ActorRigidbody> pushList = new List <ActorRigidbody>();
List <ActorRigidbody> relocateList = new List <ActorRigidbody>();
// Run global check to see if any actors are attached.
foreach (var actor in AllActors)
{
if (actor.IsAttached(solid))
{
pushList.Add(actor);
}
}
// Run regional check to find any actors that need to be moved during this movement update.
foreach (var actor in AllActors)
{
// Skip if actor has already indicated it should be moved.
if (pushList.Contains(actor))
{
continue;
}
if (PhysicsMath.IntersectPush(solid.MainCollider, actor.MainCollider, solid.Velocity, solid.ProcessingData.TimeOfImpact, out Vector2 delta))
{
pushList.Add(actor);
actor.ProcessingData.MoveDelta = delta;
}
else if (PhysicsMath.ShouldDrag(actor.MainCollider, solid.MainCollider, actor.Velocity, solid.ProcessingData.CalcVel))
{
relocateList.Add(actor);
}
}
foreach (var actor in pushList)
{
actor.Entity.Position += actor.ProcessingData.MoveDelta;
foreach (var otherSolid in AllSolids)
{
// Skip solid that actor was pushed by
if (ReferenceEquals(solid, otherSolid))
{
continue;
}
// If resultant position is inside another solid, then actor has to be crushed.
if (PhysicsMath.IsInside(actor.MainCollider, otherSolid.MainCollider))
{
Console.WriteLine($"Crush {actor} against {otherSolid}");
actor.Crush();
}
}
}
Vector2 solidDelta = solid.ProcessingData.CalcVel * solid.ProcessingData.TimeOfImpact;
solid.Entity.Position += solidDelta;
foreach (var actor in relocateList)
{
actor.Entity.Position += solidDelta;
Console.WriteLine($"relocated {actor} by {solidDelta}");
}
}
private static void CalculateSolidVelocity(SolidRigidbody solid)
{
solid.ProcessingData.CalcVel = solid.Velocity;
if (solid.Velocity == Vector2.Zero)
{
return; // Early exit if no velocity.
}
// Regional check against all static rigidbodies
foreach (var staticbody in AllStatics)
{
foreach (var collider in solid.Colliders)
{
if (solid.ProcessingData.CalcVel == Vector2.Zero)
{
return;
}
if (PhysicsMath.IsOverlapping(collider, staticbody.MainCollider))
{
if (!PhysicsMath.IsSlidingCorner(collider, staticbody.MainCollider))
{
solid.ProcessingData.CalcVel = VelocityMath.IntoPlane(solid.ProcessingData.CalcVel, PhysicsMath.GetNormal(collider, staticbody.MainCollider));
Console.WriteLine($"Solid collision against {staticbody.Entity} ({collider.LocalPosition}) to yield {solid.ProcessingData.CalcVel}");
}
}
}
}
// Regional check against other solids
foreach (var otherSolid in AllSolids)
{
if (solid.ProcessingData.CalcVel == Vector2.Zero)
{
return;
}
// Skip self
if (ReferenceEquals(solid, otherSolid))
{
continue;
}
// Skip if other solid has already processed this solid
if (otherSolid.ProcessingData.CheckedAgainst.Contains(solid))
{
continue;
}
if (PhysicsMath.IsOverlapping(solid.MainCollider, otherSolid.MainCollider))
{
if (!PhysicsMath.IsSlidingCorner(solid.MainCollider, otherSolid.MainCollider))
{
VelocityPair result = Ruleset.Get(solid, otherSolid);
Console.WriteLine($"Collided {solid.Entity} against {otherSolid.Entity} to yield {result.left} and {result.right}");
solid.ProcessingData.CalcVel = result.left;
otherSolid.ProcessingData.CalcVel = result.right;
solid.ProcessingData.CheckedAgainst.Add(otherSolid);
otherSolid.ProcessingData.CheckedAgainst.Add(solid);
}
}
}
}