19 files changed, 116 insertions, 79 deletions

diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSCharacter.cs
index 4c4e950..d91c47f 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSCharacter.cs
@@ -174,7 +174,7 @@ public sealed class BSCharacter : BSPhysObject
             BulletSimAPI.SetCcdSweptSphereRadius2(PhysBody.ptr, BSParam.CcdSweptSphereRadius);
         }
 
-        UpdatePhysicalMassProperties(RawMass);
+        UpdatePhysicalMassProperties(RawMass, false);
 
         // Make so capsule does not fall over
         BulletSimAPI.SetAngularFactorV2(PhysBody.ptr, OMV.Vector3.Zero);
@@ -224,7 +224,7 @@ public sealed class BSCharacter : BSPhysObject
                 if (PhysBody.HasPhysicalBody && PhysShape.HasPhysicalShape)
                 {
                     BulletSimAPI.SetLocalScaling2(PhysShape.ptr, Scale);
-                    UpdatePhysicalMassProperties(RawMass);
+                    UpdatePhysicalMassProperties(RawMass, true);
                     // Make sure this change appears as a property update event
                     BulletSimAPI.PushUpdate2(PhysBody.ptr);
                 }
@@ -260,7 +260,6 @@ public sealed class BSCharacter : BSPhysObject
     public override void ZeroMotion(bool inTaintTime)
     {
         _velocity = OMV.Vector3.Zero;
-        _velocityMotor.Zero();
         _acceleration = OMV.Vector3.Zero;
         _rotationalVelocity = OMV.Vector3.Zero;
 
@@ -390,7 +389,7 @@ public sealed class BSCharacter : BSPhysObject
     public override float RawMass { 
         get {return _mass; }
     }
-    public override void UpdatePhysicalMassProperties(float physMass)
+    public override void UpdatePhysicalMassProperties(float physMass, bool inWorld)
     {
         OMV.Vector3 localInertia = BulletSimAPI.CalculateLocalInertia2(PhysShape.ptr, physMass);
         BulletSimAPI.SetMassProps2(PhysBody.ptr, physMass, localInertia);
@@ -787,6 +786,7 @@ public sealed class BSCharacter : BSPhysObject
             //     the motor can be turned off. Set the velocity to zero so the zero motion is sent to the viewer.
             if (_velocityMotor.TargetValue.ApproxEquals(OMV.Vector3.Zero, 0.01f) && _velocityMotor.ErrorIsZero)
             {
+                ZeroMotion(true);
                 stepVelocity = OMV.Vector3.Zero;
                 _velocityMotor.Enabled = false;
                 DetailLog("{0},BSCharacter.UpdateProperties,taint,disableVelocityMotor,m={1}", LocalID, _velocityMotor);
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSConstraint.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSConstraint.cs
index 426bdc2..f1bed39 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSConstraint.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSConstraint.cs
@@ -36,7 +36,7 @@ public abstract class BSConstraint : IDisposable
 {
     private static string LogHeader = "[BULLETSIM CONSTRAINT]";
 
-    protected BulletSim m_world;
+    protected BulletWorld m_world;
     protected BulletBody m_body1;
     protected BulletBody m_body2;
     protected BulletConstraint m_constraint;
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSConstraint6Dof.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSConstraint6Dof.cs
index 0181d9d..d1e3f55 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSConstraint6Dof.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSConstraint6Dof.cs
@@ -39,7 +39,7 @@ public sealed class BSConstraint6Dof : BSConstraint
     public override ConstraintType Type { get { return ConstraintType.D6_CONSTRAINT_TYPE; } }
 
     // Create a btGeneric6DofConstraint
-    public BSConstraint6Dof(BulletSim world, BulletBody obj1, BulletBody obj2,
+    public BSConstraint6Dof(BulletWorld world, BulletBody obj1, BulletBody obj2,
                     Vector3 frame1, Quaternion frame1rot,
                     Vector3 frame2, Quaternion frame2rot,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
@@ -58,7 +58,7 @@ public sealed class BSConstraint6Dof : BSConstraint
                             obj1.ID, obj1.ptr.ToString(), obj2.ID, obj2.ptr.ToString());
     }
 
-    public BSConstraint6Dof(BulletSim world, BulletBody obj1, BulletBody obj2,
+    public BSConstraint6Dof(BulletWorld world, BulletBody obj1, BulletBody obj2,
                     Vector3 joinPoint,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
     {
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSConstraintCollection.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSConstraintCollection.cs
index 5c00b1a..87d1e44 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSConstraintCollection.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSConstraintCollection.cs
@@ -41,9 +41,9 @@ public sealed class BSConstraintCollection : IDisposable
     delegate bool ConstraintAction(BSConstraint constrain);
 
     private List<BSConstraint> m_constraints;
-    private BulletSim m_world;
+    private BulletWorld m_world;
 
-    public BSConstraintCollection(BulletSim world)
+    public BSConstraintCollection(BulletWorld world)
     {
         m_world = world;
         m_constraints = new List<BSConstraint>();
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSConstraintHinge.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSConstraintHinge.cs
index 7951f06..fbd1bc0 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSConstraintHinge.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSConstraintHinge.cs
@@ -36,7 +36,7 @@ public sealed class BSConstraintHinge : BSConstraint
 {
     public override ConstraintType Type { get { return ConstraintType.HINGE_CONSTRAINT_TYPE; } }
 
-    public BSConstraintHinge(BulletSim world, BulletBody obj1, BulletBody obj2,
+    public BSConstraintHinge(BulletWorld world, BulletBody obj1, BulletBody obj2,
                     Vector3 pivotInA, Vector3 pivotInB,
                     Vector3 axisInA, Vector3 axisInB,
                     bool useLinearReferenceFrameA, bool disableCollisionsBetweenLinkedBodies)
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSDynamics.cs
index 72afacc..415ad4f 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSDynamics.cs
@@ -573,6 +573,9 @@ namespace OpenSim.Region.Physics.BulletSNPlugin
                 BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia);
                 BulletSimAPI.UpdateInertiaTensor2(Prim.PhysBody.ptr);
 
+                Vector3 grav = PhysicsScene.DefaultGravity * (1f - Prim.Buoyancy);
+                BulletSimAPI.SetGravity2(Prim.PhysBody.ptr, grav);
+
                 VDetailLog("{0},BSDynamics.Refresh,mass={1},frict={2},inert={3},aDamp={4}",
                                 Prim.LocalID, m_vehicleMass, friction, localInertia, angularDamping);
             }
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSLinkset.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSLinkset.cs
index 253128b..845a113 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSLinkset.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSLinkset.cs
@@ -97,14 +97,7 @@ public abstract class BSLinkset
     }
 
     // We keep the prim's mass in the linkset structure since it could be dependent on other prims
-    protected float m_mass;
-    public float LinksetMass
-    {
-        get
-        {
-            return m_mass;
-        }
-    }
+    public float LinksetMass { get; protected set; }
 
     public virtual bool LinksetIsColliding { get { return false; } }
 
@@ -128,7 +121,7 @@ public abstract class BSLinkset
         PhysicsScene = scene;
         LinksetRoot = parent;
         m_children = new HashSet<BSPhysObject>();
-        m_mass = parent.RawMass;
+        LinksetMass = parent.RawMass;
         Rebuilding = false;
     }
 
@@ -143,7 +136,7 @@ public abstract class BSLinkset
             // Don't add the root to its own linkset
             if (!IsRoot(child))
                 AddChildToLinkset(child);
-            m_mass = ComputeLinksetMass();
+            LinksetMass = ComputeLinksetMass();
         }
         return this;
     }
@@ -162,7 +155,7 @@ public abstract class BSLinkset
                 return this;
             }
             RemoveChildFromLinkset(child);
-            m_mass = ComputeLinksetMass();
+            LinksetMass = ComputeLinksetMass();
         }
 
         // The child is down to a linkset of just itself
@@ -230,7 +223,10 @@ public abstract class BSLinkset
     // When physical properties are changed the linkset needs to recalculate
     //   its internal properties.
     // May be called at runtime or taint-time.
-    public abstract void Refresh(BSPhysObject requestor);
+    public virtual void Refresh(BSPhysObject requestor)
+    {
+        LinksetMass = ComputeLinksetMass();
+    }
 
     // Flag denoting the linkset is in the process of being rebuilt.
     // Used to know not the schedule a rebuild in the middle of a rebuild.
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSLinksetCompound.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSLinksetCompound.cs
index 23a0b8b..9a977e6 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSLinksetCompound.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSLinksetCompound.cs
@@ -89,6 +89,8 @@ public sealed class BSLinksetCompound : BSLinkset
     //   its internal properties.
     public override void Refresh(BSPhysObject requestor)
     {
+        base.Refresh(requestor);
+
         // Something changed so do the rebuilding thing
         // ScheduleRebuild();
     }
@@ -96,13 +98,13 @@ public sealed class BSLinksetCompound : BSLinkset
     // Schedule a refresh to happen after all the other taint processing.
     private void ScheduleRebuild(BSPhysObject requestor)
     {
-        DetailLog("{0},BSLinksetCompound.Refresh,schedulingRefresh,rebuilding={1}", 
+        DetailLog("{0},BSLinksetCompound.ScheduleRebuild,,rebuilding={1}", 
                             requestor.LocalID, Rebuilding);
         // When rebuilding, it is possible to set properties that would normally require a rebuild.
         //    If already rebuilding, don't request another rebuild.
         if (!Rebuilding)
         {
-            PhysicsScene.PostTaintObject("BSLinksetCompound.Refresh", LinksetRoot.LocalID, delegate()
+            PhysicsScene.PostTaintObject("BSLinksetCompound.ScheduleRebuild", LinksetRoot.LocalID, delegate()
             {
                 if (HasAnyChildren)
                     RecomputeLinksetCompound();
@@ -123,7 +125,6 @@ public sealed class BSLinksetCompound : BSLinkset
         if (IsRoot(child))
         {
             // The root is going dynamic. Make sure mass is properly set.
-            m_mass = ComputeLinksetMass();
             ScheduleRebuild(LinksetRoot);
         }
         else
@@ -377,8 +378,8 @@ public sealed class BSLinksetCompound : BSLinkset
             });
 
             // With all of the linkset packed into the root prim, it has the mass of everyone.
-            float linksetMass = LinksetMass;
-            LinksetRoot.UpdatePhysicalMassProperties(linksetMass);
+            LinksetMass = LinksetMass;
+            LinksetRoot.UpdatePhysicalMassProperties(LinksetMass, true);
         }
         finally
         {
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSLinksetConstraints.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSLinksetConstraints.cs
index 5757e64..46ff99f 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSLinksetConstraints.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSLinksetConstraints.cs
@@ -46,6 +46,8 @@ public sealed class BSLinksetConstraints : BSLinkset
     //   refresh will happen once after all the other taints are applied.
     public override void Refresh(BSPhysObject requestor)
     {
+        base.Refresh(requestor);
+
         // Queue to happen after all the other taint processing
         PhysicsScene.PostTaintObject("BSLinksetContraints.Refresh", requestor.LocalID, delegate()
             {
@@ -279,7 +281,7 @@ public sealed class BSLinksetConstraints : BSLinkset
     private void RecomputeLinksetConstraints()
     {
         float linksetMass = LinksetMass;
-        LinksetRoot.UpdatePhysicalMassProperties(linksetMass);
+        LinksetRoot.UpdatePhysicalMassProperties(linksetMass, true);
 
         // DEBUG: see of inter-linkset collisions are causing problems
         // BulletSimAPI.SetCollisionFilterMask2(LinksetRoot.BSBody.ptr, 
@@ -292,7 +294,7 @@ public sealed class BSLinksetConstraints : BSLinkset
             // A child in the linkset physically shows the mass of the whole linkset.
             // This allows Bullet to apply enough force on the child to move the whole linkset.
             // (Also do the mass stuff before recomputing the constraint so mass is not zero.)
-            child.UpdatePhysicalMassProperties(linksetMass);
+            child.UpdatePhysicalMassProperties(linksetMass, true);
 
             BSConstraint constrain;
             if (!PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain))
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSMaterials.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSMaterials.cs
index 732191f..d7941b6 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSMaterials.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSMaterials.cs
@@ -119,26 +119,26 @@ public static class BSMaterials
         Attributes[(int)MaterialAttributes.Material.Light] =
                     new MaterialAttributes("light",dDensity, dFriction, dRestitution);
         Attributes[(int)MaterialAttributes.Material.Avatar] =
-                    new MaterialAttributes("avatar",60f, 0.2f, 0f);
+                    new MaterialAttributes("avatar",3.5f, 0.2f, 0f);
 
         Attributes[(int)MaterialAttributes.Material.Stone + (int)MaterialAttributes.Material.NumberOfTypes] =
                     new MaterialAttributes("stonePhysical",dDensity, 0.8f, 0.4f);
         Attributes[(int)MaterialAttributes.Material.Metal + (int)MaterialAttributes.Material.NumberOfTypes] =
-                    new MaterialAttributes("metalPhysical",dDensity, 0.8f, 0.4f);
+                    new MaterialAttributes("metalPhysical",dDensity, 0.3f, 0.4f);
         Attributes[(int)MaterialAttributes.Material.Glass + (int)MaterialAttributes.Material.NumberOfTypes] =
-                    new MaterialAttributes("glassPhysical",dDensity, 0.8f, 0.7f);
+                    new MaterialAttributes("glassPhysical",dDensity, 0.2f, 0.7f);
         Attributes[(int)MaterialAttributes.Material.Wood + (int)MaterialAttributes.Material.NumberOfTypes] =
-                    new MaterialAttributes("woodPhysical",dDensity, 0.8f, 0.5f);
+                    new MaterialAttributes("woodPhysical",dDensity, 0.6f, 0.5f);
         Attributes[(int)MaterialAttributes.Material.Flesh + (int)MaterialAttributes.Material.NumberOfTypes] =
-                    new MaterialAttributes("fleshPhysical",dDensity, 0.8f, 0.3f);
+                    new MaterialAttributes("fleshPhysical",dDensity, 0.9f, 0.3f);
         Attributes[(int)MaterialAttributes.Material.Plastic + (int)MaterialAttributes.Material.NumberOfTypes] =
-                    new MaterialAttributes("plasticPhysical",dDensity, 0.8f, 0.7f);
+                    new MaterialAttributes("plasticPhysical",dDensity, 0.4f, 0.7f);
         Attributes[(int)MaterialAttributes.Material.Rubber + (int)MaterialAttributes.Material.NumberOfTypes] =
-                    new MaterialAttributes("rubberPhysical",dDensity, 0.8f, 0.9f);
+                    new MaterialAttributes("rubberPhysical",dDensity, 0.9f, 0.9f);
         Attributes[(int)MaterialAttributes.Material.Light + (int)MaterialAttributes.Material.NumberOfTypes] =
                     new MaterialAttributes("lightPhysical",dDensity, dFriction, dRestitution);
         Attributes[(int)MaterialAttributes.Material.Avatar + (int)MaterialAttributes.Material.NumberOfTypes] =
-                    new MaterialAttributes("avatarPhysical",60f, 0.2f, 0f);
+                    new MaterialAttributes("avatarPhysical",3.5f, 0.2f, 0f);
     }
 
     // Under the [BulletSim] section, one can change the individual material
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSParam.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSParam.cs
index 20c0939..5e93a03 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSParam.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSParam.cs
@@ -351,7 +351,7 @@ public static class BSParam
             (s) => { return AvatarStandingFriction; },
             (s,p,l,v) => { AvatarStandingFriction = v; } ),
         new ParameterDefn("AvatarDensity", "Density of an avatar. Changed on avatar recreation.",
-            60f,
+            3.5f,
             (s,cf,p,v) => { AvatarDensity = cf.GetFloat(p, v); },
             (s) => { return AvatarDensity; },
             (s,p,l,v) => { s.UpdateParameterObject((x)=>{AvatarDensity=x;}, p, l, v); } ),
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSPhysObject.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSPhysObject.cs
index 4096ef8..689da7f 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSPhysObject.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSPhysObject.cs
@@ -96,7 +96,8 @@ public abstract class BSPhysObject : PhysicsActor
     // Return the object mass without calculating it or having side effects
     public abstract float RawMass { get; }
     // Set the raw mass but also update physical mass properties (inertia, ...)
-    public abstract void UpdatePhysicalMassProperties(float mass);
+    // 'inWorld' true if the object has already been added to the dynamic world.
+    public abstract void UpdatePhysicalMassProperties(float mass, bool inWorld);
 
     // The last value calculated for the prim's inertia
     public OMV.Vector3 Inertia { get; set; }
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSPrim.cs
index a889c24..9c4ba30 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSPrim.cs
@@ -115,6 +115,8 @@ public sealed class BSPrim : BSPhysObject
         PhysBody = new BulletBody(LocalID);
         PhysShape = new BulletShape();
 
+        Linkset.Refresh(this);
+
         DetailLog("{0},BSPrim.constructor,call", LocalID);
         // do the actual object creation at taint time
         PhysicsScene.TaintedObject("BSPrim.create", delegate()
@@ -384,13 +386,13 @@ public sealed class BSPrim : BSPhysObject
     }
 
     // Return the effective mass of the object.
-    // If there are multiple items in the linkset, add them together for the root
+        // The definition of this call is to return the mass of the prim.
+        // If the simulator cares about the mass of the linkset, it will sum it itself.
     public override float Mass
     {
         get
         {
-            return Linkset.LinksetMass;
-            // return _mass;
+            return _mass;
         }
     }
 
@@ -400,22 +402,45 @@ public sealed class BSPrim : BSPhysObject
     }
     // Set the physical mass to the passed mass.
     // Note that this does not change _mass!
-    public override void UpdatePhysicalMassProperties(float physMass)
+    public override void UpdatePhysicalMassProperties(float physMass, bool inWorld)
     {
-        if (IsStatic)
+        if (PhysBody.HasPhysicalBody)
         {
-            Inertia = OMV.Vector3.Zero;
-            BulletSimAPI.SetMassProps2(PhysBody.ptr, 0f, Inertia);
-            BulletSimAPI.UpdateInertiaTensor2(PhysBody.ptr);
-        }
-        else
-        {
-            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
-            // DEBUG DEBUG BulletSimAPI.SetCenterOfMassByPosRot2(PhysBody.ptr, ForcePosition, ForceOrientation);
-            DetailLog("{0},BSPrim.UpdateMassProperties,mass={1},localInertia={2}", LocalID, physMass, Inertia);
+            if (IsStatic)
+            {
+                Inertia = OMV.Vector3.Zero;
+                BulletSimAPI.SetMassProps2(PhysBody.ptr, 0f, Inertia);
+                BulletSimAPI.UpdateInertiaTensor2(PhysBody.ptr);
+            }
+            else
+            {
+                if (inWorld)
+                {
+                    // Changing interesting properties doesn't change proxy and collision cache
+                    //    information. The Bullet solution is to re-add the object to the world
+                    //    after parameters are changed.
+                    BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, PhysBody.ptr);
+                }
+
+                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
+                // DEBUG DEBUG BulletSimAPI.SetCenterOfMassByPosRot2(PhysBody.ptr, ForcePosition, ForceOrientation);
+                DetailLog("{0},BSPrim.UpdateMassProperties,mass={1},localInertia={2},inWorld={3}", LocalID, physMass, Inertia, inWorld);
+
+                if (inWorld)
+                {
+                    BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, PhysBody.ptr,_position,_orientation);
+                }
+
+                // Must set gravity after it has been added to the world because, for unknown reasons,
+                //     adding the object resets the object's gravity to world gravity
+                OMV.Vector3 grav = PhysicsScene.DefaultGravity * (1f - Buoyancy);
+                BulletSimAPI.SetGravity2(PhysBody.ptr, grav);
+
+            }
         }
     }
 
@@ -714,7 +739,7 @@ public sealed class BSPrim : BSPhysObject
         Linkset.Refresh(this);
 
         DetailLog("{0},BSPrim.UpdatePhysicalParameters,taintExit,static={1},solid={2},mass={3},collide={4},cf={5:X},body={6},shape={7}",
-                        LocalID, IsStatic, IsSolid, _mass, SubscribedEvents(), CurrentCollisionFlags, PhysBody, PhysShape);
+                        LocalID, IsStatic, IsSolid, Mass, SubscribedEvents(), CurrentCollisionFlags, PhysBody, PhysShape);
     }
 
     // "Making dynamic" means changing to and from static.
@@ -737,7 +762,7 @@ public sealed class BSPrim : BSPhysObject
             BulletSimAPI.SetRestitution2(PhysBody.ptr, matAttrib.restitution);
 
             // Mass is zero which disables a bunch of physics stuff in Bullet
-            UpdatePhysicalMassProperties(0f);
+            UpdatePhysicalMassProperties(0f, false);
             // Set collision detection parameters
             if (BSParam.CcdMotionThreshold > 0f)
             {
@@ -777,7 +802,7 @@ public sealed class BSPrim : BSPhysObject
             // DEBUG DEBUG BulletSimAPI.SetCenterOfMassByPosRot2(Linkset.LinksetRoot.PhysBody.ptr, _position, _orientation);
 
             // A dynamic object has mass
-            UpdatePhysicalMassProperties(RawMass);
+            UpdatePhysicalMassProperties(RawMass, false);
 
             // Set collision detection parameters
             if (BSParam.CcdMotionThreshold > 0f)
@@ -950,13 +975,9 @@ public sealed class BSPrim : BSPhysObject
         set {
             _buoyancy = value;
             // DetailLog("{0},BSPrim.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
-            // Buoyancy is faked by changing the gravity applied to the object
-            if (PhysBody.HasPhysicalBody)
-            {
-                float grav = PhysicsScene.Params.gravity * (1f - _buoyancy);
-                BulletSimAPI.SetGravity2(PhysBody.ptr, new OMV.Vector3(0f, 0f, grav));
-                ActivateIfPhysical(false);
-            }
+            // Force the recalculation of the various inertia,etc variables in the object
+            UpdatePhysicalMassProperties(_mass, true);
+            ActivateIfPhysical(false);
         }
     }
 
@@ -993,7 +1014,9 @@ public sealed class BSPrim : BSPhysObject
     public override float APIDDamping { set { return; } }
 
     public override void AddForce(OMV.Vector3 force, bool pushforce) {
-        AddForce(force, pushforce, false);
+        // Since this force is being applied in only one step, make this a force per second.
+        OMV.Vector3 addForce = force / PhysicsScene.LastTimeStep;
+        AddForce(addForce, pushforce, false);
     }
     // Applying a force just adds this to the total force on the object.
     // This added force will only last the next simulation tick.
@@ -1001,8 +1024,16 @@ public sealed class BSPrim : BSPhysObject
         // for an object, doesn't matter if force is a pushforce or not
         if (force.IsFinite())
         {
+            float magnitude = force.Length();
+            if (magnitude > 20000f)
+            {
+                // Force has a limit
+                force = force / magnitude * 20000f;
+            }
+
             OMV.Vector3 addForce = force;
             DetailLog("{0},BSPrim.addForce,call,force={1}", LocalID, addForce);
+
             PhysicsScene.TaintedObject(inTaintTime, "BSPrim.AddForce", delegate()
             {
                 // Bullet adds this central force to the total force for this tick
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSScene.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSScene.cs
index 6bcea3f..1a7c34b 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSScene.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSScene.cs
@@ -74,7 +74,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
     public IMesher mesher;
     public uint WorldID { get; private set; }
-    public BulletSim World { get; private set; }
+    public BulletWorld World { get; private set; }
 
     // All the constraints that have been allocated in this instance.
     public BSConstraintCollection Constraints { get; private set; }
@@ -244,7 +244,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
         // m_log.DebugFormat("{0}: Initialize: Calling BulletSimAPI.Initialize.", LogHeader);
 
-        World = new BulletSim(0, this, BulletSimAPI.Initialize2(worldExtent, UnmanagedParams,
+        World = new BulletWorld(0, this, BulletSimAPI.Initialize2(worldExtent, UnmanagedParams,
                                         m_maxCollisionsPerFrame, ref m_collisionArray,
                                         m_maxUpdatesPerFrame,ref m_updateArray,
                                         m_DebugLogCallbackHandle));
@@ -497,13 +497,16 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
 
         InTaintTime = false; // Only used for debugging so locking is not necessary.
 
+        // The following causes the unmanaged code to output ALL the values found in ALL the objects in the world.
+        // Only enable this in a limited test world with few objects.
+        // BulletSimAPI.DumpAllInfo2(World.ptr);    // DEBUG DEBUG DEBUG
+
         // step the physical world one interval
         m_simulationStep++;
         int numSubSteps = 0;
 
         try
         {
-            //if (VehicleLoggingEnabled) DumpVehicles();  // DEBUG
             if (PhysicsLogging.Enabled) beforeTime = Util.EnvironmentTickCount();
 
             numSubSteps = BulletSimAPI.PhysicsStep2(World.ptr, timeStep, m_maxSubSteps, m_fixedTimeStep,
@@ -833,7 +836,7 @@ public sealed class BSScene : PhysicsScene, IPhysicsParameters
         {
             DetailLog("{0},BSScene.AssertInTaintTime,NOT IN TAINT TIME,Region={1},Where={2}", DetailLogZero, RegionName, whereFrom);
             m_log.ErrorFormat("{0} NOT IN TAINT TIME!! Region={1}, Where={2}", LogHeader, RegionName, whereFrom);
-            Util.PrintCallStack();  // Prints the stack into the DEBUG log file.
+            Util.PrintCallStack(DetailLog);
         }
         return InTaintTime;
     }
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSShapeCollection.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSShapeCollection.cs
index 398ece0..47fb768 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSShapeCollection.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSShapeCollection.cs
@@ -98,7 +98,7 @@ public sealed class BSShapeCollection : IDisposable
     //    higher level dependencies on the shape or body. Mostly used for LinkSets to
     //    remove the physical constraints before the body is destroyed.
     // Called at taint-time!!
-    public bool GetBodyAndShape(bool forceRebuild, BulletSim sim, BSPhysObject prim,
+    public bool GetBodyAndShape(bool forceRebuild, BulletWorld sim, BSPhysObject prim,
                     ShapeDestructionCallback shapeCallback, BodyDestructionCallback bodyCallback)
     {
         PhysicsScene.AssertInTaintTime("BSShapeCollection.GetBodyAndShape");
@@ -126,7 +126,7 @@ public sealed class BSShapeCollection : IDisposable
         return ret;
     }
 
-    public bool GetBodyAndShape(bool forceRebuild, BulletSim sim, BSPhysObject prim)
+    public bool GetBodyAndShape(bool forceRebuild, BulletWorld sim, BSPhysObject prim)
     {
         return GetBodyAndShape(forceRebuild, sim, prim, null, null);
     }
@@ -918,7 +918,7 @@ public sealed class BSShapeCollection : IDisposable
     // Updates prim.BSBody with the information about the new body if one is created.
     // Returns 'true' if an object was actually created.
     // Called at taint-time.
-    private bool CreateBody(bool forceRebuild, BSPhysObject prim, BulletSim sim, BulletShape shape,
+    private bool CreateBody(bool forceRebuild, BSPhysObject prim, BulletWorld sim, BulletShape shape,
                             BodyDestructionCallback bodyCallback)
     {
         bool ret = false;
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSTerrainHeightmap.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSTerrainHeightmap.cs
index 252953b..ba17059 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSTerrainHeightmap.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSTerrainHeightmap.cs
@@ -103,7 +103,7 @@ public sealed class BSTerrainHeightmap : BSTerrainPhys
         Vector3 centerPos;
         centerPos.X = m_mapInfo.minCoords.X + (m_mapInfo.sizeX / 2f);
         centerPos.Y = m_mapInfo.minCoords.Y + (m_mapInfo.sizeY / 2f);
-        centerPos.Z = m_mapInfo.minZ + ((m_mapInfo.maxZ - m_mapInfo.minZ) / 2f + 0.5f);
+        centerPos.Z = m_mapInfo.minZ + ((m_mapInfo.maxZ - m_mapInfo.minZ) / 2f - 0.5f);
 
         m_mapInfo.terrainBody = new BulletBody(m_mapInfo.ID,
                 BulletSimAPI.CreateBodyWithDefaultMotionState2(m_mapInfo.terrainShape.ptr,
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BSTerrainManager.cs b/OpenSim/Region/Physics/BulletSNPlugin/BSTerrainManager.cs
index dfad70e..66d62f0 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BSTerrainManager.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BSTerrainManager.cs
@@ -140,7 +140,7 @@ public sealed class BSTerrainManager : IDisposable
         m_groundPlane = new BulletBody(BSScene.GROUNDPLANE_ID,
                         BulletSimAPI.CreateBodyWithDefaultMotionState2(groundPlaneShape.ptr, BSScene.GROUNDPLANE_ID,
                                                             Vector3.Zero, Quaternion.Identity));
-        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, m_groundPlane.ptr);
+        BulletSimAPI.AddObjectToWorld2(PhysicsScene.World.ptr, m_groundPlane.ptr, Vector3.Zero, Quaternion.Identity);
         BulletSimAPI.UpdateSingleAabb2(PhysicsScene.World.ptr, m_groundPlane.ptr);
         // Ground plane does not move
         BulletSimAPI.ForceActivationState2(m_groundPlane.ptr, ActivationState.DISABLE_SIMULATION);
@@ -376,6 +376,7 @@ public sealed class BSTerrainManager : IDisposable
             DetailLog("{0},BSTerrainManager.GetTerrainHeightAtXYZ,terrainNotFound,pos={1},base={2}",
                                 BSScene.DetailLogZero, pos, terrainBaseXYZ);
         }
+
         lastHeight = ret;
         return ret;
     }
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSNPlugin/BulletSimAPI.cs
index 6af59d6..93643c9 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BulletSimAPI.cs
@@ -58,7 +58,6 @@ public enum ConstraintType : int
         MAX_CONSTRAINT_TYPE
 }
 
-
 // ===============================================================================
 [StructLayout(LayoutKind.Sequential)]
 public struct ConvexHull
diff --git a/OpenSim/Region/Physics/BulletSNPlugin/BulletSimData.cs b/OpenSim/Region/Physics/BulletSNPlugin/BulletSimData.cs
index a1ed8d8..f509dc4 100644
--- a/OpenSim/Region/Physics/BulletSNPlugin/BulletSimData.cs
+++ b/OpenSim/Region/Physics/BulletSNPlugin/BulletSimData.cs
@@ -35,9 +35,9 @@ namespace OpenSim.Region.Physics.BulletSNPlugin
 // These hold pointers to allocated objects in the unmanaged space.
 
 // The physics engine controller class created at initialization
-public struct BulletSim
+public struct BulletWorld
 {
-    public BulletSim(uint worldId, BSScene bss, object xx)
+    public BulletWorld(uint worldId, BSScene bss, object xx)
     {
         ptr = xx;
         worldID = worldId;