PongFE/PongFE/Engines/MotionEngine.cs

using System.Collections.Generic;
using System.Numerics;
using Encompass;
using MoonTools.Bonk;
using MoonTools.Structs;
using PongFE.Components;
using PongFE.Enums;
using PongFE.Messages;

namespace PongFE.Engines
{
    [Receives(typeof(MotionMessage))]
    [Sends(
        typeof(UpdatePositionMessage),
        typeof(CollisionMessage)
    )]
    [QueryWith(typeof(PositionComponent), typeof(CollisionComponent))]
    public class MotionEngine : Engine
    {
        private readonly SpatialHash<Entity> _spatialHash = new SpatialHash<Entity>(32);
        private readonly Dictionary<Entity, Vector2> _moveAmounts = new Dictionary<Entity, Vector2>();

        public override void Update(double dt)
        {
            _spatialHash.Clear();
            _moveAmounts.Clear();

            foreach (var entity in TrackedEntities)
            {
                ref readonly var positionComponent = ref GetComponent<PositionComponent>(entity);
                ref readonly var collisionComponent = ref GetComponent<CollisionComponent>(entity);

                _spatialHash.Insert(entity, collisionComponent.Rectangle, new Transform2D(positionComponent.Position));
            }

            foreach (ref readonly var entity in ReadEntities<PositionComponent>())
            {
                ref readonly var positionComponent = ref GetComponent<PositionComponent>(entity);

                _moveAmounts[entity] = Vector2.Zero;
                foreach (var motionMessage in ReadMessagesWithEntity<MotionMessage>(entity))
                {
                    _moveAmounts[entity] += motionMessage.Movement;
                }
            }

            foreach (var entity in TrackedEntities)
            {
                Vector2 moveAmount = _moveAmounts[entity];

                ref readonly var positionComponent = ref GetComponent<PositionComponent>(entity);
                var projectedPosition = positionComponent.Position + moveAmount;

                ref readonly var collisionComponent = ref GetComponent<CollisionComponent>(entity);
                var rectangle = collisionComponent.Rectangle;
                var (xHit, yHit, newPosition, collisionEntity) = SolidCollisionPosition(rectangle, positionComponent.Position, projectedPosition);

                if (xHit || yHit)
                {
                    projectedPosition = newPosition;

                    if (xHit)
                    {
                        SendMessage(new CollisionMessage(entity, collisionEntity, HitOrientation.Horizontal));
                    }
                    else
                    {
                        SendMessage(new CollisionMessage(entity, collisionEntity, HitOrientation.Vertical));
                    }
                }


                SendMessage(new UpdatePositionMessage(entity, projectedPosition));
            }
        }

        private (bool, bool, Position2D, Entity) SolidCollisionPosition(Rectangle rectangle, Position2D startPosition, Position2D endPosition)
        {
            var startX = startPosition.X;
            var endX = endPosition.X;

            var startY = startPosition.Y;
            var endY = endPosition.Y;

            bool xHit, yHit;
            int xPosition, yPosition;
            Entity xCollisionEntity, yCollisionEntity;

            (xHit, xPosition, xCollisionEntity) = SweepX(_spatialHash, rectangle, Position2D.Zero, new Position2D(startX, startY), endX - startX);
            if (!xHit) { xPosition = endX; }
            (yHit, yPosition, yCollisionEntity) = SweepY(_spatialHash, rectangle, Position2D.Zero, new Position2D(xPosition, startY), endY - startY);

            return (xHit, yHit, new Position2D(xPosition, yPosition), xHit ? xCollisionEntity : yCollisionEntity);
        }

        private (bool, int, Entity) SweepX(SpatialHash<Entity> solidSpatialHash, Rectangle rectangle, Position2D offset, Position2D startPosition, int horizontalMovement)
        {
            var sweepResult = SweepTest.Test(solidSpatialHash, rectangle, new Transform2D(offset + startPosition), new Vector2(horizontalMovement, 0));
            return (sweepResult.Hit, startPosition.X + (int)sweepResult.Motion.X, sweepResult.ID);
        }

        public static (bool, int, Entity) SweepY(SpatialHash<Entity> solidSpatialHash, Rectangle rectangle, Position2D offset, Position2D startPosition, int verticalMovement)
        {
            var sweepResult = SweepTest.Test(solidSpatialHash, rectangle, new Transform2D(offset + startPosition), new Vector2(0, verticalMovement));
            return (sweepResult.Hit, startPosition.Y + (int)sweepResult.Motion.Y, sweepResult.ID);
        }
    }
}