3 files changed, 194 insertions, 21 deletions

diff --git a/OpenSim/Framework/ILandObject.cs b/OpenSim/Framework/ILandObject.cs
index 7a50c2a..af072fe 100644
--- a/OpenSim/Framework/ILandObject.cs
+++ b/OpenSim/Framework/ILandObject.cs
@@ -64,6 +64,10 @@ namespace OpenSim.Framework
         // a estimation of a parcel center. 
         Vector2 CenterPoint { get; }
         
+        // get positions
+        Vector2? GetNearestPoint(Vector3 pos);
+        Vector2? GetNearestPointAlongDirection(Vector3 pos, Vector3 pdirection);
+
         bool ContainsPoint(int x, int y);
         
         ILandObject Copy();
diff --git a/OpenSim/Region/CoreModules/World/Land/LandObject.cs b/OpenSim/Region/CoreModules/World/Land/LandObject.cs
index ed509fe..378e996 100644
--- a/OpenSim/Region/CoreModules/World/Land/LandObject.cs
+++ b/OpenSim/Region/CoreModules/World/Land/LandObject.cs
@@ -121,6 +121,147 @@ namespace OpenSim.Region.CoreModules.World.Land
             }
         }
 
+        public Vector2? GetNearestPoint(Vector3 pos)
+        {
+            Vector3 direction = new Vector3(m_centerPoint.X - pos.X, m_centerPoint.Y - pos.Y, 0f );
+            return GetNearestPointAlongDirection(pos, direction);
+        }
+
+        public Vector2? GetNearestPointAlongDirection(Vector3 pos, Vector3 pdirection)
+        {
+            Vector2 testpos;
+            Vector2 direction;
+
+            testpos.X = pos.X / landUnit;
+            testpos.Y = pos.Y / landUnit;
+
+            if(LandBitmap[(int)testpos.X, (int)testpos.Y])
+                return new Vector2(pos.X, pos.Y); // we are already here
+
+            direction.X = pdirection.X;
+            direction.Y = pdirection.Y;
+
+            if(direction.X == 0f && direction.Y == 0f)
+                return null; // we can't look anywhere
+
+            direction.Normalize();
+
+            int minx = (int)(m_AABBmin.X / landUnit);
+            int maxx = (int)(m_AABBmax.X / landUnit);
+
+            // check against AABB
+            if(direction.X > 0f)
+            {
+                if(testpos.X >= maxx)
+                    return null;  // will never get there
+                if(testpos.X < minx)
+                    testpos.X = minx;
+            }
+            else if(direction.X < 0f)
+            {
+                if(testpos.X < minx)
+                    return null;  // will never get there
+                if(testpos.X >= maxx)
+                    testpos.X = maxx - 1;
+            }
+            else
+            {
+                if(testpos.X < minx)
+                    return null;  // will never get there
+                else if(testpos.X >= maxx)
+                    return null;  // will never get there
+            }
+
+            int miny = (int)(m_AABBmin.Y / landUnit);
+            int maxy = (int)(m_AABBmax.Y / landUnit);
+
+            if(direction.Y > 0f)
+            {
+                if(testpos.Y >= maxy)
+                    return null;  // will never get there
+                if(testpos.Y < miny)
+                    testpos.Y = miny;
+            }
+            else if(direction.Y < 0f)
+            {
+                if(testpos.Y < miny)
+                    return null;  // will never get there
+                if(testpos.Y >= maxy)
+                    testpos.Y = maxy - 1;
+            }
+            else
+            {
+                if(testpos.Y < miny)
+                    return null;  // will never get there
+                else if(testpos.Y >= maxy)
+                    return null;  // will never get there
+            }
+            
+            while(!LandBitmap[(int)testpos.X, (int)testpos.Y])
+            {
+                testpos += direction;
+
+                if(testpos.X < minx)
+                    return null;
+                if (testpos.X >= maxx)
+                    return null;
+                if(testpos.Y < miny)
+                    return null;
+                if (testpos.Y >= maxy)
+                    return null;
+            }
+
+            testpos *= landUnit;
+            float ftmp;
+
+            if(Math.Abs(direction.X) > Math.Abs(direction.Y))
+            {
+                if(direction.X < 0)
+                    testpos.X += landUnit - 0.5f;
+                else
+                    testpos.X += 0.5f;
+                ftmp = testpos.X - pos.X;
+                ftmp /= direction.X;
+                ftmp = Math.Abs(ftmp);
+                ftmp *= direction.Y;
+                ftmp += pos.Y;
+
+                if(ftmp < testpos.Y + .5f)
+                    ftmp = testpos.Y + .5f;
+                else
+                {
+                    testpos.Y += landUnit - 0.5f;
+                    if(ftmp > testpos.Y)
+                        ftmp = testpos.Y;
+                }
+                testpos.Y = ftmp;
+            }
+            else
+            {
+                if(direction.Y < 0)
+                    testpos.Y += landUnit - 0.5f;
+                else
+                    testpos.Y += 0.5f;
+                ftmp = testpos.Y - pos.Y;
+                ftmp /= direction.Y;
+                ftmp = Math.Abs(ftmp);
+                ftmp *= direction.X;
+                ftmp += pos.X;
+
+                if(ftmp < testpos.X + .5f)
+                    ftmp = testpos.X + .5f;
+                else
+                {
+                    testpos.X += landUnit - 0.5f;
+                    if(ftmp > testpos.X)
+                        ftmp = testpos.X;
+                }
+                testpos.X = ftmp;
+            }
+            return testpos;
+        }
+
+
         #region Constructors
 
         public LandObject(LandData landData, Scene scene)
@@ -791,7 +932,7 @@ namespace OpenSim.Region.CoreModules.World.Land
             {
                 for (y = 0; y < LandBitmap.GetLength(1); y++)
                 {
-                    if (LandBitmap[x, y] == true)
+                    if (LandBitmap[x, y])
                     {
                         if (min_x > x)
                             min_x = x;
@@ -856,7 +997,7 @@ namespace OpenSim.Region.CoreModules.World.Land
             m_AABBmin.X = tx;
             m_AABBmin.Y = ty;
 
-            if(m_scene == null)
+            if(m_scene == null || m_scene.Heightmap == null)
                 LandData.AABBMin = new Vector3(tx, ty, 0f);
             else
                 LandData.AABBMin = new Vector3(tx, ty, (float)m_scene.Heightmap[tx, ty]);
@@ -874,7 +1015,7 @@ namespace OpenSim.Region.CoreModules.World.Land
             m_AABBmax.X = tx;
             m_AABBmax.Y = ty;
 
-            if(m_scene == null)
+            if(m_scene == null || m_scene.Heightmap == null)
                 LandData.AABBMax = new Vector3(tx, ty, 0f);
             else
                 LandData.AABBMax = new Vector3(tx, ty, (float)m_scene.Heightmap[tx - 1, ty - 1]);
diff --git a/OpenSim/Region/Framework/Scenes/Scene.cs b/OpenSim/Region/Framework/Scenes/Scene.cs
index 60079e2..9ae0c10 100755
--- a/OpenSim/Region/Framework/Scenes/Scene.cs
+++ b/OpenSim/Region/Framework/Scenes/Scene.cs
@@ -5783,27 +5783,34 @@ Environment.Exit(1);
 
         public Vector3 GetNearestAllowedPosition(ScenePresence avatar, ILandObject excludeParcel)
         {
-            ILandObject nearestParcel = GetNearestAllowedParcel(avatar.UUID, avatar.AbsolutePosition.X, avatar.AbsolutePosition.Y, excludeParcel);
+            Vector3 pos = avatar.AbsolutePosition;
+
+            ILandObject nearestParcel = GetNearestAllowedParcel(avatar.UUID, pos.X, pos.Y, excludeParcel);
+
+            Vector3 retPos = Vector3.Zero;
 
             if (nearestParcel != null)
             {
-                Vector3 dir = Vector3.Normalize(Vector3.Multiply(avatar.Velocity, -1));
-                //Try to get a location that feels like where they came from
-                Vector3? nearestPoint = GetNearestPointInParcelAlongDirectionFromPoint(avatar.AbsolutePosition, dir, nearestParcel);
-                if (nearestPoint != null)
-                {
-                    m_log.Debug("Found a sane previous position based on velocity, sending them to: " + nearestPoint.ToString());
-                    return nearestPoint.Value;
-                }
+                Vector2? nearestPoint = null;
+                Vector3 dir = -avatar.Velocity;
+                float dirlen = dir.Length();
+                if(dirlen > 1.0f)
+                    //Try to get a location that feels like where they came from
+                    nearestPoint = nearestParcel.GetNearestPointAlongDirection(pos, dir);
+
+                if (nearestPoint == null)
+                    nearestPoint = nearestParcel.GetNearestPoint(pos);
 
-                //Sometimes velocity might be zero (local teleport), so try finding point along path from avatar to center of nearest parcel
-                Vector3 directionToParcelCenter = Vector3.Subtract(GetParcelCenterAtGround(nearestParcel), avatar.AbsolutePosition);
-                dir = Vector3.Normalize(directionToParcelCenter);
-                nearestPoint = GetNearestPointInParcelAlongDirectionFromPoint(avatar.AbsolutePosition, dir, nearestParcel);
                 if (nearestPoint != null)
                 {
-                    m_log.Debug("They had a zero velocity, sending them to: " + nearestPoint.ToString());
-                    return nearestPoint.Value;
+                    retPos.X = nearestPoint.Value.X;
+                    retPos.Y = nearestPoint.Value.Y;
+                    float h = GetGroundHeight(retPos.X, retPos.Y) + 0.8f;
+                    if(pos.Z > h)
+                        retPos.Z = pos.Z;
+                    else
+                        retPos.Z = h;
+                    return retPos;
                 }
 
                 ILandObject dest = LandChannel.GetLandObject(avatar.lastKnownAllowedPosition.X, avatar.lastKnownAllowedPosition.Y);
@@ -5833,15 +5840,36 @@ Environment.Exit(1);
 
         private Vector3? GetNearestPointInParcelAlongDirectionFromPoint(Vector3 pos, Vector3 direction, ILandObject parcel)
         {
-            Vector3 unitDirection = Vector3.Normalize(direction);
+            float maxX = RegionInfo.RegionSizeX;
+            float maxY = RegionInfo.RegionSizeY;
+            
+            // reduce resolution since regions can be very large now
+            direction *= 4.0f;
+            Vector3 testPos = pos;
             //Making distance to search go through some sane limit of distance
-            for (float distance = 0; distance < Math.Max(RegionInfo.RegionSizeX, RegionInfo.RegionSizeY) * 2; distance += .5f)
+            while(true)
             {
-                Vector3 testPos = Vector3.Add(pos, Vector3.Multiply(unitDirection, distance));
+                testPos += direction;
                 if (parcel.ContainsPoint((int)testPos.X, (int)testPos.Y))
                 {
+                    direction *= -0.125f; // .5m resolution
+                    Vector3 testPos2 = testPos - direction;
+                    for(int i = 0; i < 7; i++)
+                    {
+                        if (!parcel.ContainsPoint((int)testPos2.X, (int)testPos2.Y))
+                            return testPos;
+                        testPos = testPos2;
+                    }
                     return testPos;
                 }
+                if(testPos.X < 0)
+                    break;
+                else if (testPos.X >= maxX)
+                    break;
+                if(testPos.Y < 0)
+                    break;
+                else if (testPos.Y >= maxY)
+                    break;
             }
             return null;
         }