7 files changed, 694 insertions, 243 deletions
diff --git a/OpenSim/Region/CoreModules/Framework/Statistics/Logging/BinaryLoggingModule.cs b/OpenSim/Region/CoreModules/Framework/Statistics/Logging/BinaryLoggingModule.cs
index a75ff62..9b1b1a3 100644
--- a/OpenSim/Region/CoreModules/Framework/Statistics/Logging/BinaryLoggingModule.cs
+++ b/OpenSim/Region/CoreModules/Framework/Statistics/Logging/BinaryLoggingModule.cs
@@ -39,7 +39,7 @@ using OpenSim.Region.Framework;
 using OpenSim.Region.Framework.Interfaces;
 using OpenSim.Region.Framework.Scenes;
-namespace OpenSim.Region.CoreModules.Avatar.Attachments
+namespace OpenSim.Region.CoreModules.Statistics.Logging
 {
    [Extension(Path = "/OpenSim/RegionModules", NodeName = "RegionModule", Id = "BinaryLoggingModule")]
    public class BinaryLoggingModule : INonSharedRegionModule
diff --git a/OpenSim/Region/CoreModules/Framework/Statistics/Logging/LogWriter.cs b/OpenSim/Region/CoreModules/Framework/Statistics/Logging/LogWriter.cs
new file mode 100755
index 0000000..65e4c90
--- /dev/null
+++ b/OpenSim/Region/CoreModules/Framework/Statistics/Logging/LogWriter.cs
@@ -0,0 +1,161 @@
+/*
+ * Copyright (c) Contributors, http://opensimulator.org/
+ * See CONTRIBUTORS.TXT for a full list of copyright holders.
+ *
+ * Redistribution and use in source and binary forms, with or without
+ * modification, are permitted provided that the following conditions are met:
+ *     * Redistributions of source code must retain the above copyright
+ *       notice, this list of conditions and the following disclaimer.
+ *     * Redistributions in binary form must reproduce the above copyright
+ *       notice, this list of conditions and the following disclaimer in the
+ *       documentation and/or other materials provided with the distribution.
+ *     * Neither the name of the OpenSimulator Project nor the
+ *       names of its contributors may be used to endorse or promote products
+ *       derived from this software without specific prior written permission.
+ *
+ * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
+ * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
+ * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
+ * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
+ * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
+ * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
+ * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
+ * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
+ * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
+ * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
+ */
+using System;
+using System.IO;
+using System.Text;
+using log4net;
+namespace OpenSim.Region.CoreModules.Framework.Statistics.Logging
+{
+    /// <summary>
+    /// Class for writing a high performance, high volume log file.
+    /// Sometimes, to debug, one has a high volume logging to do and the regular
+    /// log file output is not appropriate.
+    /// Create a new instance with the parameters needed and
+    /// call Write() to output a line. Call Close() when finished.
+    /// If created with no parameters, it will not log anything.
+    /// </summary>
+    public class LogWriter : IDisposable
+    {
+        public bool Enabled { get; private set; }
+        private string m_logDirectory = ".";
+        private int m_logMaxFileTimeMin = 5;    // 5 minutes
+        public String LogFileHeader { get; set; }
+        private StreamWriter m_logFile = null;
+        private TimeSpan m_logFileLife;
+        private DateTime m_logFileEndTime;
+        private Object m_logFileWriteLock = new Object();
+        // set externally when debugging. If let 'null', this does not write any error messages.
+        public ILog ErrorLogger = null;
+        private string LogHeader = "[LOG WRITER]";
+        /// <summary>
+        /// Create a log writer that will not write anything. Good for when not enabled
+        /// but the write statements are still in the code.
+        /// </summary>
+        public LogWriter()
+        {
+            Enabled = false;
+            m_logFile = null;
+        }
+        /// <summary>
+        /// Create a log writer instance.
+        /// </summary>
+        /// <param name="dir">The directory to create the log file in. May be 'null' for default.</param>
+        /// <param name="headr">The characters that begin the log file name. May be 'null' for default.</param>
+        /// <param name="maxFileTime">Maximum age of a log file in minutes. If zero, will set default.</param>
+        public LogWriter(string dir, string headr, int maxFileTime)
+        {
+            m_logDirectory = dir == null ? "." : dir;
+            LogFileHeader = headr == null ? "log-" : headr;
+            m_logMaxFileTimeMin = maxFileTime;
+            if (m_logMaxFileTimeMin < 1)
+                m_logMaxFileTimeMin = 5;
+            m_logFileLife = new TimeSpan(0, m_logMaxFileTimeMin, 0);
+            m_logFileEndTime = DateTime.Now + m_logFileLife;
+            Enabled = true;
+        }
+        public void Dispose()
+        {
+            this.Close();
+        }
+        public void Close()
+        {
+            Enabled = false;
+            if (m_logFile != null)
+            {
+                m_logFile.Close();
+                m_logFile.Dispose();
+                m_logFile = null;
+            }
+        }
+        public void Write(string line, params object[] args)
+        {
+            if (!Enabled) return;
+            Write(String.Format(line, args));
+        }
+        public void Write(string line)
+        {
+            if (!Enabled) return;
+            try
+            {
+                lock (m_logFileWriteLock)
+                {
+                    DateTime now = DateTime.Now;
+                    if (m_logFile == null || now > m_logFileEndTime)
+                    {
+                        if (m_logFile != null)
+                        {
+                            m_logFile.Close();
+                            m_logFile.Dispose();
+                            m_logFile = null;
+                        }
+                        // First log file or time has expired, start writing to a new log file
+                        m_logFileEndTime = now + m_logFileLife;
+                        string path = (m_logDirectory.Length > 0 ? m_logDirectory
+                                    + System.IO.Path.DirectorySeparatorChar.ToString() : "")
+                                + String.Format("{0}{1}.log", LogFileHeader, now.ToString("yyyyMMddHHmmss"));
+                        m_logFile = new StreamWriter(File.Open(path, FileMode.Append, FileAccess.Write));
+                    }
+                    if (m_logFile != null)
+                    {
+                        StringBuilder buff = new StringBuilder(line.Length + 25);
+                        buff.Append(now.ToString("yyyyMMddHHmmssfff"));
+                        // buff.Append(now.ToString("yyyyMMddHHmmss"));
+                        buff.Append(",");
+                        buff.Append(line);
+                        buff.Append("\r\n");
+                        m_logFile.Write(buff.ToString());
+                    }
+                }
+            }
+            catch (Exception e)
+            {
+                if (ErrorLogger != null)
+                {
+                    ErrorLogger.ErrorFormat("{0}: FAILURE WRITING TO LOGFILE: {1}", LogHeader, e);
+                }
+                Enabled = false;
+            }
+            return;
+        }
+    }
+}
+\ No newline at end of file
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
-            // we assign to the local variables so the normal set action does not happen
+        if (_position != entprop.Position) {
-            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 {
            _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/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index eb20eb3..c197e61 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -57,7 +57,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
        private int frcount = 0;                                        // Used to limit dynamics debug output to
                                                                        // every 100th frame
-        // private BSScene m_parentScene = null;
        private BSPrim m_prim;      // the prim this dynamic controller belongs to
        // Vehicle properties
@@ -131,8 +130,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            m_type = Vehicle.TYPE_NONE;
        }
-        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
+        internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue, float timestep)
        {
+            DetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
            switch (pParam)
            {
                case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
@@ -229,8 +229,9 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            }
        }//end ProcessFloatVehicleParam
-        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
+        internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue, float timestep)
        {
+            DetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
            switch (pParam)
            {
                case Vehicle.ANGULAR_FRICTION_TIMESCALE:
@@ -265,6 +266,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
        internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
        {
+            DetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", m_prim.LocalID, pParam, pValue);
            switch (pParam)
            {
                case Vehicle.REFERENCE_FRAME:
@@ -278,6 +280,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
        internal void ProcessVehicleFlags(int pParam, bool remove)
        {
+            DetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", m_prim.LocalID, pParam, remove);
            if (remove)
            {
                if (pParam == -1)
@@ -434,6 +437,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
        internal void ProcessTypeChange(Vehicle pType)
        {
+            DetailLog("{0},ProcessTypeChange,type={1}", m_prim.LocalID, pType);
            // Set Defaults For Type
            m_type = pType;
            switch (pType)
@@ -594,11 +598,10 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                    m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
                    m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT);
                    break;
            }
        }//end SetDefaultsForType
-        internal void Step(float pTimestep,  BSScene pParentScene)
+        internal void Step(float pTimestep)
        {
            if (m_type == Vehicle.TYPE_NONE) return;
@@ -606,21 +609,34 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            if (frcount > 100)
                frcount = 0;
-            MoveLinear(pTimestep, pParentScene);
+            MoveLinear(pTimestep);
            MoveAngular(pTimestep);
            LimitRotation(pTimestep);
+            DetailLog("{0},Dynamics,done,pos={1},force={2},velocity={3},angvel={4}", 
+                    m_prim.LocalID, m_prim.Position, m_prim.Force, m_prim.Velocity, m_prim.RotationalVelocity);
        }// end Step
-        private void MoveLinear(float pTimestep, BSScene _pParentScene)
+        private void MoveLinear(float pTimestep)
        {
-            if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))  // requested m_linearMotorDirection is significant
+            // requested m_linearMotorDirection is significant
+            // if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
+            if (m_linearMotorDirection.LengthSquared() > 0.0001f)
            {
+                Vector3 origDir = m_linearMotorDirection;
+                Vector3 origVel = m_lastLinearVelocityVector;
                // add drive to body
-                Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
+                // Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
-                m_lastLinearVelocityVector += (addAmount*10);  // lastLinearVelocityVector is the current body velocity vector?
+                Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale);
+                // lastLinearVelocityVector is the current body velocity vector?
+                // RA: Not sure what the *10 is for. A correction for pTimestep?
+                // m_lastLinearVelocityVector += (addAmount*10);  
+                m_lastLinearVelocityVector += addAmount;  
                // This will work temporarily, but we really need to compare speed on an axis
                // KF: Limit body velocity to applied velocity?
+                // Limit the velocity vector to less than the last set linear motor direction
                if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
                    m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
                if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
@@ -630,76 +646,93 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                // decay applied velocity
                Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
-                //Console.WriteLine("decay: " + decayfraction);
                m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
-                //Console.WriteLine("actual: " + m_linearMotorDirection);
+                /*
+                Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/m_linearMotorTimescale;
+                m_lastLinearVelocityVector += addAmount;
+                float decayfraction = (1.0f - 1.0f / m_linearMotorDecayTimescale);
+                m_linearMotorDirection *= decayfraction;
+                 */
+                DetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},add={3},decay={4},dir={5},vel={6}",
+                    m_prim.LocalID, origDir, origVel, addAmount, decayfraction, m_linearMotorDirection, m_lastLinearVelocityVector);
            }
            else
-            {        // requested is not significant
+            {
-                    // if what remains of applied is small, zero it.
+                // if what remains of applied is small, zero it.
-                if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
+                // if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
-                    m_lastLinearVelocityVector = Vector3.Zero;
+                //     m_lastLinearVelocityVector = Vector3.Zero;
+                m_linearMotorDirection = Vector3.Zero;
+                m_lastLinearVelocityVector = Vector3.Zero;
            }
            // convert requested object velocity to world-referenced vector
-            m_dir = m_lastLinearVelocityVector;
+            Quaternion rotq = m_prim.Orientation;
-            Quaternion rot = m_prim.Orientation;
+            m_dir = m_lastLinearVelocityVector * rotq;
-            Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);    // rotq = rotation of object
-            m_dir *= rotq;                            // apply obj rotation to velocity vector
+            // Add the various forces into m_dir which will be our new direction vector (velocity)
-            // add Gravity andBuoyancy
+            // add Gravity and Buoyancy
            // KF: So far I have found no good method to combine a script-requested
            // .Z velocity and gravity. Therefore only 0g will used script-requested
            // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
            Vector3 grav = Vector3.Zero;
-            // There is some gravity, make a gravity force vector
+            // There is some gravity, make a gravity force vector that is applied after object velocity.
-            // that is applied after object velocity.
-            float objMass = m_prim.Mass;
            // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
-            grav.Z = _pParentScene.DefaultGravity.Z * objMass * (1f - m_VehicleBuoyancy);
+            grav.Z = m_prim.Scene.DefaultGravity.Z * m_prim.Mass * (1f - m_VehicleBuoyancy);
            // Preserve the current Z velocity
            Vector3 vel_now = m_prim.Velocity;
            m_dir.Z = vel_now.Z;        // Preserve the accumulated falling velocity
            Vector3 pos = m_prim.Position;
+            Vector3 posChange = pos;
 //            Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
-            Vector3 posChange = new Vector3();
-            posChange.X = pos.X - m_lastPositionVector.X;
-            posChange.Y = pos.Y - m_lastPositionVector.Y;
-            posChange.Z = pos.Z - m_lastPositionVector.Z;
            double Zchange = Math.Abs(posChange.Z);
            if (m_BlockingEndPoint != Vector3.Zero)
            {
+                bool changed = false;
                if (pos.X >= (m_BlockingEndPoint.X - (float)1))
                {
                    pos.X -= posChange.X + 1;
-                    m_prim.Position = pos;
+                    changed = true;
                }
                if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
                {
                    pos.Y -= posChange.Y + 1;
-                    m_prim.Position = pos;
+                    changed = true;
                }
                if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
                {
                    pos.Z -= posChange.Z + 1;
-                    m_prim.Position = pos;
+                    changed = true;
                }
                if (pos.X <= 0)
                {
                    pos.X += posChange.X + 1;
-                    m_prim.Position = pos;
+                    changed = true;
                }
                if (pos.Y <= 0)
                {
                    pos.Y += posChange.Y + 1;
+                    changed = true;
+                }
+                if (changed)
+                {
                    m_prim.Position = pos;
+                    DetailLog("{0},MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
+                                m_prim.LocalID, m_BlockingEndPoint, posChange, pos);
                }
            }
-            if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
+            // If below the terrain, move us above the ground a little.
+            if (pos.Z < m_prim.Scene.GetTerrainHeightAtXYZ(pos))
            {
-                pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
+                pos.Z = m_prim.Scene.GetTerrainHeightAtXYZ(pos) + 2;
                m_prim.Position = pos;
+                DetailLog("{0},MoveLinear,terrainHeight,pos={1}", m_prim.LocalID, pos);
            }
            // Check if hovering
@@ -708,11 +741,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                // We should hover, get the target height
                if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
                {
-                    m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
+                    m_VhoverTargetHeight = m_prim.Scene.GetWaterLevel() + m_VhoverHeight;
                }
                if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
                {
-                    m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
+                    m_VhoverTargetHeight = m_prim.Scene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
                }
                if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
                {
@@ -746,6 +779,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                    }
                }
+                DetailLog("{0},MoveLinear,hover,pos={1},dir={2},height={3},target={4}", m_prim.LocalID, pos, m_dir, m_VhoverHeight, m_VhoverTargetHeight);
 //                m_VhoverEfficiency = 0f;    // 0=boucy, 1=Crit.damped
 //                m_VhoverTimescale = 0f;        // time to acheive height
 //                pTimestep  is time since last frame,in secs
@@ -774,12 +809,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                {
                    grav.Z = (float)(grav.Z * 1.125);
                }
-                float terraintemp = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
+                float terraintemp = m_prim.Scene.GetTerrainHeightAtXYZ(pos);
                float postemp = (pos.Z - terraintemp);
                if (postemp > 2.5f)
                {
                    grav.Z = (float)(grav.Z * 1.037125);
                }
+                DetailLog("{0},MoveLinear,limitMotorUp,grav={1}", m_prim.LocalID, grav);
                //End Experimental Values
            }
            if ((m_flags & (VehicleFlag.NO_X)) != 0)
@@ -800,32 +836,39 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            // Apply velocity
            m_prim.Velocity = m_dir;
            // apply gravity force
-            m_prim.Force = grav;
+            // Why is this set here? The physics engine already does gravity.
+            // m_prim.AddForce(grav, false);
+            // m_prim.Force = grav;
-            // apply friction
+            // Apply friction
            Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
            m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
+            DetailLog("{0},MoveLinear,done,pos={1},vel={2},force={3},decay={4}", 
+                        m_prim.LocalID, m_lastPositionVector, m_dir, grav, decayamount);
        } // end MoveLinear()
        private void MoveAngular(float pTimestep)
        {
-            /*
+            // m_angularMotorDirection         // angular velocity requested by LSL motor
-            private Vector3 m_angularMotorDirection = Vector3.Zero;            // angular velocity requested by LSL motor
+            // m_angularMotorApply             // application frame counter
-            private int m_angularMotorApply = 0;                            // application frame counter
+            // m_angularMotorVelocity          // current angular motor velocity (ramps up and down)
-             private float m_angularMotorVelocity = 0;                        // current angular motor velocity (ramps up and down)
+            // m_angularMotorTimescale         // motor angular velocity ramp up rate
-            private float m_angularMotorTimescale = 0;                        // motor angular velocity ramp up rate
+            // m_angularMotorDecayTimescale    // motor angular velocity decay rate
-            private float m_angularMotorDecayTimescale = 0;                    // motor angular velocity decay rate
+            // m_angularFrictionTimescale      // body angular velocity  decay rate
-            private Vector3 m_angularFrictionTimescale = Vector3.Zero;        // body angular velocity  decay rate
+            // m_lastAngularVelocity           // what was last applied to body
-            private Vector3 m_lastAngularVelocity = Vector3.Zero;            // what was last applied to body
-            */
            // Get what the body is doing, this includes 'external' influences
            Vector3 angularVelocity = m_prim.RotationalVelocity;
-   //         Vector3 angularVelocity = Vector3.Zero;
            if (m_angularMotorApply > 0)
            {
+                // Rather than snapping the angular motor velocity from the old value to
+                //    a newly set velocity, this routine steps the value from the previous
+                //    value (m_angularMotorVelocity) to the requested value (m_angularMotorDirection).
+                // There are m_angularMotorApply steps.
+                Vector3 origAngularVelocity = m_angularMotorVelocity;
                // ramp up to new value
                //   current velocity  +=                         error                       /    (time to get there / step interval)
                //                               requested speed            -  last motor speed
@@ -833,23 +876,21 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) /  (m_angularMotorTimescale / pTimestep);
                m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) /  (m_angularMotorTimescale / pTimestep);
+                DetailLog("{0},MoveAngular,angularMotorApply,apply={1},origvel={2},dir={3},vel={4}", 
+                        m_prim.LocalID,m_angularMotorApply,origAngularVelocity, m_angularMotorDirection, m_angularMotorVelocity);
                m_angularMotorApply--;        // This is done so that if script request rate is less than phys frame rate the expected
                                            // velocity may still be acheived.
            }
            else
            {
-                // no motor recently applied, keep the body velocity
+                // No motor recently applied, keep the body velocity
-        /*        m_angularMotorVelocity.X = angularVelocity.X;
-                m_angularMotorVelocity.Y = angularVelocity.Y;
-                m_angularMotorVelocity.Z = angularVelocity.Z; */
                // and decay the velocity
                m_angularMotorVelocity -= m_angularMotorVelocity /  (m_angularMotorDecayTimescale / pTimestep);
            } // end motor section
            // Vertical attractor section
            Vector3 vertattr = Vector3.Zero;
            if (m_verticalAttractionTimescale < 300)
            {
                float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
@@ -871,7 +912,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                // Error is 0 (no error) to +/- 2 (max error)
                // scale it by VAservo
                verterr = verterr * VAservo;
-//if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
                // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
                // Change  Body angular velocity  X based on Y, and Y based on X. Z is not changed.
@@ -884,11 +924,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                vertattr.X += bounce * angularVelocity.X;
                vertattr.Y += bounce * angularVelocity.Y;
+                DetailLog("{0},MoveAngular,verticalAttraction,verterr={1},bounce={2},vertattr={3}", 
+                            m_prim.LocalID, verterr, bounce, vertattr);
            } // else vertical attractor is off
-    //        m_lastVertAttractor = vertattr;
+            // m_lastVertAttractor = vertattr;
            // Bank section tba
            // Deflection section tba
            // Sum velocities
@@ -898,11 +942,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            {
                m_lastAngularVelocity.X = 0;
                m_lastAngularVelocity.Y = 0;
+                DetailLog("{0},MoveAngular,noDeflectionUp,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
            }
            if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
            {
                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
+                DetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", m_prim.LocalID, m_lastAngularVelocity);
            }
             // apply friction
@@ -912,10 +958,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
            // Apply to the body
            m_prim.RotationalVelocity = m_lastAngularVelocity;
+            DetailLog("{0},MoveAngular,done,decay={1},lastAngular={2}", m_prim.LocalID, decayamount, m_lastAngularVelocity);
        } //end MoveAngular
        internal void LimitRotation(float timestep)
        {
-            Quaternion rotq = m_prim.Orientation;    // rotq = rotation of object
+            Quaternion rotq = m_prim.Orientation;
            Quaternion m_rot = rotq;
            bool changed = false;
            if (m_RollreferenceFrame != Quaternion.Identity)
@@ -923,18 +971,22 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                if (rotq.X >= m_RollreferenceFrame.X)
                {
                    m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
+                    changed = true;
                }
                if (rotq.Y >= m_RollreferenceFrame.Y)
                {
                    m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
+                    changed = true;
                }
                if (rotq.X <= -m_RollreferenceFrame.X)
                {
                    m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
+                    changed = true;
                }
                if (rotq.Y <= -m_RollreferenceFrame.Y)
                {
                    m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
+                    changed = true;
                }
                changed = true;
            }
@@ -944,8 +996,23 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                m_rot.Y = 0;
                changed = true;
            }
+            if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
+            {
+                m_rot.X = 0;
+                m_rot.Y = 0;
+                changed = true;
+            }
            if (changed)
                m_prim.Orientation = m_rot;
+            DetailLog("{0},LimitRotation,done,changed={1},orig={2},new={3}", m_prim.LocalID, changed, rotq, m_rot);
+        }
+        // Invoke the detailed logger and output something if it's enabled.
+        private void DetailLog(string msg, params Object[] args)
+        {
+            if (m_prim.Scene.VehicleLoggingEnabled)
+                m_prim.Scene.PhysicsLogging.Write(msg, args);
        }
    }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 130f1ca..71a4303 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -42,6 +42,8 @@ public sealed class BSPrim : PhysicsActor
    private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
    private static readonly string LogHeader = "[BULLETS PRIM]";
+    private void DebugLog(string mm, params Object[] xx) { if (_scene.shouldDebugLog) m_log.DebugFormat(mm, xx); }
    private IMesh _mesh;
    private PrimitiveBaseShape _pbs;
    private ShapeData.PhysicsShapeType _shapeType;
@@ -50,6 +52,7 @@ public sealed class BSPrim : PhysicsActor
    private List<ConvexResult> _hulls;
    private BSScene _scene;
+    public BSScene Scene { get { return _scene; } }
    private String _avName;
    private uint _localID = 0;
@@ -86,8 +89,8 @@ public sealed class BSPrim : PhysicsActor
    private bool _kinematic;
    private float _buoyancy;
-    private List<BSPrim> _childrenPrims;
    private BSPrim _parentPrim;
+    private List<BSPrim> _childrenPrims;
    private int _subscribedEventsMs = 0;
    private int _nextCollisionOkTime = 0;
@@ -145,9 +148,19 @@ public sealed class BSPrim : PhysicsActor
    public void Destroy()
    {
        // m_log.DebugFormat("{0}: Destroy, id={1}", LogHeader, LocalID);
+        // DetailLog("{0},Destroy", LocalID);
        // Undo any vehicle properties
        _vehicle.ProcessTypeChange(Vehicle.TYPE_NONE);
        _scene.RemoveVehiclePrim(this);     // just to make sure
+        // undo any dependance with/on other objects
+        if (_parentPrim != null)
+        {
+            // If I'm someone's child, tell them to forget about me.
+            _parentPrim.RemoveChildFromLinkset(this);
+            _parentPrim = null;
+        }
        _scene.TaintedObject(delegate()
        {
            // everything in the C# world will get garbage collected. Tell the C++ world to free stuff.
@@ -202,7 +215,8 @@ public sealed class BSPrim : PhysicsActor
    // link me to the specified parent
    public override void link(PhysicsActor obj) {
        BSPrim parent = obj as BSPrim;
-        // m_log.DebugFormat("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, obj.LocalID);
+        DebugLog("{0}: link {1}/{2} to {3}", LogHeader, _avName, _localID, obj.LocalID);
+        DetailLog("{0},link,parent={1}", LocalID, obj.LocalID);
        // TODO: decide if this parent checking needs to happen at taint time
        if (_parentPrim == null)
        {
@@ -225,7 +239,7 @@ public sealed class BSPrim : PhysicsActor
                else
                {
                    // asking to reparent a prim should not happen
-                    m_log.ErrorFormat("{0}: Reparenting a prim. ", LogHeader);
+                    m_log.ErrorFormat("{0}: link(): Reparenting a prim. ", LogHeader);
                }
            }
        }
@@ -236,7 +250,9 @@ public sealed class BSPrim : PhysicsActor
    public override void delink() {
        // TODO: decide if this parent checking needs to happen at taint time
        // Race condition here: if link() and delink() in same simulation tick, the delink will not happen
-        // m_log.DebugFormat("{0}: delink {1}/{2}", LogHeader, _avName, _localID);
+        DebugLog("{0}: delink {1}/{2}. Parent={3}", LogHeader, _avName, _localID, 
+                (_parentPrim==null ? "NULL" : _parentPrim._avName+"/"+_parentPrim.LocalID.ToString()));
+        DetailLog("{0},delink,parent={1}", LocalID, (_parentPrim==null ? "NULL" : _parentPrim.LocalID.ToString()));
        if (_parentPrim != null)
        {
            _parentPrim.RemoveChildFromLinkset(this);
@@ -252,8 +268,10 @@ public sealed class BSPrim : PhysicsActor
        {
            if (!_childrenPrims.Contains(child))
            {
+                DebugLog("{0}: AddChildToLinkset: adding child {1} to {2}", LogHeader, child.LocalID, this.LocalID);
+                DetailLog("{0},AddChildToLinkset,child={1}", LocalID, pchild.LocalID);
                _childrenPrims.Add(child);
-                child.ParentPrim = this;    // the child has gained a parent
+                child._parentPrim = this;    // the child has gained a parent
                RecreateGeomAndObject();    // rebuild my shape with the new child added
            }
        });
@@ -269,9 +287,14 @@ public sealed class BSPrim : PhysicsActor
        {
            if (_childrenPrims.Contains(child))
            {
-                BulletSimAPI.RemoveConstraint(_scene.WorldID, child.LocalID, this.LocalID);
+                DebugLog("{0}: RemoveChildFromLinkset: Removing constraint to {1}", LogHeader, child.LocalID);
+                DetailLog("{0},RemoveChildToLinkset,child={1}", LocalID, pchild.LocalID);
+                if (!BulletSimAPI.RemoveConstraintByID(_scene.WorldID, child.LocalID))
+                {
+                    m_log.ErrorFormat("{0}: RemoveChildFromLinkset: Failed remove constraint for {1}", LogHeader, child.LocalID);
+                }
                _childrenPrims.Remove(child);
-                child.ParentPrim = null;    // the child has lost its parent
+                child._parentPrim = null;    // the child has lost its parent
                RecreateGeomAndObject();    // rebuild my shape with the child removed
            }
            else
@@ -282,11 +305,6 @@ public sealed class BSPrim : PhysicsActor
        return;
    }
-    public BSPrim ParentPrim
-    {
-        set { _parentPrim = value; }
-    }
    // return true if we are the root of a linkset (there are children to manage)
    public bool IsRootOfLinkset
    {
@@ -304,20 +322,28 @@ public sealed class BSPrim : PhysicsActor
        base.RequestPhysicsterseUpdate();
    }
-    public override void LockAngularMotion(OMV.Vector3 axis) { return; }
+    public override void LockAngularMotion(OMV.Vector3 axis)
+    { 
+        DetailLog("{0},LockAngularMotion,call,axis={1}", LocalID, axis);
+        return;
+    }
    public override OMV.Vector3 Position { 
        get { 
-            // don't do the following GetObjectPosition because this function is called a zillion times
+            // child prims move around based on their parent. Need to get the latest location
+            if (_parentPrim != null)
+                _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
+            // don't do the GetObjectPosition for root elements because this function is called a zillion times
            // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
            return _position; 
        } 
        set {
            _position = value;
+            // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
            _scene.TaintedObject(delegate()
            {
+                DetailLog("{0},SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
-                // m_log.DebugFormat("{0}: setPosition: id={1}, position={2}", LogHeader, _localID, _position);
            });
        } 
    }
@@ -330,6 +356,7 @@ public sealed class BSPrim : PhysicsActor
            _force = value;
            _scene.TaintedObject(delegate()
            {
+                DetailLog("{0},SetForce,taint,force={1}", LocalID, _force);
                BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
            });
        } 
@@ -341,15 +368,23 @@ public sealed class BSPrim : PhysicsActor
        } 
        set {
            Vehicle type = (Vehicle)value;
-            _vehicle.ProcessTypeChange(type);
            _scene.TaintedObject(delegate()
            {
+                DetailLog("{0},SetVehicleType,taint,type={1}", LocalID, type);
+                _vehicle.ProcessTypeChange(type);
                if (type == Vehicle.TYPE_NONE)
                {
                    _scene.RemoveVehiclePrim(this);
                }
                else
                {
+                    _scene.TaintedObject(delegate()
+                    {
+                        // Tell the physics engine to clear state
+                        IntPtr obj = BulletSimAPI.GetBodyHandleWorldID2(_scene.WorldID, LocalID);
+                        BulletSimAPI.ClearForces2(obj);
+                    });
                    // make it so the scene will call us each tick to do vehicle things
                    _scene.AddVehiclePrim(this);
                }
@@ -359,37 +394,52 @@ public sealed class BSPrim : PhysicsActor
    }
    public override void VehicleFloatParam(int param, float value) 
    {
-        _vehicle.ProcessFloatVehicleParam((Vehicle)param, value);
+        m_log.DebugFormat("{0} VehicleFloatParam. {1} <= {2}", LogHeader, param, value);
+        _scene.TaintedObject(delegate()
+        {
+            _vehicle.ProcessFloatVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
+        });
    }
    public override void VehicleVectorParam(int param, OMV.Vector3 value) 
    {
-        _vehicle.ProcessVectorVehicleParam((Vehicle)param, value);
+        m_log.DebugFormat("{0} VehicleVectorParam. {1} <= {2}", LogHeader, param, value);
+        _scene.TaintedObject(delegate()
+        {
+            _vehicle.ProcessVectorVehicleParam((Vehicle)param, value, _scene.LastSimulatedTimestep);
+        });
    }
    public override void VehicleRotationParam(int param, OMV.Quaternion rotation) 
    {
-        _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation);
+        m_log.DebugFormat("{0} VehicleRotationParam. {1} <= {2}", LogHeader, param, rotation);
+        _scene.TaintedObject(delegate()
+        {
+            _vehicle.ProcessRotationVehicleParam((Vehicle)param, rotation);
+        });
    }
    public override void VehicleFlags(int param, bool remove) 
    {
-        _vehicle.ProcessVehicleFlags(param, remove);
+        m_log.DebugFormat("{0} VehicleFlags. {1}. Remove={2}", LogHeader, param, remove);
+        _scene.TaintedObject(delegate()
+        {
+            _vehicle.ProcessVehicleFlags(param, remove);
+        });
    }
-    // Called each simulation step to advance vehicle characteristics
+    // Called each simulation step to advance vehicle characteristics.
+    // Called from Scene when doing simulation step so we're in taint processing time.
    public void StepVehicle(float timeStep)
    {
-        _vehicle.Step(timeStep, _scene);
+        _vehicle.Step(timeStep);
    }
    // Allows the detection of collisions with inherently non-physical prims. see llVolumeDetect for more
    public override void SetVolumeDetect(int param) {
        bool newValue = (param != 0);
-        if (_isVolumeDetect != newValue)
+        _isVolumeDetect = newValue;
+        _scene.TaintedObject(delegate()
        {
-            _isVolumeDetect = newValue;
+            SetObjectDynamic();
-            _scene.TaintedObject(delegate()
+        });
-            {
-                SetObjectDynamic();
-            });
-        }
        return; 
    }
@@ -397,9 +447,11 @@ public sealed class BSPrim : PhysicsActor
    public override OMV.Vector3 CenterOfMass { get { return OMV.Vector3.Zero; } }
    public override OMV.Vector3 Velocity { 
        get { return _velocity; } 
-        set { _velocity = value; 
+        set {
+            _velocity = value;
            _scene.TaintedObject(delegate()
            {
+                DetailLog("{0},SetVelocity,taint,vel={1}", LocalID, _velocity);
                BulletSimAPI.SetObjectVelocity(_scene.WorldID, LocalID, _velocity);
            });
        } 
@@ -407,6 +459,7 @@ public sealed class BSPrim : PhysicsActor
    public override OMV.Vector3 Torque { 
        get { return _torque; } 
        set { _torque = value; 
+            DetailLog("{0},SetTorque,call,torque={1}", LocalID, _torque);
        } 
    }
    public override float CollisionScore { 
@@ -419,13 +472,21 @@ public sealed class BSPrim : PhysicsActor
        set { _acceleration = value; }
    }
    public override OMV.Quaternion Orientation { 
-        get { return _orientation; } 
+        get {
+            if (_parentPrim != null)
+            {
+                // children move around because tied to parent. Get a fresh value.
+                _orientation = BulletSimAPI.GetObjectOrientation(_scene.WorldID, LocalID);
+            }
+            return _orientation;
+        } 
        set {
            _orientation = value;
-            // m_log.DebugFormat("{0}: set orientation: id={1}, ori={2}", LogHeader, LocalID, _orientation);
+            // TODO: what does it mean if a child in a linkset changes its orientation? Rebuild the constraint?
            _scene.TaintedObject(delegate()
            {
                // _position = BulletSimAPI.GetObjectPosition(_scene.WorldID, _localID);
+                DetailLog("{0},SetOrientation,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                BulletSimAPI.SetObjectTranslation(_scene.WorldID, _localID, _position, _orientation);
            });
        } 
@@ -458,8 +519,9 @@ public sealed class BSPrim : PhysicsActor
        get { return !IsPhantom && !_isVolumeDetect; }
    }
-    // make gravity work if the object is physical and not selected
+    // Make gravity work if the object is physical and not selected
-    // no locking here because only called when it is safe
+    // No locking here because only called when it is safe
+    // Only called at taint time so it is save to call into Bullet.
    private void SetObjectDynamic()
    {
        // m_log.DebugFormat("{0}: ID={1}, SetObjectDynamic: IsStatic={2}, IsSolid={3}", LogHeader, _localID, IsStatic, IsSolid);
@@ -476,6 +538,7 @@ public sealed class BSPrim : PhysicsActor
            RecreateGeomAndObject();
        }
+        DetailLog("{0},SetObjectDynamic,taint,static={1},solid={2},mass={3}", LocalID, IsStatic, IsSolid, _mass);
        BulletSimAPI.SetObjectProperties(_scene.WorldID, LocalID, IsStatic, IsSolid, SubscribedEvents(), _mass);
    }
@@ -516,11 +579,24 @@ public sealed class BSPrim : PhysicsActor
        set { _floatOnWater = value; } 
    }
    public override OMV.Vector3 RotationalVelocity { 
-        get { return _rotationalVelocity; } 
+        get {
-        set { _rotationalVelocity = value; 
+            /*
+            OMV.Vector3 pv = OMV.Vector3.Zero;
+            // if close to zero, report zero
+            // This is copied from ODE but I'm not sure why it returns zero but doesn't
+            //    zero the property in the physics engine.
+            if (_rotationalVelocity.ApproxEquals(pv, 0.2f))
+                return pv;
+             */
+            return _rotationalVelocity;
+        } 
+        set {
+            _rotationalVelocity = value;
            // m_log.DebugFormat("{0}: RotationalVelocity={1}", LogHeader, _rotationalVelocity);
            _scene.TaintedObject(delegate()
            {
+                DetailLog("{0},SetRotationalVel,taint,rotvel={1}", LocalID, _rotationalVelocity);
                BulletSimAPI.SetObjectAngularVelocity(_scene.WorldID, LocalID, _rotationalVelocity);
            });
        } 
@@ -533,11 +609,13 @@ public sealed class BSPrim : PhysicsActor
    }
    public override float Buoyancy { 
        get { return _buoyancy; } 
-        set { _buoyancy = value;
+        set {
-        _scene.TaintedObject(delegate()
+            _buoyancy = value;
-        {
+            _scene.TaintedObject(delegate()
-            BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy);
+            {
-        });
+                DetailLog("{0},SetBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
+                BulletSimAPI.SetObjectBuoyancy(_scene.WorldID, _localID, _buoyancy);
+            });
        } 
    }
@@ -573,27 +651,45 @@ public sealed class BSPrim : PhysicsActor
    public override float APIDStrength { set { return; } }
    public override float APIDDamping { set { return; } }
+    private List<OMV.Vector3> m_accumulatedForces = new List<OMV.Vector3>();
    public override void AddForce(OMV.Vector3 force, bool pushforce) {
        if (force.IsFinite())
        {
-            _force.X += force.X;
+            // _force += force;
-            _force.Y += force.Y;
+            lock (m_accumulatedForces)
-            _force.Z += force.Z;
+                m_accumulatedForces.Add(new OMV.Vector3(force));
        }
        else
        {
            m_log.WarnFormat("{0}: Got a NaN force applied to a Character", LogHeader);
+            return;
        }
        _scene.TaintedObject(delegate()
        {
-            BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, _force);
+            lock (m_accumulatedForces)
+            {
+                if (m_accumulatedForces.Count > 0)
+                {
+                    OMV.Vector3 fSum = OMV.Vector3.Zero;
+                    foreach (OMV.Vector3 v in m_accumulatedForces)
+                    {
+                        fSum += v;
+                    }
+                    m_accumulatedForces.Clear();
+                    DetailLog("{0},SetObjectForce,taint,force={1}", LocalID, fSum);
+                    BulletSimAPI.SetObjectForce(_scene.WorldID, _localID, fSum);
+                }
+            }
        });
    }
    public override void AddAngularForce(OMV.Vector3 force, bool pushforce) { 
+        DetailLog("{0},AddAngularForce,call,angForce={1},push={2}", LocalID, force, pushforce);
        // m_log.DebugFormat("{0}: AddAngularForce. f={1}, push={2}", LogHeader, force, pushforce);
    }
    public override void SetMomentum(OMV.Vector3 momentum) { 
+        DetailLog("{0},SetMomentum,call,mom={1}", LocalID, momentum);
    }
    public override void SubscribeEvents(int ms) { 
        _subscribedEventsMs = ms;
@@ -918,6 +1014,7 @@ public sealed class BSPrim : PhysicsActor
                {
                    // m_log.DebugFormat("{0}: CreateGeom: Defaulting to sphere of size {1}", LogHeader, _size);
                    _shapeType = ShapeData.PhysicsShapeType.SHAPE_SPHERE;
+                    DetailLog("{0},CreateGeom,sphere", LocalID);
                    // Bullet native objects are scaled by the Bullet engine so pass the size in
                    _scale = _size;
                }
@@ -925,6 +1022,7 @@ public sealed class BSPrim : PhysicsActor
            else
            {
                // m_log.DebugFormat("{0}: CreateGeom: Defaulting to box. lid={1}, size={2}", LogHeader, LocalID, _size);
+                DetailLog("{0},CreateGeom,box", LocalID);
                _shapeType = ShapeData.PhysicsShapeType.SHAPE_BOX;
                _scale = _size;
            }
@@ -961,10 +1059,12 @@ public sealed class BSPrim : PhysicsActor
        // if this new shape is the same as last time, don't recreate the mesh
        if (_meshKey == newMeshKey) return;
+        DetailLog("{0},CreateGeomMesh,create,key={1}", LocalID, _meshKey);
        // Since we're recreating new, get rid of any previously generated shape
        if (_meshKey != 0)
        {
            // m_log.DebugFormat("{0}: CreateGeom: deleting old mesh. lID={1}, Key={2}", LogHeader, _localID, _meshKey);
+            DetailLog("{0},CreateGeomMesh,deleteOld,key={1}", LocalID, _meshKey);
            BulletSimAPI.DestroyMesh(_scene.WorldID, _meshKey);
            _mesh = null;
            _meshKey = 0;
@@ -981,7 +1081,6 @@ public sealed class BSPrim : PhysicsActor
        int vi = 0;
        foreach (OMV.Vector3 vv in vertices)
        {
-            // m_log.DebugFormat("{0}:  {1}: <{2:0.00}, {3:0.00}, {4:0.00}>", LogHeader, vi / 3, vv.X, vv.Y, vv.Z);
            verticesAsFloats[vi++] = vv.X;
            verticesAsFloats[vi++] = vv.Y;
            verticesAsFloats[vi++] = vv.Z;
@@ -995,6 +1094,7 @@ public sealed class BSPrim : PhysicsActor
        _shapeType = ShapeData.PhysicsShapeType.SHAPE_MESH;
        // meshes are already scaled by the meshmerizer
        _scale = new OMV.Vector3(1f, 1f, 1f);
+        DetailLog("{0},CreateGeomMesh,done", LocalID);
        return;
    }
@@ -1008,13 +1108,17 @@ public sealed class BSPrim : PhysicsActor
        // if the hull hasn't changed, don't rebuild it
        if (newHullKey == _hullKey) return;
+        DetailLog("{0},CreateGeomHull,create,key={1}", LocalID, _meshKey);
+        
        // Since we're recreating new, get rid of any previously generated shape
        if (_hullKey != 0)
        {
            // m_log.DebugFormat("{0}: CreateGeom: deleting old hull. Key={1}", LogHeader, _hullKey);
+            DetailLog("{0},CreateGeomHull,deleteOldHull,key={1}", LocalID, _meshKey);
            BulletSimAPI.DestroyHull(_scene.WorldID, _hullKey);
            _hullKey = 0;
            _hulls.Clear();
+            DetailLog("{0},CreateGeomHull,deleteOldMesh,key={1}", LocalID, _meshKey);
            BulletSimAPI.DestroyMesh(_scene.WorldID, _meshKey);
            _mesh = null;   // the mesh cannot match either
            _meshKey = 0;
@@ -1111,6 +1215,7 @@ public sealed class BSPrim : PhysicsActor
        _shapeType = ShapeData.PhysicsShapeType.SHAPE_HULL;
        // meshes are already scaled by the meshmerizer
        _scale = new OMV.Vector3(1f, 1f, 1f);
+        DetailLog("{0},CreateGeomHull,done", LocalID);
        return;
    }
@@ -1129,7 +1234,6 @@ public sealed class BSPrim : PhysicsActor
        if (IsRootOfLinkset)
        {
            // Create a linkset around this object
-            // CreateLinksetWithCompoundHull();
            CreateLinksetWithConstraints();
        }
        else
@@ -1191,33 +1295,33 @@ public sealed class BSPrim : PhysicsActor
    // TODO: make this more effeicient: a large linkset gets rebuilt over and over and prims are added
    void CreateLinksetWithConstraints()
    {
-        // m_log.DebugFormat("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, _childrenPrims.Count+1);
+        DebugLog("{0}: CreateLinkset. Root prim={1}, prims={2}", LogHeader, LocalID, _childrenPrims.Count+1);
        // remove any constraints that might be in place
        foreach (BSPrim prim in _childrenPrims)
        {
-            // m_log.DebugFormat("{0}: CreateLinkset: RemoveConstraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID);
+            DebugLog("{0}: CreateLinkset: RemoveConstraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID);
            BulletSimAPI.RemoveConstraint(_scene.WorldID, LocalID, prim.LocalID);
        }
        // create constraints between the root prim and each of the children
        foreach (BSPrim prim in _childrenPrims)
        {
-            // m_log.DebugFormat("{0}: CreateLinkset: AddConstraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID);
            // Zero motion for children so they don't interpolate
            prim.ZeroMotion();
            // relative position normalized to the root prim
-            OMV.Vector3 childRelativePosition = (prim._position - this._position) * OMV.Quaternion.Inverse(this._orientation);
+            OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(this._orientation);
+            OMV.Vector3 childRelativePosition = (prim._position - this._position) * invThisOrientation;
            // relative rotation of the child to the parent
-            OMV.Quaternion relativeRotation = OMV.Quaternion.Inverse(prim._orientation) * this._orientation;
+            OMV.Quaternion childRelativeRotation = invThisOrientation * prim._orientation;
            // this is a constraint that allows no freedom of movement between the two objects
            // http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
+            DebugLog("{0}: CreateLinkset: Adding a constraint between root prim {1} and child prim {2}", LogHeader, LocalID, prim.LocalID);
            BulletSimAPI.AddConstraint(_scene.WorldID, LocalID, prim.LocalID, 
                childRelativePosition,
-                relativeRotation,
+                childRelativeRotation,
                OMV.Vector3.Zero,
                OMV.Quaternion.Identity,
                OMV.Vector3.Zero, OMV.Vector3.Zero,
@@ -1252,78 +1356,71 @@ 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
+            _position = entprop.Position;
-            // if (_position != entprop.Position)
+            changed |= UpdatedProperties.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
+        // if (_orientation != entprop.Rotation)
+        if (!_orientation.ApproxEquals(entprop.Rotation, ROTATION_TOLERANCE))
        {
-            // Don't check for damping here -- it's done in BulletSim and SceneObjectPart.
+            _orientation = entprop.Rotation;
+            changed |= UpdatedProperties.Rotation;
-            // Only updates only for individual prims and for the root object of a linkset.
+        }
+        // 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);
+            DetailLog("{0},UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+                    LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
+            base.RequestPhysicsterseUpdate();
+        }
    }
    // I've collided with something
@@ -1362,5 +1459,11 @@ public sealed class BSPrim : PhysicsActor
            collisionCollection.Clear();
        }
    }
+    // Invoke the detailed logger and output something if it's enabled.
+    private void DetailLog(string msg, params Object[] args)
+    {
+        Scene.PhysicsLogging.Write(msg, args);
+    }
 }
 }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 417cb5f..9d41ce8 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -29,12 +29,13 @@ using System.Collections.Generic;
 using System.Runtime.InteropServices;
 using System.Text;
 using System.Threading;
-using Nini.Config;
-using log4net;
 using OpenSim.Framework;
+using OpenSim.Region.Framework;
 using OpenSim.Region.Physics.Manager;
+using Logging = OpenSim.Region.CoreModules.Framework.Statistics.Logging;
+using Nini.Config;
+using log4net;
 using OpenMetaverse;
-using OpenSim.Region.Framework;
 // TODOs for BulletSim (for BSScene, BSPrim, BSCharacter and BulletSim)
 // Debug linkset 
@@ -44,15 +45,17 @@ using OpenSim.Region.Framework;
 // Compute physics FPS reasonably
 // Based on material, set density and friction
 // More efficient memory usage when passing hull information from BSPrim to BulletSim
+// Move all logic out of the C++ code and into the C# code for easier future modifications.
 // Four states of prim: Physical, regular, phantom and selected. Are we modeling these correctly?
 //     In SL one can set both physical and phantom (gravity, does not effect others, makes collisions with ground)
 //     At the moment, physical and phantom causes object to drop through the terrain
 // Physical phantom objects and related typing (collision options )
+// Use collision masks for collision with terrain and phantom objects 
 // Check out llVolumeDetect. Must do something for that.
 // Should prim.link() and prim.delink() membership checking happen at taint time?
+// changing the position and orientation of a linked prim must rebuild the constraint with the root.
 // Mesh sharing. Use meshHash to tell if we already have a hull of that shape and only create once
 // Do attachments need to be handled separately? Need collision events. Do not collide with VolumeDetect
-// Use collision masks for collision with terrain and phantom objects 
 // Implement the genCollisions feature in BulletSim::SetObjectProperties (don't pass up unneeded collisions)
 // Implement LockAngularMotion
 // Decide if clearing forces is the right thing to do when setting position (BulletSim::SetObjectTranslation)
@@ -60,9 +63,6 @@ using OpenSim.Region.Framework;
 // Remove mesh and Hull stuff. Use mesh passed to bullet and use convexdecom from bullet.
 // Add PID movement operations. What does ScenePresence.MoveToTarget do?
 // Check terrain size. 128 or 127?
-// Multiple contact points on collision?
-//    See code in ode::near... calls to collision_accounting_events()
-//    (This might not be a problem. ODE collects all the collisions with one object in one tick.)
 // Raycast
 // 
 namespace OpenSim.Region.Physics.BulletSPlugin
@@ -72,6 +72,8 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    private static readonly ILog m_log = LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
    private static readonly string LogHeader = "[BULLETS SCENE]";
+    private void DebugLog(string mm, params Object[] xx) { if (shouldDebugLog) m_log.DebugFormat(mm, xx); }
    public string BulletSimVersion = "?";
    private Dictionary<uint, BSCharacter> m_avatars = new Dictionary<uint, BSCharacter>();
@@ -105,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;
@@ -121,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;
@@ -147,8 +154,20 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    ConfigurationParameters[] m_params;
    GCHandle m_paramsHandle;
+    public bool shouldDebugLog { get; private set; }
    private BulletSimAPI.DebugLogCallback m_DebugLogCallbackHandle;
+    // Sometimes you just have to log everything.
+    public Logging.LogWriter PhysicsLogging;
+    private bool m_physicsLoggingEnabled;
+    private string m_physicsLoggingDir;
+    private string m_physicsLoggingPrefix;
+    private int m_physicsLoggingFileMinutes;
+    private bool m_vehicleLoggingEnabled;
+    public bool VehicleLoggingEnabled { get { return m_vehicleLoggingEnabled; } }
    public BSScene(string identifier)
    {
        m_initialized = false;
@@ -169,17 +188,32 @@ public class BSScene : PhysicsScene, IPhysicsParameters
        m_updateArray = new EntityProperties[m_maxUpdatesPerFrame];
        m_updateArrayPinnedHandle = GCHandle.Alloc(m_updateArray, GCHandleType.Pinned);
+        // Enable very detailed logging.
+        // By creating an empty logger when not logging, the log message invocation code
+        // can be left in and every call doesn't have to check for null.
+        if (m_physicsLoggingEnabled)
+        {
+            PhysicsLogging = new Logging.LogWriter(m_physicsLoggingDir, m_physicsLoggingPrefix, m_physicsLoggingFileMinutes);
+        }
+        else
+        {
+            PhysicsLogging = new Logging.LogWriter();
+        }
        // Get the version of the DLL
        // TODO: this doesn't work yet. Something wrong with marshaling the returned string.
        // BulletSimVersion = BulletSimAPI.GetVersion();
        // m_log.WarnFormat("{0}: BulletSim.dll version='{1}'", LogHeader, BulletSimVersion);
        // if Debug, enable logging from the unmanaged code
-        if (m_log.IsDebugEnabled)
+        if (m_log.IsDebugEnabled || PhysicsLogging.Enabled)
        {
            m_log.DebugFormat("{0}: Initialize: Setting debug callback for unmanaged code", LogHeader);
-            // the handle is saved to it doesn't get freed after this call
+            if (PhysicsLogging.Enabled)
-            m_DebugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLogger);
+                m_DebugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLoggerPhysLog);
+            else
+                m_DebugLogCallbackHandle = new BulletSimAPI.DebugLogCallback(BulletLogger);
+            // the handle is saved in a variable to make sure it doesn't get freed after this call
            BulletSimAPI.SetDebugLogCallback(m_DebugLogCallbackHandle);
        }
@@ -209,6 +243,7 @@ public class BSScene : PhysicsScene, IPhysicsParameters
        m_meshLOD = 8f;
        m_sculptLOD = 32f;
+        shouldDebugLog = false;
        m_detailedStatsStep = 0;            // disabled
        m_maxSubSteps = 10;
@@ -217,6 +252,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;
@@ -261,7 +299,9 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                _meshSculptedPrim = pConfig.GetBoolean("MeshSculptedPrim", _meshSculptedPrim);
                _forceSimplePrimMeshing = pConfig.GetBoolean("ForceSimplePrimMeshing", _forceSimplePrimMeshing);
+                shouldDebugLog = pConfig.GetBoolean("ShouldDebugLog", shouldDebugLog);
                m_detailedStatsStep = pConfig.GetInt("DetailedStatsStep", m_detailedStatsStep);
                m_meshLOD = pConfig.GetFloat("MeshLevelOfDetail", m_meshLOD);
                m_sculptLOD = pConfig.GetFloat("SculptLevelOfDetail", m_sculptLOD);
@@ -271,6 +311,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);
@@ -303,6 +346,14 @@ public class BSScene : PhysicsScene, IPhysicsParameters
                    parms.shouldSplitSimulationIslands = ParamBoolean(pConfig, "ShouldSplitSimulationIslands", parms.shouldSplitSimulationIslands);
                    parms.shouldEnableFrictionCaching = ParamBoolean(pConfig, "ShouldEnableFrictionCaching", parms.shouldEnableFrictionCaching);
                    parms.numberOfSolverIterations = pConfig.GetFloat("NumberOfSolverIterations", parms.numberOfSolverIterations);
+                // Very detailed logging for physics debugging
+                m_physicsLoggingEnabled = pConfig.GetBoolean("PhysicsLoggingEnabled", false);
+                m_physicsLoggingDir = pConfig.GetString("PhysicsLoggingDir", ".");
+                m_physicsLoggingPrefix = pConfig.GetString("PhysicsLoggingPrefix", "physics-");
+                m_physicsLoggingFileMinutes = pConfig.GetInt("PhysicsLoggingFileMinutes", 5);
+                // Very detailed logging for vehicle debugging
+                m_vehicleLoggingEnabled = pConfig.GetBoolean("VehicleLoggingEnabled", false);
            }
        }
        m_params[0] = parms;
@@ -323,12 +374,17 @@ public class BSScene : PhysicsScene, IPhysicsParameters
        return ret;
    }
    // Called directly from unmanaged code so don't do much
    private void BulletLogger(string msg)
    {
        m_log.Debug("[BULLETS UNMANAGED]:" + msg);
    }
+    
+    // Called directly from unmanaged code so don't do much
+    private void BulletLoggerPhysLog(string msg)
+    {
+        PhysicsLogging.Write("[BULLETS UNMANAGED]:" + msg);
+    }
    public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
    {
@@ -347,34 +403,42 @@ public class BSScene : PhysicsScene, IPhysicsParameters
    public override void RemoveAvatar(PhysicsActor actor)
    {
        // m_log.DebugFormat("{0}: RemoveAvatar", LogHeader);
-        if (actor is BSCharacter)
+        BSCharacter bsactor = actor as BSCharacter;
-        {
+        if (bsactor != null)
-            ((BSCharacter)actor).Destroy();
-        }
-        try
        {
-            lock (m_avatars) m_avatars.Remove(actor.LocalID);
+            try
-        }
+            {
-        catch (Exception e)
+                lock (m_avatars) m_avatars.Remove(actor.LocalID);
-        {
+            }
-            m_log.WarnFormat("{0}: Attempt to remove avatar that is not in physics scene: {1}", LogHeader, e);
+            catch (Exception e)
+            {
+                m_log.WarnFormat("{0}: Attempt to remove avatar that is not in physics scene: {1}", LogHeader, e);
+            }
+            bsactor.Destroy();
+            // bsactor.dispose();
        }
    }
    public override void RemovePrim(PhysicsActor prim)
    {
-        // m_log.DebugFormat("{0}: RemovePrim", LogHeader);
+        BSPrim bsprim = prim as BSPrim;
-        if (prim is BSPrim)
+        if (bsprim != null)
-        {
-            ((BSPrim)prim).Destroy();
-        }
-        try
        {
-            lock (m_prims) m_prims.Remove(prim.LocalID);
+            m_log.DebugFormat("{0}: RemovePrim. id={1}/{2}", LogHeader, bsprim.Name, bsprim.LocalID);
+            try
+            {
+                lock (m_prims) m_prims.Remove(bsprim.LocalID);
+            }
+            catch (Exception e)
+            {
+                m_log.ErrorFormat("{0}: Attempt to remove prim that is not in physics scene: {1}", LogHeader, e);
+            }
+            bsprim.Destroy();
+            // bsprim.dispose();
        }
-        catch (Exception e)
+        else
        {
-            m_log.WarnFormat("{0}: Attempt to remove prim that is not in physics scene: {1}", LogHeader, e);
+            m_log.ErrorFormat("{0}: Attempt to remove prim that is not a BSPrim type.", LogHeader);
        }
    }
@@ -400,6 +464,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;
@@ -459,7 +525,6 @@ public class BSScene : PhysicsScene, IPhysicsParameters
            for (int ii = 0; ii < updatedEntityCount; ii++)
            {
                EntityProperties entprop = m_updateArray[ii];
-                // m_log.DebugFormat("{0}: entprop[{1}]: id={2}, pos={3}", LogHeader, ii, entprop.ID, entprop.Position);
                BSPrim prim;
                if (m_prims.TryGetValue(entprop.ID, out prim))
                {
@@ -532,8 +597,17 @@ public class BSScene : PhysicsScene, IPhysicsParameters
        });
    }
+    // Someday we will have complex terrain with caves and tunnels
+    // For the moment, it's flat and convex
+    public float GetTerrainHeightAtXYZ(Vector3 loc)
+    {
+        return GetTerrainHeightAtXY(loc.X, loc.Y);
+    }
    public float GetTerrainHeightAtXY(float tX, float tY)
    {
+        if (tX < 0 || tX >= Constants.RegionSize || tY < 0 || tY >= Constants.RegionSize)
+            return 30;
        return m_heightMap[((int)tX) * Constants.RegionSize + ((int)tY)];
    }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 086f0dc..babb707 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -146,6 +146,22 @@ public struct ConfigurationParameters
    public const float numericFalse = 0f;
 }
+// Values used by Bullet and BulletSim to control collisions
+public enum CollisionFlags : uint
+{
+    STATIC_OBJECT                 = 1 << 0,
+    KINEMATIC_OBJECT              = 1 << 1,
+    NO_CONTACT_RESPONSE           = 1 << 2,
+    CUSTOM_MATERIAL_CALLBACK      = 1 << 3,
+    CHARACTER_OBJECT              = 1 << 4,
+    DISABLE_VISUALIZE_OBJECT      = 1 << 5,
+    DISABLE_SPU_COLLISION_PROCESS = 1 << 6,
+    // Following used by BulletSim to control collisions
+    VOLUME_DETECT_OBJECT          = 1 << 10,
+    PHANTOM_OBJECT                = 1 << 11,
+    PHYSICAL_OBJECT               = 1 << 12,
+};
 static class BulletSimAPI {
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -214,6 +230,9 @@ public static extern bool RemoveConstraint(uint worldID, uint id1, uint id2);
 public static extern Vector3 GetObjectPosition(uint WorldID, uint id);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern Quaternion GetObjectOrientation(uint WorldID, uint id);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern bool SetObjectTranslation(uint worldID, uint id, Vector3 position, Quaternion rotation);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -268,5 +287,37 @@ public static extern void DumpBulletStatistics();
 public delegate void DebugLogCallback([MarshalAs(UnmanagedType.LPStr)]string msg);
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern void SetDebugLogCallback(DebugLogCallback callback);
+// ===============================================================================
+// ===============================================================================
+// ===============================================================================
+// A new version of the API that moves all the logic out of the C++ code and into
+//    the C# code. This will make modifications easier for the next person.
+// This interface passes the actual pointers to the objects in the unmanaged
+//    address space. All the management (calls for creation/destruction/lookup)
+//    is done in the C# code.
+// The names have a 2 tacked on. This will be removed as the code gets rebuilt
+//    and the old code is removed from the C# code.
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetSimHandle2(uint worldID);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetBodyHandleWorldID2(uint worldID, uint id);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr GetBodyHandle2(IntPtr sim, uint id);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr ClearForces2(IntPtr obj);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr SetCollisionFlags2(IntPtr obj, uint flags);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr AddToCollisionFlags2(IntPtr obj, uint flags);
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern IntPtr RemoveFromCollisionFlags2(IntPtr obj, uint flags);
 }
 }