Simple space partition algorithm which stores two Z planes and swaps between these two planes

MCSpacePartition.cs

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




/// <summary>
/// Simple space partition algorithm which stores two Z planes and swaps between these two planes
/// </summary>
public class MCSpacePartition {

	struct MCPair {
		public Vector3 point;
		public int index;
		public int z;
		public MCPair(Vector3 point, int index, int z){
			this.point = point;
			this.index = index;
			this.z = z;
		}
	}

	float epsilogDistanceSqr;
	List<MCPair>[,] data;
	int lastZ = -1;

	public MCSpacePartition(int width, int height, float epsilogDistance = 0.01f){
		epsilogDistanceSqr = epsilogDistance*epsilogDistance;
		data = new List<MCPair>[width+2,height+2];
		for (int x=0;x<data.GetLength(0);x++){
			for (int y=0;y<data.GetLength(1);y++){
				data[x,y] = new List<MCPair>();
			}
		}
	}

	void CleanUp(int z){
		for (int x=0;x<data.GetLength(0);x++){
			for (int y=0;y<data.GetLength(1);y++){
				data[x,y].RemoveAll(delegate(MCPair obj) {
					return obj.z < z;
				});
			}
		}
	}

	public void InsertPoint(Vector3 point, int index, Vec3i voxelIndex){
		data[voxelIndex.x+1, voxelIndex.y+1].Add(new MCPair(point, index, voxelIndex.z));
		if (voxelIndex.z != lastZ){
			CleanUp(lastZ);
			lastZ = voxelIndex.z;
		}
	}

	public int FindPoint(Vector3 point, Vec3i voxelIndex){
		int closestPoint = -1;
		float closestSqrDistance = float.MaxValue;
		for (int i=0;i<4;i++){
			int x = voxelIndex.x + 1;
			int y = voxelIndex.y + 1;
			if (i == 1) {
				x -= 1;
				if (x < 0){
					continue;
				}
			} else if (i == 2){
				y -= 1;
				if (y < 0){
					continue;
				}
			} else if (i == 3){
				y -= 1;
				x -= 1;
				if (y < 0 || x < 0){
					continue;
				}
			}

			foreach (var mcPair in data[x, y]){
				float sqrDist = (mcPair.point - point).magnitude;
				if (sqrDist < epsilogDistanceSqr && sqrDist < closestSqrDistance){
					closestSqrDistance = sqrDist;
					closestPoint = mcPair.index;
				}
			}
		}
		return closestPoint;
	}
}