4 files changed, 117 insertions, 23 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index f442ca2..e208d3a 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -205,7 +205,7 @@ public sealed class BSCharacter : BSPhysObject
             //   errors can creap in and the avatar will slowly float off in some direction.
             // So, the problem is that, when an avatar is standing, we cannot tell creaping error
             //   from real pushing.
-            // The code below keeps setting the velocity to zero hoping the world will keep pushing.
+            // The code below uses whether the collider is static or moving to decide whether to zero motion.
 
             _velocityMotor.Step(timeStep);
 
@@ -244,6 +244,7 @@ public sealed class BSCharacter : BSPhysObject
             }
             else
             {
+                // Supposed to be moving.
                 OMV.Vector3 stepVelocity = _velocityMotor.CurrentValue;
 
                 if (Friction != BSParam.AvatarFriction)
@@ -276,8 +277,8 @@ public sealed class BSCharacter : BSPhysObject
         });
     }
 
-    // Decide of the character is colliding with a low object and compute a force to pop the
-    //    avatar up so it has a chance of walking up and over the low object.
+    // Decide if the character is colliding with a low object and compute a force to pop the
+    //    avatar up so it can walk up and over the low objects.
     private OMV.Vector3 WalkUpStairs()
     {
         OMV.Vector3 ret = OMV.Vector3.Zero;
@@ -476,17 +477,19 @@ public sealed class BSCharacter : BSPhysObject
         if (!PhysicsScene.TerrainManager.IsWithinKnownTerrain(RawPosition))
         {
             // The character is out of the known/simulated area.
-            // Upper levels of code will handle the transition to other areas so, for
-            //     the time, we just ignore the position.
-            return ret;
+            // Force the avatar position to be within known. ScenePresence will use the position
+            //    plus the velocity to decide if the avatar is moving out of the region.
+            RawPosition = PhysicsScene.TerrainManager.ClampPositionIntoKnownTerrain(RawPosition);

+            DetailLog("{0},BSCharacter.PositionSanityCheck,notWithinKnownTerrain,clampedPos={1}", LocalID, RawPosition);
+            return true;
         }
 
         // If below the ground, move the avatar up
         float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(RawPosition);
         if (Position.Z < terrainHeight)
         {
-            DetailLog("{0},BSCharacter.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight);
-            _position.Z = terrainHeight + 2.0f;
+            DetailLog("{0},BSCharacter.PositionSanityCheck,adjustForUnderGround,pos={1},terrain={2}", LocalID, _position, terrainHeight);
+            _position.Z = terrainHeight + BSParam.AvatarBelowGroundUpCorrectionMeters;
             ret = true;
         }
         if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
@@ -806,14 +809,7 @@ public sealed class BSCharacter : BSPhysObject
     private void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
         if (force.IsFinite())
         {
-            float magnitude = force.Length();
-            if (magnitude > BSParam.MaxAddForceMagnitude)
-            {
-                // Force has a limit
-                force = force / magnitude * BSParam.MaxAddForceMagnitude;
-            }
-
-            OMV.Vector3 addForce = force;
+            OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
             // DetailLog("{0},BSCharacter.addForce,call,force={1}", LocalID, addForce);
 
             PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.AddForce", delegate()
@@ -902,6 +898,7 @@ public sealed class BSCharacter : BSPhysObject
         // Do some sanity checking for the avatar. Make sure it's above ground and inbounds.
         if (PositionSanityCheck(true))
         {
+            DetailLog("{0},BSCharacter.UpdateProperties,updatePosForSanity,pos={1}", LocalID, _position);

             entprop.Position = _position;
         }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
index fa58109..2af8468 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSParam.cs
@@ -107,6 +107,7 @@ public static class BSParam
     public static float AvatarCapsuleDepth { get; private set; }
     public static float AvatarCapsuleHeight { get; private set; }
         public static float AvatarContactProcessingThreshold { get; private set; }
+        public static float AvatarBelowGroundUpCorrectionMeters { get; private set; }
         public static float AvatarStepHeight { get; private set; }
         public static float AvatarStepApproachFactor { get; private set; }
         public static float AvatarStepForceFactor { get; private set; }
@@ -497,6 +498,10 @@ public static class BSParam
             0.1f,
             (s) => { return AvatarContactProcessingThreshold; },
             (s,v) => { AvatarContactProcessingThreshold = v; } ),
+            new ParameterDefn<float>("AvatarBelowGroundUpCorrectionMeters", "Meters to move avatar up if it seems to be below ground",
+            1.0f,
+            (s) => { return AvatarBelowGroundUpCorrectionMeters; },
+            (s,v) => { AvatarBelowGroundUpCorrectionMeters = v; } ),
             new ParameterDefn<float>("AvatarStepHeight", "Height of a step obstacle to consider step correction",
             0.3f,
             (s) => { return AvatarStepHeight; },
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
index 2e9db39..e8040d8 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs
@@ -337,6 +337,54 @@ public sealed class BSTerrainManager : IDisposable
         return GetTerrainPhysicalAtXYZ(pos, out physTerrain, out terrainBaseXYZ);
     }
 
+    // Return a new position that is over known terrain if the position is outside our terrain.
+    public Vector3 ClampPositionIntoKnownTerrain(Vector3 pPos)
+    {
+        Vector3 ret = pPos;
+
+        // Can't do this function if we don't know about any terrain.
+        if (m_terrains.Count == 0)
+            return ret;
+
+        int loopPrevention = 5;
+        Vector3 terrainBaseXYZ;
+        BSTerrainPhys physTerrain;
+        while (!GetTerrainPhysicalAtXYZ(ret, out physTerrain, out terrainBaseXYZ))
+        {
+            // The passed position is not within a known terrain area.
+
+            // First, base addresses are never negative so correct for that possible problem.
+            if (ret.X < 0f || ret.Y < 0f)
+            {
+                if (ret.X < 0f)
+                    ret.X = 0f;
+                if (ret.Y < 0f)
+                    ret.Y = 0f;
+                DetailLog("{0},BSTerrainManager.ClampPositionToKnownTerrain,zeroingNegXorY,oldPos={1},newPos={2}",
+                                            BSScene.DetailLogZero, pPos, ret);
+            }
+            else
+            {
+                // Must be off the top of a region. Find an adjacent region to move into.
+                Vector3 adjacentTerrainBase = FindAdjacentTerrainBase(terrainBaseXYZ);
+
+                ret.X = Math.Min(ret.X, adjacentTerrainBase.X + DefaultRegionSize.X);
+                ret.Y = Math.Min(ret.Y, adjacentTerrainBase.Y + DefaultRegionSize.Y);
+                DetailLog("{0},BSTerrainManager.ClampPositionToKnownTerrain,findingAdjacentRegion,adjacentRegBase={1},oldPos={2},newPos={3}",
+                                            BSScene.DetailLogZero, adjacentTerrainBase, pPos, ret);
+            }
+            if (loopPrevention-- < 0f)
+            {
+                // The 'while' is a little dangerous so this prevents looping forever if the
+                //    mapping of the terrains ever gets messed up (like nothing at <0,0>) or
+                //    the list of terrains is in transition.
+                DetailLog("{0},BSTerrainManager.ClampPositionToKnownTerrain,suppressingFindAdjacentRegionLoop", BSScene.DetailLogZero);
+                break;
+            }
+        }
+        return ret;
+    }
+
     // Given an X and Y, find the height of the terrain.
     // Since we could be handling multiple terrains for a mega-region,
     //    the base of the region is calcuated assuming all regions are
@@ -400,18 +448,60 @@ public sealed class BSTerrainManager : IDisposable
     //    the descriptor class and the 'base' fo the addresses therein.
     private bool GetTerrainPhysicalAtXYZ(Vector3 pos, out BSTerrainPhys outPhysTerrain, out Vector3 outTerrainBase)
     {
-        int offsetX = ((int)(pos.X / (int)DefaultRegionSize.X)) * (int)DefaultRegionSize.X;
-        int offsetY = ((int)(pos.Y / (int)DefaultRegionSize.Y)) * (int)DefaultRegionSize.Y;
-        Vector3 terrainBaseXYZ = new Vector3(offsetX, offsetY, 0f);
+        bool ret = false;
+
+        Vector3 terrainBaseXYZ = Vector3.Zero;
+        if (pos.X < 0f || pos.Y < 0f)
+        {
+            // We don't handle negative addresses so just make up a base that will not be found.
+            terrainBaseXYZ = new Vector3(-DefaultRegionSize.X, -DefaultRegionSize.Y, 0f);
+        }
+        else
+        {
+            int offsetX = ((int)(pos.X / (int)DefaultRegionSize.X)) * (int)DefaultRegionSize.X;
+            int offsetY = ((int)(pos.Y / (int)DefaultRegionSize.Y)) * (int)DefaultRegionSize.Y;
+            terrainBaseXYZ = new Vector3(offsetX, offsetY, 0f);
+        }
 
         BSTerrainPhys physTerrain = null;
         lock (m_terrains)
         {
-            m_terrains.TryGetValue(terrainBaseXYZ, out physTerrain);
+            ret = m_terrains.TryGetValue(terrainBaseXYZ, out physTerrain);
         }
         outTerrainBase = terrainBaseXYZ;
         outPhysTerrain = physTerrain;
-        return (physTerrain != null);
+        return ret;
+    }
+
+    // Given a terrain base, return a terrain base for a terrain that is closer to <0,0> than
+    //   this one. Usually used to return an out of bounds object to a known place.
+    private Vector3 FindAdjacentTerrainBase(Vector3 pTerrainBase)
+    {
+        Vector3 ret = pTerrainBase;
+        ret.Z = 0f;
+        lock (m_terrains)
+        {
+            // Once down to the <0,0> region, we have to be done.
+            while (ret.X > 0f && ret.Y > 0f)
+            {
+                if (ret.X > 0f)
+                {
+                    ret.X = Math.Max(0f, ret.X - DefaultRegionSize.X);
+                    DetailLog("{0},BSTerrainManager.FindAdjacentTerrainBase,reducingX,terrainBase={1}", BSScene.DetailLogZero, ret);
+                    if (m_terrains.ContainsKey(ret))
+                        break;
+                }
+                if (ret.Y > 0f)
+                {
+                    ret.Y = Math.Max(0f, ret.Y - DefaultRegionSize.Y);
+                    DetailLog("{0},BSTerrainManager.FindAdjacentTerrainBase,reducingY,terrainBase={1}", BSScene.DetailLogZero, ret);
+                    if (m_terrains.ContainsKey(ret))
+                        break;
+                }
+            }
+        }
+
+        return ret;
     }
 
     // Although no one seems to check this, I do support combining.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
index d7e800d..57a5ff2 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs
@@ -215,7 +215,8 @@ public sealed class BSTerrainMesh : BSTerrainPhys
 
             float magX = (float)sizeX / extentX;
             float magY = (float)sizeY / extentY;
-            physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,totVert={1},totInd={2},extentBase={3},magX={4},magY={5}",
+            if (physicsScene != null)
+                physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,totVert={1},totInd={2},extentBase={3},magX={4},magY={5}",
                                     BSScene.DetailLogZero, totalVertices, totalIndices, extentBase, magX, magY);
             float minHeight = float.MaxValue;
             // Note that sizeX+1 vertices are created since there is land between this and the next region.
@@ -257,7 +258,8 @@ public sealed class BSTerrainMesh : BSTerrainPhys
         }
         catch (Exception e)
         {
-            physicsScene.Logger.ErrorFormat("{0} Failed conversion of heightmap to mesh. For={1}/{2}, e={3}",
+            if (physicsScene != null)
+                physicsScene.Logger.ErrorFormat("{0} Failed conversion of heightmap to mesh. For={1}/{2}, e={3}",
                                                 LogHeader, physicsScene.RegionName, extentBase, e);
         }