1 files changed, 14 insertions, 12 deletions

diff --git a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
index 4a44438..cc1d16b 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSPlugin.cs
@@ -178,17 +178,8 @@ namespace OpenSim.Region.Physics.POSPlugin
                 }
 
                 bool forcedZ = false;
-                character.Position.X = character.Position.X + (character._target_velocity.X * timeStep);
-                if (check_all_prims(character))
-                {
-                    character.Position.X = oldposX;
-                }
-                character.Position.Y = character.Position.Y + (character._target_velocity.Y * timeStep);
-                if (check_all_prims(character))
-                {
-                    character.Position.Y = oldposY;
-                }
-                float terrainheight = _heightMap[(int)character.Position.Y * 256 + (int)character.Position.X];
+
+                float terrainheight = _heightMap[(int)(character.Position.Y + (character._target_velocity.Y * timeStep)) * 256 + (int)(character.Position.X + (character._target_velocity.X * timeStep))];
                 if (character.Position.Z + (character._target_velocity.Z * timeStep) < terrainheight + 2)
                 {
                     character.Position.Z = terrainheight + 1.0f;
@@ -208,13 +199,24 @@ namespace OpenSim.Region.Physics.POSPlugin
                     character.Position.Z = oldposZ;                 //  first try Z axis
                     if (check_all_prims(character))
                     {
-                        character.Position.Z = oldposZ + 0.4f;                 // try harder
+                        character.Position.Z = oldposZ + 0.5f;                 // try harder
                     }
                     else
                     {
                         forcedZ = true;
                     }
                 }            
+                character.Position.X = character.Position.X + (character._target_velocity.X * timeStep);
+                if (check_all_prims(character))
+                {
+                    character.Position.X = oldposX;
+                }
+                character.Position.Y = character.Position.Y + (character._target_velocity.Y * timeStep);
+                if (check_all_prims(character))
+                {
+                    character.Position.Y = oldposY;
+                }
+                
 
                 if (character.Position.Y < 0)
                 {