From 34cbc738a84b7946a87e8bfd3bb04869519b3dee Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Tue, 20 Nov 2012 20:36:49 -0800 Subject: [PATCH] BulletSim: enablement and debugging of mesh terrain. --- .../BulletSPlugin/BSTerrainHeightmap.cs | 28 +-- .../Physics/BulletSPlugin/BSTerrainManager.cs | 26 +- .../Physics/BulletSPlugin/BSTerrainMesh.cs | 229 +++++++++++++++++- .../Physics/BulletSPlugin/BulletSimAPI.cs | 6 +- 4 files changed, 253 insertions(+), 36 deletions(-) diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs index 3bb63cdd1c..e9fd0cb442 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs @@ -44,10 +44,11 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys { static string LogHeader = "[BULLETSIM TERRAIN HEIGHTMAP]"; - BulletHeightMapInfo m_mapInfo; + BulletHeightMapInfo m_mapInfo = null; - public BSTerrainHeightmap(BSScene physicsScene, uint id, Vector3 regionSize) - : base(physicsScene) + // Constructor to build a default, flat heightmap terrain. + public BSTerrainHeightmap(BSScene physicsScene, Vector3 regionBase, uint id, Vector3 regionSize) + : base(physicsScene, regionBase, id) { Vector3 minTerrainCoords = new Vector3(0f, 0f, BSTerrainManager.HEIGHT_INITIALIZATION - BSTerrainManager.HEIGHT_EQUAL_FUDGE); Vector3 maxTerrainCoords = new Vector3(regionSize.X, regionSize.Y, BSTerrainManager.HEIGHT_INITIALIZATION); @@ -60,21 +61,23 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys m_mapInfo = new BulletHeightMapInfo(id, initialMap, IntPtr.Zero); m_mapInfo.minCoords = minTerrainCoords; m_mapInfo.maxCoords = maxTerrainCoords; + m_mapInfo.terrainRegionBase = TerrainBase; // Don't have to free any previous since we just got here. BuildHeightmapTerrain(); } // This minCoords and maxCoords passed in give the size of the terrain (min and max Z // are the high and low points of the heightmap). - public BSTerrainHeightmap(BSScene physicsScene, uint id, float[] initialMap, + public BSTerrainHeightmap(BSScene physicsScene, Vector3 regionBase, uint id, float[] initialMap, Vector3 minCoords, Vector3 maxCoords) - : base(physicsScene) + : base(physicsScene, regionBase, id) { m_mapInfo = new BulletHeightMapInfo(id, initialMap, IntPtr.Zero); m_mapInfo.minCoords = minCoords; m_mapInfo.maxCoords = maxCoords; m_mapInfo.minZ = minCoords.Z; m_mapInfo.maxZ = maxCoords.Z; + m_mapInfo.terrainRegionBase = TerrainBase; // Don't have to free any previous since we just got here. BuildHeightmapTerrain(); @@ -135,12 +138,10 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys { if (m_mapInfo.terrainBody.ptr != IntPtr.Zero) { - if (BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr)) - { - // Frees both the body and the shape. - BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr); - BulletSimAPI.ReleaseHeightMapInfo2(m_mapInfo.Ptr); - } + BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr); + // Frees both the body and the shape. + BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, m_mapInfo.terrainBody.ptr); + BulletSimAPI.ReleaseHeightMapInfo2(m_mapInfo.Ptr); } } m_mapInfo = null; @@ -165,10 +166,5 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys } return ret; } - - public override Vector3 TerrainBase - { - get { return m_mapInfo.terrainRegionBase; } - } } } diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs index db042997d1..ed0dfa8f10 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainManager.cs @@ -45,14 +45,18 @@ namespace OpenSim.Region.Physics.BulletSPlugin public abstract class BSTerrainPhys : IDisposable { public BSScene PhysicsScene { get; private set; } + // Base of the region in world coordinates. Coordinates inside the region are relative to this. + public Vector3 TerrainBase { get; private set; } + public uint ID { get; private set; } - public BSTerrainPhys(BSScene physicsScene) + public BSTerrainPhys(BSScene physicsScene, Vector3 regionBase, uint id) { PhysicsScene = physicsScene; + TerrainBase = regionBase; + ID = id; } public abstract void Dispose(); public abstract float GetHeightAtXYZ(Vector3 pos); - public abstract Vector3 TerrainBase { get; } } // ========================================================================================== @@ -133,7 +137,7 @@ public sealed class BSTerrainManager (uint)CollisionFilterGroups.GroundPlaneFilter, (uint)CollisionFilterGroups.GroundPlaneMask); // Build an initial terrain and put it in the world. This quickly gets replaced by the real region terrain. - BSTerrainPhys initialTerrain = new BSTerrainHeightmap(PhysicsScene, BSScene.TERRAIN_ID, DefaultRegionSize); + BSTerrainPhys initialTerrain = new BSTerrainHeightmap(PhysicsScene, Vector3.Zero, BSScene.TERRAIN_ID, DefaultRegionSize); m_terrains.Add(Vector3.Zero, initialTerrain); } @@ -208,10 +212,9 @@ public sealed class BSTerrainManager BSScene.DetailLogZero, minCoords, maxCoords, inTaintTime); // Find high and low points of passed heightmap. - // The min and max passed in are usually the region objects can exist in (maximum + // The min and max passed in is usually the area objects can be in (maximum // object height, for instance). The terrain wants the bounding box for the // terrain so we replace passed min and max Z with the actual terrain min/max Z. - // limit, for float minZ = float.MaxValue; float maxZ = float.MinValue; foreach (float height in heightMap) @@ -219,6 +222,11 @@ public sealed class BSTerrainManager if (height < minZ) minZ = height; if (height > maxZ) maxZ = height; } + if (minZ == maxZ) + { + // If min and max are the same, reduce min a little bit so a good bounding box is created. + minZ -= BSTerrainManager.HEIGHT_EQUAL_FUDGE; + } minCoords.Z = minZ; maxCoords.Z = maxZ; @@ -240,7 +248,9 @@ public sealed class BSTerrainManager if (MegaRegionParentPhysicsScene == null) { - BSTerrainPhys newTerrainPhys = new BSTerrainHeightmap(PhysicsScene, id, + // BSTerrainPhys newTerrainPhys = new BSTerrainHeightmap(PhysicsScene, terrainRegionBase, id, + // heightMap, minCoords, maxCoords); + BSTerrainPhys newTerrainPhys = new BSTerrainMesh(PhysicsScene, terrainRegionBase, id, heightMap, minCoords, maxCoords); m_terrains.Add(terrainRegionBase, newTerrainPhys); @@ -282,8 +292,8 @@ public sealed class BSTerrainManager { DetailLog("{0},UpdateTerrain:NewTerrain,taint,baseX={1},baseY={2}", BSScene.DetailLogZero, minCoordsX.X, minCoordsX.Y); - BSTerrainPhys newTerrainPhys = new BSTerrainHeightmap(PhysicsScene, newTerrainID, - heightMapX, minCoordsX, maxCoordsX); + BSTerrainPhys newTerrainPhys = new BSTerrainHeightmap(PhysicsScene, terrainRegionBase, + newTerrainID, heightMapX, minCoordsX, maxCoordsX); m_terrains.Add(terrainRegionBase, newTerrainPhys); m_terrainModified = true; diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs index 387c78b643..bbb014a87b 100755 --- a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainMesh.cs @@ -44,30 +44,241 @@ public sealed class BSTerrainMesh : BSTerrainPhys { static string LogHeader = "[BULLETSIM TERRAIN MESH]"; - public BSTerrainMesh(BSScene physicsScene, uint id, Vector3 regionSize) - : base(physicsScene) + private float[] m_savedHeightMap; + int m_sizeX; + int m_sizeY; + + BulletShape m_terrainShape; + BulletBody m_terrainBody; + + public BSTerrainMesh(BSScene physicsScene, Vector3 regionBase, uint id, Vector3 regionSize) + : base(physicsScene, regionBase, id) { } - public BSTerrainMesh(BSScene physicsScene /* parameters for making mesh */) - : base(physicsScene) + public BSTerrainMesh(BSScene physicsScene, Vector3 regionBase, uint id /* parameters for making mesh */) + : base(physicsScene, regionBase, id) { } + // Create terrain mesh from a heightmap. + public BSTerrainMesh(BSScene physicsScene, Vector3 regionBase, uint id, float[] initialMap, + Vector3 minCoords, Vector3 maxCoords) + : base(physicsScene, regionBase, id) + { + int indicesCount; + int[] indices; + int verticesCount; + float[] vertices; + + m_savedHeightMap = initialMap; + + m_sizeX = (int)(maxCoords.X - minCoords.X); + m_sizeY = (int)(maxCoords.Y - minCoords.Y); + + if (!BSTerrainMesh.ConvertHeightmapToMesh(PhysicsScene, initialMap, m_sizeX, m_sizeY, + (float)m_sizeX, (float)m_sizeY, + Vector3.Zero, 1.0f, + out indicesCount, out indices, out verticesCount, out vertices)) + { + // DISASTER!! + PhysicsScene.DetailLog("{0},BSTerrainMesh.create,failedConversionOfHeightmap", ID); + PhysicsScene.Logger.ErrorFormat("{0} Failed conversion of heightmap to mesh! base={1}", LogHeader, TerrainBase); + // Something is very messed up and a crash is in our future. + return; + } + PhysicsScene.DetailLog("{0},BSTerrainMesh.create,afterConvertHeightmapToMesh,ver={1},ind={2}", + ID, verticesCount, indicesCount); + + m_terrainShape = new BulletShape(BulletSimAPI.CreateMeshShape2(PhysicsScene.World.ptr, + indicesCount, indices, verticesCount, vertices), + PhysicsShapeType.SHAPE_MESH); + if (m_terrainShape.ptr == IntPtr.Zero) + { + // DISASTER!! + PhysicsScene.DetailLog("{0},BSTerrainMesh.create,failedCreationOfShape", ID); + physicsScene.Logger.ErrorFormat("{0} Failed creation of terrain mesh! base={1}", LogHeader, TerrainBase); + // Something is very messed up and a crash is in our future. + return; + } + PhysicsScene.DetailLog("{0},BSTerrainMesh.create,afterCreateShape,shape={1}", ID, m_terrainShape); + + // The terrain object initial position is at the center of the object + Vector3 centerPos; + centerPos.X = minCoords.X + (m_sizeX / 2f); + centerPos.Y = minCoords.Y + (m_sizeY / 2f); + centerPos.Z = minCoords.Z + ((maxCoords.Z - minCoords.Z) / 2f); + Quaternion rot = Quaternion.Identity; + + PhysicsScene.DetailLog("{0},BSTerrainMesh.create,creatingBody,centerPos={1},rot={2}", ID, centerPos, rot); + m_terrainBody = new BulletBody(id, BulletSimAPI.CreateBodyWithDefaultMotionState2( + m_terrainShape.ptr, ID, centerPos, rot)); + if (m_terrainBody.ptr == IntPtr.Zero) + { + // DISASTER!! + physicsScene.Logger.ErrorFormat("{0} Failed creation of terrain body! base={1}", LogHeader, TerrainBase); + // Something is very messed up and a crash is in our future. + return; + } + PhysicsScene.DetailLog("{0},BSTerrainMesh.create,afterCreateBody,body={1}", ID, m_terrainBody); + + // Set current terrain attributes + BulletSimAPI.SetFriction2(m_terrainBody.ptr, PhysicsScene.Params.terrainFriction); + BulletSimAPI.SetHitFraction2(m_terrainBody.ptr, PhysicsScene.Params.terrainHitFraction); + BulletSimAPI.SetRestitution2(m_terrainBody.ptr, PhysicsScene.Params.terrainRestitution); + BulletSimAPI.SetCollisionFlags2(m_terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT); + + // Static objects are not very massive. + BulletSimAPI.SetMassProps2(m_terrainBody.ptr, 0f, Vector3.Zero); + + // Return the new terrain to the world of physical objects + BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, m_terrainBody.ptr); + + // redo its bounding box now that it is in the world + BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, m_terrainBody.ptr); + + BulletSimAPI.SetCollisionFilterMask2(m_terrainBody.ptr, + (uint)CollisionFilterGroups.TerrainFilter, + (uint)CollisionFilterGroups.TerrainMask); + + // Make it so the terrain will not move or be considered for movement. + BulletSimAPI.ForceActivationState2(m_terrainBody.ptr, ActivationState.DISABLE_SIMULATION); + } + public override void Dispose() { - return; + if (m_terrainBody.ptr != IntPtr.Zero) + { + BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, m_terrainBody.ptr); + // Frees both the body and the shape. + BulletSimAPI.DestroyObject2(PhysicsScene.World.ptr, m_terrainBody.ptr); + } } public override float GetHeightAtXYZ(Vector3 pos) { - return 12345f; + // For the moment use the saved heightmap to get the terrain height. + // TODO: raycast downward to find the true terrain below the position. + float ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET; + + int mapIndex = (int)pos.Y * m_sizeY + (int)pos.X; + try + { + ret = m_savedHeightMap[mapIndex]; + } + catch + { + // Sometimes they give us wonky values of X and Y. Give a warning and return something. + PhysicsScene.Logger.WarnFormat("{0} Bad request for terrain height. terrainBase={1}, pos={2}", + LogHeader, TerrainBase, pos); + ret = BSTerrainManager.HEIGHT_GETHEIGHT_RET; + } + return ret; } - public override Vector3 TerrainBase + // Convert the passed heightmap to mesh information suitable for CreateMeshShape2(). + // Return 'true' if successfully created. + public static bool ConvertHeightmapToMesh( + BSScene physicsScene, + float[] heightMap, int sizeX, int sizeY, // parameters of incoming heightmap + float extentX, float extentY, // zero based range for output vertices + Vector3 extentBase, // base to be added to all vertices + float magnification, // number of vertices to create between heightMap coords + out int indicesCountO, out int[] indicesO, + out int verticesCountO, out float[] verticesO) { - get { return Vector3.Zero; } - } + bool ret = false; + int indicesCount = 0; + int verticesCount = 0; + int[] indices = new int[0]; + float[] vertices = new float[0]; + + // Simple mesh creation which assumes magnification == 1, sizeX == extentX and sizeY == extentY. + // TODO: do a more general solution that scales, adds new vertices and smoothes the result. + + try + { + // One vertice per heightmap value plus the vertices off the top and bottom edge. + int totalVertices = (sizeX + 1) * (sizeY + 1); + vertices = new float[totalVertices * 3]; + int totalIndices = sizeX * sizeY * 6; + indices = new int[totalIndices]; + + physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,totVert={1},totInd={2}", + BSScene.DetailLogZero, totalVertices, totalIndices); + float magX = (float)sizeX / extentX; + float magY = (float)sizeY / extentY; + // Note that sizeX+1 vertices are created since there is land between this and the next region. + for (int yy = 0; yy <= sizeY; yy++) + { + for (int xx = 0; xx <= sizeX; xx++) // Hint: the "<=" means we got through sizeX + 1 times + { + int offset = yy * sizeX + xx; + // Extend the height from the height from the last row or column + if (yy == sizeY) offset -= sizeX; + if (xx == sizeX) offset -= 1; + float height = heightMap[offset]; + vertices[verticesCount + 0] = (float)xx * magX + extentBase.X; + vertices[verticesCount + 1] = (float)yy * magY + extentBase.Y; + vertices[verticesCount + 2] = height + extentBase.Z; + if (physicsScene.PhysicsLogging.Enabled && verticesCount < 900) // DEBUG DEBUG DEBUG + { + Vector3 genVertex = new Vector3( + vertices[verticesCount + 0], + vertices[verticesCount + 1], + vertices[verticesCount + 2]); + physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,ii={1},vertex={2}", + BSScene.DetailLogZero, verticesCount/3, genVertex); + } + verticesCount += 3; + } + } + verticesCount = verticesCount / 3; + physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,completeVerts,verCount={1}", + BSScene.DetailLogZero, verticesCount); + + for (int yy = 0; yy < sizeY; yy++) + { + for (int xx = 0; xx < sizeX; xx++) + { + int offset = yy * sizeX + xx; + // Each vertices is presumed to be the upper left corner of a box of two triangles + indices[indicesCount + 0] = offset; + indices[indicesCount + 1] = offset + 1; + indices[indicesCount + 2] = offset + sizeX + 1; // accounting for the extra column + indices[indicesCount + 3] = offset + 1; + indices[indicesCount + 4] = offset + sizeX + 2; + indices[indicesCount + 5] = offset + sizeX + 1; + if (indicesCount < (300 * 6)) // DEBUG DEBUG DEBUG + physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,i0={1},i1={2},i2={3},i3={4},i4={5},i5={6}", // DEEBUG DEBUG DEBUG + BSScene.DetailLogZero, + indices[indicesCount + 0], + indices[indicesCount + 1], + indices[indicesCount + 2], + indices[indicesCount + 3], + indices[indicesCount + 4], + indices[indicesCount + 5] + ); + indicesCount += 6; + } + } + physicsScene.DetailLog("{0},BSTerrainMesh.ConvertHeightMapToMesh,completeIndices,indCount={1}", // DEEBUG DEBUG DEBUG + LogHeader, indicesCount); // DEBUG + ret = true; + } + catch (Exception e) + { + physicsScene.Logger.ErrorFormat("{0} Failed conversion of heightmap to mesh. Base={1}, e={2}", + LogHeader, extentBase, e); + } + + indicesCountO = indicesCount; + indicesO = indices; + verticesCountO = verticesCount; + verticesO = vertices; + + return ret; + } } } diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs index bab3b3d9df..a2271a9fa8 100644 --- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs +++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs @@ -391,13 +391,13 @@ public enum CollisionFilterGroups : uint ObjectFilter = BSolidFilter, ObjectMask = BAllFilter, StaticObjectFilter = BStaticFilter, - StaticObjectMask = BAllFilter, + StaticObjectMask = BAllFilter & ~BStaticFilter, // static objects don't collide with each other LinksetFilter = BLinksetFilter, - LinksetMask = BAllFilter & ~BLinksetFilter, + LinksetMask = BAllFilter & ~BLinksetFilter, // linkset objects don't collide with each other VolumeDetectFilter = BSensorTrigger, VolumeDetectMask = ~BSensorTrigger, TerrainFilter = BTerrainFilter, - TerrainMask = BAllFilter & ~BStaticFilter, + TerrainMask = BAllFilter & ~BStaticFilter, // static objects on the ground don't collide GroundPlaneFilter = BGroundPlaneFilter, GroundPlaneMask = BAllFilter