Prevent teleports from ending underground

OpenSim/Region/Framework/Scenes/Scene.cs

@@ -3760,5 +3760,46 @@ namespace OpenSim.Region.Framework.Scenes
                 return (Scene)MainConsole.Instance.ConsoleScene;
             return null;
         }
+
+        public float GetGroundHeight(float x, float y)
+        {
+            if (x < 0)
+                x = 0;
+            if (x >= Heightmap.Width)
+                x = Heightmap.Width - 1;
+            if (y < 0)
+                y = 0;
+            if (y >= Heightmap.Height)
+                y = Heightmap.Height - 1;
+
+            Vector3 p0 = new Vector3(x, y, (float)Heightmap[(int)x, (int)y]);
+            Vector3 p1 = new Vector3(p0);
+            Vector3 p2 = new Vector3(p0);
+
+            p1.X += 1.0f;
+            if (p1.X < Heightmap.Width)
+                p1.Z = (float)Heightmap[(int)p1.X, (int)p1.Y];
+
+            p2.Y += 1.0f;
+            if (p2.Y < Heightmap.Height)
+                p2.Z = (float)Heightmap[(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;
+        }
     }
 }

OpenSim/Region/Framework/Scenes/ScenePresence.cs

@@ -1020,6 +1020,17 @@ namespace OpenSim.Region.Framework.Scenes
                 look = new Vector3(0.99f, 0.042f, 0);
             }
 
+            // Prevent teleporting to an underground location
+            // (may crash client otherwise)
+            //
+            Vector3 pos = AbsolutePosition;
+            float ground = m_scene.GetGroundHeight(pos.X, pos.Y);
+            if (pos.Z < ground + 1.5f)
+            {
+                pos.Z = ground + 1.5f;
+                AbsolutePosition = pos;
+            }
+
             if (m_isChildAgent)
             {
                 m_isChildAgent = false;