1 files changed, 31 insertions, 26 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 13c2539..c34c05a 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -137,6 +137,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             get { return Type != Vehicle.TYPE_NONE && Prim.IsPhysical; }
         }
 
+        #region Vehicle parameter setting
         internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
         {
             VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", Prim.LocalID, pParam, pValue);
@@ -546,6 +547,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_verticalAttractionMotor.FrictionTimescale = new Vector3(BSMotor.Infinite, BSMotor.Infinite, 0.1f);
             m_verticalAttractionMotor.PhysicsScene = PhysicsScene;  // DEBUG DEBUG DEBUG (enables detail logging)
         }
+        #endregion // Vehicle parameter setting
 
         // Some of the properties of this prim may have changed.
         // Do any updating needed for a vehicle
@@ -854,6 +856,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // The movement computed in the linear motor is relative to the vehicle
             //     coordinates. Rotate the movement to world coordinates.
             linearMotorContribution *= VehicleOrientation;
+            // All the contributions after this are world relative (mostly Z modifications)
 
             // ==================================================================
             // Buoyancy: force to overcome gravity.
@@ -925,7 +928,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // TODO: Consider taking the rotated size of the object or possibly casting a ray.
             if (VehiclePosition.Z < GetTerrainHeight(VehiclePosition))
             {
-                // TODO: correct position by applying force rather than forcing position.
+                // Force position because applying force won't get the vehicle through the terrain
                 Vector3 newPosition = VehiclePosition;
                 newPosition.Z = GetTerrainHeight(VehiclePosition) + 1f;
                 VehiclePosition = newPosition;
@@ -980,14 +983,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
 
                     // TODO: implement m_VhoverEfficiency correctly
-                    if (Math.Abs(verticalError) > m_VhoverEfficiency)
-                    {
-                        ret = new Vector3(0f, 0f, verticalCorrectionVelocity);
-                    }
+                    ret = new Vector3(0f, 0f, verticalCorrectionVelocity);
                 }
 
-                VDetailLog("{0},  MoveLinear,hover,pos={1},ret={2},hoverTS={3},height={4},target={5}",
-                                Prim.LocalID, VehiclePosition, ret, m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight);
+                VDetailLog("{0},  MoveLinear,hover,pos={1},eff={2},hoverTS={3},height={4},target={5},ret={6}",
+                                Prim.LocalID, VehiclePosition, m_VhoverEfficiency, m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight, ret);
             }
 
             return ret;
@@ -1109,6 +1109,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                                     + deflectionContribution
                                     + bankingContribution;
 
+            // Add of the above computation are made relative to vehicle coordinates.
+            // Convert to world coordinates.
+            m_lastAngularVelocity *= VehicleOrientation;
+
             // ==================================================================
             // Apply the correction velocity.
             // TODO: Should this be applied as an angular force (torque)?
@@ -1220,19 +1224,21 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         public Vector3 ComputeAngularDeflection()
         {
             Vector3 ret = Vector3.Zero;
-            return ret;         // DEBUG DEBUG DEBUG
-            // Disable angular deflection for the moment.
+
             // Since angularMotorUp and angularDeflection are computed independently, they will calculate
             //     approximately the same X or Y correction. When added together (when contributions are combined)
             //     this creates an over-correction and then wabbling as the target is overshot.
             // TODO: rethink how the different correction computations inter-relate.
 
-            if (m_angularDeflectionEfficiency != 0)
+            if (m_angularDeflectionEfficiency != 0 && VehicleVelocity != Vector3.Zero)
             {
                 // The direction the vehicle is moving
                 Vector3 movingDirection = VehicleVelocity;
                 movingDirection.Normalize();
 
+                // If the vehicle is going backward, it is still pointing forward
+                movingDirection *= Math.Sign(VehicleForwardSpeed);
+
                 // The direction the vehicle is pointing
                 Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
                 pointingDirection.Normalize();
@@ -1241,6 +1247,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 Vector3 deflectionError = movingDirection - pointingDirection;
 
                 // Don't try to correct very large errors (not our job)
+                // if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = PIOverTwo * Math.Sign(deflectionError.X);
+                // if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = PIOverTwo * Math.Sign(deflectionError.Y);
+                // if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = PIOverTwo * Math.Sign(deflectionError.Z);
                 if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = 0f;
                 if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = 0f;
                 if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = 0f;
@@ -1296,33 +1305,29 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
             if (m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
             {
-                // This works by rotating a unit vector to the orientation of the vehicle. The
-                //    roll (tilt) will be Y component of a tilting Z vector (zero for no tilt
-                //    up to one for full over).
+                // Rotate a UnitZ vector (pointing up) to how the vehicle is oriented.
+                // As the vehicle rolls to the right or left, the Y value will increase from
+                //     zero (straight up) to 1 or -1 (full tilt right  or left)
                 Vector3 rollComponents = Vector3.UnitZ * VehicleOrientation;
-
+                
                 // Figure out the yaw value for this much roll.
-                float turnComponent = rollComponents.Y * rollComponents.Y * m_bankingEfficiency;
-                // Keep the sign
-                if (rollComponents.Y < 0f)
-                    turnComponent = -turnComponent;
-
-                // TODO: there must be a better computation of the banking force.
-                float bankingTurnForce = turnComponent;
+                // Squared because that seems to give a good value
+                float yawAngle = (float)Math.Asin(rollComponents.Y * rollComponents.Y) * m_bankingEfficiency;
 
                 //        actual error  =       static turn error            +           dynamic turn error
-                float mixedBankingError = bankingTurnForce * (1f - m_bankingMix) + bankingTurnForce * m_bankingMix * VehicleForwardSpeed;
+                float mixedYawAngle = yawAngle * (1f - m_bankingMix) + yawAngle * m_bankingMix * VehicleForwardSpeed;
+
                 // TODO: the banking effect should not go to infinity but what to limit it to?
-                mixedBankingError = ClampInRange(-20f, mixedBankingError, 20f);
+                mixedYawAngle = ClampInRange(-20f, mixedYawAngle, 20f);
 
                 // Build the force vector to change rotation from what it is to what it should be
-                ret.Z = -mixedBankingError;
+                ret.Z = -mixedYawAngle;
 
                 // Don't do it all at once.
                 ret /= m_bankingTimescale;
 
-                VDetailLog("{0},  MoveAngular,Banking,rollComp={1},speed={2},turnComp={3},bankErr={4},mixedBankErr={5},ret={6}",
-                            Prim.LocalID, rollComponents, VehicleForwardSpeed, turnComponent, bankingTurnForce, mixedBankingError, ret);
+                VDetailLog("{0},  MoveAngular,Banking,rollComp={1},speed={2},rollComp={3},yAng={4},mYAng={5},ret={6}",
+                            Prim.LocalID, rollComponents, VehicleForwardSpeed, rollComponents, yawAngle, mixedYawAngle, ret);
             }
             return ret;
         }