diff --git a/OpenSim/Region/Framework/Scenes/ScenePresence.cs b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
index 620db94f34..98e90b993a 100644
--- a/OpenSim/Region/Framework/Scenes/ScenePresence.cs
+++ b/OpenSim/Region/Framework/Scenes/ScenePresence.cs
@@ -1635,7 +1635,6 @@ namespace OpenSim.Region.Framework.Scenes
                         if (LocalVectorToTarget3D.Y > 0) //MoveLeft
                         {
                             m_movementflag += (byte)(uint)Dir_ControlFlags.DIR_CONTROL_FLAG_LEFT;
-                            //AgentControlFlags
                             AgentControlFlags |= (uint)Dir_ControlFlags.DIR_CONTROL_FLAG_LEFT;
                             updated = true;
                         }
@@ -1704,6 +1703,9 @@ namespace OpenSim.Region.Framework.Scenes
 //                pos.Z = AbsolutePosition.Z;
 //            }
 
+            float terrainHeight = (float)m_scene.Heightmap[(int)pos.X, (int)pos.Y];
+            pos.Z = Math.Max(terrainHeight, pos.Z);
+
             m_moveToPositionInProgress = true;
             MoveToPositionTarget = pos;