BulletSim: add BSPhysObject code to manage registrations of preStep events. Use same to implement setForce and setTorque so the values are restored at the beginning of each step (since Bullet zeros forces applied last step). Simplify implementation of AddForce and AddTorque by relying on the addition of forces in Bullet.

3 files changed, 104 insertions, 66 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 7bde1c1..b392d75 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -134,6 +134,8 @@ public sealed class BSCharacter : BSPhysObject
     // called when this character is being destroyed and the resources should be released
     public override void Destroy()
     {
+        base.Destroy();
+
         DetailLog("{0},BSCharacter.Destroy", LocalID);
         PhysicsScene.TaintedObject("BSCharacter.destroy", delegate()
         {
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 92a5f2f..9525a11 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -45,6 +45,16 @@ namespace OpenSim.Region.Physics.BulletSPlugin
  *      ForceVariableName: direct reference (store and fetch) to the value in the physics engine.
  *  The last two (and certainly the last one) should be referenced only in taint-time.
  */
+
+/*
+ * As of 20121221, the following are the call sequences (going down) for different script physical functions:
+ * llApplyImpulse       llApplyRotImpulse           llSetTorque             llSetForce
+ * SOP.ApplyImpulse     SOP.ApplyAngularImpulse     SOP.SetAngularImpulse   SOP.SetForce
+ * SOG.ApplyImpulse     SOG.ApplyAngularImpulse     SOG.SetAngularImpulse
+ * PA.AddForce          PA.AddAngularForce          PA.Torque = v           PA.Force = v
+ * BS.ApplyCentralForce BS.ApplyTorque              
+ */
+
 public abstract class BSPhysObject : PhysicsActor
 {
     protected BSPhysObject()
@@ -69,6 +79,12 @@ public abstract class BSPhysObject : PhysicsActor
         CollidingGroundStep = 0;
     }
 
+    // Tell the object to clean up.
+    public virtual void Destroy()
+    {
+        UnRegisterAllPreStepActions();
+    }
+
     public BSScene PhysicsScene { get; protected set; }
     // public override uint LocalID { get; set; } // Use the LocalID definition in PhysicsActor
     public string PhysObjectName { get; protected set; }
@@ -130,9 +146,6 @@ public abstract class BSPhysObject : PhysicsActor
     // Update the physical location and motion of the object. Called with data from Bullet.
     public abstract void UpdateProperties(EntityProperties entprop);
 
-    // Tell the object to clean up.
-    public abstract void Destroy();
-
     public abstract OMV.Vector3 RawPosition { get; set; }
     public abstract OMV.Vector3 ForcePosition { get; set; }
 
@@ -280,11 +293,48 @@ public abstract class BSPhysObject : PhysicsActor
 
     #endregion // Collisions
 
+    #region Per Simulation Step actions
+    // There are some actions that must be performed for a physical object before each simulation step.
+    // These actions are optional so, rather than scanning all the physical objects and asking them
+    //     if they have anything to do, a physical object registers for an event call before the step is performed.
+    // This bookkeeping makes it easy to add, remove and clean up after all these registrations.
+    private Dictionary<string, BSScene.PreStepAction> RegisteredActions = new Dictionary<string, BSScene.PreStepAction>();
+    protected void RegisterPreStepAction(string op, uint id, BSScene.PreStepAction actn)
+    {
+        string identifier = op + "-" + id.ToString();
+        RegisteredActions[identifier] = actn;
+        PhysicsScene.BeforeStep += actn;
+    }
+
+    // Unregister a pre step action. Safe to call if the action has not been registered.
+    protected void UnRegisterPreStepAction(string op, uint id)
+    {
+        string identifier = op + "-" + id.ToString();
+        if (RegisteredActions.ContainsKey(identifier))
+        {
+            PhysicsScene.BeforeStep -= RegisteredActions[identifier];
+            RegisteredActions.Remove(identifier);
+        }
+    }
+
+    protected void UnRegisterAllPreStepActions()
+    {
+        foreach (KeyValuePair<string, BSScene.PreStepAction> kvp in RegisteredActions)
+        {
+            PhysicsScene.BeforeStep -= kvp.Value;
+        }
+        RegisteredActions.Clear();
+    }
+
+    
+    #endregion // Per Simulation Step actions
+
     // High performance detailed logging routine used by the physical objects.
     protected void DetailLog(string msg, params Object[] args)
     {
         if (PhysicsScene.PhysicsLogging.Enabled)
             PhysicsScene.DetailLog(msg, args);
     }
+
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index e43bf8e..e6aeebb 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -129,6 +129,7 @@ public sealed class BSPrim : BSPhysObject
     public override void Destroy()
     {
         // m_log.DebugFormat("{0}: Destroy, id={1}", LogHeader, LocalID);
+        base.Destroy();
 
         // Undo any links between me and any other object
         BSPhysObject parentBefore = Linkset.LinksetRoot;
@@ -434,12 +435,22 @@ public sealed class BSPrim : BSPhysObject
         get { return _force; }
         set {
             _force = value;
-            PhysicsScene.TaintedObject("BSPrim.setForce", delegate()
+            if (_force != OMV.Vector3.Zero)
             {
-                // DetailLog("{0},BSPrim.setForce,taint,force={1}", LocalID, _force);
-                if (PhysBody.HasPhysicalBody)
-                    BulletSimAPI.SetObjectForce2(PhysBody.ptr, _force);
-            });
+                // If the force is non-zero, it must be reapplied each tick because
+                //    Bullet clears the forces applied last frame.
+                RegisterPreStepAction("BSPrim.setForce", LocalID,
+                    delegate(float timeStep)
+                    {
+                        if (PhysBody.HasPhysicalBody)
+                            BulletSimAPI.ApplyCentralForce2(PhysBody.ptr, _force);
+                    }
+                );
+            }
+            else
+            {
+                UnRegisterPreStepAction("BSPrim.setForce", LocalID);
+            }
         }
     }
 
@@ -550,7 +561,22 @@ public sealed class BSPrim : BSPhysObject
         get { return _torque; }
         set {
             _torque = value;
-            AddAngularForce(_torque, false, false);
+            if (_torque != OMV.Vector3.Zero)
+            {
+                // If the torque is non-zero, it must be reapplied each tick because
+                //    Bullet clears the forces applied last frame.
+                RegisterPreStepAction("BSPrim.setTorque", LocalID,
+                    delegate(float timeStep)
+                    {
+                        if (PhysBody.HasPhysicalBody)
+                            AddAngularForce(_torque, false, true);
+                    }
+                );
+            }
+            else
+            {
+                UnRegisterPreStepAction("BSPrim.setTorque", LocalID);
+            }
             // DetailLog("{0},BSPrim.SetTorque,call,torque={1}", LocalID, _torque);
         }
     }
@@ -969,56 +995,32 @@ public sealed class BSPrim : BSPhysObject
     public override float APIDStrength { set { return; } }
     public override float APIDDamping { set { return; } }
 
-    private List<OMV.Vector3> m_accumulatedForces = new List<OMV.Vector3>();
     public override void AddForce(OMV.Vector3 force, bool pushforce) {
         AddForce(force, pushforce, false);
     }
     // Applying a force just adds this to the total force on the object.
+    // This added force will only last the next simulation tick.
     public void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
         // for an object, doesn't matter if force is a pushforce or not
         if (force.IsFinite())
         {
-            // _force += force;
-            lock (m_accumulatedForces)
-                m_accumulatedForces.Add(new OMV.Vector3(force));
+            OMV.Vector3 addForce = force;
+            DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce);
+            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
+            {
+                // Bullet adds this central force to the total force for this tick
+                DetailLog("{0},BSPrim.addForce,taint,force={1}", LocalID, addForce);
+                if (PhysBody.HasPhysicalBody)
+                    BulletSimAPI.ApplyCentralForce2(PhysBody.ptr, addForce);
+            });
         }
         else
         {
             m_log.WarnFormat("{0}: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
             return;
         }
-        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
-        {
-            OMV.Vector3 fSum = OMV.Vector3.Zero;
-            lock (m_accumulatedForces)
-            {
-                // Sum the accumulated additional forces for one big force to apply once.
-                foreach (OMV.Vector3 v in m_accumulatedForces)
-                {
-                    fSum += v;
-                }
-                m_accumulatedForces.Clear();
-            }
-            DetailLog("{0},BSPrim.AddForce,taint,force={1}", LocalID, fSum);
-            if (fSum != OMV.Vector3.Zero)
-                if (PhysBody.HasPhysicalBody)
-                    BulletSimAPI.ApplyCentralForce2(PhysBody.ptr, fSum);
-        });
     }
 
-    // An impulse force is scaled by the mass of the object.
-    public void ApplyForceImpulse(OMV.Vector3 impulse, bool inTaintTime)
-    {
-        OMV.Vector3 applyImpulse = impulse;
-        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ApplyForceImpulse", delegate()
-        {
-            DetailLog("{0},BSPrim.ApplyForceImpulse,taint,tImpulse={1}", LocalID, applyImpulse);
-            if (PhysBody.HasPhysicalBody)
-                BulletSimAPI.ApplyCentralImpulse2(PhysBody.ptr, applyImpulse);
-        });
-    }
-
-    private List<OMV.Vector3> m_accumulatedAngularForces = new List<OMV.Vector3>();
     public override void AddAngularForce(OMV.Vector3 force, bool pushforce) {
         AddAngularForce(force, pushforce, false);
     }
@@ -1026,36 +1028,20 @@ public sealed class BSPrim : BSPhysObject
     {
         if (force.IsFinite())
         {
-            // _force += force;
-            lock (m_accumulatedAngularForces)
-                m_accumulatedAngularForces.Add(new OMV.Vector3(force));
+            OMV.Vector3 angForce = force;
+            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddAngularForce", delegate()
+            {
+                if (PhysBody.HasPhysicalBody)
+                    BulletSimAPI.ApplyTorque2(PhysBody.ptr, angForce);
+            });
         }
         else
         {
             m_log.WarnFormat("{0}: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
             return;
         }
-        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddAngularForce", delegate()
-        {
-            OMV.Vector3 fSum = OMV.Vector3.Zero;
-            lock (m_accumulatedAngularForces)
-            {
-                // Sum the accumulated additional forces for one big force to apply once.
-                foreach (OMV.Vector3 v in m_accumulatedAngularForces)
-                {
-                    fSum += v;
-                }
-                m_accumulatedAngularForces.Clear();
-            }
-            DetailLog("{0},BSPrim.AddAngularForce,taint,aForce={1}", LocalID, fSum);
-            if (fSum != OMV.Vector3.Zero)
-            {
-                if (PhysBody.HasPhysicalBody)
-                    BulletSimAPI.ApplyTorque2(PhysBody.ptr, fSum);
-                _torque = fSum;
-            }
-        });
     }
+
     // A torque impulse.
     // ApplyTorqueImpulse adds torque directly to the angularVelocity.
     // AddAngularForce accumulates the force and applied it to the angular velocity all at once.