BulletSim: repair vehicle problems introduced in previous 'improvements'. Fix line endings in BSParams.

OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs

@@ -634,28 +634,33 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             m_knownHas = 0;
             m_knownChanged = 0;
         }
+        // Push all the changed values back into the physics engine
         private void PushKnownChanged()
         {
             if (m_knownChanged != 0)
             {
                 if ((m_knownChanged & m_knownChangedPosition) != 0)
-                    Prim.ForcePosition = VehiclePosition;
+                    Prim.ForcePosition = m_knownPosition;
+
                 if ((m_knownChanged & m_knownChangedOrientation) != 0)
-                    Prim.ForceOrientation = VehicleOrientation;
+                    Prim.ForceOrientation = m_knownOrientation;
+
                 if ((m_knownChanged & m_knownChangedVelocity) != 0)
                 {
-                    Prim.ForceVelocity = VehicleVelocity;
+                    Prim.ForceVelocity = m_knownVelocity;
                     BulletSimAPI.SetInterpolationLinearVelocity2(Prim.PhysBody.ptr, VehicleVelocity);
                 }
+
                 if ((m_knownChanged & m_knownChangedForce) != 0)
                     Prim.AddForce((Vector3)m_knownForce, false, true);
 
                 if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
                 {
-                    Prim.ForceRotationalVelocity = VehicleRotationalVelocity;
+                    Prim.ForceRotationalVelocity = m_knownRotationalVelocity;
                     // Fake out Bullet by making it think the velocity is the same as last time.
-                    BulletSimAPI.SetInterpolationAngularVelocity2(Prim.PhysBody.ptr, VehicleRotationalVelocity);
+                    BulletSimAPI.SetInterpolationAngularVelocity2(Prim.PhysBody.ptr, m_knownRotationalVelocity);
                 }
+
                 if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
                     Prim.AddAngularForce((Vector3)m_knownRotationalForce, false, true);
 
@@ -667,7 +672,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         }
 
         // 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.
+        //    is used to fetch the height only once for each vehicle simulation step.
         private float GetTerrainHeight(Vector3 pos)
         {
             if ((m_knownHas & m_knownChangedTerrainHeight) == 0)
@@ -699,12 +704,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_knownPosition = Prim.ForcePosition;
                     m_knownHas |= m_knownChangedPosition;
                 }
-                return (Vector3)m_knownPosition;
+                return m_knownPosition;
             }
             set
             {
                 m_knownPosition = value;
                 m_knownChanged |= m_knownChangedPosition;
+                m_knownHas |= m_knownChangedPosition;
             }
         }
 
@@ -717,12 +723,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_knownOrientation = Prim.ForceOrientation;
                     m_knownHas |= m_knownChangedOrientation;
                 }
-                return (Quaternion)m_knownOrientation;
+                return m_knownOrientation;
             }
             set
             {
                 m_knownOrientation = value;
                 m_knownChanged |= m_knownChangedOrientation;
+                m_knownHas |= m_knownChangedOrientation;
             }
         }
 
@@ -741,13 +748,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 m_knownVelocity = value;
                 m_knownChanged |= m_knownChangedVelocity;
+                m_knownHas |= m_knownChangedVelocity;
             }
         }
 
         private void VehicleAddForce(Vector3 aForce)
         {
+            if ((m_knownHas & m_knownChangedForce) == 0)
+            {
+                m_knownForce = Vector3.Zero;
+            }
             m_knownForce += aForce;
             m_knownChanged |= m_knownChangedForce;
+            m_knownHas |= m_knownChangedForce;
         }
 
         private Vector3 VehicleRotationalVelocity
@@ -765,12 +778,18 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 m_knownRotationalVelocity = value;
                 m_knownChanged |= m_knownChangedRotationalVelocity;
+                m_knownHas |= m_knownChangedRotationalVelocity;
             }
         }
         private void VehicleAddAngularForce(Vector3 aForce)
         {
+            if ((m_knownHas & m_knownChangedRotationalForce) == 0)
+            {
+                m_knownRotationalForce = Vector3.Zero;
+            }
             m_knownRotationalForce += aForce;
             m_knownChanged |= m_knownChangedRotationalForce;
+            m_knownHas |= m_knownChangedRotationalForce;
         }
         // Vehicle relative forward velocity
         private Vector3 VehicleForwardVelocity
@@ -782,7 +801,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_knownForwardVelocity = VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleOrientation));
                     m_knownHas |= m_knownChangedForwardVelocity;
                 }
-                return (Vector3)m_knownForwardVelocity;
+                return m_knownForwardVelocity;
             }
         }
         private float VehicleForwardSpeed

OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs

@@ -268,12 +268,14 @@ public class BSPIDVMotor : BSVMotor
     public Vector3 proportionFactor { get; set; }
     public Vector3 integralFactor { get; set; }
     public Vector3 derivFactor { get; set; }
+
     // Arbritrary factor range.
     // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct.
     public float EfficiencyHigh = 0.4f;
     public float EfficiencyLow = 4.0f;
 
-    Vector3 IntegralFactor { get; set; }
+    // Running integration of the error
+    Vector3 RunningIntegration { get; set; }
 
     public BSPIDVMotor(string useName)
         : base(useName)
@@ -281,7 +283,7 @@ public class BSPIDVMotor : BSVMotor
         proportionFactor = new Vector3(1.00f, 1.00f, 1.00f);
         integralFactor = new Vector3(1.00f, 1.00f, 1.00f);
         derivFactor = new Vector3(1.00f, 1.00f, 1.00f);
-        IntegralFactor = Vector3.Zero;
+        RunningIntegration = Vector3.Zero;
         LastError = Vector3.Zero;
     }
 
@@ -312,14 +314,18 @@ public class BSPIDVMotor : BSVMotor
     public override Vector3 Step(float timeStep, Vector3 error)
     {
         // Add up the error so we can integrate over the accumulated errors
-        IntegralFactor += error * timeStep;
+        RunningIntegration += error * timeStep;
 
         // A simple derivitive is the rate of change from the last error.
         Vector3 derivFactor = (error - LastError) * timeStep;
         LastError = error;
 
         // Correction = -(proportionOfPresentError +      accumulationOfPastError    +     rateOfChangeOfError)
-        Vector3 ret   = -(error * proportionFactor + IntegralFactor * integralFactor + derivFactor * derivFactor);
+        Vector3 ret   = -(
+                          error * proportionFactor
+                        + RunningIntegration * integralFactor 
+                        + derivFactor * derivFactor
+                        );
 
         return ret;
     }

OpenSim/Region/Physics/BulletSPlugin/BSScene.cs

@@ -864,7 +864,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         {
             DetailLog("{0},BSScene.AssertInTaintTime,NOT IN TAINT TIME,Region={1},Where={2}", DetailLogZero, RegionName, whereFrom);
             m_log.ErrorFormat("{0} NOT IN TAINT TIME!! Region={1}, Where={2}", LogHeader, RegionName, whereFrom);
-            Util.PrintCallStack(DetailLog);  // Prints the stack into the DEBUG log file.
+            Util.PrintCallStack(DetailLog);
         }
         return InTaintTime;
     }