BulletSim: enablement and debugging of mesh terrain.

4 files changed, 253 insertions, 36 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs b/OpenSim/Region/Physics/BulletSPlugin/BSTerrainHeightmap.cs
index 3bb63cd..e9fd0cb 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 db04299..ed0dfa8 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 387c78b..bbb014a 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 bab3b3d..a2271a9 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