1 files changed, 72 insertions, 5 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index e54bf75..e37a4a0 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -267,6 +267,7 @@ public sealed class BSPrim : BSPhysObject
         set {
             _position = value;
             // TODO: what does it mean to set the position of a child prim?? Rebuild the constraint?
+            PositionSanityCheck();
             PhysicsScene.TaintedObject("BSPrim.setPosition", delegate()
             {
                 // DetailLog("{0},BSPrim.SetPosition,taint,pos={1},orient={2}", LocalID, _position, _orientation);
@@ -275,6 +276,63 @@ public sealed class BSPrim : BSPhysObject
         }
     }
 
+    // Check that the current position is sane and, if not, modify the position to make it so.
+    // Check for being below terrain and being out of bounds.
+    // Returns 'true' of the position was made sane by some action.
+    private bool PositionSanityCheck()
+    {
+        bool ret = false;
+      
+        // If totally below the ground, move the prim up
+        // TODO: figure out the right solution for this... only for dynamic objects?
+        /*
+        float terrainHeight = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);
+        if (Position.Z < terrainHeight)
+        {
+            DetailLog("{0},BSPrim.PositionAdjustUnderGround,call,pos={1},terrain={2}", LocalID, _position, terrainHeight);
+            _position.Z = terrainHeight + 2.0f;
+            ret = true;
+        }
+         */
+        if ((CurrentCollisionFlags & CollisionFlags.BS_FLOATS_ON_WATER) != 0)
+        {
+            float waterHeight = PhysicsScene.GetWaterLevelAtXYZ(_position);
+            if (Position.Z < waterHeight)
+            {
+                _position.Z = waterHeight;
+                ret = true;
+            }
+        }
+
+        // TODO: check for out of bounds
+        return ret;
+    }
+
+    // A version of the sanity check that also makes sure a new position value is
+    //    pushed back to the physics engine. This routine would be used by anyone
+    //    who is not already pushing the value.
+    private bool PositionSanityCheck2(bool atTaintTime)
+    {
+        bool ret = false;
+        if (PositionSanityCheck())
+        {
+            // The new position value must be pushed into the physics engine but we can't
+            //    just assign to "Position" because of potential call loops.
+            BSScene.TaintCallback sanityOperation = delegate()
+            {
+                DetailLog("{0},BSPrim.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
+                BulletSimAPI.SetObjectTranslation(PhysicsScene.WorldID, LocalID, _position, _orientation);
+            };
+            if (atTaintTime)
+                sanityOperation();
+            else
+                PhysicsScene.TaintedObject("BSPrim.PositionSanityCheck", sanityOperation);
+
+            ret = true;
+        }
+        return ret;
+    }
+
     // Return the effective mass of the object.
     // If there are multiple items in the linkset, add them together for the root
     public override float Mass
@@ -481,11 +539,10 @@ public sealed class BSPrim : BSPhysObject
         // This is a NOOP if the object is not in the world (BulletSim and Bullet ignore objects not found).
         BulletSimAPI.RemoveObjectFromWorld2(PhysicsScene.World.ptr, BSBody.ptr);
 
-
         // Set up the object physicalness (does gravity and collisions move this object)
         MakeDynamic(IsStatic);
 
-        // Do any vehicle stuff
+        // Update vehicle specific parameters
         _vehicle.Refresh();
 
         // Arrange for collision events if the simulator wants them
@@ -556,7 +613,6 @@ public sealed class BSPrim : BSPhysObject
             // A dynamic object has mass
             IntPtr collisionShapePtr = BulletSimAPI.GetCollisionShape2(BSBody.ptr);
             OMV.Vector3 inertia = BulletSimAPI.CalculateLocalInertia2(collisionShapePtr, Mass);
-            // OMV.Vector3 inertia = OMV.Vector3.Zero;
             BulletSimAPI.SetMassProps2(BSBody.ptr, _mass, inertia);
             BulletSimAPI.UpdateInertiaTensor2(BSBody.ptr);
 
@@ -566,7 +622,7 @@ public sealed class BSPrim : BSPhysObject
             BulletSimAPI.SetSleepingThresholds2(BSBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
             BulletSimAPI.SetContactProcessingThreshold2(BSBody.ptr, PhysicsScene.Params.contactProcessingThreshold);
 
-            // There can be special things needed for implementing linksets.
+            // There might be special things needed for implementing linksets.
             Linkset.MakeDynamic(this);
 
             // Force activation of the object so Bullet will act on it.
@@ -656,7 +712,16 @@ public sealed class BSPrim : BSPhysObject
         }
     }
     public override bool FloatOnWater {
-        set { _floatOnWater = value; }
+        set { 
+            _floatOnWater = value;
+            PhysicsScene.TaintedObject("BSPrim.setFloatOnWater", delegate()
+            {
+                if (_floatOnWater)
+                    CurrentCollisionFlags = BulletSimAPI.AddToCollisionFlags2(BSBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+                else
+                    CurrentCollisionFlags = BulletSimAPI.RemoveFromCollisionFlags2(BSBody.ptr, CollisionFlags.BS_FLOATS_ON_WATER);
+            });
+        }
     }
     public override OMV.Vector3 RotationalVelocity {
         get {
@@ -1198,6 +1263,8 @@ public sealed class BSPrim : BSPhysObject
             _acceleration = entprop.Acceleration;
             _rotationalVelocity = entprop.RotationalVelocity;
 
+            PositionSanityCheck2(true);
+
             DetailLog("{0},BSPrim.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
                     LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);