From 36ee8e39411c1eedf72ba082b159528dec8c7880 Mon Sep 17 00:00:00 2001
From: Robert Adams
Date: Mon, 14 Aug 2017 21:20:59 -0700
Subject: BUlletSim: return better terrain height in
 BSTerrainHeightMap.GetTerrainHeightAtXYZ(). Partial fix for Mantis 8011.
 Problem is that computed terrain height is different than mesh height in the
 physics engine. For small shapes, they would have their position corrected to
 above terrain so they would never collide.

---
 .../PhysicsModules/BulletS/BSTerrainHeightmap.cs   | 24 ++++++++++++++++++----
 1 file changed, 20 insertions(+), 4 deletions(-)

(limited to 'OpenSim/Region/PhysicsModules')

diff --git a/OpenSim/Region/PhysicsModules/BulletS/BSTerrainHeightmap.cs b/OpenSim/Region/PhysicsModules/BulletS/BSTerrainHeightmap.cs
index 42fc11b..efdf479 100755
--- a/OpenSim/Region/PhysicsModules/BulletS/BSTerrainHeightmap.cs
+++ b/OpenSim/Region/PhysicsModules/BulletS/BSTerrainHeightmap.cs
@@ -141,14 +141,30 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
     }
 
     // The passed position is relative to the base of the region.
+    // There are many assumptions herein that the heightmap increment is 1.
     public override float GetTerrainHeightAtXYZ(Vector3 pos)
     {
         float ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET;
 
-        int mapIndex = (int)pos.Y * (int)m_mapInfo.sizeY + (int)pos.X;
-        try
-        {
-            ret = m_mapInfo.heightMap[mapIndex];
+        try {
+            int baseX = (int)pos.X;
+            int baseY = (int)pos.Y;
+            int maxX = (int)m_mapInfo.sizeX;
+            int maxY = (int)m_mapInfo.sizeY;
+            float diffX = pos.X - (float)baseX;
+            float diffY = pos.Y - (float)baseY;
+
+            float mapHeight1 = m_mapInfo.heightMap[baseY * maxY + baseX];
+            float mapHeight2 = m_mapInfo.heightMap[Math.Min(baseY + 1, maxY - 1) * maxY + baseX];
+            float mapHeight3 = m_mapInfo.heightMap[baseY * maxY + Math.Min(baseX + 1, maxX  - 1)];
+            float mapHeight4 = m_mapInfo.heightMap[Math.Min(baseY + 1, maxY - 1) * maxY +  Math.Min(baseX + 1, maxX  - 1)];
+
+            float Xrise = (mapHeight4 - mapHeight3) * diffX;
+            float Yrise = (mapHeight2 - mapHeight1) * diffY;
+
+            ret = mapHeight1 + ((Xrise + Yrise) / 2f);
+            m_physicsScene.DetailLog("{0},BSTerrainHeightMap,GetTerrainHeightAtXYZ,pos={1},{2}/{3}/{4}/{5},ret={6}",
+                    BSScene.DetailLogZero, pos, mapHeight1, mapHeight2, mapHeight3, mapHeight4, ret);
         }
         catch
         {
-- 
cgit v1.1