BulletSim: Add Force* operations to objects to allow direct push to engine.

Update BSDynamics to use same (don't want to delay updates til next taint-time.
Suppress queuing a taint update for position and orientation calls if value
    does not change.
Move Bullet timing statistics call from C# back to C++ code.
Throttle taints per simulation step and add parameter to set.
By default, don't create hulls for physical objects. Add a
    parameter to turn on and off.

7 files changed, 100 insertions, 29 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 2a52e01..c23ccd5 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -332,6 +332,13 @@ public class BSCharacter : BSPhysObject
             });
         }
     }
+    public override OMV.Vector3 ForceVelocity {
+        get { return _velocity; }
+        set {
+            _velocity = value;
+            BulletSimAPI.SetObjectVelocity(PhysicsScene.WorldID, LocalID, _velocity);
+        }
+    }
     public override OMV.Vector3 Torque {
         get { return _torque; }
         set { _torque = value;
@@ -432,6 +439,10 @@ public class BSCharacter : BSPhysObject
         get { return _rotationalVelocity; }
         set { _rotationalVelocity = value; }
     }
+    public override OMV.Vector3 ForceRotationalVelocity {
+        get { return _rotationalVelocity; }
+        set { _rotationalVelocity = value; }
+    }
     public override bool Kinematic {
         get { return _kinematic; }
         set { _kinematic = value; }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 3fb2253..76230a1 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -481,7 +481,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_lastPositionVector = Prim.ForcePosition;
 
             VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
-                    Prim.LocalID, Prim.Position, Prim.Force, Prim.Velocity, Prim.RotationalVelocity);
+                    Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity);
         }// end Step
 
         // Apply the effect of the linear motor.
@@ -540,7 +540,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // add Gravity and Buoyancy
             // There is some gravity, make a gravity force vector that is applied after object velocity.
             // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
-            Vector3 grav = Prim.PhysicsScene.DefaultGravity * (Prim.Mass * (1f - m_VehicleBuoyancy));
+            Vector3 grav = Prim.PhysicsScene.DefaultGravity * (Prim.MassRaw * (1f - m_VehicleBuoyancy));
 
             /*
              * RA: Not sure why one would do this
@@ -678,10 +678,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 m_newVelocity.Z = 0;
 
             // Apply velocity
-            Prim.Velocity = m_newVelocity;
+            Prim.ForceVelocity = m_newVelocity;
             // apply gravity force
             // Why is this set here? The physics engine already does gravity.
-            // m_prim.AddForce(grav, false);
+            Prim.AddForce(grav, false);
 
             // Apply friction
             Vector3 keepFraction = Vector3.One - (Vector3.One / (m_linearFrictionTimescale / pTimestep));
@@ -704,7 +704,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // m_lastAngularVelocity           // what was last applied to body
 
             // Get what the body is doing, this includes 'external' influences
-            Vector3 angularVelocity = Prim.RotationalVelocity;
+            Vector3 angularVelocity = Prim.ForceRotationalVelocity;
 
             if (m_angularMotorApply > 0)
             {
@@ -810,7 +810,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
 
             // Apply to the body
-            Prim.RotationalVelocity = m_lastAngularVelocity;
+            Prim.ForceRotationalVelocity = m_lastAngularVelocity;
 
             VDetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", Prim.LocalID, decayamount, m_lastAngularVelocity);
         } //end MoveAngular
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 0665292..cae599c 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -85,6 +85,10 @@ public abstract class BSPhysObject : PhysicsActor
 
     public abstract OMV.Quaternion ForceOrientation { get; set; }
 
+    public abstract OMV.Vector3 ForceVelocity { get; set; }
+
+    public abstract OMV.Vector3 ForceRotationalVelocity { get; set; }
+
     #region Collisions
 
     // Requested number of milliseconds between collision events. Zero means disabled.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 98a18a1..d408be0 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -265,6 +265,11 @@ public sealed class BSPrim : BSPhysObject
             return _position;
         }
         set {
+            // If you must push the position into the physics engine, use ForcePosition.
+            if (_position == value)
+            {
+                return;
+            }
             _position = value;
             // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
             PositionSanityCheck();
@@ -453,7 +458,6 @@ public sealed class BSPrim : BSPhysObject
         }
         return;
     }
-
     public override OMV.Vector3 Velocity {
         get { return _velocity; }
         set {
@@ -465,6 +469,13 @@ public sealed class BSPrim : BSPhysObject
             });
         }
     }
+    public override OMV.Vector3 ForceVelocity {
+        get { return _velocity; }
+        set {
+            _velocity = value;
+            BulletSimAPI.SetLinearVelocity2(BSBody.ptr, _velocity);
+        }
+    }
     public override OMV.Vector3 Torque {
         get { return _torque; }
         set { _torque = value;
@@ -490,6 +501,8 @@ public sealed class BSPrim : BSPhysObject
             return _orientation;
         }
         set {
+            if (_orientation == value)
+                return;
             _orientation = value;
             // TODO: what does it mean if a child in a linkset changes its orientation? Rebuild the constraint?
             PhysicsScene.TaintedObject("BSPrim.setOrientation", delegate()
@@ -621,9 +634,9 @@ public sealed class BSPrim : BSPhysObject
             // There can be special things needed for implementing linksets
             Linkset.MakeStatic(this);
             // The activation state is 'disabled' so Bullet will not try to act on it.
-            // BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.DISABLE_SIMULATION);
+            BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.DISABLE_SIMULATION);
             // Start it out sleeping and physical actions could wake it up.
-            BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ISLAND_SLEEPING);
+            // BulletSimAPI.ForceActivationState2(BSBody.ptr, ActivationState.ISLAND_SLEEPING);
 
             BSBody.collisionFilter = CollisionFilterGroups.StaticObjectFilter;
             BSBody.collisionMask = CollisionFilterGroups.StaticObjectMask;
@@ -640,6 +653,9 @@ public sealed class BSPrim : BSPhysObject
             // per http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=3382
             BulletSimAPI.ClearAllForces2(BSBody.ptr);
 
+            // For good measure, make sure the transform is set through to the motion state
+            BulletSimAPI.SetTranslation2(BSBody.ptr, _position, _orientation);
+
             // A dynamic object has mass
             IntPtr collisionShapePtr = BulletSimAPI.GetCollisionShape2(BSBody.ptr);
             OMV.Vector3 inertia = BulletSimAPI.CalculateLocalInertia2(collisionShapePtr, Mass);
@@ -776,6 +792,15 @@ public sealed class BSPrim : BSPhysObject
             });
         }
     }
+    public override OMV.Vector3 ForceRotationalVelocity {
+        get {
+            return _rotationalVelocity;
+        }
+        set {
+            _rotationalVelocity = value;
+            BulletSimAPI.SetAngularVelocity2(BSBody.ptr, _rotationalVelocity);
+        }
+    }
     public override bool Kinematic {
         get { return _kinematic; }
         set { _kinematic = value;
@@ -1307,7 +1332,7 @@ public sealed class BSPrim : BSPhysObject
             /*
         else
         {
-            // For debugging, we can also report the movement of children
+            // For debugging, report the movement of children
             DetailLog("{0},BSPrim.UpdateProperties,child,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
                     LocalID, entprop.Position, entprop.Rotation, entprop.Velocity,
                     entprop.Acceleration, entprop.RotationalVelocity);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index eed915d..33ac116 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -90,10 +90,6 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     // let my minuions use my logger
     public ILog Logger { get { return m_log; } }
 
-    // If non-zero, the number of simulation steps between calls to the physics
-    //    engine to output detailed physics stats. Debug logging level must be on also.
-    private int m_detailedStatsStep = 0;
-
     public IMesher mesher;
     // Level of Detail values kept as float because that's what the Meshmerizer wants
     public float MeshLOD { get; private set; }
@@ -112,6 +108,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     private float m_fixedTimeStep;
     private long m_simulationStep = 0;
     public long SimulationStep { get { return m_simulationStep; } }
+    private int m_taintsToProcessPerStep;
 
     // 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
@@ -131,6 +128,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
 
     public bool ShouldMeshSculptedPrim { get; private set; }   // cause scuplted prims to get meshed
     public bool ShouldForceSimplePrimMeshing { get; private set; }   // if a cube or sphere, let Bullet do internal shapes
+    public bool ShouldUseHullsForPhysicalObjects { get; private set; }   // 'true' if should create hulls for physical objects
 
     public float PID_D { get; private set; }    // derivative
     public float PID_P { get; private set; }    // proportional
@@ -582,15 +580,6 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             }
         }
 
-        // If enabled, call into the physics engine to dump statistics
-        if (m_detailedStatsStep > 0)
-        {
-            if ((m_simulationStep % m_detailedStatsStep) == 0)
-            {
-                BulletSimAPI.DumpBulletStatistics();
-            }
-        }
-
         // The physics engine returns the number of milliseconds it simulated this call.
         // These are summed and normalized to one second and divided by 1000 to give the reported physics FPS.
         // Since Bullet normally does 5 or 6 substeps, this will normally sum to about 60 FPS.
@@ -617,7 +606,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         BSPhysObject collidee = null;
         PhysObjects.TryGetValue(collidingWith, out collidee);
 
-        DetailLog("{0},BSScene.SendCollision,collide,id={1},with={2}", DetailLogZero, localID, collidingWith);
+        // DetailLog("{0},BSScene.SendCollision,collide,id={1},with={2}", DetailLogZero, localID, collidingWith);
 
         if (collider.Collide(collidingWith, collidee, collidePoint, collideNormal, penetration))
         {
@@ -704,6 +693,35 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         if (_taintedObjects.Count > 0)  // save allocating new list if there is nothing to process
         {
             // swizzle a new list into the list location so we can process what's there
+            int taintCount = m_taintsToProcessPerStep;
+            TaintCallbackEntry oneCallback = new TaintCallbackEntry();
+            while (_taintedObjects.Count > 0 && taintCount-- > 0)
+            {
+                bool gotOne = false;
+                lock (_taintLock)
+                {
+                    if (_taintedObjects.Count > 0)
+                    {
+                        oneCallback = _taintedObjects[0];
+                        _taintedObjects.RemoveAt(0);
+                        gotOne = true;
+                    }
+                }
+                if (gotOne)
+                {
+                    try
+                    {
+                        DetailLog("{0},BSScene.ProcessTaints,doTaint,id={1}", DetailLogZero, oneCallback.ident); // DEBUG DEBUG DEBUG
+                        oneCallback.callback();
+                    }
+                    catch (Exception e)
+                    {
+                        m_log.ErrorFormat("{0}: ProcessTaints: {1}: Exception: {2}", LogHeader, oneCallback.ident, e);
+                    }
+                }
+            }
+            /*
+            // swizzle a new list into the list location so we can process what's there
             List<TaintCallbackEntry> oldList;
             lock (_taintLock)
             {
@@ -724,6 +742,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                 }
             }
             oldList.Clear();
+             */
         }
     }
 
@@ -835,6 +854,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s,cf,p,v) => { s.ShouldForceSimplePrimMeshing = cf.GetBoolean(p, s.BoolNumeric(v)); },
             (s) => { return s.NumericBool(s.ShouldForceSimplePrimMeshing); },
             (s,p,l,v) => { s.ShouldForceSimplePrimMeshing = s.BoolNumeric(v); } ),
+        new ParameterDefn("UseHullsForPhysicalObjects", "If true, create hulls for physical objects",
+            ConfigurationParameters.numericFalse,
+            (s,cf,p,v) => { s.ShouldUseHullsForPhysicalObjects = cf.GetBoolean(p, s.BoolNumeric(v)); },
+            (s) => { return s.NumericBool(s.ShouldUseHullsForPhysicalObjects); },
+            (s,p,l,v) => { s.ShouldUseHullsForPhysicalObjects = s.BoolNumeric(v); } ),
 
         new ParameterDefn("MeshLevelOfDetail", "Level of detail to render meshes (32, 16, 8 or 4. 32=most detailed)",
             8f,
@@ -877,6 +901,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s,cf,p,v) => { s.m_maxUpdatesPerFrame = cf.GetInt(p, (int)v); },
             (s) => { return (float)s.m_maxUpdatesPerFrame; },
             (s,p,l,v) => { s.m_maxUpdatesPerFrame = (int)v; } ),
+        new ParameterDefn("MaxTaintsToProcessPerStep", "Number of update taints to process before each simulation step",
+            100f,
+            (s,cf,p,v) => { s.m_taintsToProcessPerStep = cf.GetInt(p, (int)v); },
+            (s) => { return (float)s.m_taintsToProcessPerStep; },
+            (s,p,l,v) => { s.m_taintsToProcessPerStep = (int)v; } ),
         new ParameterDefn("MaxObjectMass", "Maximum object mass (10000.01)",
             10000.01f,
             (s,cf,p,v) => { s.MaximumObjectMass = cf.GetFloat(p, v); },
@@ -1086,11 +1115,11 @@ public class BSScene : PhysicsScene, IPhysicsParameters
             (s) => { return s.m_params[0].linkConstraintSolverIterations; },
             (s,p,l,v) => { s.m_params[0].linkConstraintSolverIterations = v; } ),
 
-        new ParameterDefn("DetailedStats", "Frames between outputting detailed phys stats. (0 is off)",
+        new ParameterDefn("LogPhysicsStatisticsFrames", "Frames between outputting detailed phys stats. (0 is off)",
             0f,
-            (s,cf,p,v) => { s.m_detailedStatsStep = cf.GetInt(p, (int)v); },
-            (s) => { return (float)s.m_detailedStatsStep; },
-            (s,p,l,v) => { s.m_detailedStatsStep = (int)v; } ),
+            (s,cf,p,v) => { s.m_params[0].physicsLoggingFrames = cf.GetInt(p, (int)v); },
+            (s) => { return (float)s.m_params[0].physicsLoggingFrames; },
+            (s,p,l,v) => { s.m_params[0].physicsLoggingFrames = (int)v; } ),
     };
 
     // Convert a boolean to our numeric true and false values
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
index 283b601..e619b48 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapeCollection.cs
@@ -407,7 +407,7 @@ public class BSShapeCollection : IDisposable
         //     made. Native shapes are best used in either case.
         if (!haveShape)
         {
-            if (prim.IsPhysical)
+            if (prim.IsPhysical && PhysicsScene.ShouldUseHullsForPhysicalObjects)
             {
                 // Update prim.BSShape to reference a hull of this shape.
                 ret = GetReferenceToHull(prim, shapeData, pbs, shapeCallback);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index bb4d399..7a60afb 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -306,6 +306,8 @@ public struct ConfigurationParameters
     public float linkConstraintCFM;
     public float linkConstraintSolverIterations;
 
+    public float physicsLoggingFrames;
+
     public const float numericTrue = 1f;
     public const float numericFalse = 0f;
 }