1 files changed, 90 insertions, 55 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 5887249..a5acd86 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -91,6 +91,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private Vector3 m_lastVertAttractor = Vector3.Zero;             // what VA was last applied to body
 
         //Deflection properties
+        private BSVMotor m_angularDeflectionMotor = new BSVMotor("AngularDeflection");
         private float m_angularDeflectionEfficiency = 0;
         private float m_angularDeflectionTimescale = 0;
         private float m_linearDeflectionEfficiency = 0;
@@ -102,6 +103,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private float m_bankingTimescale = 0;
 
         //Hover and Buoyancy properties
+        private BSVMotor m_hoverMotor = new BSVMotor("Hover");
         private float m_VhoverHeight = 0f;
         private float m_VhoverEfficiency = 0f;
         private float m_VhoverTimescale = 0f;
@@ -118,6 +120,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         // Timescale > cutoff  means no vert attractor.
         private float m_verticalAttractionTimescale = 510f;
 
+        // Just some recomputed constants:
+        static readonly float PIOverFour = ((float)Math.PI) / 4f;
+        static readonly float PIOverTwo = ((float)Math.PI) / 2f;
+
         public BSDynamics(BSScene myScene, BSPrim myPrim)
         {
             PhysicsScene = myScene;
@@ -563,9 +569,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // Vehicles report collision events so we know when it's on the ground
                 BulletSimAPI.AddToCollisionFlags2(Prim.PhysBody.ptr, CollisionFlags.BS_VEHICLE_COLLISIONS);
 
-                // DEBUG DEBUG DEBUG: use uniform inertia to smooth movement added by Bullet
-                // Vector3 localInertia = new Vector3(1f, 1f, 1f);
-                // Vector3 localInertia = new Vector3(m_vehicleMass, m_vehicleMass, m_vehicleMass);
                 Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(Prim.PhysShape.ptr, m_vehicleMass);
                 BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia);
                 BulletSimAPI.UpdateInertiaTensor2(Prim.PhysBody.ptr);
@@ -599,21 +602,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         #region Known vehicle value functions
         // Vehicle physical parameters that we buffer from constant getting and setting.
-        // The "m_known*" variables are initialized to 'null', fetched only if referenced
-        //      and stored back into the physics engine only if updated.
+        // The "m_known*" values are unknown until they are fetched and the m_knownHas flag is set.
+        //      Changing is remembered and the parameter is stored back into the physics engine only if updated.
         //      This does two things: 1) saves continuious calls into unmanaged code, and
         //      2) signals when a physics property update must happen back to the simulator
         //      to update values modified for the vehicle.
         private int m_knownChanged;
-        private float? m_knownTerrainHeight;
-        private float? m_knownWaterLevel;
-        private Vector3? m_knownPosition;
-        private Vector3? m_knownVelocity;
+        private int m_knownHas;
+        private float m_knownTerrainHeight;
+        private float m_knownWaterLevel;
+        private Vector3 m_knownPosition;
+        private Vector3 m_knownVelocity;
         private Vector3 m_knownForce;
-        private Quaternion? m_knownOrientation;
-        private Vector3? m_knownRotationalVelocity;
+        private Quaternion m_knownOrientation;
+        private Vector3 m_knownRotationalVelocity;
         private Vector3 m_knownRotationalForce;
-        private float? m_knownForwardSpeed;
+        private Vector3 m_knownForwardVelocity;    // vehicle relative forward speed
 
         private const int m_knownChangedPosition           = 1 << 0;
         private const int m_knownChangedVelocity           = 1 << 1;
@@ -621,18 +625,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private const int m_knownChangedOrientation        = 1 << 3;
         private const int m_knownChangedRotationalVelocity = 1 << 4;
         private const int m_knownChangedRotationalForce    = 1 << 5;
+        private const int m_knownChangedTerrainHeight      = 1 << 6;
+        private const int m_knownChangedWaterLevel         = 1 << 7;
+        private const int m_knownChangedForwardVelocity    = 1 << 8;
 
         private void ForgetKnownVehicleProperties()
         {
-            m_knownTerrainHeight = null;
-            m_knownWaterLevel = null;
-            m_knownPosition = null;
-            m_knownVelocity = null;
-            m_knownForce = Vector3.Zero;
-            m_knownOrientation = null;
-            m_knownRotationalVelocity = null;
-            m_knownRotationalForce = Vector3.Zero;
-            m_knownForwardSpeed = null;
+            m_knownHas = 0;
             m_knownChanged = 0;
         }
         private void PushKnownChanged()
@@ -671,17 +670,23 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //    is used ot fetch the height only once for each vehicle simulation step.
         private float GetTerrainHeight(Vector3 pos)
         {
-            if (m_knownTerrainHeight == null)
+            if ((m_knownHas & m_knownChangedTerrainHeight) == 0)
+            {
                 m_knownTerrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
-            return (float)m_knownTerrainHeight;
+                m_knownHas |= m_knownChangedTerrainHeight;
+            }
+            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 == null)
+            if ((m_knownHas & m_knownChangedWaterLevel) == 0)
+            {
                 m_knownWaterLevel = Prim.PhysicsScene.TerrainManager.GetWaterLevelAtXYZ(pos);
+                m_knownHas |= m_knownChangedWaterLevel;
+            }
             return (float)m_knownWaterLevel;
         }
 
@@ -689,8 +694,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             get
             {
-                if (m_knownPosition == null)
+                if ((m_knownHas & m_knownChangedPosition) == 0)
+                {
                     m_knownPosition = Prim.ForcePosition;
+                    m_knownHas |= m_knownChangedPosition;
+                }
                 return (Vector3)m_knownPosition;
             }
             set
@@ -704,8 +712,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             get
             {
-                if (m_knownOrientation == null)
+                if ((m_knownHas & m_knownChangedOrientation) == 0)
+                {
                     m_knownOrientation = Prim.ForceOrientation;
+                    m_knownHas |= m_knownChangedOrientation;
+                }
                 return (Quaternion)m_knownOrientation;
             }
             set
@@ -719,8 +730,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             get
             {
-                if (m_knownVelocity == null)
+                if ((m_knownHas & m_knownChangedVelocity) == 0)
+                {
                     m_knownVelocity = Prim.ForceVelocity;
+                    m_knownHas |= m_knownChangedVelocity;
+                }
                 return (Vector3)m_knownVelocity;
             }
             set
@@ -740,8 +754,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             get
             {
-                if (m_knownRotationalVelocity == null)
+                if ((m_knownHas & m_knownChangedRotationalVelocity) == 0)
+                {
                     m_knownRotationalVelocity = Prim.ForceRotationalVelocity;
+                    m_knownHas |= m_knownChangedRotationalVelocity;
+                }
                 return (Vector3)m_knownRotationalVelocity;
             }
             set
@@ -755,13 +772,24 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_knownRotationalForce += aForce;
             m_knownChanged |= m_knownChangedRotationalForce;
         }
+        // Vehicle relative forward velocity
+        private Vector3 VehicleForwardVelocity
+        {
+            get
+            {
+                if ((m_knownHas & m_knownChangedForwardVelocity) == 0)
+                {
+                    m_knownForwardVelocity = VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleOrientation));
+                    m_knownHas |= m_knownChangedForwardVelocity;
+                }
+                return (Vector3)m_knownForwardVelocity;
+            }
+        }
         private float VehicleForwardSpeed
         {
             get
             {
-                if (m_knownForwardSpeed == null)
-                    m_knownForwardSpeed = (VehicleVelocity * Quaternion.Inverse(VehicleOrientation)).X;
-                return (float)m_knownForwardSpeed;
+                return VehicleForwardVelocity.X;
             }
         }
 
@@ -832,13 +860,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // ==================================================================
             // Clamp high or low velocities
             float newVelocityLengthSq = newVelocity.LengthSquared();
-            // if (newVelocityLengthSq > 1e6f)
             if (newVelocityLengthSq > 1000f)
             {
                 newVelocity /= newVelocity.Length();
                 newVelocity *= 1000f;
             }
-            // else if (newVelocityLengthSq < 1e-6f)
             else if (newVelocityLengthSq < 0.001f)
                 newVelocity = Vector3.Zero;
 
@@ -1003,7 +1029,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // Not colliding if the vehicle is off the ground
                 if (!Prim.IsColliding)
                 {
-                    // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
                     // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
                     ret = new Vector3(0, 0, -distanceAboveGround);
                 }
@@ -1026,7 +1051,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         //     set directly on the vehicle.
         private void MoveAngular(float pTimestep)
         {
-            // The user wants how many radians per second angular change?
+            // The user wants this many radians per second angular change?
             Vector3 angularMotorContribution = m_angularMotor.Step(pTimestep);
 
             // ==================================================================
@@ -1135,31 +1160,26 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 //         zero and one.
                 // The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees.
 
+                // Y error means needed rotation around X axis and visa versa.
+                // Since the error goes from zero to one, the asin is the corresponding angle.
+                ret.X = (float)Math.Asin(verticalError.Y);
+                // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.)
+                ret.Y = -(float)Math.Asin(verticalError.X);
+
                 // If verticalError.Z is negative, the vehicle is upside down. Add additional push.
                 if (verticalError.Z < 0f)
                 {
-                    verticalError.X = 2f - verticalError.X;
-                    verticalError.Y = 2f - verticalError.Y;
+                    ret.X += PIOverFour;
+                    ret.Y += PIOverFour;
                 }
 
-                // Y error means needed rotation around X axis and visa versa.
-                ret.X =    verticalError.Y;
-                ret.Y =  - verticalError.X;
-                ret.Z = 0f;
-
-                // Scale the correction force by how far we're off from vertical.
-                // Z error of one says little error. As Z gets smaller, the vehicle is leaning farther over.
-                float clampedSqrZError = ClampInRange(0.01f, verticalError.Z * verticalError.Z, 1f);
-                float vertForce = 1f / clampedSqrZError;
-
-                ret *= vertForce;
-
-                // Correction happens over a number of seconds.
+                // 'ret' is now the necessary velocity to correct tilt in one second.
+                //     Correction happens over a number of seconds.
                 Vector3 unscaledContrib = ret;
                 ret /= m_verticalAttractionTimescale;
 
-                VDetailLog("{0},  MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},vertForce={3},eff={4},vertAttr={5}",
-                                Prim.LocalID, verticalError, unscaledContrib, vertForce, m_verticalAttractionEfficiency, ret);
+                VDetailLog("{0},  MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},eff={3},ts={4},vertAttr={5}",
+                                Prim.LocalID, verticalError, unscaledContrib, m_verticalAttractionEfficiency, m_verticalAttractionTimescale, ret);
             }
             return ret;
         }
@@ -1172,7 +1192,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         public Vector3 ComputeAngularDeflection()
         {
             Vector3 ret = Vector3.Zero;
-            return ret;     // DEBUG DEBUG DEBUG  debug one force at a time
+            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)
             {
@@ -1184,15 +1209,24 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
                 pointingDirection.Normalize();
 
-                // The difference between what is and what should be
+                // The difference between what is and what should be.
                 Vector3 deflectionError = movingDirection - pointingDirection;
 
+                // Don't try to correct very large errors (not our job)
+                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;
+
+                // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
+
                 // Scale the correction by recovery timescale and efficiency
-                ret = (-deflectionError * VehicleForwardSpeed) * m_angularDeflectionEfficiency;
+                ret = (-deflectionError) * m_angularDeflectionEfficiency;
                 ret /= m_angularDeflectionTimescale;
 
                 VDetailLog("{0},  MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
                     Prim.LocalID, movingDirection, pointingDirection, deflectionError, ret);
+                VDetailLog("{0},  MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3}",
+                    Prim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale);
             }
             return ret;
         }
@@ -1308,6 +1342,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private float ClampInRange(float low, float val, float high)
         {
             return Math.Max(low, Math.Min(val, high));
+            // return Utils.Clamp(val, low, high);
         }
 
         // Invoke the detailed logger and output something if it's enabled.