BulletSim: reorganize linear movement routine into separate subroutines enabling external calibration routines and unit tests.

1 files changed, 79 insertions, 58 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index eb4d06a..74eb9ab 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -630,13 +630,64 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
             Vector3 grav = Prim.PhysicsScene.DefaultGravity * (1f - m_VehicleBuoyancy);
 
-            // Current vehicle position
             Vector3 pos = Prim.ForcePosition;
+            float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
+
+            Vector3 terrainHeightContribution = ComputeLinearTerrainHeightCorrection(pTimestep, ref pos, terrainHeight);
+
+            Vector3 hoverContribution = ComputeLinearHover(pTimestep, ref pos, terrainHeight);
+
+            ComputeLinearBlockingEndPoint(pTimestep, ref pos);
+
+            Vector3 limitMotorUpContribution = ComputeLinearMotorUp(pTimestep, pos, terrainHeight);
+
+            // ==================================================================
+            Vector3 newVelocity = linearMotorContribution
+                            + terrainHeightContribution
+                            + hoverContribution
+                            + limitMotorUpContribution;
+
+            // If not changing some axis, reduce out velocity
+            if ((m_flags & (VehicleFlag.NO_X)) != 0)
+                newVelocity.X = 0;
+            if ((m_flags & (VehicleFlag.NO_Y)) != 0)
+                newVelocity.Y = 0;
+            if ((m_flags & (VehicleFlag.NO_Z)) != 0)
+                newVelocity.Z = 0;
+
+            // ==================================================================
+            // Clamp REALLY high or low velocities
+            float newVelocityLengthSq = newVelocity.LengthSquared();
+            if (newVelocityLengthSq > 1e6f)
+            {
+                newVelocity /= newVelocity.Length();
+                newVelocity *= 1000f;
+            }
+            else if (newVelocityLengthSq < 1e-6f)
+                newVelocity = Vector3.Zero;
 
             // ==================================================================
-            Vector3 terrainHeightContribution = Vector3.Zero;
+            // Stuff new linear velocity into the vehicle
+            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;
+            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);
+
+        } // end MoveLinear()
+
+        public Vector3 ComputeLinearTerrainHeightCorrection(float pTimestep, ref Vector3 pos, float terrainHeight)
+        {
+            Vector3 ret = Vector3.Zero;
             // If below the terrain, move us above the ground a little.
-            float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
             // 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;
@@ -648,10 +699,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 Prim.ForcePosition = pos;
                 VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
             }
+            return ret;
+        }
+
+        public Vector3 ComputeLinearHover(float pTimestep, ref Vector3 pos, float terrainHeight)
+        {
+            Vector3 ret = Vector3.Zero;
 
-            // ==================================================================
-            Vector3 hoverContribution = Vector3.Zero;
-            // Check if hovering
             // m_VhoverEfficiency: 0=bouncy, 1=totally damped
             // m_VhoverTimescale: time to achieve height
             if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
@@ -692,24 +746,29 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     // Replace Vertical speed with correction figure if significant
                     if (verticalError > 0.01f)
                     {
-                        hoverContribution = new Vector3(0f, 0f, verticalCorrectionVelocity);
+                        ret = new Vector3(0f, 0f, verticalCorrectionVelocity);
                         //KF: m_VhoverEfficiency is not yet implemented
                     }
                     else if (verticalError < -0.01)
                     {
-                        hoverContribution = new Vector3(0f, 0f, -verticalCorrectionVelocity);
+                        ret = new Vector3(0f, 0f, -verticalCorrectionVelocity);
                     }
                 }
 
                 VDetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}",
-                                Prim.LocalID, pos, hoverContribution, m_VhoverHeight, m_VhoverTargetHeight);
+                                Prim.LocalID, pos, ret, m_VhoverHeight, m_VhoverTargetHeight);
             }
 
-            // ==================================================================
+            return ret;
+        }
+
+        public bool ComputeLinearBlockingEndPoint(float pTimestep, ref Vector3 pos)
+        {
+            bool changed = false;
+
             Vector3 posChange = pos - m_lastPositionVector;
             if (m_BlockingEndPoint != Vector3.Zero)
             {
-                bool changed = false;
                 if (pos.X >= (m_BlockingEndPoint.X - (float)1))
                 {
                     pos.X -= posChange.X + 1;
@@ -742,9 +801,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                                 Prim.LocalID, m_BlockingEndPoint, posChange, pos);
                 }
             }
+            return changed;
+        }
 
-            // ==================================================================
-            Vector3 limitMotorUpContribution = Vector3.Zero;
+        public Vector3 ComputeLinearMotorUp(float pTimestep, Vector3 pos, float terrainHeight)
+        {
+            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.
@@ -753,58 +815,17 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 {
                     // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
                     // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
-                    limitMotorUpContribution = new Vector3(0, 0, -distanceAboveGround);
+                    ret = new Vector3(0, 0, -distanceAboveGround);
                 }
                 // TODO: this calculation is all wrong. From the description at
                 //     (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
                 //     has a decay factor. This says this force should
                 //     be computed with a motor.
                 VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}",
-                                    Prim.LocalID, distanceAboveGround, limitMotorUpContribution);
+                                    Prim.LocalID, distanceAboveGround, ret);
             }
-
-            // ==================================================================
-            Vector3 newVelocity = linearMotorContribution
-                            + terrainHeightContribution
-                            + hoverContribution
-                            + limitMotorUpContribution;
-
-            // If not changing some axis, reduce out velocity
-            if ((m_flags & (VehicleFlag.NO_X)) != 0)
-                newVelocity.X = 0;
-            if ((m_flags & (VehicleFlag.NO_Y)) != 0)
-                newVelocity.Y = 0;
-            if ((m_flags & (VehicleFlag.NO_Z)) != 0)
-                newVelocity.Z = 0;
-
-            // ==================================================================
-            // Clamp REALLY high or low velocities
-            float newVelocityLengthSq = newVelocity.LengthSquared();
-            if (newVelocityLengthSq > 1e6f)
-            {
-                newVelocity /= newVelocity.Length();
-                newVelocity *= 1000f;
-            }
-            else if (newVelocityLengthSq < 1e-6f)
-                newVelocity = Vector3.Zero;
-
-            // ==================================================================
-            // Stuff new linear velocity into the vehicle
-            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;
-            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);
-
-        } // end MoveLinear()
+            return ret;
+        }
 
         // =======================================================================
         // =======================================================================