Minecraft Collision Engine

using System;
using System.Collections;
using System.Collections.Generic;
using System.Timers;
using UnityEngine;

public static class CollisionEngine
{
    private static Queue<DynamicCollider> dynamicColliders;

    public static void RegisterDynamicCollider(DynamicCollider collider)
    {
        if (dynamicColliders == null)
        {
            dynamicColliders = new Queue<DynamicCollider>();
        }

        if (collider.isRegistered == false)
        {
            dynamicColliders.Enqueue(collider);
            collider.isRegistered = true;
        }
    }

    public static void UnregisterDynamicCollider(DynamicCollider collider)
    {
        if (collider.isRegistered)
        {
            Queue<DynamicCollider> newColliders = new Queue<DynamicCollider>();
            for (int i = 0; i < dynamicColliders.Count; i++)
            {
                DynamicCollider col = dynamicColliders.Dequeue();
                if (col != collider)
                    newColliders.Enqueue(col);
            }
            collider.isRegistered = false;
            dynamicColliders = newColliders;
        }
    }

    public static void RunPhysicsSimulation(float time)
    {
        Queue<DynamicCollider> cols = new Queue<DynamicCollider>(dynamicColliders);

        while (cols.Count > 0)
        {
            DynamicCollider col = cols.Dequeue();
            col.PhysicsUpdate(time);

            DoCollision(col, time);
        }
    }

    private static void DoCollision(DynamicCollider col, float time)
    {
        Pos3Int colliderPos = new Pos3Int((int)col.transform.position.x, (int)col.transform.position.y, (int)col.transform.position.z);
        Queue<ushort> blocksInRadius = new Queue<ushort>();
        float lowestCollisionTime = 1f;
        float highestRemainingTime = 0f;
        Vector3 foundNormal = Vector3.zero;

        for (int x = colliderPos.x - Mathf.RoundToInt(col.colliderSize.x) + Mathf.RoundToInt(col.colliderOffset.x) - 1; x <= colliderPos.x + Mathf.RoundToInt(col.colliderSize.x) + Mathf.RoundToInt(col.colliderOffset.x) + 1; x++)
        {
            for (int z = colliderPos.z - Mathf.RoundToInt(col.colliderSize.z) + Mathf.RoundToInt(col.colliderOffset.z) - 1; z <= colliderPos.z + Mathf.RoundToInt(col.colliderSize.z) + Mathf.RoundToInt(col.colliderOffset.z) + 1; z++)
            {
                Pos2Int chunkPos = new Pos2Int(x / 16, z / 16);
                WorldChunk data = World.FindChunkData(chunkPos);
                if (data != null)
                {
                    for (int y = colliderPos.y - Mathf.RoundToInt(col.colliderSize.y) + Mathf.RoundToInt(col.colliderOffset.y) - 1; y <= colliderPos.y + Mathf.RoundToInt(col.colliderSize.y) + Mathf.RoundToInt(col.colliderOffset.z) + 1; y++)
                    {
                        if (y >= 0 && y < 256)
                        {
                            int xChunk = Mathf.Abs(x) % 16;
                            int zChunk = Mathf.Abs(z) % 16;

                            ushort block = data.chunkInfo[xChunk + zChunk * 16 + y * 256];
                            if (block != 0 && BlockDictionary.GetBlockInformation(block).hasCollision)
                            {
                                Pos3 vel = new Pos3(col.velocity.x * time, col.velocity.y * time, col.velocity.z * time);
                                //if (AABBBroadCheck(broadStart, broadSize, new Pos3Int(x, y, z)))
                                //{
                                    Vector3 normal;
                                    float colTime;
                                    CollisionHandeling(block, new Pos3(x, y, z), col, vel, out normal, out colTime);
                                    if (colTime < lowestCollisionTime)
                                    {
                                        lowestCollisionTime = colTime;
                                        foundNormal = normal;
                                        Debug.Log("Closest collision is " + BlockDictionary.GetBlockInformation(block).internalName);
                                    }
                                //}
                            }
                        }
                    }
                }
            }
        }
        highestRemainingTime = 1 - lowestCollisionTime;

        if(lowestCollisionTime > 0)
            col.transform.position += col.velocity * time * lowestCollisionTime;

        // slide
        // push
        /*
        float magnitude = Mathf.Sqrt((col.velocity.x * col.velocity.x + col.velocity.y * col.velocity.y)) * highestRemainingTime;
        float dotprod = col.velocity.x * foundNormal.y + col.velocity.y * foundNormal.x;
        if (dotprod > 0.0f)
            dotprod = 1.0f;
        else if (dotprod < 0.0f)
            dotprod = -1.0f;
        col.velocity.x = dotprod * foundNormal.y * magnitude;
        col.velocity.y = dotprod * foundNormal.x * magnitude;*/
        //col.velocity.x = foundNormal.x * col.velocity.x;
        //col.velocity.y = foundNormal.y * col.velocity.y;
        //col.transform.position += col.velocity * time;
        Debug.Log(col.velocity + " " + foundNormal);
        col.velocity.x = (foundNormal.x == 0) ? col.velocity.x : 0;
        col.velocity.y = (foundNormal.y == 0) ? col.velocity.y : 0;
        col.velocity.z = (foundNormal.z == 0) ? col.velocity.z : 0;
        Debug.Log(col.velocity + " " + foundNormal.x);
        col.hitNormal = foundNormal;
        /*
        if (highestRemainingTime > 0)
        {
            float dotprod = col.velocity.x * foundNormal.x + col.velocity.y * foundNormal.y;
            col.velocity = col.velocity + dotprod * foundNormal;
        }*/
    }

    private static bool AABBBroadCheck(Vector3 areaStart, Vector3 size, Pos3Int cubePos)
    {
        if (areaStart.x + size.x > cubePos.x && areaStart.x < cubePos.x + 1 &&
            areaStart.y + size.y > cubePos.y && areaStart.y < cubePos.y + 1 &&
            areaStart.z + size.z > cubePos.x && areaStart.z < cubePos.z + 1)
        {
            return true;
        }
        return false;
    }

    private static void GenerateBroadArea(Vector3 start, Vector3 size, out Vector3 finishedStart, out Vector3 finishedSize)
    {
        if (size.x < 0)
        {
            start.x += size.x;
            size.x = -size.x;
        }
        if (size.y < 0)
        {
            start.y += size.y;
            size.y = -size.y;
        }
        if (size.z < 0)
        {
            start.z += size.z;
            size.z = -size.z;
        }
        finishedStart = start;
        finishedSize = size;
        Debug.Log(finishedStart + " " + finishedSize);
    }

    private static void CollisionHandeling(ushort blockId, Pos3 blockPosition, DynamicCollider toCheck, Pos3 velocity, out Vector3 normal, out float collisionTime)
    {
        //Pos3 toCheckPosition = new Pos3(toCheck.transform.position.x + toCheck.colliderOffset.x, toCheck.transform.position.y + toCheck.colliderOffset.y, toCheck.transform.position.z + toCheck.colliderOffset.z) + velocity;

        normal = Vector3.zero;
        collisionTime = SweptAABB(toCheck, blockPosition, velocity, out normal.x, out normal.y, out normal.z);
    }

    private static float SweptAABB(DynamicCollider col, Pos3 blockPos, Pos3 velocity, out float normalX, out float normalY, out float normalZ)
    {
        float xInvEntry, yInvEntry, zInvEntry;
        float xInvExit, yInvExit, zInvExit;
        Pos3 colPos = new Pos3(col.transform.position.x + col.colliderOffset.x, col.transform.position.y + col.colliderOffset.y, col.transform.position.z + col.colliderOffset.z);
        // find the distance between the objects on the near and far sides for both x and y
        if (velocity.x > 0.0f)
        {
            xInvEntry = blockPos.x - (colPos.x + col.colliderSize.x);
            xInvExit = (blockPos.x + 1) - colPos.x;
        }
        else
        {
            xInvEntry = (blockPos.x + 1) - colPos.x;
            xInvExit = blockPos.x - (colPos.x + col.colliderSize.x);
        }

        if (velocity.y > 0.0f)
        {
            yInvEntry = blockPos.y - (colPos.y + col.colliderSize.y);
            yInvExit = (blockPos.y + 1) - colPos.y;
        }
        else
        {
            yInvEntry = (blockPos.y + 1) - colPos.y;
            yInvExit = blockPos.y - (colPos.y + col.colliderSize.y);
        }
        /*
        if (velocity.z > 0.0f)
        {
            zInvEntry = blockPos.z - (colPos.z + col.colliderSize.z);
            zInvExit = (blockPos.z + 1) - colPos.z;
        }
        else
        {
            zInvEntry = (blockPos.z + 1) - colPos.z;
            zInvExit = blockPos.z - (colPos.z + col.colliderSize.z);
        }*/

        // find time of collision and time of leaving for each axis (if statement is to prevent divide by zero)
        float xEntry, yEntry, zEntry;
        float xExit, yExit, zExit;
        if (velocity.x == 0.0f)
        {
            xEntry = float.NegativeInfinity;//-std::numeric_limits::infinity();
            xExit = float.PositiveInfinity;//std::numeric_limits::infinity();
        }
        else
        {
            xEntry = xInvEntry / velocity.x;
            xExit = xInvExit / velocity.x;
        }

        if (velocity.y == 0.0f)
        {
            yEntry = float.NegativeInfinity;//-std::numeric_limits::infinity();
            yExit = float.PositiveInfinity;//std::numeric_limits::infinity();
        }
        else
        {
            yEntry = yInvEntry / velocity.y;
            yExit = yInvExit / velocity.y;
        }
        /*
        if (velocity.z == 0.0f)
        {
            zEntry = float.NegativeInfinity;//-std::numeric_limits::infinity();
            zExit = float.PositiveInfinity;//std::numeric_limits::infinity();
        }
        else
        {
            zEntry = zInvEntry / col.velocity.z;
            zExit = zInvExit / col.velocity.z;
        }
        */

        // find the earliest/latest times of collision
        float entryTime = Mathf.Max(xEntry, yEntry);//, zEntry);
        float exitTime = Mathf.Min(xExit, yExit);//, zExit);

        if (entryTime > exitTime || xEntry < 0.0f && yEntry < 0.0f ||/* zEntry < 0.0f ||*/ xEntry > 1.0f || yEntry > 1.0f /*|| zEntry > 1.0f*/)
        {
            normalX = 0.0f;
            normalY = 0.0f;
            normalZ = 0.0f;
            return 1.0f;
        }
        else // if there was a collision
        {
            // calculate normal of collided surface
            if (xEntry > yEntry)// && xEntry > zEntry)
            {
                if (xInvEntry < 0.0f)
                {
                    normalX = 1.0f;
                    normalY = 0.0f;
                    normalZ = 0.0f;
                }
                else
                {
                    normalX = -1.0f;
                    normalY = 0.0f;
                    normalZ = 0.0f;
                }
            }
            else// if (yEntry > zEntry)
            {
                if (yInvEntry < 0.0f)
                {
                    normalX = 0.0f;
                    normalY = 1.0f;
                    normalZ = 0.0f;
                }
                else
                {
                    normalX = 0.0f;
                    normalY = -1.0f;
                    normalZ = 0.0f;
                }
            }
            /*
            else
            {
                if (zInvEntry < 0.0f)
                {
                    normalX = 0.0f;
                    normalY = 0.0f;
                    normalZ = 1.0f;
                }
                else
                {
                    normalX = 0.0f;
                    normalY = 0.0f;
                    normalZ = -1.0f;
                }
            }
            */
            // return the time of collision
            return entryTime;
        }
    }
}