3 files changed, 104 insertions, 22 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
index b88ec3c..5cf8953 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSCharacter.cs
@@ -234,6 +234,15 @@ public class BSCharacter : BSPhysObject
             _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;
@@ -242,18 +251,22 @@ public class BSCharacter : BSPhysObject
     // 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()
+    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.
-            PhysicsScene.TaintedObject("BSCharacter.PositionSanityCheck", delegate()
+            BSScene.TaintCallback sanityOperation = delegate()
             {
                 DetailLog("{0},BSCharacter.PositionSanityCheck,taint,pos={1},orient={2}", LocalID, _position, _orientation);
                 BulletSimAPI.SetObjectTranslation(PhysicsScene.WorldID, LocalID, _position, _orientation);
-            });
+            };
+            if (atTaintTime)
+                sanityOperation();
+            else
+                PhysicsScene.TaintedObject("BSCharacter.PositionSanityCheck", sanityOperation);
             ret = true;
         }
         return ret;
@@ -378,7 +391,16 @@ public class BSCharacter : BSPhysObject
         set { _collidingObj = value; }
     }
     public override bool FloatOnWater {
-        set { _floatOnWater = value; }
+        set { 
+            _floatOnWater = value;
+            PhysicsScene.TaintedObject("BSCharacter.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 { return _rotationalVelocity; }
@@ -493,15 +515,14 @@ public class BSCharacter : BSPhysObject
         _velocity = entprop.Velocity;
         _acceleration = entprop.Acceleration;
         _rotationalVelocity = entprop.RotationalVelocity;
+        // Do some sanity checking for the avatar. Make sure it's above ground and inbounds.
+        PositionSanityCheck2(true);
+
         // Avatars don't report their changes the usual way. Changes are checked for in the heartbeat loop.
         // base.RequestPhysicsterseUpdate();
 
-        // Do some sanity checking for the avatar. Make sure it's above ground and inbounds.
-        PositionSanityCheck2();
-
-        float heightHere = PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(_position);   // only for debug
-        DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5},terrain={6}",
-                LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity, heightHere);
+        DetailLog("{0},BSCharacter.UpdateProperties,call,pos={1},orient={2},vel={3},accel={4},rotVel={5}",
+                LocalID, _position, _orientation, _velocity, _acceleration, _rotationalVelocity);
     }
 }
 }
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);
 
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
index 1125d7e..d49e5f3 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BulletSimAPI.cs
@@ -344,10 +344,7 @@ public enum CollisionFlags : uint
     CF_DISABLE_SPU_COLLISION_PROCESS = 1 << 6,
     // Following used by BulletSim to control collisions
     BS_SUBSCRIBE_COLLISION_EVENTS    = 1 << 10,
-    // BS_VOLUME_DETECT_OBJECT          = 1 << 11,
-    // BS_PHANTOM_OBJECT                = 1 << 12,
-    // BS_PHYSICAL_OBJECT               = 1 << 13,
-    // BS_TERRAIN_OBJECT                = 1 << 14,
+    BS_FLOATS_ON_WATER               = 1 << 11,
     BS_NONE                          = 0,
     BS_ALL                           = 0xFFFFFFFF,
 
@@ -356,9 +353,6 @@ public enum CollisionFlags : uint
     BS_ACTIVE = CF_STATIC_OBJECT
                 | CF_KINEMATIC_OBJECT
                 | CF_NO_CONTACT_RESPONSE
-    //            | BS_VOLUME_DETECT_OBJECT
-    //            | BS_PHANTOM_OBJECT
-    //            | BS_PHYSICAL_OBJECT,
 };
 
 // Values for collisions groups and masks