MoonTools.Bonk/Bonk/NarrowPhase/SimplexVertexBuffer.cs

using System.Collections.Generic;
using System.Numerics;
using MoonTools.Core.Structs;

namespace MoonTools.Core.Bonk
{
    internal unsafe struct SimplexVertexBuffer
    {
        private const int Size = 35;

        public int Length { get; private set; }

        public SimplexVertexBuffer(IEnumerable<Position2D> positions)
        {
            var i = 0;
            foreach (var position in positions)
            {
                if (i == Size) { break; }
                var vertex = position.ToVector2();
                _simplexXBuffer[i] = vertex.X;
                _simplexYBuffer[i] = vertex.Y;
                i++;
            }
            Length = i;
        }

        public Vector2 this[int key]
        {
            get => new Vector2(_simplexXBuffer[key], _simplexYBuffer[key]);
            private set
            {
                _simplexXBuffer[key] = value.X;
                _simplexYBuffer[key] = value.Y;
            }
        }

        public void Insert(int index, Vector2 value)
        {
            for (var i = Length; i > index; i--)
            {
                this[i] = this[i - 1];
            }
            this[index] = value;
            Length++;
        }

        private fixed float _simplexXBuffer[Size];
        private fixed float _simplexYBuffer[Size];
    }
}