SFML SpriteBatch

#define LOOKUP
using SFML.Graphics;
using SFML.Window;
using System;
using System.Collections.Generic;
using System.Diagnostics;


namespace SFML.Graphics
{
    /// <summary>
    /// Provides optimized drawing of sprites
    /// </summary>
    public class SpriteBatch
    {

        public int Count {get { return count; }}

#if(LOOKUP)

        private const int LookupSize = 1024; //has to be power of 2
        static readonly private float[] getSin, getCos;
        static SpriteBatch()
        {
            getSin = new float[LookupSize];
            getCos = new float[LookupSize];

            for (int i = 0; i < LookupSize; i++)
            {
                getSin[i] = (float)Math.Sin(i * Math.PI / LookupSize * 2);
                getCos[i] = (float)Math.Cos(i * Math.PI / LookupSize * 2);
            }

            float max = 0;
            for (int i = 1; i < LookupSize; i++)
            {
                max = Math.Max(max, Math.Abs(getSin[i] - getSin[i - 1]));
            }
            max /= 2;
            Debug.WriteLine("Max sin/cos error is: " + max);
        }
#endif


        Vertex[] vertices = new Vertex[100 * 4];
        int count;
        RenderStates state;

        public RenderTarget RenderTarget;

        /// <summary>
        /// Setting this causes Display
        /// </summary>
        public BlendMode BlendMode
        {
            get { return state.BlendMode; }
            set
            {
                Display(false);
                state.BlendMode = value;
            }
        }

        /// <summary>
        /// Setting this causes Display
        /// </summary>
        public Shader Shader
        {
            get { return state.Shader; }
            set
            {
                Display(false);
                state.Shader = value;
            }
        }

        public SpriteBatch(RenderTarget rt)
        {
            state = RenderStates.Default;
            this.RenderTarget = rt;


        }

        /// <summary>
        /// Sends buffered sprites to GPU.
        /// </summary>
        /// <param name="resetState">true will reset internal  RenderStates to default value</param>
        public void Display(bool resetState = true, bool flush = true)
        {
            if (count > 0)
                RenderTarget.Draw(vertices, 0, (uint)count * 4, PrimitiveType.Quads, state);
            if (resetState)
                state = RenderStates.Default;
            if (flush)
                count = 0;
        }

        public void Flush()
        {
            count = 0;
        }

        public void Draw(IEnumerable<Sprite> sprites)
        {
            foreach (var s in sprites)
                Draw(s);
        }

        public void Draw(Sprite sprite)
        {
            Draw(sprite.Texture, sprite.Position, sprite.TextureRect, sprite.Color, sprite.Scale, sprite.Origin, sprite.Rotation);
        }

        private const int Max = 4000000;


        private const float PI = 3.14f;

        int Create(Texture texture)
        {
            if (texture != state.Texture)
            {
                Display(false);
                state.Texture = texture;
            }

            if (count >= (vertices.Length / 4))
            {
                //Display(false);
                if (vertices.Length < Max)
                {
                    Array.Resize(ref vertices, Math.Min(vertices.Length * 2, Max));
                    //vertices = new Vertex[vertices.Length * 2];
                }
            }
            return 4 * count++;
        }

        /// <summary>
        /// Setting this will cause Display if provided texture is different than previous
        /// </summary>
        public unsafe void Draw(Texture texture, Vector2f position, IntRect rec, Color color, Vector2f scale, Vector2f origin, float rotation = 0)
        {
            var index = Create(texture);
#if(LOOKUP)
            int rot = (int)(rotation * (LookupSize / 360f) + 0.5f) & (LookupSize - 1);
            //int rot = (int) (rotation*(LookupSize/360f) + 0.5f)%LookupSize;
            //if (rot < 0) rot += LookupSize;

            float cos = getCos[rot];
            float sin = getSin[rot];
#else
            float sin = 0, cos = 1;
            if (rotation != 0)
            {
                rotation = rotation * (float)Math.PI / 180;
                sin = (float)Math.Sin(rotation);
                cos = (float)Math.Cos(rotation);
            }
#endif

            scale.X *= rec.Width;
            scale.Y *= rec.Height;
            var pX = -origin.X * scale.X;
            var pY = -origin.Y * scale.Y;

            fixed (Vertex* fptr = vertices)
            {
                var ptr = fptr + index;

                ptr->Position.X = pX * cos - pY * sin + position.X;
                ptr->Position.Y = pX * sin + pY * cos + position.Y;
                ptr->TexCoords.X = rec.Left;
                ptr->TexCoords.Y = rec.Top;
                ptr->Color = color;
                ptr++;

                pX += scale.X;
                ptr->Position.X = pX * cos - pY * sin + position.X;
                ptr->Position.Y = pX * sin + pY * cos + position.Y;
                ptr->TexCoords.X = rec.Left + rec.Width;
                ptr->TexCoords.Y = rec.Top;
                ptr->Color = color;
                ptr++;

                pY += scale.Y;
                ptr->Position.X = pX * cos - pY * sin + position.X;
                ptr->Position.Y = pX * sin + pY * cos + position.Y;
                ptr->TexCoords.X = rec.Left + rec.Width;
                ptr->TexCoords.Y = rec.Top + rec.Height;
                ptr->Color = color;
                ptr++;

                pX -= scale.X;
                ptr->Position.X = pX * cos - pY * sin + position.X;
                ptr->Position.Y = pX * sin + pY * cos + position.Y;
                ptr->TexCoords.X = rec.Left;
                ptr->TexCoords.Y = rec.Top + rec.Height;
                ptr->Color = color;
            }
        }

        public unsafe void Draw(Texture texture, FloatRect rec, IntRect src, Color color)
        {
            var index = Create(texture);

            fixed (Vertex* fptr = vertices)
            {
                var ptr = fptr + index;

                ptr->Position.X = rec.Left;
                ptr->Position.Y = rec.Top;
                ptr->TexCoords.X = src.Left;
                ptr->TexCoords.Y = src.Top;
                ptr->Color = color;
                ptr++;

                ptr->Position.X = rec.Left + rec.Width;
                ptr->Position.Y = rec.Top;
                ptr->TexCoords.X = src.Left + src.Width;
                ptr->TexCoords.Y = src.Top;
                ptr->Color = color;
                ptr++;

                ptr->Position.X = rec.Left +rec.Width;
                ptr->Position.Y = rec.Top + rec.Height;
                ptr->TexCoords.X = src.Left + src.Width;
                ptr->TexCoords.Y = src.Top + src.Height;
                ptr->Color = color;
                ptr++;

                ptr->Position.X = rec.Left;
                ptr->Position.Y = rec.Top + rec.Height;
                ptr->TexCoords.X = src.Left;
                ptr->TexCoords.Y = src.Top + src.Height;
                ptr->Color = color;
            }
        }
    }
}