BulletSim: Add IsSelected attribute to physical objects. Have vehicles check to see if physical before trying to step. Replace vehicle gravity application. Previously relying on Bullet to apply gravity but since vehicles over-ride the velocity calculation, gravity never had a chance to accelerate the body down. Added AddForceImpulse as well as AddForce for those who need to apply immediate velocity updates. Use the impulse to apply the linear motion.

4 files changed, 162 insertions, 50 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 939d38a..aadb583 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -328,6 +328,10 @@ public sealed class BSCharacter : BSPhysObject
     public override bool Selected {
         set { _selected = value; }
     }
+    public override bool IsSelected
+    {
+        get { return _selected; }
+    }
     public override void CrossingFailure() { return; }
     public override void link(PhysicsActor obj) { return; }
     public override void delink() { return; }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 2e44ab6..80fdfb9 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -108,10 +108,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private float m_VhoverEfficiency = 0f;
         private float m_VhoverTimescale = 0f;
         private float m_VhoverTargetHeight = -1.0f;     // if <0 then no hover, else its the current target height
-        private float m_VehicleBuoyancy = 0f;           //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
-                    // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
-                    // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
-                    // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
+        // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
+        private float m_VehicleBuoyancy = 0f;
+        private Vector3 m_VehicleGravity = Vector3.Zero;    // Gravity computed when buoyancy set
 
         //Attractor properties
         private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction");
@@ -149,14 +148,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 enableAngularVerticalAttraction = false;
                 enableAngularDeflection = false;
                 enableAngularBanking = false;
-                VDetailLog("{0},BSDynamics.SetupVehicleDebugging,settingDebugMode");
+                VDetailLog("{0},BSDynamics.SetupVehicleDebugging,settingDebugMode", Prim.LocalID);
             }
         }
 
         // Return 'true' if this vehicle is doing vehicle things
         public bool IsActive
         {
-            get { return Type != Vehicle.TYPE_NONE && Prim.IsPhysical; }
+            get { return (Type != Vehicle.TYPE_NONE && !Prim.IsStatic); }
         }
 
         #region Vehicle parameter setting
@@ -190,6 +189,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     break;
                 case Vehicle.BUOYANCY:
                     m_VehicleBuoyancy = ClampInRange(-1f, pValue, 1f);
+                    m_VehicleGravity = Prim.ComputeGravity(m_VehicleBuoyancy);
                     break;
                 case Vehicle.HOVER_EFFICIENCY:
                     m_VhoverEfficiency = ClampInRange(0f, pValue, 1f);
@@ -562,12 +562,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                                 1f);
             m_angularMotor.PhysicsScene = PhysicsScene;  // DEBUG DEBUG DEBUG (enables detail logging)
 
+            /*  Not implemented
             m_verticalAttractionMotor = new BSVMotor("VerticalAttraction", m_verticalAttractionTimescale,
                                 BSMotor.Infinite, BSMotor.InfiniteVector,
                                 m_verticalAttractionEfficiency);
             // Z goes away and we keep X and Y
             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
 
@@ -599,11 +601,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                 // Set the gravity for the vehicle depending on the buoyancy
                 // TODO: what should be done if prim and vehicle buoyancy differ?
-                Vector3 grav = Prim.ComputeGravity(m_VehicleBuoyancy);
-                PhysicsScene.PE.SetGravity(Prim.PhysBody, grav);
+                m_VehicleGravity = Prim.ComputeGravity(m_VehicleBuoyancy);
+                // The actual vehicle gravity is set to zero in Bullet so we can do all the application of same.
+                PhysicsScene.PE.SetGravity(Prim.PhysBody, Vector3.Zero);
 
                 VDetailLog("{0},BSDynamics.Refresh,mass={1},frict={2},inert={3},aDamp={4},grav={5}",
-                                Prim.LocalID, m_vehicleMass, friction, Prim.Inertia, angularDamping, grav);
+                        Prim.LocalID, m_vehicleMass, friction, Prim.Inertia, angularDamping, m_VehicleGravity);
             }
             else
             {
@@ -643,6 +646,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private Vector3 m_knownPosition;
         private Vector3 m_knownVelocity;
         private Vector3 m_knownForce;
+        private Vector3 m_knownForceImpulse;
         private Quaternion m_knownOrientation;
         private Vector3 m_knownRotationalVelocity;
         private Vector3 m_knownRotationalForce;
@@ -651,12 +655,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private const int m_knownChangedPosition           = 1 << 0;
         private const int m_knownChangedVelocity           = 1 << 1;
         private const int m_knownChangedForce              = 1 << 2;
-        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 const int m_knownChangedForceImpulse       = 1 << 3;
+        private const int m_knownChangedOrientation        = 1 << 4;
+        private const int m_knownChangedRotationalVelocity = 1 << 5;
+        private const int m_knownChangedRotationalForce    = 1 << 6;
+        private const int m_knownChangedTerrainHeight      = 1 << 7;
+        private const int m_knownChangedWaterLevel         = 1 << 8;
+        private const int m_knownChangedForwardVelocity    = 1 << 9;
 
         private void ForgetKnownVehicleProperties()
         {
@@ -677,21 +682,29 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 if ((m_knownChanged & m_knownChangedVelocity) != 0)
                 {
                     Prim.ForceVelocity = m_knownVelocity;
-                    PhysicsScene.PE.SetInterpolationLinearVelocity(Prim.PhysBody, VehicleVelocity);
+                    // Fake out Bullet by making it think the velocity is the same as last time.
+                    // Bullet does a bunch of smoothing for changing parameters.
+                    //    Since the vehicle is demanding this setting, we override Bullet's smoothing
+                    //    by telling Bullet the value was the same last time.
+                    PhysicsScene.PE.SetInterpolationLinearVelocity(Prim.PhysBody, m_knownVelocity);
                 }
 
                 if ((m_knownChanged & m_knownChangedForce) != 0)
                     Prim.AddForce((Vector3)m_knownForce, false, true);
 
+                if ((m_knownChanged & m_knownChangedForceImpulse) != 0)
+                    Prim.AddForceImpulse((Vector3)m_knownForceImpulse, false, true);
+
                 if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
                 {
                     Prim.ForceRotationalVelocity = m_knownRotationalVelocity;
-                    // Fake out Bullet by making it think the velocity is the same as last time.
                     PhysicsScene.PE.SetInterpolationAngularVelocity(Prim.PhysBody, m_knownRotationalVelocity);
                 }
 
                 if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
+                {
                     Prim.AddAngularForce((Vector3)m_knownRotationalForce, false, true);
+                }
 
                 // If we set one of the values (ie, the physics engine didn't do it) we must force
                 //      an UpdateProperties event to send the changes up to the simulator.
@@ -781,15 +794,26 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
         }
 
-        private void VehicleAddForce(Vector3 aForce)
+        private void VehicleAddForce(Vector3 pForce)
         {
             if ((m_knownHas & m_knownChangedForce) == 0)
             {
                 m_knownForce = Vector3.Zero;
+                m_knownHas |= m_knownChangedForce;
             }
-            m_knownForce += aForce;
+            m_knownForce += pForce;
             m_knownChanged |= m_knownChangedForce;
-            m_knownHas |= m_knownChangedForce;
+        }
+
+        private void VehicleAddForceImpulse(Vector3 pImpulse)
+        {
+            if ((m_knownHas & m_knownChangedForceImpulse) == 0)
+            {
+                m_knownForceImpulse = Vector3.Zero;
+                m_knownHas |= m_knownChangedForceImpulse;
+            }
+            m_knownForceImpulse += pImpulse;
+            m_knownChanged |= m_knownChangedForceImpulse;
         }
 
         private Vector3 VehicleRotationalVelocity
@@ -868,20 +892,14 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             if (PhysicsScene.VehiclePhysicalLoggingEnabled)
                 PhysicsScene.PE.DumpRigidBody(PhysicsScene.World, Prim.PhysBody);
 
-            VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
-                    Prim.LocalID, VehiclePosition, Prim.Force, VehicleVelocity, VehicleRotationalVelocity);
+            VDetailLog("{0},BSDynamics.Step,done,pos={1}, force={2},velocity={3},angvel={4}",
+                    Prim.LocalID, VehiclePosition, m_knownForce, VehicleVelocity, VehicleRotationalVelocity);
         }
 
         // Apply the effect of the linear motor and other linear motions (like hover and float).
         private void MoveLinear(float pTimestep)
         {
-            Vector3 linearMotorContribution = m_linearMotor.Step(pTimestep);
-
-            // The movement computed in the linear motor is relative to the vehicle
-            //     coordinates. Rotate the movement to world coordinates.
-            linearMotorContribution *= VehicleOrientation;
-
-            VehicleVelocity = linearMotorContribution;
+            ComputeLinearVelocity(pTimestep);
 
             ComputeLinearTerrainHeightCorrection(pTimestep);
 
@@ -891,6 +909,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
             ComputeLinearMotorUp(pTimestep);
 
+            ApplyGravity(pTimestep);
+
             // If not changing some axis, reduce out velocity
             if ((m_flags & (VehicleFlag.NO_X | VehicleFlag.NO_Y | VehicleFlag.NO_Z)) != 0)
             {
@@ -919,6 +939,43 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
         } // end MoveLinear()
 
+        public void ComputeLinearVelocity(float pTimestep)
+        {
+            Vector3 linearMotorStep = m_linearMotor.Step(pTimestep);
+
+            // The movement computed in the linear motor is relative to the vehicle
+            //     coordinates. Rotate the movement to world coordinates.
+            Vector3 linearMotorVelocity = linearMotorStep * VehicleOrientation;
+
+            // If we're a ground vehicle, don't loose any Z action (like gravity acceleration).
+            float mixFactor = 1f;   // 1 means use all linear motor Z value, 0 means use all existing Z
+            if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
+            {
+                if (!Prim.IsColliding)
+                {
+                    // If a ground vehicle and not on the ground, I want gravity effect
+                    mixFactor = 0.2f;
+                }
+            }
+            else
+            {
+                // I'm not a ground vehicle but don't totally loose the effect of the environment
+                mixFactor = 0.8f;
+            }
+            linearMotorVelocity.Z = mixFactor * linearMotorVelocity.Z + (1f - mixFactor) * VehicleVelocity.Z;
+
+            // What we want to contribute to the vehicle's existing velocity
+            Vector3 linearMotorForce = linearMotorVelocity - VehicleVelocity;
+
+            // Act against the inertia of the vehicle
+            linearMotorForce *= m_vehicleMass;
+
+            VehicleAddForceImpulse(linearMotorForce);
+
+            VDetailLog("{0},  MoveLinear,velocity,vehVel={1},step={2},stepVel={3},mix={4},force={5}",
+                        Prim.LocalID, VehicleVelocity, linearMotorStep, linearMotorVelocity, mixFactor, linearMotorForce);
+        }
+
         public void ComputeLinearTerrainHeightCorrection(float pTimestep)
         {
             // If below the terrain, move us above the ground a little.
@@ -979,7 +1036,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
 
                     // TODO: implement m_VhoverEfficiency correctly
-                    VehicleVelocity += new Vector3(0f, 0f, verticalCorrectionVelocity);
+                    VehicleAddForceImpulse(new Vector3(0f, 0f, verticalCorrectionVelocity));
 
                     VDetailLog("{0},  MoveLinear,hover,pos={1},eff={2},hoverTS={3},height={4},target={5},err={6},corrVel={7}",
                                     Prim.LocalID, VehiclePosition, m_VhoverEfficiency,
@@ -1096,6 +1153,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
         }
 
+        private void ApplyGravity(float pTimeStep)
+        {
+            Vector3 appliedGravity = m_VehicleGravity * m_vehicleMass;
+            VehicleAddForce(appliedGravity);
+
+            VDetailLog("{0},  MoveLinear,applyGravity,vehGrav={1},appliedForce-{2}", 
+                            Prim.LocalID, m_VehicleGravity, appliedGravity);
+        }
+
         // =======================================================================
         // =======================================================================
         // Apply the effect of the angular motor.
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index e8575f6..2c84293 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -135,6 +135,7 @@ public abstract class BSPhysObject : PhysicsActor
     public virtual OMV.Vector3 Scale { get; set; }
     public abstract bool IsSolid { get; }
     public abstract bool IsStatic { get; }
+    public abstract bool IsSelected { get; }
 
     // Materialness
     public MaterialAttributes.Material Material { get; private set; }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 50ba343..02d06b4 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -204,6 +204,10 @@ public sealed class BSPrim : BSPhysObject
             }
         }
     }
+    public override bool IsSelected
+    {
+        get { return _isSelected; }
+    }
     public override void CrossingFailure() { return; }
 
     // link me to the specified parent
@@ -1153,33 +1157,70 @@ public sealed class BSPrim : BSPhysObject
     // This added force will only last the next simulation tick.
     public void AddForce(OMV.Vector3 force, bool pushforce, bool inTaintTime) {
         // for an object, doesn't matter if force is a pushforce or not
-        if (!IsStatic && force.IsFinite())
+        if (!IsStatic)
         {
-            float magnitude = force.Length();
-            if (magnitude > BSParam.MaxAddForceMagnitude)
+            if (force.IsFinite())
             {
-                // Force has a limit
-                force = force / magnitude * BSParam.MaxAddForceMagnitude;
-            }
+                float magnitude = force.Length();
+                if (magnitude > BSParam.MaxAddForceMagnitude)
+                {
+                    // Force has a limit
+                    force = force / magnitude * BSParam.MaxAddForceMagnitude;
+                }
 
-            OMV.Vector3 addForce = force;
-            // DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce);
+                OMV.Vector3 addForce = force;
+                // DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce);
 
-            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
-            {
-                // Bullet adds this central force to the total force for this tick
-                DetailLog("{0},BSPrim.addForce,taint,force={1}", LocalID, addForce);
-                if (PhysBody.HasPhysicalBody)
+                PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
                 {
-                    PhysicsScene.PE.ApplyCentralForce(PhysBody, addForce);
-                    ActivateIfPhysical(false);
-                }
-            });
+                    // Bullet adds this central force to the total force for this tick
+                    DetailLog("{0},BSPrim.addForce,taint,force={1}", LocalID, addForce);
+                    if (PhysBody.HasPhysicalBody)
+                    {
+                        PhysicsScene.PE.ApplyCentralForce(PhysBody, addForce);
+                        ActivateIfPhysical(false);
+                    }
+                });
+            }
+            else
+            {
+                m_log.WarnFormat("{0}: AddForce: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
+                return;
+            }
         }
-        else
+    }
+
+    public void AddForceImpulse(OMV.Vector3 impulse, bool pushforce, bool inTaintTime) {
+        // for an object, doesn't matter if force is a pushforce or not
+        if (!IsStatic)
         {
-            m_log.WarnFormat("{0}: Got a NaN force applied to a prim. LocalID={1}", LogHeader, LocalID);
-            return;
+            if (impulse.IsFinite())
+            {
+                float magnitude = impulse.Length();
+                if (magnitude > BSParam.MaxAddForceMagnitude)
+                {
+                    // Force has a limit
+                    impulse = impulse / magnitude * BSParam.MaxAddForceMagnitude;
+                }
+
+                // DetailLog("{0},BSPrim.addForceImpulse,call,impulse={1}", LocalID, impulse);
+                OMV.Vector3 addImpulse = impulse;
+                PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddImpulse", delegate()
+                {
+                    // Bullet adds this impulse immediately to the velocity
+                    DetailLog("{0},BSPrim.addForceImpulse,taint,impulseforce={1}", LocalID, addImpulse);
+                    if (PhysBody.HasPhysicalBody)
+                    {
+                        PhysicsScene.PE.ApplyCentralImpulse(PhysBody, addImpulse);
+                        ActivateIfPhysical(false);
+                    }
+                });
+            }
+            else
+            {
+                m_log.WarnFormat("{0}: AddForceImpulse: Got a NaN impulse applied to a prim. LocalID={1}", LogHeader, LocalID);
+                return;
+            }
         }
     }