BulletSim: a better version of llMoveToTarget that doesn't go crazy.

There is still some overshoot but mostly fixes Mantis 6693.
Fix bug where moveToTarget was active for non-physical objects
and while selected.
Fix bug where move target was not getting changed if the script
changed the target during a move.

5 files changed, 98 insertions, 19 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs b/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs
index 75ff24e..bdf4bc0 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSActorMoveToTarget.cs
@@ -50,7 +50,8 @@ public class BSActorMoveToTarget : BSActor
     // BSActor.isActive
     public override bool isActive
     {
-        get { return Enabled; }
+        // MoveToTarget only works on physical prims
+        get { return Enabled && m_controllingPrim.IsPhysicallyActive; }
     }
 
     // Release any connections and resources used by the actor.
@@ -102,16 +103,28 @@ public class BSActorMoveToTarget : BSActor
             // We're taking over after this.
             m_controllingPrim.ZeroMotion(true);
 
-            m_targetMotor = new BSVMotor("BSActorMoveToTargget.Activate",
-                                        m_controllingPrim.MoveToTargetTau,                    // timeScale
-                                        BSMotor.Infinite,           // decay time scale
-                                        1f                          // efficiency
+            /* Someday use the PID controller
+            m_targetMotor = new BSPIDVMotor("BSActorMoveToTarget-" + m_controllingPrim.LocalID.ToString());
+            m_targetMotor.TimeScale = m_controllingPrim.MoveToTargetTau;
+            m_targetMotor.Efficiency = 1f;
+             */
+            m_targetMotor = new BSVMotor("BSActorMoveToTarget-" + m_controllingPrim.LocalID.ToString(),
+                                        m_controllingPrim.MoveToTargetTau,  // timeScale
+                                        BSMotor.Infinite,                   // decay time scale
+                                        1f                                  // efficiency
             );
             m_targetMotor.PhysicsScene = m_physicsScene; // DEBUG DEBUG so motor will output detail log messages.
             m_targetMotor.SetTarget(m_controllingPrim.MoveToTargetTarget);
             m_targetMotor.SetCurrent(m_controllingPrim.RawPosition);
 
-            m_physicsScene.BeforeStep += Mover;
+            // m_physicsScene.BeforeStep += Mover;
+            m_physicsScene.BeforeStep += Mover2;
+        }
+        else
+        {
+            // If already allocated, make sure the target and other paramters are current
+            m_targetMotor.SetTarget(m_controllingPrim.MoveToTargetTarget);
+            m_targetMotor.SetCurrent(m_controllingPrim.RawPosition);
         }
     }
 
@@ -119,12 +132,16 @@ public class BSActorMoveToTarget : BSActor
     {
         if (m_targetMotor != null)
         {
-            m_physicsScene.BeforeStep -= Mover;
+            // m_physicsScene.BeforeStep -= Mover;
+            m_physicsScene.BeforeStep -= Mover2;
             m_targetMotor = null;
         }
     }
 
-    // Called just before the simulation step. Update the vertical position for hoverness.
+    // Origional mover that set the objects position to move to the target.
+    // The problem was that gravity would keep trying to push the object down so
+    //    the overall downward velocity would increase to infinity.
+    // Called just before the simulation step.
     private void Mover(float timeStep)
     {
         // Don't do hovering while the object is selected.
@@ -142,6 +159,7 @@ public class BSActorMoveToTarget : BSActor
             m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover,zeroMovement,movePos={1},pos={2},mass={3}",
                                     m_controllingPrim.LocalID, movePosition, m_controllingPrim.RawPosition, m_controllingPrim.Mass);
             m_controllingPrim.ForcePosition = m_targetMotor.TargetValue;
+            m_controllingPrim.ForceVelocity = OMV.Vector3.Zero;
             // Setting the position does not cause the physics engine to generate a property update. Force it.
             m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody);
         }
@@ -151,7 +169,51 @@ public class BSActorMoveToTarget : BSActor
             // Setting the position does not cause the physics engine to generate a property update. Force it.
             m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody);
         }
-        m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover,move,fromPos={1},movePos={2}", m_controllingPrim.LocalID, origPosition, movePosition);
+        m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover,move,fromPos={1},movePos={2}",
+                                        m_controllingPrim.LocalID, origPosition, movePosition);
+    }
+
+    // Version of mover that applies forces to move the physical object to the target.
+    // Also overcomes gravity so the object doesn't just drop to the ground.
+    // Called just before the simulation step.
+    private void Mover2(float timeStep)
+    {
+        // Don't do hovering while the object is selected.
+        if (!isActive)
+            return;
+
+        OMV.Vector3 origPosition = m_controllingPrim.RawPosition;     // DEBUG DEBUG (for printout below)
+        OMV.Vector3 addedForce = OMV.Vector3.Zero;
+
+        // CorrectionVector is the movement vector required this step
+        OMV.Vector3 correctionVector = m_targetMotor.Step(timeStep, m_controllingPrim.RawPosition);
+
+        // If we are very close to our target, turn off the movement motor.
+        if (m_targetMotor.ErrorIsZero())
+        {
+            m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover3,zeroMovement,pos={1},mass={2}",
+                                    m_controllingPrim.LocalID, m_controllingPrim.RawPosition, m_controllingPrim.Mass);
+            m_controllingPrim.ForcePosition = m_targetMotor.TargetValue;
+            m_controllingPrim.ForceVelocity = OMV.Vector3.Zero;
+            // Setting the position does not cause the physics engine to generate a property update. Force it.
+            m_physicsScene.PE.PushUpdate(m_controllingPrim.PhysBody);
+        }
+        else
+        {
+            // First force to move us there -- the motor return a timestep scaled value.
+            addedForce = correctionVector / timeStep;
+            // Remove the existing velocity (only the moveToTarget force counts)
+            addedForce -= m_controllingPrim.RawVelocity;
+            // Overcome gravity. 
+            addedForce -= m_controllingPrim.Gravity;
+
+            // Add enough force to overcome the mass of the object
+            addedForce *= m_controllingPrim.Mass;
+
+            m_controllingPrim.AddForce(addedForce, false /* pushForce */, true /* inTaintTime */);
+        }
+        m_physicsScene.DetailLog("{0},BSActorMoveToTarget.Mover3,move,fromPos={1},addedForce={2}",
+                                        m_controllingPrim.LocalID, origPosition, addedForce);
     }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 48f842e..5ef6992 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -626,7 +626,7 @@ public sealed class BSCharacter : BSPhysObject
         OMV.Vector3 addForce = force / PhysScene.LastTimeStep;
         AddForce(addForce, pushforce, false);
     }
-    private void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
+    public override void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
         if (force.IsFinite())
         {
             OMV.Vector3 addForce = Util.ClampV(force, BSParam.MaxAddForceMagnitude);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index ef662b5..1214703 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -144,7 +144,6 @@ public class BSVMotor : BSMotor
 
         Vector3 correction = Vector3.Zero;
         Vector3 error = TargetValue - CurrentValue;
-        LastError = error;
         if (!ErrorIsZero(error))
         {
             correction = StepError(timeStep, error);
@@ -179,6 +178,7 @@ public class BSVMotor : BSMotor
             MDetailLog("{0},  BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}",
                         BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue);
         }
+        LastError = error;
 
         return correction;
     }
@@ -293,7 +293,6 @@ public class BSFMotor : BSMotor
 
         float correction = 0f;
         float error = TargetValue - CurrentValue;
-        LastError = error;
         if (!ErrorIsZero(error))
         {
             correction = StepError(timeStep, error);
@@ -328,6 +327,7 @@ public class BSFMotor : BSMotor
             MDetailLog("{0},  BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
                         BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
         }
+        LastError = error;
 
         return CurrentValue;
     }
@@ -363,7 +363,7 @@ public class BSFMotor : BSMotor
 
 // ============================================================================
 // ============================================================================
-// Proportional, Integral, Derivitive Motor
+// Proportional, Integral, Derivitive ("PID") Motor
 // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
 public class BSPIDVMotor : BSVMotor
 {
@@ -434,15 +434,14 @@ public class BSPIDVMotor : BSVMotor
 
         // A simple derivitive is the rate of change from the last error.
         Vector3 derivitive = (error - LastError) * timeStep;
-        LastError = error;
 
         // Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError)
-        Vector3 ret   =   error * timeStep   * proportionFactor * FactorMix.X
-                        + RunningIntegration * integralFactor   * FactorMix.Y
-                        + derivitive         * derivFactor      * FactorMix.Z
+        Vector3 ret   =   error / TimeScale * timeStep   * proportionFactor * FactorMix.X
+                        + RunningIntegration / TimeScale * integralFactor   * FactorMix.Y
+                        + derivitive / TimeScale         * derivFactor      * FactorMix.Z
                         ;
 
-        MDetailLog("{0},BSPIDVMotor.step,ts={1},err={2},runnInt={3},deriv={4},ret={5}",
+        MDetailLog("{0},  BSPIDVMotor.step,ts={1},err={2},runnInt={3},deriv={4},ret={5}",
                         BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivitive, ret);
 
         return ret;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index a4c5e08..a0d5c42 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -210,6 +210,7 @@ public abstract class BSPhysObject : PhysicsActor
         AddAngularForce(force, pushforce, false);
     }
     public abstract void AddAngularForce(OMV.Vector3 force, bool pushforce, bool inTaintTime);
+    public abstract void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime);
 
     public abstract OMV.Vector3 ForceRotationalVelocity { get; set; }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 95bdc7b..90f74df 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -450,6 +450,9 @@ public class BSPrim : BSPhysObject
                 Gravity = ComputeGravity(Buoyancy);
                 PhysScene.PE.SetGravity(PhysBody, Gravity);
 
+                OMV.Vector3 currentScale = PhysScene.PE.GetLocalScaling(PhysShape.physShapeInfo);   // DEBUG DEBUG
+                DetailLog("{0},BSPrim.UpdateMassProperties,currentScale{1},shape={2}", LocalID, currentScale, PhysShape.physShapeInfo);   // DEBUG DEBUG
+
                 Inertia = PhysScene.PE.CalculateLocalInertia(PhysShape.physShapeInfo, physMass);
                 PhysScene.PE.SetMassProps(PhysBody, physMass, Inertia);
                 PhysScene.PE.UpdateInertiaTensor(PhysBody);
@@ -1040,6 +1043,20 @@ public class BSPrim : BSPhysObject
         }
     }
 
+    public override OMV.Vector3 PIDTarget
+    {
+        set
+        {
+            base.PIDTarget = value;
+            BSActor actor;
+            if (PhysicalActors.TryGetActor(MoveToTargetActorName, out actor))
+            {
+                // if the actor exists, tell it to refresh its values.
+                actor.Refresh();
+            }
+            
+        }
+    }
     // Used for llSetHoverHeight and maybe vehicle height
     // Hover Height will override MoveTo target's Z
     public override bool PIDHoverActive {
@@ -1063,7 +1080,7 @@ public class BSPrim : BSPhysObject
 
     // 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) {
+    public override void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
         // for an object, doesn't matter if force is a pushforce or not
         if (IsPhysicallyActive)
         {