BulletSim: Add PID variables to physical scene. Not PIDing yet, but soon.

Cleaned up code and got rid of compile warnings.

3 files changed, 97 insertions, 101 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index dc0c008..09e1f0c 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -41,7 +41,7 @@ public class BSCharacter : PhysicsActor
 
     private BSScene _scene;
     private String _avName;
-    private bool _stopped;
+    // private bool _stopped;
     private Vector3 _size;
     private Vector3 _scale;
     private PrimitiveBaseShape _pbs;
@@ -134,9 +134,9 @@ public class BSCharacter : PhysicsActor
     {
         base.RequestPhysicsterseUpdate();
     }
-
+    // No one calls this method so I don't know what it could possibly mean
     public override bool Stopped { 
-        get { return _stopped; } 
+        get { return false; } 
     }
     public override Vector3 Size { 
         get { return _size; } 
@@ -391,52 +391,47 @@ public class BSCharacter : PhysicsActor
         _mass = _density * _avatarVolume;
     }
 
-    // Set to 'true' if the individual changed items should be checked
-    //   (someday RequestPhysicsTerseUpdate() will take a bitmap of changed properties)
-    const bool SHOULD_CHECK_FOR_INDIVIDUAL_CHANGES = false;
-
     // The physics engine says that properties have updated. Update same and inform
     // the world that things have changed.
     public void UpdateProperties(EntityProperties entprop)
     {
+        /*
         bool changed = false;
-        if (SHOULD_CHECK_FOR_INDIVIDUAL_CHANGES) {
-            // we assign to the local variables so the normal set action does not happen
-            if (_position != entprop.Position) {
-                _position = entprop.Position;
-                changed = true;
-            }
-            if (_orientation != entprop.Rotation) {
-                _orientation = entprop.Rotation;
-                changed = true;
-            }
-            if (_velocity != entprop.Velocity) {
-                _velocity = entprop.Velocity;
-                changed = true;
-            }
-            if (_acceleration != entprop.Acceleration) {
-                _acceleration = entprop.Acceleration;
-                changed = true;
-            }
-            if (_rotationalVelocity != entprop.RotationalVelocity) {
-                _rotationalVelocity = entprop.RotationalVelocity;
-                changed = true;
-            }
-            if (changed) {
-                // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
-                // Avatar movement is not done by generating this event. There is code in the heartbeat
-                //   loop that updates avatars.
-                // base.RequestPhysicsterseUpdate();
-            }
-        }
-        else {
+        // we assign to the local variables so the normal set action does not happen
+        if (_position != entprop.Position) {
             _position = entprop.Position;
+            changed = true;
+        }
+        if (_orientation != entprop.Rotation) {
             _orientation = entprop.Rotation;
+            changed = true;
+        }
+        if (_velocity != entprop.Velocity) {
             _velocity = entprop.Velocity;
+            changed = true;
+        }
+        if (_acceleration != entprop.Acceleration) {
             _acceleration = entprop.Acceleration;
+            changed = true;
+        }
+        if (_rotationalVelocity != entprop.RotationalVelocity) {
             _rotationalVelocity = entprop.RotationalVelocity;
+            changed = true;
+        }
+        if (changed) {
+            // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
+            // Avatar movement is not done by generating this event. There is code in the heartbeat
+            //   loop that updates avatars.
             // base.RequestPhysicsterseUpdate();
         }
+        */
+        _position = entprop.Position;
+        _orientation = entprop.Rotation;
+        _velocity = entprop.Velocity;
+        _acceleration = entprop.Acceleration;
+        _rotationalVelocity = entprop.RotationalVelocity;
+        // Avatars don't report theirr changes the usual way. Changes are checked for in the heartbeat loop.
+        // base.RequestPhysicsterseUpdate();
     }
 
     // Called by the scene when a collision with this object is reported
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 9b28a06..23b276e 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -148,7 +148,7 @@ public sealed class BSPrim : PhysicsActor
     {
         // m_log.DebugFormat("{0}: Destroy, id={1}", LogHeader, LocalID);
         // Undo any vehicle properties
-        _vehicle.ProcessTypeChange(Vehicle.TYPE_NONE);
+        _vehicle.ProcessTypeChange(Vehicle.TYPE_NONE, 1f);
         _scene.RemoveVehiclePrim(this);     // just to make sure
 
         // undo any dependance with/on other objects
@@ -353,7 +353,7 @@ public sealed class BSPrim : PhysicsActor
         } 
         set {
             Vehicle type = (Vehicle)value;
-            _vehicle.ProcessTypeChange(type);
+            _vehicle.ProcessTypeChange(type, _scene.LastSimulatedTimestep);
             _scene.TaintedObject(delegate()
             {
                 if (type == Vehicle.TYPE_NONE)
@@ -371,11 +371,11 @@ public sealed class BSPrim : PhysicsActor
     }
     public override void VehicleFloatParam(int param, float value) 
     {
-        _vehicle.ProcessFloatVehicleParam((Vehicle)param, value);
+        _vehicle.ProcessFloatVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
     }
     public override void VehicleVectorParam(int param, OMV.Vector3 value) 
     {
-        _vehicle.ProcessVectorVehicleParam((Vehicle)param, value);
+        _vehicle.ProcessVectorVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
     }
     public override void VehicleRotationParam(int param, OMV.Quaternion rotation) 
     {
@@ -1262,78 +1262,66 @@ public sealed class BSPrim : PhysicsActor
     const float POSITION_TOLERANCE = 0.05f;
     const float ACCELERATION_TOLERANCE = 0.01f;
     const float ROTATIONAL_VELOCITY_TOLERANCE = 0.01f;
-    const bool SHOULD_DAMP_UPDATES = false;
 
     public void UpdateProperties(EntityProperties entprop)
     {
+        /*
         UpdatedProperties changed = 0;
-        if (SHOULD_DAMP_UPDATES)
+        // assign to the local variables so the normal set action does not happen
+        // if (_position != entprop.Position)
+        if (!_position.ApproxEquals(entprop.Position, POSITION_TOLERANCE))
         {
-            // assign to the local variables so the normal set action does not happen
-            // if (_position != entprop.Position)
-            if (!_position.ApproxEquals(entprop.Position, POSITION_TOLERANCE))
-            {
-                _position = entprop.Position;
-                // m_log.DebugFormat("{0}: UpdateProperties: id={1}, pos = {2}", LogHeader, LocalID, _position);
-                changed |= UpdatedProperties.Position;
-            }
-            // if (_orientation != entprop.Rotation)
-            if (!_orientation.ApproxEquals(entprop.Rotation, ROTATION_TOLERANCE))
-            {
-                _orientation = entprop.Rotation;
-                // m_log.DebugFormat("{0}: UpdateProperties: id={1}, rot = {2}", LogHeader, LocalID, _orientation);
-                changed |= UpdatedProperties.Rotation;
-            }
-            // if (_velocity != entprop.Velocity)
-            if (!_velocity.ApproxEquals(entprop.Velocity, VELOCITY_TOLERANCE))
-            {
-                _velocity = entprop.Velocity;
-                // m_log.DebugFormat("{0}: UpdateProperties: velocity = {1}", LogHeader, _velocity);
-                changed |= UpdatedProperties.Velocity;
-            }
-            // if (_acceleration != entprop.Acceleration)
-            if (!_acceleration.ApproxEquals(entprop.Acceleration, ACCELERATION_TOLERANCE))
-            {
-                _acceleration = entprop.Acceleration;
-                // m_log.DebugFormat("{0}: UpdateProperties: acceleration = {1}", LogHeader, _acceleration);
-                changed |= UpdatedProperties.Acceleration;
-            }
-            // if (_rotationalVelocity != entprop.RotationalVelocity)
-            if (!_rotationalVelocity.ApproxEquals(entprop.RotationalVelocity, ROTATIONAL_VELOCITY_TOLERANCE))
-            {
-                _rotationalVelocity = entprop.RotationalVelocity;
-                // m_log.DebugFormat("{0}: UpdateProperties: rotationalVelocity = {1}", LogHeader, _rotationalVelocity);
-                changed |= UpdatedProperties.RotationalVel;
-            }
-            if (changed != 0)
-            {
-                // m_log.DebugFormat("{0}: UpdateProperties: id={1}, c={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
-                // Only update the position of single objects and linkset roots
-                if (this._parentPrim == null)
-                {
-                    // m_log.DebugFormat("{0}: RequestTerseUpdate. id={1}, ch={2}, pos={3}, rot={4}", LogHeader, LocalID, changed, _position, _orientation);
-                    base.RequestPhysicsterseUpdate();
-                }
-            }
+            _position = entprop.Position;
+            changed |= UpdatedProperties.Position;
         }
-        else
+        // if (_orientation != entprop.Rotation)
+        if (!_orientation.ApproxEquals(entprop.Rotation, ROTATION_TOLERANCE))
         {
-            // Don't check for damping here -- it's done in BulletSim and SceneObjectPart.
-
-            // Only updates only for individual prims and for the root object of a linkset.
+            _orientation = entprop.Rotation;
+            changed |= UpdatedProperties.Rotation;
+        }
+        // if (_velocity != entprop.Velocity)
+        if (!_velocity.ApproxEquals(entprop.Velocity, VELOCITY_TOLERANCE))
+        {
+            _velocity = entprop.Velocity;
+            changed |= UpdatedProperties.Velocity;
+        }
+        // if (_acceleration != entprop.Acceleration)
+        if (!_acceleration.ApproxEquals(entprop.Acceleration, ACCELERATION_TOLERANCE))
+        {
+            _acceleration = entprop.Acceleration;
+            changed |= UpdatedProperties.Acceleration;
+        }
+        // if (_rotationalVelocity != entprop.RotationalVelocity)
+        if (!_rotationalVelocity.ApproxEquals(entprop.RotationalVelocity, ROTATIONAL_VELOCITY_TOLERANCE))
+        {
+            _rotationalVelocity = entprop.RotationalVelocity;
+            changed |= UpdatedProperties.RotationalVel;
+        }
+        if (changed != 0)
+        {
+            // Only update the position of single objects and linkset roots
             if (this._parentPrim == null)
             {
-                // Assign to the local variables so the normal set action does not happen
-                _position = entprop.Position;
-                _orientation = entprop.Rotation;
-                _velocity = entprop.Velocity;
-                _acceleration = entprop.Acceleration;
-                _rotationalVelocity = entprop.RotationalVelocity;
-                // m_log.DebugFormat("{0}: RequestTerseUpdate. id={1}, ch={2}, pos={3}, rot={4}, vel={5}, acc={6}, rvel={7}", 
-                //         LogHeader, LocalID, changed, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
                 base.RequestPhysicsterseUpdate();
             }
         }
+        */
+
+        // Don't check for damping here -- it's done in BulletSim and SceneObjectPart.
+        // Updates only for individual prims and for the root object of a linkset.
+        if (this._parentPrim == null)
+        {
+            // Assign to the local variables so the normal set action does not happen
+            _position = entprop.Position;
+            _orientation = entprop.Rotation;
+            _velocity = entprop.Velocity;
+            _acceleration = entprop.Acceleration;
+            _rotationalVelocity = entprop.RotationalVelocity;
+            // m_log.DebugFormat("{0}: RequestTerseUpdate. id={1}, ch={2}, pos={3}, rot={4}, vel={5}, acc={6}, rvel={7}", 
+            //         LogHeader, LocalID, changed, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
+            base.RequestPhysicsterseUpdate();
+        }
     }
 
     // I've collided with something
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index eb1d798..150326e 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -107,6 +107,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     private long m_simulationStep = 0;
     public long SimulationStep { get { return m_simulationStep; } }
 
+    public float LastSimulatedTimestep { get; private set; }
+
     // A value of the time now so all the collision and update routines do not have to get their own
     // Set to 'now' just before all the prims and actors are called for collisions and updates
     private int m_simulationNowTime;
@@ -123,6 +125,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
     private bool _meshSculptedPrim = true;         // cause scuplted prims to get meshed
     private bool _forceSimplePrimMeshing = false;   // if a cube or sphere, let Bullet do internal shapes
 
+    public float PID_D { get; private set; }    // derivative
+    public float PID_P { get; private set; }    // proportional
+
     public const uint TERRAIN_ID = 0;       // OpenSim senses terrain with a localID of zero
     public const uint GROUNDPLANE_ID = 1;
 
@@ -222,6 +227,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         m_maxUpdatesPerFrame = 2048;
         m_maximumObjectMass = 10000.01f;
 
+        PID_D = 2200f;
+        PID_P = 900f;
+
         parms.defaultFriction = 0.5f;
         parms.defaultDensity = 10.000006836f; // Aluminum g/cm3
         parms.defaultRestitution = 0f;
@@ -278,6 +286,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                 m_maxUpdatesPerFrame = pConfig.GetInt("MaxUpdatesPerFrame", m_maxUpdatesPerFrame);
                 m_maximumObjectMass = pConfig.GetFloat("MaxObjectMass", m_maximumObjectMass);
 
+                PID_D = pConfig.GetFloat("PIDDerivative", PID_D);
+                PID_P = pConfig.GetFloat("PIDProportional", PID_P);
+
                 parms.defaultFriction = pConfig.GetFloat("DefaultFriction", parms.defaultFriction);
                 parms.defaultDensity = pConfig.GetFloat("DefaultDensity", parms.defaultDensity);
                 parms.defaultRestitution = pConfig.GetFloat("DefaultRestitution", parms.defaultRestitution);
@@ -415,6 +426,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
         int collidersCount;
         IntPtr collidersPtr;
 
+        LastSimulatedTimestep = timeStep;
+
         // prevent simulation until we've been initialized
         if (!m_initialized) return 10.0f;