public Sweep SweepCollideInto(AABBCollider colliding, Vector3D collidingVelocity, AABBCollider[] colliders)
{
//AABB box = collider.AABB;
AABB col = colliding.AABB;
Vector3D delta = collidingVelocity;
colliders = colliders.OrderBy(c => Vector3D.Distance(col.Position, c.Center)).ToArray();
Sweep nearest = new Sweep();
nearest.Time = 1.0D;
nearest.Position = col.Position + delta;
for (int i = 0, il = colliders.Length; i < il; i++)
{
Sweep sweep = this.SweepCollide(colliding, delta, colliders[i]);
if (sweep.Time < nearest.Time)
{
nearest = sweep;
}
}
return(nearest);
}
public Sweep SweepCollide(AABBCollider colliding, Vector3D collidingVelocity, AABBCollider collider)
{
AABB box = colliding.AABB;
AABB col = collider.AABB;
Vector3D delta = collidingVelocity;
Sweep sweep = new Sweep();
if (delta == Vector3D.Zero)
{
sweep.Position = box.Position;
sweep.Hit = this.Collide(colliding, collider);
if (sweep.Hit.HasHit)
{
sweep.Hit.Time = 0.0D;
sweep.Time = 0.0D;
}
else
{
sweep.Time = 1.0;
}
return(sweep);
}
sweep.Hit = new RayBoxCollisionProvider().Collide(new Ray(box.Position, delta, delta.Length), collider, box.Extents);
if (sweep.Hit.HasHit)
{
sweep.Time = WMath.Clamp(sweep.Hit.Time - Physics.Epsilon, 0, 1);
sweep.Position = box.Position + delta * sweep.Time;
Vector3D direction = delta.Normalized;
sweep.Hit.Position = new Vector3D(
WMath.Clamp(sweep.Hit.Position.X + direction.X * box.Extents.X, col.Position.X - col.Extents.X,
col.Position.X + col.Extents.X),
WMath.Clamp(sweep.Hit.Position.Y + direction.Y * box.Extents.Y, col.Position.Y - col.Extents.Y,
col.Position.Y + col.Extents.Y),
WMath.Clamp(sweep.Hit.Position.Z + direction.Z * box.Extents.Z, col.Position.Z - col.Extents.Z,
col.Position.Z + col.Extents.Z)
);
}
else
{
sweep.Position = box.Position + delta;
sweep.Time = 1.0D;
}
return(sweep);
}