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

Cleaned up code and got rid of compile warnings.

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,16 +391,12 @@ 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;
@@ -428,16 +424,15 @@ public class BSCharacter : PhysicsActor
             //   loop that updates avatars.
             // base.RequestPhysicsterseUpdate();
         }
-        }
+        */
-        else {
         _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
     // The collision, if it should be reported to the character, is placed in a collection

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,65 +1262,54 @@ 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))
         {
             _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();
             }
         }
-        }
+        */
-        else
-        {
-            // 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.
+        // 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
@@ -1334,7 +1323,6 @@ public sealed class BSPrim : PhysicsActor
             base.RequestPhysicsterseUpdate();
         }
     }
-    }
 
     // I've collided with something
     CollisionEventUpdate collisionCollection = null;

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;