OpenSim.Region.ScriptEngine.Shared.Api.LSL_Api.GroundIntersection C# (CSharp) Method

        private ContactResult? GroundIntersection(Vector3 rayStart, Vector3 rayEnd)
        {
            double[,] heightfield = World.Heightmap.GetDoubles();
            List<ContactResult> contacts = new List<ContactResult>();

            double min = 2048.0;
            double max = 0.0;

            // Find the min and max of the heightfield
            for (int x = 0 ; x < World.Heightmap.Width ; x++)
            {
                for (int y = 0 ; y < World.Heightmap.Height ; y++)
                {
                    if (heightfield[x, y] > max)
                        max = heightfield[x, y];
                    if (heightfield[x, y] < min)
                        min = heightfield[x, y];
                }
            }


            // A ray extends past rayEnd, but doesn't go back before
            // rayStart. If the start is above the highest point of the ground
            // and the ray goes up, we can't hit the ground. Ever.
            if (rayStart.Z > max && rayEnd.Z >= rayStart.Z)
                return null;

            // Same for going down
            if (rayStart.Z < min && rayEnd.Z <= rayStart.Z)
                return null;

            List<Tri> trilist = new List<Tri>();

            // Create our triangle list
            for (int x = 1 ; x < World.Heightmap.Width ; x++)
            {
                for (int y = 1 ; y < World.Heightmap.Height ; y++)
                {
                    Tri t1 = new Tri();
                    Tri t2 = new Tri();

                    Vector3 p1 = new Vector3(x-1, y-1, (float)heightfield[x-1, y-1]);
                    Vector3 p2 = new Vector3(x, y-1, (float)heightfield[x, y-1]);
                    Vector3 p3 = new Vector3(x, y, (float)heightfield[x, y]);
                    Vector3 p4 = new Vector3(x-1, y, (float)heightfield[x-1, y]);

                    t1.p1 = p1;
                    t1.p2 = p2;
                    t1.p3 = p3;

                    t2.p1 = p3;
                    t2.p2 = p4;
                    t2.p3 = p1;

                    trilist.Add(t1);
                    trilist.Add(t2);
                }
            }

            // Ray direction
            Vector3 rayDirection = rayEnd - rayStart;

            foreach (Tri t in trilist)
            {
                // Compute triangle plane normal and edges
                Vector3 u = t.p2 - t.p1;
                Vector3 v = t.p3 - t.p1;
                Vector3 n = Vector3.Cross(u, v);

                if (n == Vector3.Zero)
                    continue;

                Vector3 w0 = rayStart - t.p1;
                double a = -Vector3.Dot(n, w0);
                double b = Vector3.Dot(n, rayDirection);

                // Not intersecting the plane, or in plane (same thing)
                // Ignoring this MAY cause the ground to not be detected
                // sometimes
                if (Math.Abs(b) < 0.000001)
                    continue;

                double r = a / b;

                // ray points away from plane
                if (r < 0.0)
                    continue;

                Vector3 ip = rayStart + Vector3.Multiply(rayDirection, (float)r);

                float uu = Vector3.Dot(u, u);
                float uv = Vector3.Dot(u, v);
                float vv = Vector3.Dot(v, v);
                Vector3 w = ip - t.p1;
                float wu = Vector3.Dot(w, u);
                float wv = Vector3.Dot(w, v);
                float d = uv * uv - uu * vv;

                float cs = (uv * wv - vv * wu) / d;
                if (cs < 0 || cs > 1.0)
                    continue;
                float ct = (uv * wu - uu * wv) / d;
                if (ct < 0 || (cs + ct) > 1.0)
                    continue;

                // Add contact point
                ContactResult result = new ContactResult ();
                result.ConsumerID = 0;
                result.Depth = Vector3.Distance(rayStart, ip);
                result.Normal = n;
                result.Pos = ip;

                contacts.Add(result);
            }

            if (contacts.Count == 0)
                return null;

            contacts.Sort(delegate(ContactResult a, ContactResult b)
            {
                return (int)(a.Depth - b.Depth);
            });

            return contacts[0];
        }
/*