TextureFromPoints4

//  http://forum.unity3d.com/threads/144235-A-Fast-KD-Tree-or-Unity-Alternative
//  All rights reserved. Copyright NPSF3000 2012  Attribution Required.

//  I think I have enough data to give an example of how I'd tackle the problem.
//  This is written as a console application in VS because it has a few more features
//  and niceties to reduce my prototyping time.  Feel free to use it as a console app,
//  or port it unity or even windows forms!

//  Tested on a i3 2120, windows 7 64, x86 build, plenty of ram and a SSD.

//  This version has had multithreading implemented.
//  Version 4 : Optimisations [Fast BMP Generation, Flexible Save Format] resulting in 2000% speedup

using System;
using System.Collections.Generic;
using System.Diagnostics;
using System.Drawing;
using System.Drawing.Imaging;
using System.Linq;
using System.Runtime.InteropServices;
using System.Threading.Tasks;
using System.IO;

namespace TextureFromPoints4
{
    class Program
    {
        const int numSensors = 70000;
        const int textureSize = 1024;
        const int intervals = 200;

        const bool useFastBMPGenerator = true;

        //BMP is much faster than PNG to save, but at the cost of no compression.
        //Simple tests show a 530% time saving [7ms vs 44ms] but 100% larger file. [1MB vs 500KB per 1024x1024 image, 70k poins]
        static ImageFormat imageType = ImageFormat.Bmp;

        //Disabling debug mode removes some console output that can slow the program by 1s [25%]
        const bool debugMode = false;

        const string directory = @"C:\TextureFromPoints4\";
        //Deletes all files in the data directory at start
        const bool cleanDirAtStart = true;

        //Changing hash size can have dramatic effect on the my Spacial Collection's performance.
        //Ideal hash size relies upon the densisty of sensors:pixels -
        //For example the hash size of 2 will fill a 1024x1024 texture with 70,000 points in 0.5s.
        //However, if you reduce the number of points to 700 it will take 13 seconds.
        //Setting hash to less than 1 will use a simple formula to guess the correct size.
        //Otherwise the code will use whatever variable you choose.
        static int hash = -1;

        //If maxThreads is under 1 the system default will be used.
        static int maxThreads = -1;

        static Random rnd = new Random();

        static void Main(string[] args)
        {
            var pOptions = new ParallelOptions();
            if (maxThreads > 0) pOptions.MaxDegreeOfParallelism = maxThreads;

            if (hash < 1)
            {
                var magic = Math.Sqrt(textureSize * textureSize / numSensors) / 2;  //Magic Formula
                hash = (int)Math.Pow(2, (int)(Math.Log(magic - 1, 2)) + 1);  //Rounds up to nearest power of 2.
                if (hash < 1) hash = 1;
                Console.WriteLine("Rough Hash Generated: {0}  [Magic Density : {1}]", hash, magic);
            }
            else
                Console.WriteLine("Using Harcoded Hash: {0}", hash);
            Console.WriteLine("");

            while (true)
            {
                var dir = Directory.CreateDirectory(directory);
                if (cleanDirAtStart) foreach (FileInfo file in dir.GetFiles()) file.Delete();


                Console.WriteLine("Starting");
                Console.WriteLine("");

                var swTotalTime = Stopwatch.StartNew();

                //Step one: get the raw data:
                var sensors = GetData();

                //Step 2&3 - generate the sensor texture.  We create a new spacial hash for every thread.
                //This decreases texture geration time from 700ms to about 500ms.
                //As such, at these settings the improvement is minimal - tough different workloads may vary.

                var sw = Stopwatch.StartNew();
                int splitCount = Environment.ProcessorCount / 2;

                Sensor[,] sensorTexture = new Sensor[textureSize, textureSize];

                Parallel.For(0, splitCount, pOptions, i =>
                {
                    //Generate the spacial hash
                    var spacialhash = new SimpleSpacialCollection(textureSize, hash);
                    foreach (var sensor in sensors) spacialhash.AddSensor(sensor);

                    //remember the offset
                    int offsetSize = textureSize / splitCount;
                    int offset = offsetSize * i;
                    //Create the array to store the elements
                    var result = new Sensor[offsetSize, textureSize];

                    //Fill the result
                    for (int x = 0; x < offsetSize; x++)
                        for (int y = 0; y < textureSize; y++)
                            result[x, y] = spacialhash.GetNearest(x + offset, y);

                    //Copies the result to the sensorTexture.
                    Array.Copy(result, 0, sensorTexture, offset * textureSize, result.Length);
                });
                sw.Stop();
                Console.WriteLine("Mapped {0} sensors to {1}x{1} pixels in {2}ms", numSensors, textureSize, sw.ElapsedMilliseconds);

                //Step 4&5 - Creating and saving textures.
                //It makes sense to combine IO and Compute bound tasks together - so IO isn't idling while computing and vice vera
                var sw5 = Stopwatch.StartNew();
                Parallel.For(0, intervals, pOptions, i =>
                {
                    var sw1 = Stopwatch.StartNew();
                    var bmp = useFastBMPGenerator ? GenerateBMPv2(sensorTexture, i) : GenerateBMP(sensorTexture, i);
                    sw1.Stop();
                    if (debugMode) Console.WriteLine("Created Texture {0} in {1}ms", i, sw1.ElapsedMilliseconds);

                    sw1 = Stopwatch.StartNew();
                    bmp.Save(directory + i + "." + imageType.ToString().ToLower(), imageType);
                    bmp.Dispose();  //This is new - keeps memory consumption down!
                    sw1.Stop();
                    if (debugMode) Console.WriteLine("Saved Texture {0} in {1}ms", i, sw1.ElapsedMilliseconds);
                });
                sw5.Stop();

                Console.WriteLine("Created & Saved {0} Textures as {2} in {1}ms", intervals, sw5.ElapsedMilliseconds, imageType.ToString().ToLower());

                swTotalTime.Stop();

                Console.WriteLine("");
                Console.WriteLine("All finished in {0}ms", swTotalTime.ElapsedMilliseconds);
                Console.ReadLine();

                //With Multithreading 200 images, 70k points, 1024 by 1024 takes 86s - faster than the single threaded version at 188.7s.
                //With new v4 optimisations, this now down to ~4 seconds.
            }
        }

        private static Bitmap GenerateBMP(Sensor[,] sensorTexture, long i)
        {
            var bmp = new Bitmap(textureSize, textureSize);
            for (int x = 0; x < textureSize; x++)
                for (int y = 0; y < textureSize; y++)
                {
                    var sensor = sensorTexture[x, y];
                    bmp.SetPixel(x, y, Color.FromArgb((int)(255 * sensor.data[i]), (int)(255 * sensor.data[i]), (int)(255 * sensor.data[i])));
                }
            return bmp;
        }

        //http://stackoverflow.com/questions/6782489/create-bitmap-from-byte-array-of-pixel-data
        private static Bitmap GenerateBMPv2(Sensor[,] sensorTexture, long i)
        {
            var b = new Bitmap(textureSize, textureSize, PixelFormat.Format8bppIndexed);
            ColorPalette ncp = b.Palette;
            for (int k = 0; k < 256; k++)
                ncp.Entries[k] = Color.FromArgb(255, k, k, k);
            b.Palette = ncp;

            var BoundsRect = new Rectangle(0, 0, textureSize, textureSize);
            BitmapData bmpData = b.LockBits(BoundsRect, ImageLockMode.WriteOnly, b.PixelFormat);
            IntPtr ptr = bmpData.Scan0;

            var bytes = new byte[bmpData.Stride * b.Height];

            var width = bmpData.Width;
            var height = bmpData.Height;
            var stride = bmpData.Stride;

            //Console.WriteLine(" {0} : {1} : {2} ", width, height, stride);
            for (int x = 0; x < width; x++)
                for (int y = 0; y < height; y++)
                {
                    var sensor = sensorTexture[x, y];
                    bytes[x + y * stride] = (byte)(255 * sensor.data[i]);
                }

            Marshal.Copy(bytes, 0, ptr, bytes.Length);
            b.UnlockBits(bmpData);
            return b;
        }

        private static Sensor[] GetData()
        {
            //  I don't care how you manage it, but I figure you need a sensor
            //  that has a 2d position [corrisponding with the texture co-ords]
            //  and has a array of values that shows teh sensor's value every interval.
            //  So I faked itL
            var sensors = new Sensor[numSensors];

            for (int i = 0; i < numSensors; i++)
            {
                var sensor = new Sensor();
                sensor.position = new Vector2(textureSize);
                sensor.data = Enumerable.Range(0, intervals).Select(_ => (float)rnd.NextDouble()).ToArray();
                sensors[i] = sensor;
            }
            return sensors;
        }

        public struct Sensor
        {
            public Vector2 position;
            public float[] data;
        }

        public struct Vector2
        {
            public float x;
            public float y;

            public Vector2(float x, float y)
            {
                this.x = x;
                this.y = y;
            }

            public Vector2(float randomDistance)
            {
                this.x = (float)rnd.NextDouble() * randomDistance;
                this.y = (float)rnd.NextDouble() * randomDistance;
            }

            public static Vector2 operator -(Vector2 a, Vector2 b)
            {
                return new Vector2(a.x - b.x, a.y - b.y);
            }

            public float sqrMagnitude()
            {
                return x * x + y * y;
            }

            public float sqrDistanceToPoint(Vector2 point)
            {
                var x = this.x - point.x;
                var y = this.y - point.y;
                return x * x + y * y;
            }
        }

        public class SimpleSpacialCollection
        {
            List<Sensor>[,] buckets;
            public readonly int bounds;
            public readonly int hash;

            public int count { get; private set; }

            public SimpleSpacialCollection(int bounds, int hash)
            {
                this.bounds = bounds;
                this.hash = hash;
                buckets = new List<Sensor>[bounds / hash + 1, bounds / hash + 1];
                count = 0;
            }

            public void AddSensor(Sensor sensor)
            {
                GetBucket(sensor.position).Add(sensor);
                count++;
            }

            List<Sensor> GetBucket(Vector2 v2)
            {
                return GetBucket((int)v2.x, (int)v2.y);
            }

            List<Sensor> GetBucket(int x, int y)
            {
                var bucket = buckets[x / hash, y / hash];
                if (bucket == null)
                {
                    bucket = new List<Sensor>();
                    buckets[x / hash, y / hash] = bucket;
                }
                return bucket;
            }

            public Sensor GetNearest(int pointX, int pointY)
            {
                if (count == 0) throw new Exception("Need more than 0 elements!");

                var candidates = new List<Sensor>();

                int maxX, minX, maxY, minY;

                maxX = minX = pointX / hash;
                maxY = minY = pointY / hash;

                do
                {
                    maxX++; maxY++; minX--; minY--;

                    if (minX < 0) minX = 0;
                    if (minY < 0) minY = 0;
                    if (maxX > buckets.GetLength(0)) maxX = buckets.GetLength(0);
                    if (maxY > buckets.GetLength(1)) maxY = buckets.GetLength(1);

                    for (int x = minX; x < maxX; x++)
                        for (int y = minY; y < maxY; y++)
                        {
                            var bucket = buckets[x, y];
                            if (bucket != null) candidates.AddRange(bucket);
                        }
                } while (candidates.Count == 0);

                var point = new Vector2(pointX, pointY);

                Sensor nearest = candidates[0];
                float nearestDistance = nearest.position.sqrDistanceToPoint(point);

                for (int i = 1; i < candidates.Count; i++)
                {
                    var candidate = candidates[i];
                    if (candidate.position.sqrDistanceToPoint(point) < nearestDistance)
                    {
                        nearest = candidate;
                        nearestDistance = candidate.position.sqrDistanceToPoint(point);
                    }
                }
                return nearest;
            }
        }
    }
}