Fixes Mantis #3260. Thank you kindly, MCortez for a patch that:

llSetHoverHeight() should not clamp the x/y position of an object the way MoveTo does, 
and it should recalculate the absolute height to hover at as an object moves to reflect 
the current ground/water height under it.
Correctly implementing required adjusting the Physics interfaces and implementing at 
the physics plug-in level. The attached is a patch that correctly implements 
llSetHoverHeight() including updates to the ODE physics plug-in.

9 files changed, 177 insertions, 2 deletions

diff --git a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
index 3081077..284837b 100644
--- a/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
+++ b/OpenSim/Region/Physics/BasicPhysicsPlugin/BasicPhysicsPlugin.cs
@@ -447,9 +447,17 @@ namespace OpenSim.Region.Physics.BasicPhysicsPlugin
         public override void CrossingFailure()
         {
         }
+
         public override PhysicsVector PIDTarget { set { return; } }
         public override bool PIDActive { set { return; } }
         public override float PIDTau { set { return; } }
+
+        public override float PIDHoverHeight { set { return; } }
+        public override bool PIDHoverActive { set { return; } }
+        public override PIDHoverType PIDHoverType { set { return; } }
+        public override float PIDHoverTau { set { return; } }
+
+
         public override void SubscribeEvents(int ms)
         {
 
diff --git a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
index 8891163..ea72c8c 100644
--- a/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
+++ b/OpenSim/Region/Physics/BulletXPlugin/BulletXPlugin.cs
@@ -1225,6 +1225,13 @@ namespace OpenSim.Region.Physics.BulletXPlugin
         public override PhysicsVector PIDTarget { set { return; } }
         public override bool PIDActive { set { return; } }
         public override float PIDTau { set { return; } }
+
+        public override float PIDHoverHeight { set { return; } }
+        public override bool PIDHoverActive { set { return; } }
+        public override PIDHoverType PIDHoverType { set { return; } }
+        public override float PIDHoverTau { set { return; } }
+
+
         public override void SubscribeEvents(int ms)
         {
 
diff --git a/OpenSim/Region/Physics/Manager/PhysicsActor.cs b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
index 0f75c46..c4bb889 100644
--- a/OpenSim/Region/Physics/Manager/PhysicsActor.cs
+++ b/OpenSim/Region/Physics/Manager/PhysicsActor.cs
@@ -44,6 +44,14 @@ namespace OpenSim.Region.Physics.Manager
         Ground = 3
     }
 
+    public enum PIDHoverType
+    {
+        Ground
+        , GroundAndWater
+        , Water
+        , Absolute
+    }
+
     public class CollisionEventUpdate : EventArgs
     {
         // Raising the event on the object, so don't need to provide location..  further up the tree knows that info.
@@ -204,9 +212,20 @@ namespace OpenSim.Region.Physics.Manager
         public abstract PhysicsVector RotationalVelocity { get; set; }
         public abstract bool Kinematic { get; set; }
         public abstract float Buoyancy { get; set; }
+
+        // Used for MoveTo
         public abstract PhysicsVector PIDTarget { set;}
-        public abstract bool PIDActive { set;}
+        public abstract bool  PIDActive { set;}
         public abstract float PIDTau { set; }
+
+        // Used for llSetHoverHeight and maybe vehicle height
+        // Hover Height will override MoveTo target's Z
+        public abstract bool PIDHoverActive { set;}
+        public abstract float PIDHoverHeight { set;}
+        public abstract PIDHoverType PIDHoverType { set;}
+        public abstract float PIDHoverTau { set;}
+
+
         public abstract void AddForce(PhysicsVector force, bool pushforce);
         public abstract void AddAngularForce(PhysicsVector force, bool pushforce);
         public abstract void SetMomentum(PhysicsVector momentum);
@@ -430,6 +449,11 @@ namespace OpenSim.Region.Physics.Manager
         public override bool PIDActive { set { return; } }
         public override float PIDTau { set { return; } }
 
+        public override float PIDHoverHeight { set { return; } }
+        public override bool PIDHoverActive { set { return; } }
+        public override PIDHoverType PIDHoverType { set { return; } }
+        public override float PIDHoverTau { set { return; } }
+
         public override void SetMomentum(PhysicsVector momentum)
         {
         }
diff --git a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
index 9991ab1..a62409c 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODECharacter.cs
@@ -922,9 +922,16 @@ namespace OpenSim.Region.Physics.OdePlugin
         public override void CrossingFailure()
         {
         }
+
         public override PhysicsVector PIDTarget { set { return; } }
         public override bool PIDActive { set { return; } }
         public override float PIDTau { set { return; } }
+
+        public override float PIDHoverHeight { set { return; } }
+        public override bool PIDHoverActive { set { return; } }
+        public override PIDHoverType PIDHoverType { set { return; } }
+        public override float PIDHoverTau { set { return; } }
+
         public override void SubscribeEvents(int ms)
         {
             m_eventsubscription = ms;
diff --git a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
index ae0bb11..f164048 100644
--- a/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
+++ b/OpenSim/Region/Physics/OdePlugin/ODEPrim.cs
@@ -69,11 +69,20 @@ namespace OpenSim.Region.Physics.OdePlugin
         private float m_PIDTau = 0f;
         private float PID_D = 35f;
         private float PID_G = 25f;
+        private bool m_usePID = false;
+
+        private float m_PIDHoverHeight = 0f;
+        private float m_PIDHoverTau = 0f;
+        private bool m_useHoverPID = false;
+        private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
+        private float m_targetHoverHeight = 0f;
+        private float m_groundHeight = 0f;
+        private float m_waterHeight = 0f;
+
         private float m_tensor = 5f;
         private int body_autodisable_frames = 20;
         private IMesh primMesh = null;
 
-        private bool m_usePID = false;
 
         private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
                                                         | CollisionCategories.Space
@@ -1554,6 +1563,91 @@ namespace OpenSim.Region.Physics.OdePlugin
                     }
                 }
 
+                // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
+                if (m_useHoverPID && !m_usePID)
+                {
+                    // If we're using the PID controller, then we have no gravity
+                    fz = (-1 * _parent_scene.gravityz) * m_mass;
+
+                    //  no lock; for now it's only called from within Simulate()
+
+                    // If the PID Controller isn't active then we set our force
+                    // calculating base velocity to the current position
+
+                    if ((m_PIDTau < 1))
+                    {
+                        PID_G = PID_G / m_PIDTau;
+                    }
+
+                    if ((PID_G - m_PIDTau) <= 0)
+                    {
+                        PID_G = m_PIDTau + 1;
+                    }
+                    
+
+                    // Where are we, and where are we headed?
+                    d.Vector3 pos = d.BodyGetPosition(Body);
+                    d.Vector3 vel = d.BodyGetLinearVel(Body);
+
+                    // determine what our target height really is based on HoverType
+                    switch (m_PIDHoverType)
+                    {
+                        case PIDHoverType.Absolute:
+                            m_targetHoverHeight = m_PIDHoverHeight;
+                            break;
+                        case PIDHoverType.Ground:
+                            m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                            m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                            break;
+                        case PIDHoverType.GroundAndWater:
+                            m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                            m_waterHeight  = _parent_scene.GetWaterLevel();
+                            if (m_groundHeight > m_waterHeight)
+                            {
+                                m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
+                            }
+                            else
+                            {
+                                m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
+                            }
+                            break;
+                        case PIDHoverType.Water:
+                            m_waterHeight = _parent_scene.GetWaterLevel();
+                            m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
+                            break;
+                    }
+
+
+                    _target_velocity =
+                        new PhysicsVector(0.0f, 0.0f,
+                            (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
+                            );
+
+                    //  if velocity is zero, use position control; otherwise, velocity control
+
+                    if (_target_velocity.IsIdentical(PhysicsVector.Zero, 0.1f))
+                    {
+                        //  keep track of where we stopped.  No more slippin' & slidin'
+
+                        // We only want to deactivate the PID Controller if we think we want to have our surrogate
+                        // react to the physics scene by moving it's position.
+                        // Avatar to Avatar collisions
+                        // Prim to avatar collisions
+
+                        d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
+                        d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
+                        d.BodyAddForce(Body, 0, 0, fz);
+                        return;
+                    }
+                    else
+                    {
+                        _zeroFlag = false;
+
+                        // We're flying and colliding with something
+                        fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
+                    }
+                }
+
                 fx *= m_mass;
                 fy *= m_mass;
                 //fz *= m_mass;
@@ -2622,6 +2716,11 @@ namespace OpenSim.Region.Physics.OdePlugin
         public override bool PIDActive { set { m_usePID = value; } }
         public override float PIDTau { set { m_PIDTau = value; } }
 
+        public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
+        public override bool PIDHoverActive { set { m_useHoverPID = value; } }
+        public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
+        public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
+
         private void createAMotor(PhysicsVector axis)
         {
             if (Body == IntPtr.Zero)
diff --git a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
index 359e7b3..b94f374 100644
--- a/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
+++ b/OpenSim/Region/Physics/OdePlugin/OdePlugin.cs
@@ -3031,6 +3031,11 @@ namespace OpenSim.Region.Physics.OdePlugin
         {
         }
 
+        public float GetWaterLevel()
+        {
+            return waterlevel;
+        }
+
         public override void SetWaterLevel(float baseheight)
         {
             waterlevel = baseheight;
diff --git a/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs b/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs
index 4230a57..0320931 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSCharacter.cs
@@ -288,6 +288,12 @@ namespace OpenSim.Region.Physics.POSPlugin
             set { return; }
         }
 
+        public override float PIDHoverHeight { set { return; } }
+        public override bool PIDHoverActive { set { return; } }
+        public override PIDHoverType PIDHoverType { set { return; } }
+        public override float PIDHoverTau { set { return; } }
+
+
         public override void SubscribeEvents(int ms)
         {
         }
diff --git a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
index bf96c35..526a2c6 100644
--- a/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
+++ b/OpenSim/Region/Physics/POSPlugin/POSPrim.cs
@@ -283,6 +283,12 @@ namespace OpenSim.Region.Physics.POSPlugin
             set { return; }
         }
 
+        public override float PIDHoverHeight { set { return; } }
+        public override bool PIDHoverActive { set { return; } }
+        public override PIDHoverType PIDHoverType { set { return; } }
+        public override float PIDHoverTau { set { return; } }
+
+
         public override void SubscribeEvents(int ms)
         {
         }
diff --git a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
index 120eed9..7bbca9d 100644
--- a/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
+++ b/OpenSim/Region/Physics/PhysXPlugin/PhysXPlugin.cs
@@ -490,9 +490,16 @@ namespace OpenSim.Region.Physics.PhysXPlugin
         {
 
         }
+
         public override PhysicsVector PIDTarget { set { return; } }
         public override bool PIDActive { set { return; } }
         public override float PIDTau { set { return; } }
+
+        public override float PIDHoverHeight { set { return; } }
+        public override bool PIDHoverActive { set { return; } }
+        public override PIDHoverType PIDHoverType { set { return; } }
+        public override float PIDHoverTau { set { return; } }
+
         public override void SubscribeEvents(int ms)
         {
 
@@ -765,10 +772,16 @@ namespace OpenSim.Region.Physics.PhysXPlugin
         public override void CrossingFailure()
         {
         }
+
         public override PhysicsVector PIDTarget { set { return; } }
         public override bool PIDActive { set { return; } }
         public override float PIDTau { set { return; } }
 
+        public override float PIDHoverHeight { set { return; } }
+        public override bool PIDHoverActive { set { return; } }
+        public override PIDHoverType PIDHoverType { set { return; } }
+        public override float PIDHoverTau { set { return; } }
+
         public override void SubscribeEvents(int ms)
         {