OpenSim.Region.Framework.Scenes.Scene.GetGroundHeight C# (CSharp) Method

        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;
        }