11 files changed, 593 insertions, 105 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index 2a5397e..f33c124 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -103,7 +103,7 @@ public sealed class BSCharacter : BSPhysObject
         PhysicsScene.TaintedObject("BSCharacter.create", delegate()
         {
             DetailLog("{0},BSCharacter.create,taint", LocalID);
-            // New body and shape into BSBody and BSShape
+            // New body and shape into PhysBody and PhysShape
             PhysicsScene.Shapes.GetBodyAndShape(true, PhysicsScene.World, this, null, null);
 
             SetPhysicalProperties();
@@ -126,7 +126,7 @@ public sealed class BSCharacter : BSPhysObject
     {
         BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, PhysBody.ptr);
 
-        ZeroMotion();
+        ZeroMotion(true);
         ForcePosition = _position;
         // Set the velocity and compute the proper friction
         ForceVelocity = _velocity;
@@ -218,18 +218,31 @@ public sealed class BSCharacter : BSPhysObject
     // Do it to the properties so the values get set in the physics engine.
     // Push the setting of the values to the viewer.
     // Called at taint time!
-    public override void ZeroMotion()
+    public override void ZeroMotion(bool inTaintTime)
     {
         _velocity = OMV.Vector3.Zero;
         _acceleration = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
 
         // Zero some other properties directly into the physics engine
-        BulletSimAPI.SetLinearVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
-        BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
-        BulletSimAPI.SetInterpolationVelocity2(PhysBody.ptr, OMV.Vector3.Zero, OMV.Vector3.Zero);
-        BulletSimAPI.ClearForces2(PhysBody.ptr);
+        PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
+        {
+            BulletSimAPI.ClearAllForces2(PhysBody.ptr);
+        });
     }
+    public override void ZeroAngularMotion(bool inTaintTime)
+    {
+        _rotationalVelocity = OMV.Vector3.Zero;
+
+        PhysicsScene.TaintedObject(inTaintTime, "BSCharacter.ZeroMotion", delegate()
+        {
+            BulletSimAPI.SetInterpolationAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+            BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+            // The next also get rid of applied linear force but the linear velocity is untouched.
+            BulletSimAPI.ClearForces2(PhysBody.ptr);
+        });
+    }
+
 
     public override void LockAngularMotion(OMV.Vector3 axis) { return; }
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 819635a..dbc9039 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -54,10 +54,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
     public sealed class BSDynamics
     {
+        private static string LogHeader = "[BULLETSIM VEHICLE]";
+
         private BSScene PhysicsScene { get; set; }
         // the prim this dynamic controller belongs to
         private BSPrim Prim { get; set; }
 
+        // mass of the vehicle fetched each time we're calles
+        private float m_vehicleMass;
+
         // Vehicle properties
         public Vehicle Type { get; set; }
 
@@ -516,7 +521,29 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // Friction effects are handled by this vehicle code
                 BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, 0f);
                 BulletSimAPI.SetHitFraction2(Prim.PhysBody.ptr, 0f);
+
+                // BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, 0.8f);
+
+                VDetailLog("{0},BSDynamics.Refresh,zeroingFriction and adding damping", Prim.LocalID);
+            }
+        }
+
+        public bool RemoveBodyDependencies(BSPhysObject prim)
+        {
+            // If active, we need to add our properties back when the body is rebuilt.
+            return IsActive;
+        }
+
+        public void RestoreBodyDependencies(BSPhysObject prim)
+        {
+            if (Prim.LocalID != prim.LocalID)
+            {
+                // The call should be on us by our prim. Error if not.
+                PhysicsScene.Logger.ErrorFormat("{0} RestoreBodyDependencies: called by not my prim. passedLocalID={1}, vehiclePrimLocalID={2}",
+                                LogHeader, prim.LocalID, Prim.LocalID);
+                return;
             }
+            Refresh();
         }
 
         // One step of the vehicle properties for the next 'pTimestep' seconds.
@@ -533,16 +560,26 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // m_lastLinearVelocityVector = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation);        // DEBUG:
             // END DEBUG
 
+            m_vehicleMass = Prim.Linkset.LinksetMass;
+
             MoveLinear(pTimestep);
+            // Commented out for debug
             MoveAngular(pTimestep);
-            LimitRotation(pTimestep);
+            // Prim.ApplyTorqueImpulse(-Prim.RotationalVelocity * m_vehicleMass, false);    // DEBUG DEBUG
+            // Prim.ForceRotationalVelocity = -Prim.RotationalVelocity;    // DEBUG DEBUG
 
-            // DEBUG: Trying to figure out why Bullet goes crazy when the root prim is moved.
-            // BulletSimAPI.SetInterpolationVelocity2(Prim.BSBody.ptr, m_newVelocity, m_lastAngularVelocity); // DEBUG DEBUG DEBUG
+            LimitRotation(pTimestep);
 
             // remember the position so next step we can limit absolute movement effects
             m_lastPositionVector = Prim.ForcePosition;
 
+            VDetailLog("{0},BSDynamics.Step,frict={1},grav={2},inertia={3},mass={4}",        // DEBUG DEBUG
+                                    Prim.LocalID, 
+                                    BulletSimAPI.GetFriction2(Prim.PhysBody.ptr),
+                                    BulletSimAPI.GetGravity2(Prim.PhysBody.ptr),
+                                    Prim.Inertia,
+                                    m_vehicleMass
+                                    );
             VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
                     Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity);
         }// end Step
@@ -555,25 +592,26 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // m_lastLinearVelocityVector is the current speed we are moving in that direction
             if (m_linearMotorDirection.LengthSquared() > 0.001f)
             {
-                Vector3 origDir = m_linearMotorDirection;
-                Vector3 origVel = m_lastLinearVelocityVector;
+                Vector3 origDir = m_linearMotorDirection;       // DEBUG
+                Vector3 origVel = m_lastLinearVelocityVector;   // DEBUG
+                // DEBUG: the vehicle velocity rotated to be relative to vehicle coordinates for comparison
                 Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation);   // DEBUG
 
-                // add drive to body
+                // Add (desiredVelocity - lastAppliedVelocity) / howLongItShouldTakeToComplete
                 Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep;
-                // lastLinearVelocityVector is the current body velocity vector
                 m_lastLinearVelocityVector += addAmount;
 
                 float decayFactor = (1.0f / m_linearMotorDecayTimescale) * pTimestep;
                 m_linearMotorDirection *= (1f - decayFactor);
 
-                Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep;
-                m_lastLinearVelocityVector *= (Vector3.One - frictionFactor);
-
                 // Rotate new object velocity from vehicle relative to world coordinates
                 m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation;
 
-                VDetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lmVel={8},newVel={9}",
+                // Apply friction for next time
+                Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep;
+                m_lastLinearVelocityVector *= (Vector3.One - frictionFactor);
+
+                VDetailLog("{0},MoveLinear,nonZero,origlmDir={1},origlvVel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lvVec={8},newVel={9}",
                                 Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, frictionFactor, 
                                 m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity);
             }
@@ -607,7 +645,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // If below the terrain, move us above the ground a little.
             float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
             // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset.
-            //     Need to add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
+            //     TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
             // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation;
             // if (rotatedSize.Z < terrainHeight)
             if (pos.Z < terrainHeight)
@@ -638,13 +676,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
                 {
-                    // If body is aready heigher, use its height as target height
-                    if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
+                    // If body is already heigher, use its height as target height
+                    if (pos.Z > m_VhoverTargetHeight)
+                        m_VhoverTargetHeight = pos.Z;
                 }
                 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
                 {
-                    if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
+                    if (Math.Abs(pos.Z - m_VhoverTargetHeight) > 0.2f)
                     {
+                        pos.Z = m_VhoverTargetHeight;
                         Prim.ForcePosition = pos;
                     }
                 }
@@ -709,25 +749,28 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
             }
 
-            // Limit absolute vertical change
-            float Zchange = Math.Abs(posChange.Z);
+            #region downForce
+            Vector3 downForce = Vector3.Zero;
+
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
-                if (Zchange > .3)
-                    grav.Z = (float)(grav.Z * 3);
-                if (Zchange > .15)
-                    grav.Z = (float)(grav.Z * 2);
-                if (Zchange > .75)
-                    grav.Z = (float)(grav.Z * 1.5);
-                if (Zchange > .05)
-                    grav.Z = (float)(grav.Z * 1.25);
-                if (Zchange > .025)
-                    grav.Z = (float)(grav.Z * 1.125);
-                float postemp = (pos.Z - terrainHeight);
-                if (postemp > 2.5f)
-                    grav.Z = (float)(grav.Z * 1.037125);
-                VDetailLog("{0},MoveLinear,limitMotorUp,grav={1}", Prim.LocalID, grav);
+                // If the vehicle is motoring into the sky, get it going back down.
+                // Is this an angular force or both linear and angular??
+                float distanceAboveGround = pos.Z - terrainHeight;
+                if (distanceAboveGround > 2f)
+                {
+                    // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
+                    // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
+                    downForce = new Vector3(0, 0, -distanceAboveGround);
+                }
+                // TODO: this calculation is all wrong. From the description at
+                //     (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
+                //     has a decay factor. This says this force should
+                //     be computed with a motor.
+                VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", 
+                                    Prim.LocalID, distanceAboveGround, downForce);
             }
+            #endregion // downForce
 
             // If not changing some axis, reduce out velocity
             if ((m_flags & (VehicleFlag.NO_X)) != 0)
@@ -737,13 +780,28 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             if ((m_flags & (VehicleFlag.NO_Z)) != 0)
                 m_newVelocity.Z = 0;
 
-            // Apply velocity
+            // Clamp REALLY high or low velocities
+            if (m_newVelocity.LengthSquared() > 1e6f)
+            {
+                m_newVelocity /= m_newVelocity.Length();
+                m_newVelocity *= 1000f;
+            }
+            else if (m_newVelocity.LengthSquared() < 1e-6f)
+                m_newVelocity = Vector3.Zero;
+
+            // Stuff new linear velocity into the vehicle
             Prim.ForceVelocity = m_newVelocity;
-            // Prim.AddForce(m_newVelocity * Prim.Linkset.LinksetMass, false);
-            Prim.AddForce(grav * Prim.Linkset.LinksetMass, false);
+            // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false);    // DEBUG DEBUG
 
-            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},grav={4}",
-                    Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, grav);
+            Vector3 totalDownForce = downForce + grav;
+            if (totalDownForce != Vector3.Zero)
+            {
+                Prim.AddForce(totalDownForce * m_vehicleMass, false);
+                // Prim.ApplyForceImpulse(totalDownForce * m_vehicleMass, false);
+            }
+
+            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}",
+                    Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, Prim.Velocity, totalDownForce);
 
         } // end MoveLinear()
 
@@ -765,7 +823,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 Vector3 origDir = m_angularMotorDirection;
 
                 //       new velocity    +=                      error                       /  (  time to get there   / step interval)
-                //                             requested speed     -    last motor speed
+                //                           requested direction   - current vehicle direction
                 m_angularMotorVelocity += (m_angularMotorDirection - m_angularMotorVelocity) /  (m_angularMotorTimescale / pTimestep);
                 // decay requested direction
                 m_angularMotorDirection *= (1.0f - (pTimestep * 1.0f/m_angularMotorDecayTimescale));
@@ -784,10 +842,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             Vector3 deflection = Vector3.Zero;
             Vector3 banking = Vector3.Zero;
 
+            // If vertical attaction timescale is reasonable and we applied an angular force last time...
             if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
             {
                 float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale;
-                if (Prim.Linkset.LinksetIsColliding)
+                if (Prim.IsColliding)
                     VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale);
 
                 VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
@@ -806,7 +865,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     verticalError.X = 2.0f - verticalError.X;
                     verticalError.Y = 2.0f - verticalError.Y;
                 }
-                // scale it by VAservo
+                // scale it by VAservo (timestep and timescale)
                 verticalError = verticalError * VAservo;
 
                 // As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y
@@ -822,25 +881,30 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 vertattr.X += bounce * angularVelocity.X;
                 vertattr.Y += bounce * angularVelocity.Y;
 
-                VDetailLog("{0},MoveAngular,verticalAttraction,verticalError={1},bounce={2},vertattr={3}",
-                            Prim.LocalID, verticalError, bounce, vertattr);
+                VDetailLog("{0},MoveAngular,verticalAttraction,VAservo={1},effic={2},verticalError={3},bounce={4},vertattr={5}",
+                            Prim.LocalID, VAservo, m_verticalAttractionEfficiency, verticalError, bounce, vertattr);
 
             }
             #endregion // Vertical attactor
 
             #region Deflection
 
-            //Forward is the prefered direction, but if the reference frame has changed, we need to take this into account as well
             if (m_angularDeflectionEfficiency != 0)
             {
-                Vector3 preferredAxisOfMotion =
+                // Compute a scaled vector that points in the preferred axis (X direction)
+                Vector3 scaledDefaultDirection =
                     new Vector3((pTimestep * 10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale)), 0, 0);
-                preferredAxisOfMotion *= Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
+                // Adding the current vehicle orientation and reference frame displaces the orientation to the frame.
+                // Rotate the scaled default axix relative to the actual vehicle direction giving where it should point.
+                Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
 
+                // Scale by efficiency and timescale
                 deflection = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
 
                 VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", 
                                         Prim.LocalID, preferredAxisOfMotion, deflection);
+                // This deflection computation is not correct.
+                deflection = Vector3.Zero;
             }
 
             #endregion
@@ -875,7 +939,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
                 else
                     banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X) * 4;
-                if (!Prim.Linkset.LinksetIsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
+                if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
                     //If they are colliding, we probably shouldn't shove the prim around... probably
                 {
                     float angVelZ = m_angularMotorVelocity.X*-1;
@@ -904,6 +968,40 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // Sum velocities
             m_lastAngularVelocity = m_angularMotorVelocity + vertattr + banking + deflection;
 
+            #region Linear Motor Offset
+
+            //Offset section
+            if (m_linearMotorOffset != Vector3.Zero)
+            {
+                //Offset of linear velocity doesn't change the linear velocity,
+                //   but causes a torque to be applied, for example...
+                //
+                //      IIIII     >>>   IIIII
+                //      IIIII     >>>    IIIII
+                //      IIIII     >>>     IIIII
+                //          ^
+                //          |  Applying a force at the arrow will cause the object to move forward, but also rotate
+                //
+                //
+                // The torque created is the linear velocity crossed with the offset
+
+                // NOTE: this computation does should be in the linear section
+                //    because there we know the impulse being applied.
+                Vector3 torqueFromOffset = Vector3.Zero;
+                // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
+                if (float.IsNaN(torqueFromOffset.X))
+                    torqueFromOffset.X = 0;
+                if (float.IsNaN(torqueFromOffset.Y))
+                    torqueFromOffset.Y = 0;
+                if (float.IsNaN(torqueFromOffset.Z))
+                    torqueFromOffset.Z = 0;
+                torqueFromOffset *= m_vehicleMass;
+                Prim.ApplyTorqueImpulse(torqueFromOffset, true);
+                VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
+            }
+
+            #endregion
+
             if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
             {
                 m_lastAngularVelocity.X = 0;
@@ -914,25 +1012,25 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
             {
                 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
-                VDetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
+                Prim.ZeroAngularMotion(true);
+                VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
+            }
+            else
+            {
+                // Apply to the body.
+                // The above calculates the absolute angular velocity needed. Angular velocity is massless.
+                // Since we are stuffing the angular velocity directly into the object, the computed
+                //     velocity needs to be scaled by the timestep.
+                Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep) - Prim.ForceRotationalVelocity);
+                Prim.ForceRotationalVelocity = applyAngularForce;
+
+                // Decay the angular movement for next time
+                Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep;
+                m_lastAngularVelocity *= Vector3.One - decayamount;
+
+                VDetailLog("{0},MoveAngular,done,newRotVel={1},decay={2},lastAngular={3}", 
+                                    Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity);
             }
-
-            // Apply to the body
-            // The above calculates the absolute angular velocity needed
-            // Prim.ForceRotationalVelocity = m_lastAngularVelocity;
-
-            // Apply a force to overcome current angular velocity
-            Vector3 applyAngularForce = (m_lastAngularVelocity - Prim.ForceRotationalVelocity) * Prim.Linkset.LinksetMass;
-            // Vector3 applyAngularForce = (m_lastAngularVelocity - Prim.ForceRotationalVelocity);
-            // Prim.AddAngularForce(applyAngularForce, false);
-            Prim.ApplyTorqueImpulse(applyAngularForce, false);
-
-             // Apply friction for next time
-            Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep;
-            m_lastAngularVelocity *= Vector3.One - decayamount;
-
-            VDetailLog("{0},MoveAngular,done,applyAForce={1},decay={2},lastAngular={3}", 
-                                Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity);
         } //end MoveAngular
 
         internal void LimitRotation(float timestep)
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
index 3a92f93..436e043 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
@@ -49,6 +49,9 @@ public abstract class BSLinkset
 
         switch ((int)physScene.Params.linksetImplementation)
         {
+            case (int)LinksetImplementation.Constraint:
+                ret = new BSLinksetConstraints(physScene, parent);
+                break;
             case (int)LinksetImplementation.Compound:
                 ret = new BSLinksetCompound(physScene, parent);
                 break;
@@ -56,7 +59,7 @@ public abstract class BSLinkset
                 // ret = new BSLinksetManual(physScene, parent);
                 break;
             default:
-                ret = new BSLinksetConstraints(physScene, parent);
+                ret = new BSLinksetCompound(physScene, parent);
                 break;
         }
         return ret;
@@ -97,7 +100,6 @@ public abstract class BSLinkset
     {
         get
         {
-            m_mass = ComputeLinksetMass();
             return m_mass;
         }
     }
@@ -138,6 +140,7 @@ public abstract class BSLinkset
             // Don't add the root to its own linkset
             if (!IsRoot(child))
                 AddChildToLinkset(child);
+            m_mass = ComputeLinksetMass();
         }
         return this;
     }
@@ -156,6 +159,7 @@ public abstract class BSLinkset
                 return this;
             }
             RemoveChildFromLinkset(child);
+            m_mass = ComputeLinksetMass();
         }
 
         // The child is down to a linkset of just itself
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
index 12c6d7a..3238c85 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
@@ -264,6 +264,8 @@ public sealed class BSLinksetCompound : BSLinkset
         float linksetMass = LinksetMass;
         LinksetRoot.UpdatePhysicalMassProperties(linksetMass);
 
+        BulletSimAPI.RecalculateCompoundShapeLocalAabb2(LinksetRoot.PhysShape.ptr);
+
         // DEBUG: see of inter-linkset collisions are causing problems for constraint linksets.
         // BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr, 
         //                     (uint)CollisionFilterGroups.LinksetFilter, (uint)CollisionFilterGroups.LinksetMask);
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
index d2387fb..c855fda 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
@@ -184,7 +184,7 @@ public sealed class BSLinksetConstraints : BSLinkset
     private BSConstraint BuildConstraint(BSPhysObject rootPrim, BSPhysObject childPrim)
     {
         // Zero motion for children so they don't interpolate
-        childPrim.ZeroMotion();
+        childPrim.ZeroMotion(true);
 
         // Relative position normalized to the root prim
         // Essentually a vector pointing from center of rootPrim to center of childPrim
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
new file mode 100755
index 0000000..bc6e4c4
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -0,0 +1,104 @@
+using System;

+using System.Collections.Generic;

+using System.Text;

+using OpenMetaverse;

+

+namespace OpenSim.Region.Physics.BulletSPlugin

+{

+public abstract class BSMotor

+{

+    public virtual void Reset() { }

+    public virtual void Zero() { }

+}

+// Can all the incremental stepping be replaced with motor classes?

+public class BSVMotor : BSMotor

+{

+    public Vector3 FrameOfReference { get; set; }

+    public Vector3 Offset { get; set; }

+

+    public float TimeScale { get; set; }

+    public float TargetValueDecayTimeScale { get; set; }

+    public Vector3 CurrentValueReductionTimescale { get; set; }

+    public float Efficiency { get; set; }

+

+    public Vector3 TargetValue { get; private set; }

+    public Vector3 CurrentValue { get; private set; }

+

+

+

+    BSVMotor(float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency)

+    {

+        TimeScale = timeScale;

+        TargetValueDecayTimeScale = decayTimeScale;

+        CurrentValueReductionTimescale = frictionTimeScale;

+        Efficiency = efficiency;

+    }

+    public void SetCurrent(Vector3 current)

+    {

+        CurrentValue = current;

+    }

+    public void SetTarget(Vector3 target)

+    {

+        TargetValue = target;

+    }

+    public Vector3 Step(float timeStep)

+    {

+        if (CurrentValue.LengthSquared() > 0.001f)

+        {

+            // Vector3 origDir = Target;       // DEBUG

+            // Vector3 origVel = CurrentValue;   // DEBUG

+

+            // Add (desiredVelocity - currentAppliedVelocity) / howLongItShouldTakeToComplete

+            Vector3 addAmount = (TargetValue - CurrentValue)/(TargetValue) * timeStep;

+            CurrentValue += addAmount;

+

+            float decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;

+            TargetValue *= (1f - decayFactor);

+

+            Vector3 frictionFactor = (Vector3.One / CurrentValueReductionTimescale) * timeStep;

+            CurrentValue *= (Vector3.One - frictionFactor);

+        }

+        else

+        {

+            // if what remains of direction is very small, zero it.

+            TargetValue = Vector3.Zero;

+            CurrentValue = Vector3.Zero;

+

+            // VDetailLog("{0},MoveLinear,zeroed", Prim.LocalID);

+        }

+        return CurrentValue;

+    }

+}

+

+public class BSFMotor : BSMotor

+{

+    public float TimeScale { get; set; }

+    public float DecayTimeScale { get; set; }

+    public float Friction { get; set; }

+    public float Efficiency { get; set; }

+

+    public float Target { get; private set; }

+    public float CurrentValue { get; private set; }

+

+    BSFMotor(float timeScale, float decayTimescale, float friction, float efficiency)

+    {

+    }

+    public void SetCurrent(float target)

+    {

+    }

+    public void SetTarget(float target)

+    {

+    }

+    public float Step(float timeStep)

+    {

+        return 0f;

+    }

+}

+public class BSPIDMotor : BSMotor

+{

+    // TODO: write and use this one

+    BSPIDMotor()

+    {

+    }

+}

+}

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
index 7127aaf..e803072 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
@@ -34,9 +34,17 @@ using OpenSim.Region.Physics.Manager;
 
 namespace OpenSim.Region.Physics.BulletSPlugin
 {
-// Class to wrap all objects.
-// The rest of BulletSim doesn't need to keep checking for avatars or prims
-// unless the difference is significant.
+/*
+ * Class to wrap all objects.
+ * The rest of BulletSim doesn't need to keep checking for avatars or prims
+ *        unless the difference is significant.
+ * 
+ *  Variables in the physicsl objects are in three forms:
+ *      VariableName: used by the simulator and performs taint operations, etc
+ *      RawVariableName: direct reference to the BulletSim storage for the variable value
+ *      ForceVariableName: direct reference (store and fetch) to the value in the physics engine.
+ *  The last two (and certainly the last one) should be referenced only in taint-time.
+ */
 public abstract class BSPhysObject : PhysicsActor
 {
     protected void BaseInitialize(BSScene parentScene, uint localID, string name, string typeName)
@@ -67,6 +75,9 @@ public abstract class BSPhysObject : PhysicsActor
     // Set the raw mass but also update physical mass properties (inertia, ...)
     public abstract void UpdatePhysicalMassProperties(float mass);
 
+    // The last value calculated for the prim's inertia
+    public OMV.Vector3 Inertia { get; set; }
+
     // Reference to the physical body (btCollisionObject) of this object
     public BulletBody PhysBody;
     // Reference to the physical shape (btCollisionShape) of this object
@@ -96,7 +107,8 @@ public abstract class BSPhysObject : PhysicsActor
     public abstract bool IsStatic { get; }
 
     // Stop all physical motion.
-    public abstract void ZeroMotion();
+    public abstract void ZeroMotion(bool inTaintTime);
+    public abstract void ZeroAngularMotion(bool inTaintTime);
 
     // Step the vehicle simulation for this object. A NOOP if the vehicle was not configured.
     public virtual void StepVehicle(float timeStep) { }
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index aaa0d93..14eb505 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -25,8 +25,6 @@
  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  */
 
-// Uncomment this it enable code to do all shape an body memory management
-//    in the C# code.
 using System;
 using System.Reflection;
 using System.Collections.Generic;
@@ -236,14 +234,27 @@ public sealed class BSPrim : BSPhysObject
     // Do it to the properties so the values get set in the physics engine.
     // Push the setting of the values to the viewer.
     // Called at taint time!
-    public override void ZeroMotion()
+    public override void ZeroMotion(bool inTaintTime)
     {
         _velocity = OMV.Vector3.Zero;
         _acceleration = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
 
         // Zero some other properties in the physics engine
-        BulletSimAPI.ClearAllForces2(PhysBody.ptr);
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ZeroMotion", delegate()
+        {
+            BulletSimAPI.ClearAllForces2(PhysBody.ptr);
+        });
+    }
+    public override void ZeroAngularMotion(bool inTaintTime)
+    {
+        _rotationalVelocity = OMV.Vector3.Zero;
+        // Zero some other properties in the physics engine
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ZeroMotion", delegate()
+        {
+            BulletSimAPI.SetInterpolationAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+            BulletSimAPI.SetAngularVelocity2(PhysBody.ptr, OMV.Vector3.Zero);
+        });
     }
 
     public override void LockAngularMotion(OMV.Vector3 axis)
@@ -371,17 +382,18 @@ public sealed class BSPrim : BSPhysObject
     {
         if (IsStatic)
         {
-            BulletSimAPI.SetMassProps2(PhysBody.ptr, 0f, OMV.Vector3.Zero);
+            Inertia = OMV.Vector3.Zero;
+            BulletSimAPI.SetMassProps2(PhysBody.ptr, 0f, Inertia);
             BulletSimAPI.UpdateInertiaTensor2(PhysBody.ptr);
         }
         else
         {
-            OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(PhysShape.ptr, physMass);
-            BulletSimAPI.SetMassProps2(PhysBody.ptr, physMass, localInertia);
+            Inertia = BulletSimAPI.CalculateLocalInertia2(PhysShape.ptr, physMass);
+            BulletSimAPI.SetMassProps2(PhysBody.ptr, physMass, Inertia);
+            BulletSimAPI.UpdateInertiaTensor2(PhysBody.ptr);
             // center of mass is at the zero of the object
-            BulletSimAPI.SetCenterOfMassByPosRot2(PhysBody.ptr, ForcePosition, ForceOrientation);
-            // BulletSimAPI.UpdateInertiaTensor2(BSBody.ptr);
-            DetailLog("{0},BSPrim.UpdateMassProperties,mass={1},localInertia={2}", LocalID, physMass, localInertia);
+            // DEBUG DEBUG BulletSimAPI.SetCenterOfMassByPosRot2(PhysBody.ptr, ForcePosition, ForceOrientation);
+            DetailLog("{0},BSPrim.UpdateMassProperties,mass={1},localInertia={2}", LocalID, physMass, Inertia);
         }
     }
 
@@ -582,7 +594,7 @@ public sealed class BSPrim : BSPhysObject
                     // DetailLog("{0},setIsPhysical,taint,isPhys={1}", LocalID, _isPhysical);
                     SetObjectDynamic(true);
                     // whether phys-to-static or static-to-phys, the object is not moving.
-                    ZeroMotion();
+                    ZeroMotion(true);
                 });
             }
         }
@@ -648,6 +660,7 @@ public sealed class BSPrim : BSPhysObject
         // Recompute any linkset parameters.
         // When going from non-physical to physical, this re-enables the constraints that
         //     had been automatically disabled when the mass was set to zero.
+        // For compound based linksets, this enables and disables interactions of the children.
         Linkset.Refresh(this);
 
         DetailLog("{0},BSPrim.UpdatePhysicalParameters,taintExit,static={1},solid={2},mass={3},collide={4},cf={5:X},body={6},shape={7}",
@@ -666,9 +679,9 @@ public sealed class BSPrim : BSPhysObject
             // Become a Bullet 'static' object type
             CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(PhysBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
             // Stop all movement
-            ZeroMotion();
+            ZeroMotion(true);
             // Center of mass is at the center of the object
-            BulletSimAPI.SetCenterOfMassByPosRot2(Linkset.LinksetRoot.PhysBody.ptr, _position, _orientation);
+            // DEBUG DEBUG BulletSimAPI.SetCenterOfMassByPosRot2(Linkset.LinksetRoot.PhysBody.ptr, _position, _orientation);
             // Mass is zero which disables a bunch of physics stuff in Bullet
             UpdatePhysicalMassProperties(0f);
             // Set collision detection parameters
@@ -704,7 +717,7 @@ public sealed class BSPrim : BSPhysObject
             BulletSimAPI.SetTranslation2(PhysBody.ptr, _position, _orientation);
 
             // Center of mass is at the center of the object
-            BulletSimAPI.SetCenterOfMassByPosRot2(Linkset.LinksetRoot.PhysBody.ptr, _position, _orientation);
+            // DEBUG DEBUG BulletSimAPI.SetCenterOfMassByPosRot2(Linkset.LinksetRoot.PhysBody.ptr, _position, _orientation);
 
             // A dynamic object has mass
             UpdatePhysicalMassProperties(RawMass);
@@ -958,6 +971,16 @@ public sealed class BSPrim : BSPhysObject
         });
     }
 
+    public void ApplyForceImpulse(OMV.Vector3 impulse, bool inTaintTime)
+    {
+        OMV.Vector3 applyImpulse = impulse;
+        PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ApplyForceImpulse", delegate()
+        {
+            DetailLog("{0},BSPrim.ApplyForceImpulse,taint,tImpulse={1}", LocalID, applyImpulse);
+            BulletSimAPI.ApplyCentralImpulse2(PhysBody.ptr, applyImpulse);
+        });
+    }
+
     private List<OMV.Vector3> m_accumulatedAngularForces = new List<OMV.Vector3>();
     public override void AddAngularForce(OMV.Vector3 force, bool pushforce) {
         AddAngularForce(force, pushforce, false);
@@ -1001,7 +1024,6 @@ public sealed class BSPrim : BSPhysObject
         OMV.Vector3 applyImpulse = impulse;
         PhysicsScene.TaintedObject(inTaintTime, "BSPrim.ApplyTorqueImpulse", delegate()
         {
-            DetailLog("{0},BSPrim.ApplyTorqueImpulse,taint,tImpulse={1}", LocalID, applyImpulse);
             BulletSimAPI.ApplyTorqueImpulse2(PhysBody.ptr, applyImpulse);
         });
     }
@@ -1315,9 +1337,10 @@ public sealed class BSPrim : BSPhysObject
         // If this prim is part of a linkset, we must remove and restore the physical
         //    links if the body is rebuilt.
         bool needToRestoreLinkset = false;
+        bool needToRestoreVehicle = false;
 
         // Create the correct physical representation for this type of object.
-        // Updates BSBody and BSShape with the new information.
+        // Updates PhysBody and PhysShape with the new information.
         // Ignore 'forceRebuild'. This routine makes the right choices and changes of necessary.
         // Returns 'true' if either the body or the shape was changed.
         PhysicsScene.Shapes.GetBodyAndShape(false, PhysicsScene.World, this, null, delegate(BulletBody dBody)
@@ -1326,6 +1349,7 @@ public sealed class BSPrim : BSPhysObject
             // Remove all the physical dependencies on the old body.
             // (Maybe someday make the changing of BSShape an event handled by BSLinkset.)
             needToRestoreLinkset = Linkset.RemoveBodyDependencies(this);
+            needToRestoreVehicle = _vehicle.RemoveBodyDependencies(this);
         });
 
         if (needToRestoreLinkset)
@@ -1333,6 +1357,11 @@ public sealed class BSPrim : BSPhysObject
             // If physical body dependencies were removed, restore them
             Linkset.RestoreBodyDependencies(this);
         }
+        if (needToRestoreVehicle)
+        {
+            // If physical body dependencies were removed, restore them
+            _vehicle.RestoreBodyDependencies(this);
+        }
 
         // Make sure the properties are set on the new object
         UpdatePhysicalParameters();
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
index 740f339..1cc607a 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSScene.cs
@@ -491,7 +491,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
         // Some of the prims operate with special vehicle properties
         ProcessVehicles(timeStep);
-        numTaints += _taintOperations.Count;
         ProcessTaints();    // the vehicles might have added taints
 
         // step the physical world one interval
@@ -500,7 +499,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
         try
         {
-            // DumpVehicles();  // DEBUG
+            if (VehicleLoggingEnabled) DumpVehicles();  // DEBUG
             if (PhysicsLogging.Enabled) beforeTime = Util.EnvironmentTickCount();
 
             numSubSteps = BulletSimAPI.PhysicsStep2(World.ptr, timeStep, m_maxSubSteps, m_fixedTimeStep,
@@ -509,7 +508,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             if (PhysicsLogging.Enabled) simTime = Util.EnvironmentTickCountSubtract(beforeTime);
             DetailLog("{0},Simulate,call, frame={1}, nTaints={2}, simTime={3}, substeps={4}, updates={5}, colliders={6}",
                         DetailLogZero, m_simulationStep, numTaints, simTime, numSubSteps, updatedEntityCount, collidersCount);
-            // DumpVehicles();  // DEBUG
+            if (VehicleLoggingEnabled) DumpVehicles();  // DEBUG
         }
         catch (Exception e)
         {
@@ -521,7 +520,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             collidersCount = 0;
         }
 
-
         // Don't have to use the pointers passed back since we know it is the same pinned memory we passed in
 
         // Get a value for 'now' so all the collision and update routines don't have to get their own
@@ -724,6 +722,9 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     {
         if (_taintOperations.Count > 0)  // save allocating new list if there is nothing to process
         {
+            /*
+            // Code to limit the number of taints processed per step. Meant to limit step time.
+            // Unsure if a good idea as code assumes that taints are done before the step.
             int taintCount = m_taintsToProcessPerStep;
             TaintCallbackEntry oneCallback = new TaintCallbackEntry();
             while (_taintOperations.Count > 0 && taintCount-- > 0)
@@ -752,13 +753,17 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
                     }
                 }
             }
-            /*
+            if (_taintOperations.Count > 0)
+            {
+                DetailLog("{0},BSScene.ProcessTaints,leftTaintsOnList,numNotProcessed={1}", DetailLogZero, _taintOperations.Count);
+            }
+             */
             // swizzle a new list into the list location so we can process what's there
             List<TaintCallbackEntry> oldList;
             lock (_taintLock)
             {
-                oldList = _taintedObjects;
-                _taintedObjects = new List<TaintCallbackEntry>();
+                oldList = _taintOperations;
+                _taintOperations = new List<TaintCallbackEntry>();
             }
 
             foreach (TaintCallbackEntry tcbe in oldList)
@@ -774,7 +779,6 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
                 }
             }
             oldList.Clear();
-             */
         }
     }
 
@@ -1043,7 +1047,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             (s) => { return (float)s.m_maxUpdatesPerFrame; },
             (s,p,l,v) => { s.m_maxUpdatesPerFrame = (int)v; } ),
         new ParameterDefn("MaxTaintsToProcessPerStep", "Number of update taints to process before each simulation step",
-            100f,
+            500f,
             (s,cf,p,v) => { s.m_taintsToProcessPerStep = cf.GetInt(p, (int)v); },
             (s) => { return (float)s.m_taintsToProcessPerStep; },
             (s,p,l,v) => { s.m_taintsToProcessPerStep = (int)v; } ),
@@ -1097,13 +1101,13 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
             (s,cf,p,v) => { s.m_params[0].linearDamping = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].linearDamping; },
             (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].linearDamping, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetDamping2(o.PhysBody.ptr, v, v); } ),
+            (s,o,v) => { BulletSimAPI.SetDamping2(o.PhysBody.ptr, v, s.m_params[0].angularDamping); } ),
         new ParameterDefn("AngularDamping", "Factor to damp angular movement per second (0.0 - 1.0)",
             0f,
             (s,cf,p,v) => { s.m_params[0].angularDamping = cf.GetFloat(p, v); },
             (s) => { return s.m_params[0].angularDamping; },
             (s,p,l,v) => { s.UpdateParameterObject(ref s.m_params[0].angularDamping, p, l, v); },
-            (s,o,v) => { BulletSimAPI.SetDamping2(o.PhysBody.ptr, v, v); } ),
+            (s,o,v) => { BulletSimAPI.SetDamping2(o.PhysBody.ptr, s.m_params[0].linearDamping, v); } ),
         new ParameterDefn("DeactivationTime", "Seconds before considering an object potentially static",
             0.2f,
             (s,cf,p,v) => { s.m_params[0].deactivationTime = cf.GetFloat(p, v); },
@@ -1473,7 +1477,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
     {
         PhysicsLogging.Write(msg, args);
         // Add the Flush() if debugging crashes. Gets all the messages written out.
-        PhysicsLogging.Flush();
+        // PhysicsLogging.Flush();
     }
     // Used to fill in the LocalID when there isn't one. It's the correct number of characters.
     public const string DetailLogZero = "0000000000";
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
new file mode 100755
index 0000000..5e2c4a8
--- /dev/null
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSShapes.cs
@@ -0,0 +1,213 @@
+/*

+ * 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 copyrightD

+ *       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.Collections.Generic;

+using System.Linq;

+using System.Text;

+

+namespace OpenSim.Region.Physics.BulletSPlugin

+{

+public abstract class BSShape

+{

+    public IntPtr ptr { get; set; }

+    public ShapeData.PhysicsShapeType type { get; set; }

+    public System.UInt64 key { get; set; }

+    public int referenceCount { get; set; }

+    public DateTime lastReferenced { get; set; }

+

+    protected void Initialize()

+    {

+        ptr = IntPtr.Zero;

+        type = ShapeData.PhysicsShapeType.SHAPE_UNKNOWN;

+        key = 0;

+        referenceCount = 0;

+        lastReferenced = DateTime.Now;

+    }

+

+    // Get a reference to a physical shape. Create if it doesn't exist

+    public static BSShape GetShapeReference(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)

+    {

+        BSShape ret = null;

+

+        if (prim.PreferredPhysicalShape == ShapeData.PhysicsShapeType.SHAPE_AVATAR)

+        {

+            // an avatar capsule is close to a native shape (it is not shared)

+            ret = BSShapeNative.GetReference(physicsScene, prim, ShapeData.PhysicsShapeType.SHAPE_AVATAR,

+                                        ShapeData.FixedShapeKey.KEY_CAPSULE);

+            physicsScene.DetailLog("{0},BSShape.GetShapeReference,avatarCapsule,shape={1}", prim.LocalID, ret);

+        }

+

+        // Compound shapes are handled special as they are rebuilt from scratch.

+        // This isn't too great a hardship since most of the child shapes will already been created.

+        if (ret == null  && prim.PreferredPhysicalShape == ShapeData.PhysicsShapeType.SHAPE_COMPOUND)

+        {

+            // Getting a reference to a compound shape gets you the compound shape with the root prim shape added

+            ret = BSShapeCompound.GetReference(prim);

+            physicsScene.DetailLog("{0},BSShapeCollection.CreateGeom,compoundShape,shape={1}", prim.LocalID, ret);

+        }

+

+        if (ret == null)

+            ret = GetShapeReferenceNonSpecial(physicsScene, forceRebuild, prim);

+

+        return ret;

+    }

+    public static BSShape GetShapeReferenceNonSpecial(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)

+    {

+        return null;

+    }

+    public static BSShape GetShapeReferenceNonNative(BSScene physicsScene, bool forceRebuild, BSPhysObject prim)

+    {

+        return null;

+    }

+

+    // Release the use of a physical shape.

+    public abstract void Dereference(BSScene physicsScene);

+

+    // All shapes have a static call to get a reference to the physical shape

+    // protected abstract static BSShape GetReference();

+

+    public override string ToString()

+    {

+        StringBuilder buff = new StringBuilder();

+        buff.Append("<p=");

+        buff.Append(ptr.ToString("X"));

+        buff.Append(",s=");

+        buff.Append(type.ToString());

+        buff.Append(",k=");

+        buff.Append(key.ToString("X"));

+        buff.Append(",c=");

+        buff.Append(referenceCount.ToString());

+        buff.Append(">");

+        return buff.ToString();

+    }

+}

+

+public class BSShapeNull : BSShape

+{

+    public BSShapeNull()

+    {

+        base.Initialize();

+    }

+    public static BSShape GetReference() { return new BSShapeNull();  }

+    public override void Dereference(BSScene physicsScene) { /* The magic of garbage collection will make this go away */ }

+}

+

+public class BSShapeNative : BSShape

+{

+    private static string LogHeader = "[BULLETSIM SHAPE NATIVE]";

+    public BSShapeNative()

+    {

+        base.Initialize();

+    }

+    public static BSShape GetReference(BSScene physicsScene, BSPhysObject prim, 

+                    ShapeData.PhysicsShapeType shapeType, ShapeData.FixedShapeKey shapeKey) 

+    {

+        // Native shapes are not shared and are always built anew.

+        return new BSShapeNative(physicsScene, prim, shapeType, shapeKey);

+    }

+

+    private BSShapeNative(BSScene physicsScene, BSPhysObject prim,

+                    ShapeData.PhysicsShapeType shapeType, ShapeData.FixedShapeKey shapeKey)

+    {

+        ShapeData nativeShapeData = new ShapeData();

+        nativeShapeData.Type = shapeType;

+        nativeShapeData.ID = prim.LocalID;

+        nativeShapeData.Scale = prim.Scale;

+        nativeShapeData.Size = prim.Scale;

+        nativeShapeData.MeshKey = (ulong)shapeKey;

+        nativeShapeData.HullKey = (ulong)shapeKey;

+

+       

+        if (shapeType == ShapeData.PhysicsShapeType.SHAPE_AVATAR)

+        {

+            ptr = BulletSimAPI.BuildCapsuleShape2(physicsScene.World.ptr, 1f, 1f, prim.Scale);

+            physicsScene.DetailLog("{0},BSShapeCollection.BuiletPhysicalNativeShape,capsule,scale={1}", prim.LocalID, prim.Scale);

+        }

+        else

+        {

+            ptr = BulletSimAPI.BuildNativeShape2(physicsScene.World.ptr, nativeShapeData);

+        }

+        if (ptr == IntPtr.Zero)

+        {

+            physicsScene.Logger.ErrorFormat("{0} BuildPhysicalNativeShape failed. ID={1}, shape={2}",

+                                    LogHeader, prim.LocalID, shapeType);

+        }

+        type = shapeType;

+        key = (UInt64)shapeKey;

+    }

+    // Make this reference to the physical shape go away since native shapes are not shared.

+    public override void Dereference(BSScene physicsScene)

+    {

+        // Native shapes are not tracked and are released immediately

+        physicsScene.DetailLog("{0},BSShapeCollection.DereferenceShape,deleteNativeShape,shape={1}", BSScene.DetailLogZero, this);

+        BulletSimAPI.DeleteCollisionShape2(physicsScene.World.ptr, ptr);

+        ptr = IntPtr.Zero;

+        // Garbage collection will free up this instance.

+    }

+}

+

+public class BSShapeMesh : BSShape

+{

+    private static string LogHeader = "[BULLETSIM SHAPE MESH]";

+    private static Dictionary<System.UInt64, BSShapeMesh> Meshes = new Dictionary<System.UInt64, BSShapeMesh>();

+

+    public BSShapeMesh()

+    {

+        base.Initialize();

+    }

+    public static BSShape GetReference() { return new BSShapeNull();  }

+    public override void Dereference(BSScene physicsScene) { }

+}

+

+public class BSShapeHull : BSShape

+{

+    private static string LogHeader = "[BULLETSIM SHAPE HULL]";

+    private static Dictionary<System.UInt64, BSShapeHull> Hulls = new Dictionary<System.UInt64, BSShapeHull>();

+

+    public BSShapeHull()

+    {

+        base.Initialize();

+    }

+    public static BSShape GetReference() { return new BSShapeNull();  }

+    public override void Dereference(BSScene physicsScene) { }

+}

+

+public class BSShapeCompound : BSShape

+{

+    private static string LogHeader = "[BULLETSIM SHAPE COMPOUND]";

+    public BSShapeCompound()

+    {

+        base.Initialize();

+    }

+    public static BSShape GetReference(BSPhysObject prim) 

+    { 

+        return new BSShapeNull();

+    }

+    public override void Dereference(BSScene physicsScene) { }

+}

+}

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 702bd77..07149d8 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -632,6 +632,9 @@ public static extern IntPtr RemoveChildShapeFromCompoundShapeIndex2(IntPtr cShap
 public static extern void RemoveChildShapeFromCompoundShape2(IntPtr cShape, IntPtr removeShape);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void RecalculateCompoundShapeLocalAabb2(IntPtr cShape);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern IntPtr DuplicateCollisionShape2(IntPtr sim, IntPtr srcShape, uint id);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
@@ -919,6 +922,12 @@ public static extern Vector3 GetGravity2(IntPtr obj);
 public static extern void SetDamping2(IntPtr obj, float lin_damping, float ang_damping);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetLinearDamping2(IntPtr obj, float lin_damping);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
+public static extern void SetAngularDamping2(IntPtr obj, float ang_damping);
+
+[DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]
 public static extern float GetLinearDamping2(IntPtr obj);
 
 [DllImport("BulletSim", CallingConvention = CallingConvention.Cdecl), SuppressUnmanagedCodeSecurity]