2 files changed, 74 insertions, 27 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 7fd7b82..bf8a004 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -80,6 +80,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private Quaternion m_referenceFrame = Quaternion.Identity;
 
         // Linear properties
+        private BSVMotor m_linearMotor = new BSVMotor("LinearMotor");
         private Vector3 m_linearMotorDirection = Vector3.Zero;          // velocity requested by LSL, decayed by time
         private Vector3 m_linearMotorOffset = Vector3.Zero;             // the point of force can be offset from the center
         private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero;   // velocity requested by LSL
@@ -152,7 +153,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
-                    m_angularMotorDecayTimescale = Math.Max(pValue, 0.01f);
+                    m_angularMotorDecayTimescale = Math.Max(0.01f, Math.Min(pValue,120));
                     break;
                 case Vehicle.ANGULAR_MOTOR_TIMESCALE:
                     m_angularMotorTimescale = Math.Max(pValue, 0.01f);
@@ -185,10 +186,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
                     break;
                 case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
-                    m_linearMotorDecayTimescale = Math.Max(pValue, 0.01f);
+                    m_linearMotorDecayTimescale = Math.Max(0.01f, Math.Min(pValue,120));
+                    m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale;
                     break;
                 case Vehicle.LINEAR_MOTOR_TIMESCALE:
                     m_linearMotorTimescale = Math.Max(pValue, 0.01f);
+                    m_linearMotor.TimeScale = m_linearMotorTimescale;
                     break;
                 case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
                     m_verticalAttractionEfficiency = Math.Max(0.1f, Math.Min(pValue, 1f));
@@ -208,10 +211,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
                     m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
+                    m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
                     break;
                 case Vehicle.LINEAR_MOTOR_DIRECTION:
                     m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
                     m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
+                    m_linearMotor.SetTarget(m_linearMotorDirection);
                     break;
                 case Vehicle.LINEAR_MOTOR_OFFSET:
                     m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
@@ -238,10 +243,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     break;
                 case Vehicle.LINEAR_FRICTION_TIMESCALE:
                     m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    m_linearMotor.FrictionTimescale = m_linearFrictionTimescale;
                     break;
                 case Vehicle.LINEAR_MOTOR_DIRECTION:
                     m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
                     m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
+                    m_linearMotor.SetTarget(m_linearMotorDirection);
                     break;
                 case Vehicle.LINEAR_MOTOR_OFFSET:
                     m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
@@ -319,6 +326,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                     m_referenceFrame = Quaternion.Identity;
                     m_flags = (VehicleFlag)0;
+
                     break;
 
                 case Vehicle.TYPE_SLED:
@@ -510,6 +518,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                                     | VehicleFlag.HOVER_GLOBAL_HEIGHT);
                     break;
             }
+
+            // Update any physical parameters based on this type.
+            Refresh();
+
+            m_linearMotor = new BSVMotor("LinearMotor", m_linearMotorTimescale, m_linearMotorDecayTimescale, m_linearFrictionTimescale, 1f);
+            m_linearMotor.PhysicsScene = PhysicsScene;  // DEBUG DEBUG DEBUG (enables detail logging)
         }
 
         // Some of the properties of this prim may have changed.
@@ -518,18 +532,25 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             if (IsActive)
             {
-                VDetailLog("{0},BSDynamics.Refresh", Prim.LocalID);
                 m_vehicleMass = Prim.Linkset.LinksetMass;
 
                 // Friction effects are handled by this vehicle code
-                BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, 0f);
-                BulletSimAPI.SetHitFraction2(Prim.PhysBody.ptr, 0f);
-
-                BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, PhysicsScene.Params.vehicleAngularDamping);
-
-                BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, new Vector3(1f, 1f, 1f));
-
-
+                float friction = 0f;
+                BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, friction);
+
+                // Moderate angular movement introduced by Bullet.
+                // TODO: possibly set AngularFactor and LinearFactor for the type of vehicle.
+                //     Maybe compute linear and angular factor and damping from params.
+                float angularDamping = PhysicsScene.Params.vehicleAngularDamping;
+                BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, angularDamping);
+
+                // 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);
+                BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia);
+
+                VDetailLog("{0},BSDynamics.Refresh,frict={1},inert={2},aDamp={3}", 
+                                Prim.LocalID, friction, localInertia, angularDamping);
             }
         }
 
@@ -591,6 +612,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         // Also does hover and float.
         private void MoveLinear(float pTimestep)
         {
+            /*
             // m_linearMotorDirection is the target direction we are moving relative to the vehicle coordinates
             // m_lastLinearVelocityVector is the current speed we are moving in that direction
             if (m_linearMotorDirection.LengthSquared() > 0.001f)
@@ -627,6 +649,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                 VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);
             }
+            */
+
+            m_newVelocity = m_linearMotor.Step(pTimestep);
+
+            // Rotate new object velocity from vehicle relative to world coordinates
+            m_newVelocity *= Prim.ForceOrientation;
 
             // m_newVelocity is velocity computed from linear motor in world coordinates
 
@@ -785,12 +813,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 m_newVelocity.Z = 0;
 
             // Clamp REALLY high or low velocities
-            if (m_newVelocity.LengthSquared() > 1e6f)
+            float newVelocityLengthSq = m_newVelocity.LengthSquared();
+            if (newVelocityLengthSq > 1e6f)
             {
                 m_newVelocity /= m_newVelocity.Length();
                 m_newVelocity *= 1000f;
             }
-            else if (m_newVelocity.LengthSquared() < 1e-6f)
+            else if (newVelocityLengthSq < 1e-6f)
                 m_newVelocity = Vector3.Zero;
 
             // Stuff new linear velocity into the vehicle
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index b8bdd87..68eec2d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -7,13 +7,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
 public abstract class BSMotor
 {
-    public BSMotor()
+    public BSMotor(string useName)
     {
+        UseName = useName;
         PhysicsScene = null;
     }
     public virtual void Reset() { }
     public virtual void Zero() { }
 
+    public string UseName { get; private set; }
     // Used only for outputting debug information. Might not be set so check for null.
     public BSScene PhysicsScene { get; set; }
     protected void MDetailLog(string msg, params Object[] parms)
@@ -35,17 +37,25 @@ public class BSVMotor : BSMotor
 
     public float TimeScale { get; set; }
     public float TargetValueDecayTimeScale { get; set; }
-    public Vector3 CurrentValueReductionTimescale { get; set; }
+    public Vector3 FrictionTimescale { get; set; }
     public float Efficiency { get; set; }
 
     public Vector3 TargetValue { get; private set; }
     public Vector3 CurrentValue { get; private set; }
 
-    BSVMotor(float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) : base()
+    public BSVMotor(string useName)
+        : base(useName)
+    {
+        TimeScale = TargetValueDecayTimeScale = Efficiency = 1f;
+        FrictionTimescale = Vector3.Zero;
+        CurrentValue = TargetValue = Vector3.Zero;
+    }
+    public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) 
+        : this(useName)
     {
         TimeScale = timeScale;
         TargetValueDecayTimeScale = decayTimeScale;
-        CurrentValueReductionTimescale = frictionTimeScale;
+        FrictionTimescale = frictionTimeScale;
         Efficiency = efficiency;
         CurrentValue = TargetValue = Vector3.Zero;
     }
@@ -60,10 +70,10 @@ public class BSVMotor : BSMotor
     public Vector3 Step(float timeStep)
     {
         Vector3 returnCurrent = Vector3.Zero;
-        if (CurrentValue.LengthSquared() > 0.001f)
+        if (!CurrentValue.ApproxEquals(TargetValue, 0.01f))
         {
-            // Vector3 origDir = Target;       // DEBUG
-            // Vector3 origVel = CurrentValue;   // DEBUG
+            Vector3 origTarget = TargetValue;       // DEBUG
+            Vector3 origCurrVal = CurrentValue;   // DEBUG
 
             // Add (desiredVector - currentAppliedVector) / howLongItShouldTakeToComplete
             Vector3 addAmount = (TargetValue - CurrentValue)/TimeScale * timeStep;
@@ -74,11 +84,17 @@ public class BSVMotor : BSMotor
             float decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
             TargetValue *= (1f - decayFactor);
 
-            Vector3 frictionFactor = (Vector3.One / CurrentValueReductionTimescale) * timeStep;
+            Vector3 frictionFactor = Vector3.Zero;
+            frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
             CurrentValue *= (Vector3.One - frictionFactor);
 
-            MDetailLog("{0},BSVMotor.Step,nonZero,curr={1},target={2},add={3},decay={4},frict={5},ret={6}",
-                                    BSScene.DetailLogZero, TargetValue, CurrentValue, 
+            MDetailLog("{0},BSVMotor.Step,nonZero,{1},origTarget={2},origCurr={3},timeStep={4},timeScale={5},addAmnt={6},targetDecay={7},decayFact={8},fricTS={9},frictFact={10}",
+                                BSScene.DetailLogZero, UseName, origTarget, origCurrVal,
+                                timeStep, TimeScale, addAmount,
+                                TargetValueDecayTimeScale, decayFactor,
+                                FrictionTimescale, frictionFactor);
+            MDetailLog("{0},BSVMotor.Step,nonZero,{1},curr={2},target={3},add={4},decay={5},frict={6},ret={7}",
+                                    BSScene.DetailLogZero, UseName, TargetValue, CurrentValue, 
                                     addAmount, decayFactor, frictionFactor, returnCurrent);
         }
         else
@@ -87,8 +103,8 @@ public class BSVMotor : BSMotor
             CurrentValue = Vector3.Zero;
             TargetValue = Vector3.Zero;
 
-            MDetailLog("{0},BSVMotor.Step,zero,curr={1},target={2},ret={3}",
-                                    BSScene.DetailLogZero, TargetValue, CurrentValue, returnCurrent);
+            MDetailLog("{0},BSVMotor.Step,zero,{1},curr={2},target={3},ret={4}",
+                                    BSScene.DetailLogZero, UseName, TargetValue, CurrentValue, returnCurrent);
 
         }
         return returnCurrent;
@@ -105,7 +121,8 @@ public class BSFMotor : BSMotor
     public float Target { get; private set; }
     public float CurrentValue { get; private set; }
 
-    BSFMotor(float timeScale, float decayTimescale, float friction, float efficiency) : base()
+    public BSFMotor(string useName, float timeScale, float decayTimescale, float friction, float efficiency)
+        : base(useName)
     {
     }
     public void SetCurrent(float target)
@@ -122,7 +139,8 @@ public class BSFMotor : BSMotor
 public class BSPIDMotor : BSMotor
 {
     // TODO: write and use this one
-    BSPIDMotor() : base()
+    public BSPIDMotor(string useName)
+        : base(useName)
     {
     }
 }