Pass collisions and updates in pinned memory (saves marshaling).

Fix folding feet by using collision normals.
Add constraint specification.

4 files changed, 151 insertions, 98 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index d9e236f..dbd7285 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -185,7 +185,7 @@ public class BSCharacter : PhysicsActor
         get { return _force; } 
         set {
             _force = value;
-            m_log.DebugFormat("{0}: Force = {1}", LogHeader, _force);
+            // m_log.DebugFormat("{0}: Force = {1}", LogHeader, _force);
             _scene.TaintedObject(delegate()
             {
                 BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
@@ -411,8 +411,9 @@ public class BSCharacter : PhysicsActor
             _collidingGroundStep = _scene.SimulationStep;
         }
 
+        // throttle collisions to the rate specified in the subscription
         if (_subscribedEventsMs == 0) return;   // don't want collisions
-        int nowTime = Util.EnvironmentTickCount();
+        int nowTime = _scene.SimulationNowTime;
         if (nowTime < (_lastCollisionTime + _subscribedEventsMs)) return;
         _lastCollisionTime = nowTime;
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 82a8803..f2ab2d9 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -261,6 +261,7 @@ public sealed class BSPrim : PhysicsActor
         {
             if (_childrenPrims.Contains(child))
             {
+                BulletSimAPI.RemoveConstraint(_scene.WorldID, child.LocalID, this.LocalID);
                 _childrenPrims.Remove(child);
                 child.ParentPrim = null;    // the child has lost its parent
                 RecreateGeomAndObject();    // rebuild my shape with the child removed
@@ -1018,47 +1019,8 @@ public sealed class BSPrim : PhysicsActor
         if (IsRootOfLinkset)
         {
             // Create a linkset around this object
-            /*
-             * NOTE: the original way of creating a linkset was to create a compound hull in the
-             * root which consisted of the hulls of all the children. This didn't work well because
-             * OpenSimulator needs updates and collisions for all the children and the physics
-             * engine didn't create events for the children when the root hull was moved.
-             * This code creates the compound hull.
-            // If I am the root prim of a linkset, replace my physical shape with all the
-            // pieces of the children.
-            // All of the children should have called CreateGeom so they have a hull
-            // in the physics engine already. Here we pull together all of those hulls
-            // into one shape.
-            int totalPrimsInLinkset = _childrenPrims.Count + 1;
-            // m_log.DebugFormat("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, totalPrimsInLinkset);
-            ShapeData[] shapes = new ShapeData[totalPrimsInLinkset];
-            FillShapeInfo(out shapes[0]);
-            int ii = 1;
-            foreach (BSPrim prim in _childrenPrims)
-            {
-                // m_log.DebugFormat("{0}: CreateLinkset: adding prim {1}", LogHeader, prim.LocalID);
-                prim.FillShapeInfo(out shapes[ii]);
-                ii++;
-            }
-            BulletSimAPI.CreateLinkset(_scene.WorldID, totalPrimsInLinkset, shapes);
-             */
-            // Create the linkset by putting constraints between the objects of the set so they cannot move
-            // relative to each other.
-            // m_log.DebugFormat("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, _childrenPrims.Count+1);
-
-            // remove any constraints that might be in place
-            foreach (BSPrim prim in _childrenPrims)
-            {
-                BulletSimAPI.RemoveConstraint(_scene.WorldID, LocalID, prim.LocalID);
-            }
-            // create constraints between the root prim and each of the children
-            foreach (BSPrim prim in _childrenPrims)
-            {
-                // this is a constraint that allows no freedom of movement between the two objects
-                // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
-                BulletSimAPI.AddConstraint(_scene.WorldID, LocalID, prim.LocalID, OMV.Vector3.Zero, OMV.Vector3.Zero,
-                    OMV.Vector3.Zero, OMV.Vector3.Zero, OMV.Vector3.Zero, OMV.Vector3.Zero);
-            }
+            CreateLinksetWithCompoundHull();
+            // CreateLinksetWithConstraints();
         }
         else
         {
@@ -1070,6 +1032,73 @@ public sealed class BSPrim : PhysicsActor
         }
     }
 
+    // Create a linkset by creating a compound hull at the root prim that consists of all
+    // the children.
+    void CreateLinksetWithCompoundHull()
+    {
+        // If I am the root prim of a linkset, replace my physical shape with all the
+        // pieces of the children.
+        // All of the children should have called CreateGeom so they have a hull
+        // in the physics engine already. Here we pull together all of those hulls
+        // into one shape.
+        int totalPrimsInLinkset = _childrenPrims.Count + 1;
+        // m_log.DebugFormat("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, totalPrimsInLinkset);
+        ShapeData[] shapes = new ShapeData[totalPrimsInLinkset];
+        FillShapeInfo(out shapes[0]);
+        int ii = 1;
+        foreach (BSPrim prim in _childrenPrims)
+        {
+            // m_log.DebugFormat("{0}: CreateLinkset: adding prim {1}", LogHeader, prim.LocalID);
+            prim.FillShapeInfo(out shapes[ii]);
+            ii++;
+        }
+        BulletSimAPI.CreateLinkset(_scene.WorldID, totalPrimsInLinkset, shapes);
+    }
+
+    // Create the linkset by putting constraints between the objects of the set so they cannot move
+    // relative to each other.
+    void CreateLinksetWithConstraints()
+    {
+        // m_log.DebugFormat("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, _childrenPrims.Count+1);
+
+        // remove any constraints that might be in place
+        foreach (BSPrim prim in _childrenPrims)
+        {
+            // m_log.DebugFormat("{0}: CreateObject: RemoveConstraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID);
+            BulletSimAPI.RemoveConstraint(_scene.WorldID, LocalID, prim.LocalID);
+        }
+        // create constraints between the root prim and each of the children
+        foreach (BSPrim prim in _childrenPrims)
+        {
+            // this is a constraint that allows no freedom of movement between the two objects
+            // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
+            // m_log.DebugFormat("{0}: CreateObject: AddConstraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID);
+
+            // relative position normalized to the root prim
+            OMV.Vector3 childRelativePosition = (prim._position - this._position) * OMV.Quaternion.Inverse(this._orientation);
+            // OMV.Quaternion relativeRotation = OMV.Quaternion.Identity;
+
+            // rotation is pointing up the vector between the object centers
+            OMV.Quaternion relativeRotation = OMV.Quaternion.CreateFromAxisAngle(childRelativePosition, 0f);
+
+            /* // the logic for relative rotation from http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=6391
+            OMV.Vector3 rrn = childRelativePosition;
+            rrn.Normalize();
+            rrn /= rrn.X;
+            OMV.Matrix4 rotmat = new OMV.Matrix4(rrn.X, rrn.Y, rrn.Z, 0, 0, 1, 0, 0, 0, 0, 1, 0, 0, 0, 0, 1);
+            OMV.Quaternion relativeRotation = OMV.Quaternion.CreateFromRotationMatrix(rotmat);
+             */
+
+            BulletSimAPI.AddConstraint(_scene.WorldID, LocalID, prim.LocalID, 
+                childRelativePosition / 2,
+                relativeRotation,
+                -childRelativePosition / 2,
+                relativeRotation,
+                OMV.Vector3.Zero, OMV.Vector3.Zero,
+                OMV.Vector3.Zero, OMV.Vector3.Zero);
+        }
+    }
+
     // Copy prim's info into the BulletSim shape description structure
     public void FillShapeInfo(out ShapeData shape)
     {
@@ -1118,50 +1147,53 @@ public sealed class BSPrim : PhysicsActor
     // The physics engine says that properties have updated. Update same and inform
     // the world that things have changed.
     // TODO: do we really need to check for changed? Maybe just copy values and call RequestPhysicsterseUpdate()
-    private int UpPropPosition      = 1 << 0;
-    private int UpPropRotation      = 1 << 1;
-    private int UpPropVelocity      = 1 << 2;
-    private int UpPropAcceleration  = 1 << 3;
-    private int UpPropAngularVel    = 1 << 4;
+    enum UpdatedProperties {
+        Position      = 1 << 0,
+        Rotation      = 1 << 1,
+        Velocity      = 1 << 2,
+        Acceleration  = 1 << 3,
+        AngularVel    = 1 << 4
+    }
 
     public void UpdateProperties(EntityProperties entprop)
     {
-        int changed = 0;
+        UpdatedProperties changed = 0;
         // assign to the local variables so the normal set action does not happen
         if (_position != entprop.Position)
         {
             _position = entprop.Position;
             // m_log.DebugFormat("{0}: UpdateProperties: position = {1}", LogHeader, _position);
-            changed |= UpPropPosition;
+            changed |= UpdatedProperties.Position;
         }
         if (_orientation != entprop.Rotation)
         {
             _orientation = entprop.Rotation;
             // m_log.DebugFormat("{0}: UpdateProperties: rotation = {1}", LogHeader, _orientation);
-            changed |= UpPropRotation;
+            changed |= UpdatedProperties.Rotation;
         }
         if (_velocity != entprop.Velocity)
         {
             _velocity = entprop.Velocity;
             // m_log.DebugFormat("{0}: UpdateProperties: velocity = {1}", LogHeader, _velocity);
-            changed |= UpPropVelocity;
+            changed |= UpdatedProperties.Velocity;
         }
         if (_acceleration != entprop.Acceleration)
         {
             _acceleration = entprop.Acceleration;
             // m_log.DebugFormat("{0}: UpdateProperties: acceleration = {1}", LogHeader, _acceleration);
-            changed |= UpPropAcceleration;
+            changed |= UpdatedProperties.Acceleration;
         }
         if (_rotationalVelocity != entprop.AngularVelocity)
         {
             _rotationalVelocity = entprop.AngularVelocity;
             // m_log.DebugFormat("{0}: UpdateProperties: rotationalVelocity = {1}", LogHeader, _rotationalVelocity);
-            changed |= UpPropAngularVel;
+            changed |= UpdatedProperties.AngularVel;
         }
         if (changed != 0)
         {
             // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
-            base.RequestPhysicsterseUpdate();
+            if (this._parentPrim == null)
+                base.RequestPhysicsterseUpdate();
         }
     }
 
@@ -1178,7 +1210,7 @@ public sealed class BSPrim : PhysicsActor
 
         if (_subscribedEventsMs == 0) return;   // nothing in the object is waiting for collision events
         // throttle the collisions to the number of milliseconds specified in the subscription
-        int nowTime = Util.EnvironmentTickCount();
+        int nowTime = _scene.SimulationNowTime;
         if (nowTime < (_lastCollisionTime + _subscribedEventsMs)) return;
         _lastCollisionTime = nowTime;
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index beffd21..062945f 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -38,9 +38,7 @@ using OpenSim.Region.Framework;
 
 // TODOs for BulletSim (for BSScene, BSPrim, BSCharacter and BulletSim)
 // Fix folding up feet
-// Fix terrain. Only flat terrain works. Terrain with shape is oriented wrong? Origined wrong?
 // Parameterize BulletSim. Pass a structure of parameters to the C++ code. Capsule size, friction, ...
-// Shift drag duplication of objects does not work
 // Adjust character capsule size when height is adjusted (ScenePresence.SetHeight)
 // Test sculpties
 // Compute physics FPS reasonably
@@ -82,6 +80,19 @@ public class BSScene : PhysicsScene
     private long m_simulationStep = 0;
     public long SimulationStep { get { return m_simulationStep; } }
 
+    // A value of the time now so all the collision and update routines do not have to get their own
+    // Set to 'now' just before all the prims and actors are called for collisions and updates
+    private int m_simulationNowTime;
+    public int SimulationNowTime { get { return m_simulationNowTime; } }
+
+    private int m_maxCollisionsPerFrame = 2048;
+    private CollisionDesc[] m_collisionArray;
+    private GCHandle m_collisionArrayPinnedHandle;
+
+    private int m_maxUpdatesPerFrame = 2048;
+    private EntityProperties[] m_updateArray;
+    private GCHandle m_updateArrayPinnedHandle;
+
     private bool _meshSculptedPrim = true;         // cause scuplted prims to get meshed
     private bool _forceSimplePrimMeshing = false;   // if a cube or sphere, let Bullet do internal shapes
     public float maximumMassObject = 10000.01f;
@@ -129,8 +140,19 @@ public class BSScene : PhysicsScene
         _taintedObjects = new List<TaintCallback>();
 
         mesher = meshmerizer;
+        // The bounding box for the simulated world
+        Vector3 worldExtent = new Vector3(Constants.RegionSize, Constants.RegionSize, 4096f);
+
+        // Allocate pinned memory to pass back object property updates and collisions from simulation step
+        m_collisionArray = new CollisionDesc[m_maxCollisionsPerFrame];
+        m_collisionArrayPinnedHandle = GCHandle.Alloc(m_collisionArray, GCHandleType.Pinned);
+        m_updateArray = new EntityProperties[m_maxUpdatesPerFrame];
+        m_updateArrayPinnedHandle = GCHandle.Alloc(m_updateArray, GCHandleType.Pinned);
+
         // m_log.DebugFormat("{0}: Initialize: Calling BulletSimAPI.Initialize.", LogHeader);
-        m_worldID = BulletSimAPI.Initialize(new Vector3(Constants.RegionSize, Constants.RegionSize, 4096f));
+        m_worldID = BulletSimAPI.Initialize(worldExtent, 
+                                        m_maxCollisionsPerFrame, m_collisionArrayPinnedHandle.AddrOfPinnedObject(),
+                                        m_maxUpdatesPerFrame, m_updateArrayPinnedHandle.AddrOfPinnedObject());
     }
 
     // Called directly from unmanaged code so don't do much
@@ -188,19 +210,7 @@ public class BSScene : PhysicsScene
     }
 
     public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
-                                              Vector3 size, Quaternion rotation)  // deprecated
-    {
-        return null;
-    }
-    public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
-                                              Vector3 size, Quaternion rotation, bool isPhysical)
-    {
-        m_log.ErrorFormat("{0}: CALL TO AddPrimShape in BSScene. NOT IMPLEMENTED", LogHeader);
-        return null;
-    }
-
-    public override PhysicsActor AddPrimShape(uint localID, string primName, PrimitiveBaseShape pbs, Vector3 position,
-                                              Vector3 size, Quaternion rotation, bool isPhysical)
+                                              Vector3 size, Quaternion rotation, bool isPhysical, uint localID)
     {
         // m_log.DebugFormat("{0}: AddPrimShape2: {1}", LogHeader, primName);
         IMesh mesh = null;
@@ -225,10 +235,8 @@ public class BSScene : PhysicsScene
     {
         int updatedEntityCount;
         IntPtr updatedEntitiesPtr;
-        IntPtr[] updatedEntities;
         int collidersCount;
         IntPtr collidersPtr;
-        int[] colliders;    // should be uint but Marshal.Copy does not have that overload
 
         // update the prim states while we know the physics engine is not busy
         ProcessTaints();
@@ -242,32 +250,33 @@ public class BSScene : PhysicsScene
         int numSubSteps = BulletSimAPI.PhysicsStep(m_worldID, timeStep, m_maxSubSteps, m_fixedTimeStep, 
                     out updatedEntityCount, out updatedEntitiesPtr, out collidersCount, out collidersPtr);
 
-        // if there were collisions, they show up here
+        // Don't have to use the pointers passed back since we know it is the same pinned memory we passed in
+
+        // Get a value for 'now' so all the collision and update routines don't have to get their own
+        m_simulationNowTime = Util.EnvironmentTickCount();
+
+        // If there were collisions, process them by sending the event to the prim.
+        // Collisions must be processed before updates.
         if (collidersCount > 0)
         {
-            colliders = new int[collidersCount];
-            Marshal.Copy(collidersPtr, colliders, 0, collidersCount);
-            for (int ii = 0; ii < collidersCount; ii+=2)
+            for (int ii = 0; ii < collidersCount; ii++)
             {
-                uint cA = (uint)colliders[ii];
-                uint cB = (uint)colliders[ii+1];
-                SendCollision(cA, cB);
-                SendCollision(cB, cA);
+                uint cA = m_collisionArray[ii].aID;
+                uint cB = m_collisionArray[ii].bID;
+                Vector3 point = m_collisionArray[ii].point;
+                Vector3 normal = m_collisionArray[ii].normal;
+                SendCollision(cA, cB, point, normal, 0.01f);
+                SendCollision(cB, cA, point, -normal, 0.01f);
             }
         }
 
-        // if any of the objects had updated properties, they are returned in the updatedEntity structure
-        // TODO: figure out how to pass all of the EntityProperties structures in one marshal call.
+        // If any of the objects had updated properties, tell the object it has been changed by the physics engine
         if (updatedEntityCount > 0)
         {
-            updatedEntities = new IntPtr[updatedEntityCount];
-            // fetch all the pointers to all the EntityProperties structures for these updates
-            Marshal.Copy(updatedEntitiesPtr, updatedEntities, 0, updatedEntityCount);
             for (int ii = 0; ii < updatedEntityCount; ii++)
             {
-                IntPtr updatePointer = updatedEntities[ii];
-                EntityProperties entprop = (EntityProperties)Marshal.PtrToStructure(updatePointer, typeof(EntityProperties));
-                // m_log.DebugFormat("{0}: entprop: id={1}, pos={2}", LogHeader, entprop.ID, entprop.Position);
+                EntityProperties entprop = m_updateArray[ii];
+                // m_log.DebugFormat("{0}: entprop[{1}]: id={2}, pos={3}", LogHeader, ii, entprop.ID, entprop.Position);
                 BSCharacter actor;
                 if (m_avatars.TryGetValue(entprop.ID, out actor))
                 {
@@ -282,12 +291,12 @@ public class BSScene : PhysicsScene
             }
         }
 
-        // fps calculation wrong. This calculation returns about 1 in normal operation.
+        // fps calculation wrong. This calculation always returns about 1 in normal operation.
         return timeStep / (numSubSteps * m_fixedTimeStep) * 1000f;
     }
 
     // Something has collided
-    private void SendCollision(uint localID, uint collidingWith)
+    private void SendCollision(uint localID, uint collidingWith, Vector3 collidePoint, Vector3 collideNormal, float penitration)
     {
         if (localID == TERRAIN_ID || localID == GROUNDPLANE_ID)
         {
@@ -303,12 +312,12 @@ public class BSScene : PhysicsScene
 
         BSPrim prim;
         if (m_prims.TryGetValue(localID, out prim)) {
-            prim.Collide(collidingWith, type, Vector3.Zero, Vector3.UnitZ, 0.01f);
+            prim.Collide(collidingWith, type, collidePoint, collideNormal, 0.01f);
             return;
         }
         BSCharacter actor;
         if (m_avatars.TryGetValue(localID, out actor)) {
-            actor.Collide(collidingWith, type, Vector3.Zero, Vector3.UnitZ, 0.01f);
+            actor.Collide(collidingWith, type, collidePoint, collideNormal, 0.01f);
             return;
         }
         return;
@@ -317,7 +326,6 @@ public class BSScene : PhysicsScene
     public override void GetResults() { }
 
     public override void SetTerrain(float[] heightMap) {
-        m_log.DebugFormat("{0}: SetTerrain", LogHeader);
         m_heightMap = heightMap;
         this.TaintedObject(delegate()
         {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index b2bc0d7..ace8158 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -78,7 +78,13 @@ public struct RaycastHit
     public float Fraction;
     public Vector3 Normal;
 }
-
+public struct CollisionDesc
+{
+    public uint aID;
+    public uint bID;
+    public Vector3 point;
+    public Vector3 normal;
+}
 public struct EntityProperties
 {
     public uint ID;
@@ -92,7 +98,8 @@ public struct EntityProperties
 static class BulletSimAPI {
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
-public static extern uint Initialize(Vector3 maxPosition);
+public static extern uint Initialize(Vector3 maxPosition, int maxCollisions, IntPtr collisionArray, 
+                        int maxUpdates, IntPtr updateArray);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void SetHeightmap(uint worldID, [MarshalAs(UnmanagedType.LPArray)] float[] heightMap);
@@ -124,7 +131,12 @@ public static extern void CreateLinkset(uint worldID, int objectCount, ShapeData
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void AddConstraint(uint worldID, uint id1, uint id2, 
-    Vector3 frame1, Vector3 frame2, Vector3 lowLinear, Vector3 hiLinear, Vector3 lowAngular, Vector3 hiAngular);
+    Vector3 frame1, Quaternion frame1rot,
+    Vector3 frame2, Quaternion frame2rot,
+    Vector3 lowLinear, Vector3 hiLinear, Vector3 lowAngular, Vector3 hiAngular);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern bool RemoveConstraintByID(uint worldID, uint id1);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool RemoveConstraint(uint worldID, uint id1, uint id2);