BulletSim: remove time scaling of computed vehicle absolute velocity since Bullet will scale the movement by the time slice. Restore LIMIT_MOTOR_UP to definitition of BOAT simce some vehicle engines use it even for land vehicles. Push vehicle parameter updates through the regular property update to solve vehicles floating off when they should be stopped.

3 files changed, 73 insertions, 48 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index fcee1de..fcc1224 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -127,6 +127,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private float m_verticalAttractionEfficiency = 1.0f;        // damped
         private float m_verticalAttractionTimescale = 500f;         // Timescale > 300  means no vert attractor.
 
+        // Local
+        private float m_knownTerrainHeight;
+        private float m_knownWaterLevel;
+
         public BSDynamics(BSScene myScene, BSPrim myPrim)
         {
             PhysicsScene = myScene;
@@ -443,9 +447,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY
                                     | VehicleFlag.HOVER_GLOBAL_HEIGHT
                                     | VehicleFlag.LIMIT_ROLL_ONLY
-                                    | VehicleFlag.LIMIT_MOTOR_UP
                                     | VehicleFlag.HOVER_UP_ONLY);
                     m_flags |= (VehicleFlag.NO_DEFLECTION_UP
+                                    | VehicleFlag.LIMIT_MOTOR_UP
                                     | VehicleFlag.HOVER_WATER_ONLY);
                     break;
                 case Vehicle.TYPE_AIRPLANE:
@@ -596,11 +600,32 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             Refresh();
         }
 
+        // Since the computation of terrain height can be a little involved, this routine
+        //    is used ot fetch the height only once for each vehicle simulation step.
+        private float GetTerrainHeight(Vector3 pos)
+        {
+            if (m_knownTerrainHeight == float.MinValue)
+                m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
+            return m_knownTerrainHeight;
+        }
+
+        // Since the computation of water level can be a little involved, this routine
+        //    is used ot fetch the level only once for each vehicle simulation step.
+        private float GetWaterLevel(Vector3 pos)
+        {
+            if (m_knownWaterLevel == float.MinValue)
+                m_knownWaterLevel = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos);
+            return m_knownWaterLevel;
+        }
+
         // One step of the vehicle properties for the next 'pTimestep' seconds.
         internal void Step(float pTimestep)
         {
             if (!IsActive) return;
 
+            // zap values so they will be fetched when needed
+            m_knownTerrainHeight = m_knownWaterLevel = float.MinValue;
+
             MoveLinear(pTimestep);
             MoveAngular(pTimestep);
 
@@ -609,6 +634,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // remember the position so next step we can limit absolute movement effects
             m_lastPositionVector = Prim.ForcePosition;
 
+            // Force the physics engine to decide whether values have updated.
+            // TODO: this is only necessary if pos, velocity, ... were updated. Is it quicker
+            //      to check for changes here or just push the update?
+            BulletSimAPI.PushUpdate2(Prim.PhysBody.ptr);
+
             VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
                     Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity);
         }
@@ -629,15 +659,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
 
             Vector3 pos = Prim.ForcePosition;
-            float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
 
-            Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep, ref pos, terrainHeight);
+            Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(ref pos);
 
-            Vector3 hoverContribution = ComputeLinearHover(pTimestep, ref pos, terrainHeight);
+            Vector3 hoverContribution = ComputeLinearHover(ref pos);
 
-            ComputeLinearBlockingEndPoint(pTimestep, ref pos);
+            ComputeLinearBlockingEndPoint(ref pos);
 
-            Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep, pos, terrainHeight);
+            Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pos);
 
             // ==================================================================
             Vector3 newVelocity = linearMotorContribution
@@ -665,42 +694,40 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 newVelocity = Vector3.Zero;
 
             // ==================================================================
-            // Stuff new linear velocity into the vehicle
+            // Stuff new linear velocity into the vehicle.
+            // Since the velocity is just being set, it is not scaled by pTimeStep. Bullet will do that for us.
             Prim.ForceVelocity = newVelocity;
-            // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false);    // DEBUG DEBUG
 
             // Other linear forces are applied as forces.
-            Vector3 totalDownForce = grav * m_vehicleMass;
+            Vector3 totalDownForce = grav * m_vehicleMass * pTimestep;
             if (totalDownForce != Vector3.Zero)
             {
                 Prim.AddForce(totalDownForce, false);
             }
 
-            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}",
-                                Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector,
-                                newVelocity, Prim.Velocity, totalDownForce);
+            VDetailLog("{0},MoveLinear,done,newVel={1},totDown={2},linContrib={3},terrContrib={4},hoverContrib={5},limitContrib={6}",
+                                Prim.LocalID, newVelocity, totalDownForce,
+                                linearMotorContribution, terrainHeightContribution, hoverContribution, limitMotorUpContribution
+            );
 
         } // end MoveLinear()
 
-        public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep, ref Vector3 pos, float terrainHeight)
+        public Vector3 ComputeLinearTerrainHeightCorrection(ref Vector3 pos)
         {
             Vector3 ret = Vector3.Zero;
             // If below the terrain, move us above the ground a little.
-            // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset.
-            //     TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
-            // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation;
-            // if (rotatedSize.Z < terrainHeight)
-            if (pos.Z < terrainHeight)
+            // TODO: Consider taking the rotated size of the object or possibly casting a ray.
+            if (pos.Z < GetTerrainHeight(pos))
             {
                 // TODO: correct position by applying force rather than forcing position.
-                pos.Z = terrainHeight + 2;
+                pos.Z = GetTerrainHeight(pos) + 2;
                 Prim.ForcePosition = pos;
-                VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
+                VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, GetTerrainHeight(pos), pos);
             }
             return ret;
         }
 
-        public Vector3 ComputeLinearHover(float pTimestep, ref Vector3 pos, float terrainHeight)
+        public Vector3 ComputeLinearHover(ref Vector3 pos)
         {
             Vector3 ret = Vector3.Zero;
 
@@ -711,11 +738,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // We should hover, get the target height
                 if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos) + m_VhoverHeight;
+                    m_VhoverTargetHeight = GetWaterLevel(pos) + m_VhoverHeight;
                 }
                 if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                 {
-                    m_VhoverTargetHeight = terrainHeight + m_VhoverHeight;
+                    m_VhoverTargetHeight = GetTerrainHeight(pos) + m_VhoverHeight;
                 }
                 if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                 {
@@ -739,16 +766,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
                 else
                 {
-                    float verticalError = pos.Z - m_VhoverTargetHeight;
-                    float verticalCorrectionVelocity = pTimestep * (verticalError / m_VhoverTimescale);
+                    // Error is positive if below the target and negative if above.
+                    float verticalError = m_VhoverTargetHeight - pos.Z;
+                    float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
 
-                    // TODO: implement m_VhoverEfficiency
-                    if (verticalError > 0.01f)
-                    {
-                        // If error is positive (we're above the target height), push down
-                        ret = new Vector3(0f, 0f, -verticalCorrectionVelocity);
-                    }
-                    else if (verticalError < -0.01)
+                    // TODO: implement m_VhoverEfficiency correctly
+                    if (Math.Abs(verticalError) > m_VhoverEfficiency)
                     {
                         ret = new Vector3(0f, 0f, verticalCorrectionVelocity);
                     }
@@ -761,7 +784,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             return ret;
         }
 
-        public bool ComputeLinearBlockingEndPoint(float pTimestep, ref Vector3 pos)
+        public bool ComputeLinearBlockingEndPoint(ref Vector3 pos)
         {
             bool changed = false;
 
@@ -810,13 +833,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //    VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering
         //    when they are in mid jump. 
         // TODO: this code is wrong. Also, what should it do for boats?
-        public Vector3 ComputeLinearMotorUp(float pTimestep, Vector3 pos, float terrainHeight)
+        public Vector3 ComputeLinearMotorUp(Vector3 pos)
         {
             Vector3 ret = Vector3.Zero;
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
                 // If the vehicle is motoring into the sky, get it going back down.
-                float distanceAboveGround = pos.Z - terrainHeight;
+                // float distanceAboveGround = pos.Z - Math.Max(GetTerrainHeight(pos), GetWaterLevel(pos));
+                float distanceAboveGround = pos.Z - GetTerrainHeight(pos);
                 if (distanceAboveGround > 1f)
                 {
                     // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
@@ -933,7 +957,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
                 // TODO: zeroing is good but it also sets values in unmanaged code. Remove the stores when idle.
-                VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
+                VDetailLog("{0},MoveAngular,done,zero,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
                 Prim.ZeroAngularMotion(true);
             }
             else
@@ -948,7 +972,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // Unscale the force by the angular factor so it overwhelmes the Bullet additions.
                 Prim.ForceRotationalVelocity = applyAngularForce;
 
-                VDetailLog("{0},MoveAngular,done,angMotor={1},vertAttr={2},bank={3},deflect={4},newAngForce={5},lastAngular={6}",
+                VDetailLog("{0},MoveAngular,done,nonZero,angMotor={1},vertAttr={2},bank={3},deflect={4},newAngForce={5},lastAngular={6}",
                                     Prim.LocalID,
                                     angularMotorContribution, verticalAttractionContribution,
                                     bankingContribution, deflectionContribution,
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index eca1452..b256887 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -101,6 +101,14 @@ public class BSVMotor : BSMotor
     {
         TargetValue = target;
     }
+
+    // A form of stepping that does not take the time quantum into account.
+    // The caller must do the right thing later.
+    public Vector3 Step()
+    {
+        return Step(1f);
+    }
+
     public Vector3 Step(float timeStep)
     {
         Vector3 returnCurrent = Vector3.Zero;
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 3fb0300..54b4167 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -348,7 +348,9 @@ public sealed class BSPrim : BSPhysObject
         if (ret)
         {
             // Apply upforce and overcome gravity.
-            AddForce(upForce - PhysicsScene.DefaultGravity, false, inTaintTime);
+            OMV.Vector3 correctionForce = upForce - PhysicsScene.DefaultGravity;
+            DetailLog("{0},BSPrim.PositionSanityCheck,applyForce,pos={1},upForce={2},correctionForce={3}", LocalID, _position, upForce, correctionForce);
+            AddForce(correctionForce, false, inTaintTime);
         }
         return ret;
     }
@@ -839,15 +841,6 @@ public sealed class BSPrim : BSPhysObject
     }
     public override OMV.Vector3 RotationalVelocity {
         get {
-            /*
-            OMV.Vector3 pv = OMV.Vector3.Zero;
-            // if close to zero, report zero
-            // This is copied from ODE but I'm not sure why it returns zero but doesn't
-            //    zero the property in the physics engine.
-            if (_rotationalVelocity.ApproxEquals(pv, 0.2f))
-                return pv;
-             */
-
             return _rotationalVelocity;
         }
         set {
@@ -1409,7 +1402,7 @@ public sealed class BSPrim : BSPhysObject
             LastEntityProperties = CurrentEntityProperties;
             CurrentEntityProperties = entprop;
 
-            OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;
+            OMV.Vector3 direction = OMV.Vector3.UnitX * _orientation;   // DEBUG DEBUG DEBUG
             DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},dir={3},vel={4},rotVel={5}",
                     LocalID, _position, _orientation, direction, _velocity, _rotationalVelocity);