diff --git a/OpenSim/Region/Framework/Scenes/Scene.cs b/OpenSim/Region/Framework/Scenes/Scene.cs index 461d146278..80295d753c 100755 --- a/OpenSim/Region/Framework/Scenes/Scene.cs +++ b/OpenSim/Region/Framework/Scenes/Scene.cs @@ -5642,44 +5642,65 @@ Environment.Exit(1); // does a linear approximation of the height at this intermediate point. public float GetGroundHeight(float x, float y) { + int ix; + int iy; + float dx; + float dy; + + // make position fit into array if (x < 0) - x = 0; - if (x >= Heightmap.Width) - x = Heightmap.Width - 1; + { + ix = 0; + dx = 0; + } + else if (x < Heightmap.Width - 1) + { + ix = (int)x; + dx = x - ix; + } + else // out world use external height + { + ix = Heightmap.Width - 2; + dx = 0; + } if (y < 0) - y = 0; - if (y >= Heightmap.Height) - y = Heightmap.Height - 1; + { + iy = 0; + dy = 0; + } + else if (y < Heightmap.Height - 1) + { + iy = (int)y; + dy = y - iy; + } + else + { + iy = Heightmap.Height - 2; + dy = 0; + } - Vector3 p0 = new Vector3(x, y, (float)Heightmap[(int)x, (int)y]); - Vector3 p1 = p0; - Vector3 p2 = p0; + float h1; + float h2; + float h0 = Heightmap[ix, iy]; // 0,0 vertice - p1.X += 1.0f; - if (p1.X < Heightmap.Width) - p1.Z = (float)Heightmap[(int)p1.X, (int)p1.Y]; - - p2.Y += 1.0f; - if (p2.Y < Heightmap.Height) - p2.Z = (float)Heightmap[(int)p2.X, (int)p2.Y]; - - Vector3 v0 = new Vector3(p1.X - p0.X, p1.Y - p0.Y, p1.Z - p0.Z); - Vector3 v1 = new Vector3(p2.X - p0.X, p2.Y - p0.Y, p2.Z - p0.Z); - - v0.Normalize(); - v1.Normalize(); - - Vector3 vsn = new Vector3( - (v0.Y * v1.Z) - (v0.Z * v1.Y), - (v0.Z * v1.X) - (v0.X * v1.Z), - (v0.X * v1.Y) - (v0.Y * v1.X) - ); - vsn.Normalize(); - - float xdiff = x - (float)((int)x); - float ydiff = y - (float)((int)y); - - return (((vsn.X * xdiff) + (vsn.Y * ydiff)) / (-1 * vsn.Z)) + p0.Z; + if (dy > dx) + { + ++iy; + h2 = Heightmap[ix, iy]; // 0,1 vertice + h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0 + ++ix; + h2 = (Heightmap[ix, iy] - h2) * dx; // 1,1 vertice minus 0,1 + } + else + { + ++ix; + h2 = Heightmap[ix, iy]; // vertice 1,0 + h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0 + ++iy; + h2 = (Heightmap[ix, iy] - h2) * dy; // 1,1 vertice minus 1,0 + } + + return h0 + h1 + h2; } private void CheckHeartbeat() diff --git a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs index 3bbae08406..89b5b1ad70 100644 --- a/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs +++ b/OpenSim/Region/PhysicsModules/ubOde/ODEScene.cs @@ -1915,22 +1915,10 @@ namespace OpenSim.Region.PhysicsModule.ubOde public unsafe float GetTerrainHeightAtXY(float x, float y) { - if (TerrainGeom == IntPtr.Zero) - return 0f; - - if (m_terrainHeights == null || m_terrainHeights.Length == 0) - return 0f; - // TerrainHeightField for ODE as offset 1m x += 1f; y += 1f; - // make position fit into array - if (x < 0) - x = 0; - if (y < 0) - y = 0; - // integer indexs int ix; int iy; @@ -1938,7 +1926,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde float dx; float dy; - if (x < m_heightmapWidthSamples - 1) + // make position fit into array + if (x < 0) + { + ix = 0; + dx = 0; + } + else if (x < m_heightmapWidthSamples - 1) { ix = (int)x; dx = x - ix; @@ -1948,7 +1942,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde ix = m_heightmapWidthSamples - 2; dx = 0; } - if (y < m_heightmapHeightSamples - 1) + if (y < 0) + { + iy = 0; + dy = 0; + } + else if (y < m_heightmapHeightSamples - 1) { iy = (int)y; dy = y - iy; @@ -1969,20 +1968,6 @@ namespace OpenSim.Region.PhysicsModule.ubOde fixed(float* heightsb = &m_terrainHeights[iy]) { float* heights = heightsb; - /* - if ((dx + dy) <= 1.0f) - { - h0 = ((float)heights[iy]); // 0,0 vertice - h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 - h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 - } - else - { - h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice - h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 - h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 - } - */ h0 = *heights; // 0,0 vertice if (dy>dx) @@ -2007,32 +1992,22 @@ namespace OpenSim.Region.PhysicsModule.ubOde public unsafe Vector3 GetTerrainNormalAtXY(float x, float y) { - Vector3 norm = new Vector3(0, 0, 1); - - if (TerrainGeom == IntPtr.Zero) - return norm; - - if (m_terrainHeights == null || m_terrainHeights.Length == 0) - return norm; - // TerrainHeightField for ODE as offset 1m x += 1f; y += 1f; - // make position fit into array - if (x < 0) - x = 0; - if (y < 0) - y = 0; - - // integer indexs int ix; int iy; - // interpolators offset float dx; float dy; - if (x < m_heightmapWidthSamples - 1) + // make position fit into array + if (x < 0) + { + ix = 0; + dx = 0; + } + else if (x < m_heightmapWidthSamples - 1) { ix = (int)x; dx = x - ix; @@ -2042,7 +2017,12 @@ namespace OpenSim.Region.PhysicsModule.ubOde ix = m_heightmapWidthSamples - 2; dx = 0; } - if (y < m_heightmapHeightSamples - 1) + if (y < 0) + { + iy = 0; + dy = 0; + } + else if (y < m_heightmapHeightSamples - 1) { iy = (int)y; dy = y - iy; @@ -2060,7 +2040,9 @@ namespace OpenSim.Region.PhysicsModule.ubOde iy *= m_heightmapWidthSamples; iy += ix; // all indexes have iy + ix - fixed(float* heightsB = &m_terrainHeights[iy]) + float rx; + float ry; + fixed (float* heightsB = &m_terrainHeights[iy]) { float* heights = heightsB; if (dy > dx) @@ -2069,8 +2051,8 @@ namespace OpenSim.Region.PhysicsModule.ubOde heights += m_heightmapWidthSamples; h0 = *heights; // 0,1 h2 = *(heights + 1); // 1,1 vertice - norm.X = h0 - h2; - norm.Y = h1 - h0; + rx = h0 - h2; + ry = h1 - h0; } else { @@ -2078,12 +2060,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde heights++; h0 = *heights; // 1,0 vertice h1 = *(heights + m_heightmapWidthSamples); // vertice 1,1 - norm.X = h2 - h0; - norm.Y = h0 - h1; + rx = h2 - h0; + ry = h0 - h1; } } - norm.Normalize(); - return norm; + h0 = rx * rx + ry * ry + 1.0f; + h0 = 1.0f / (float) Math.Sqrt(h0); + return new Vector3(rx * h0, ry * h0, h0); } private void InitTerrain()