public float GetGroundHeight(float x, float y)
{
if (x < 0)
x = 0;
if (x >= Voxels.Width)
x = Voxels.Width - 1;
if (y < 0)
y = 0;
if (y >= Voxels.Length)
y = Voxels.Length - 1;
double[,] heights = Voxels.GetDoubles();
Vector3 p0 = new Vector3(x, y, (float)heights[(int)x, (int)y]);
Vector3 p1 = new Vector3(p0);
Vector3 p2 = new Vector3(p0);
p1.X += 1.0f;
if (p1.X < Voxels.Width)
p1.Z = (float)heights[(int)p1.X, (int)p1.Y];
p2.Y += 1.0f;
if (p2.Y < Voxels.Length)
p2.Z = (float)heights[(int)p2.X, (int)p2.Y];
Vector3 v0 = new Vector3(p1.X - p0.X, p1.Y - p0.Y, p1.Z - p0.Z);
Vector3 v1 = new Vector3(p2.X - p0.X, p2.Y - p0.Y, p2.Z - p0.Z);
v0.Normalize();
v1.Normalize();
Vector3 vsn = new Vector3();
vsn.X = (v0.Y * v1.Z) - (v0.Z * v1.Y);
vsn.Y = (v0.Z * v1.X) - (v0.X * v1.Z);
vsn.Z = (v0.X * v1.Y) - (v0.Y * v1.X);
vsn.Normalize();
float xdiff = x - (float)((int)x);
float ydiff = y - (float)((int)y);
return (((vsn.X * xdiff) + (vsn.Y * ydiff)) / (-1 * vsn.Z)) + p0.Z;
}