1 files changed, 97 insertions, 24 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 0323b0d..4dff927 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -70,6 +70,8 @@ public class BSPrim : BSPhysObject
     private bool _kinematic;
     private float _buoyancy;
 
+    private int CrossingFailures { get; set; }
+
     public BSDynamics VehicleController { get; private set; }
 
     private BSVMotor _targetMotor;
@@ -197,7 +199,20 @@ public class BSPrim : BSPhysObject
     {
         get { return _isSelected; }
     }
-    public override void CrossingFailure() { return; }
+
+    public override void CrossingFailure()
+    {
+        CrossingFailures++;
+        if (CrossingFailures > BSParam.CrossingFailuresBeforeOutOfBounds)
+        {
+            base.RaiseOutOfBounds(RawPosition);
+        }
+        else if (CrossingFailures == BSParam.CrossingFailuresBeforeOutOfBounds)
+        {
+            m_log.WarnFormat("{0} Too many crossing failures for {1}", LogHeader, Name);
+        }
+        return;
+    }
 
     // link me to the specified parent
     public override void link(PhysicsActor obj) {
@@ -239,50 +254,98 @@ public class BSPrim : BSPhysObject
         });
     }
 
+    bool TryExperimentalLockAxisCode = false;
+    BSConstraint LockAxisConstraint = null;
     public override void LockAngularMotion(OMV.Vector3 axis)
     {
         DetailLog("{0},BSPrim.LockAngularMotion,call,axis={1}", LocalID, axis);
 
+        // "1" means free, "0" means locked
         OMV.Vector3 locking = new OMV.Vector3(1f, 1f, 1f);
         if (axis.X != 1) locking.X = 0f;
         if (axis.Y != 1) locking.Y = 0f;
         if (axis.Z != 1) locking.Z = 0f;
         LockedAxis = locking;
 
-        /*  Not implemented yet
-        if (LockedAxis != LockedAxisFree)
+        if (TryExperimentalLockAxisCode && LockedAxis != LockedAxisFree)
         {
-            // Something is locked so start the thingy that keeps that axis from changing
-            RegisterPreUpdatePropertyAction("BSPrim.LockAngularMotion", delegate(ref EntityProperties entprop)
+            // Lock that axis by creating a 6DOF constraint that has one end in the world and
+            //    the other in the object.
+            // http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?p=20817
+            // http://www.bulletphysics.org/Bullet/phpBB3/viewtopic.php?p=26380
+
+            PhysicsScene.TaintedObject("BSPrim.LockAngularMotion", delegate()
             {
-                if (LockedAxis != LockedAxisFree)
+                CleanUpLockAxisPhysicals(true /* inTaintTime */);
+
+                BSConstraint6Dof axisConstrainer = new BSConstraint6Dof(PhysicsScene.World, PhysBody, 
+                                    OMV.Vector3.Zero, OMV.Quaternion.Inverse(RawOrientation),
+                                    true /* useLinearReferenceFrameB */, true /* disableCollisionsBetweenLinkedBodies */);
+                LockAxisConstraint = axisConstrainer;
+                PhysicsScene.Constraints.AddConstraint(LockAxisConstraint);
+
+                // The constraint is tied to the world and oriented to the prim.
+
+                // Free to move linearly
+                OMV.Vector3 linearLow = OMV.Vector3.Zero;
+                OMV.Vector3 linearHigh = PhysicsScene.TerrainManager.DefaultRegionSize;
+                axisConstrainer.SetLinearLimits(linearLow, linearHigh);
+
+                // Angular with some axis locked
+                float f2PI = (float)Math.PI * 2f;
+                OMV.Vector3 angularLow = new OMV.Vector3(-f2PI, -f2PI, -f2PI);
+                OMV.Vector3 angularHigh = new OMV.Vector3(f2PI, f2PI, f2PI);
+                if (LockedAxis.X != 1f)
                 {
-                    if (IsPhysicallyActive)
-                    {
-                        // Bullet can lock axis but it only works for global axis.
-                        // Check if this prim is aligned on global axis and use Bullet's
-                        //    system if so.
-
-                        ForceOrientation = entprop.Rotation;
-                        ForceRotationalVelocity = entprop.RotationalVelocity;
-                    }
+                    angularLow.X = 0f;
+                    angularHigh.X = 0f;
                 }
-                else
+                if (LockedAxis.Y != 1f)
+                {
+                    angularLow.Y = 0f;
+                    angularHigh.Y = 0f;
+                }
+                if (LockedAxis.Z != 1f)
                 {
-                    UnRegisterPreUpdatePropertyAction("BSPrim.LockAngularMotion");
+                    angularLow.Z = 0f;
+                    angularHigh.Z = 0f;
                 }
+                axisConstrainer.SetAngularLimits(angularLow, angularHigh);
+
+                DetailLog("{0},BSPrim.LockAngularMotion,create,linLow={1},linHi={2},angLow={3},angHi={4}",
+                                            LocalID, linearLow, linearHigh, angularLow, angularHigh);
 
+                // Constants from one of the posts mentioned above and used in Bullet's ConstraintDemo.
+                axisConstrainer.TranslationalLimitMotor(true /* enable */, 5.0f, 0.1f);
+
+                axisConstrainer.RecomputeConstraintVariables(RawMass);
             });
         }
         else
         {
             // Everything seems unlocked
-            UnRegisterPreUpdatePropertyAction("BSPrim.LockAngularMotion");
+            CleanUpLockAxisPhysicals(false /* inTaintTime */);
         }
-         */
 
         return;
     }
+    // Get rid of any constraint built for LockAxis
+    // Most often the constraint is removed when the constraint collection is cleaned for this prim.
+    private void CleanUpLockAxisPhysicals(bool inTaintTime)
+    {
+        if (LockAxisConstraint != null)
+        {
+            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.CleanUpLockAxisPhysicals", delegate()
+            {
+                if (LockAxisConstraint != null)
+                {
+                    PhysicsScene.Constraints.RemoveAndDestroyConstraint(LockAxisConstraint);
+                    LockAxisConstraint = null;
+                    DetailLog("{0},BSPrim.CleanUpLockAxisPhysicals,destroyingConstraint", LocalID);
+                }
+            });
+        }
+    }
 
     public override OMV.Vector3 RawPosition
     {
@@ -762,6 +825,7 @@ public class BSPrim : BSPhysObject
                     SetObjectDynamic(true);
                     // whether phys-to-static or static-to-phys, the object is not moving.
                     ZeroMotion(true);
+
                 });
             }
         }
@@ -885,6 +949,8 @@ public class BSPrim : BSPhysObject
 
             // For good measure, make sure the transform is set through to the motion state
             ForcePosition = _position;
+            ForceVelocity = _velocity;
+            ForceRotationalVelocity = _rotationalVelocity;
 
             // A dynamic object has mass
             UpdatePhysicalMassProperties(RawMass, false);
@@ -1064,15 +1130,19 @@ public class BSPrim : BSPhysObject
             _buoyancy = value;
             // DetailLog("{0},BSPrim.setForceBuoyancy,taint,buoy={1}", LocalID, _buoyancy);
             // Force the recalculation of the various inertia,etc variables in the object
-            DetailLog("{0},BSPrim.ForceBuoyancy,buoy={1},mass={2}", LocalID, _buoyancy, _mass);
-            UpdatePhysicalMassProperties(_mass, true);
+            UpdatePhysicalMassProperties(RawMass, true);
+            DetailLog("{0},BSPrim.ForceBuoyancy,buoy={1},mass={2},grav={3}", LocalID, _buoyancy, RawMass, Gravity);
             ActivateIfPhysical(false);
         }
     }
 
     // Used for MoveTo
     public override OMV.Vector3 PIDTarget {
-        set { _PIDTarget = value; }
+        set
+        {
+            // TODO: add a sanity check -- don't move more than a region or something like that.
+            _PIDTarget = value;
+        }
     }
     public override float PIDTau {
         set { _PIDTau = value; }
@@ -1126,7 +1196,9 @@ public class BSPrim : BSPhysObject
                     }
                     else
                     {
-                        ForcePosition = movePosition;
+                        _position = movePosition;
+                        PositionSanityCheck(true /* intaintTime */);
+                        ForcePosition = _position;
                     }
                     DetailLog("{0},BSPrim.PIDTarget,move,fromPos={1},movePos={2}", LocalID, origPosition, movePosition);
                 });
@@ -1303,6 +1375,7 @@ public class BSPrim : BSPhysObject
             {
                 if (PhysBody.HasPhysicalBody)
                 {
+                    DetailLog("{0},BSPrim.AddAngularForce,taint,angForce={1}", LocalID, angForce);
                     PhysicsScene.PE.ApplyTorque(PhysBody, angForce);
                     ActivateIfPhysical(false);
                 }
@@ -1667,7 +1740,7 @@ public class BSPrim : BSPhysObject
         // _velocity = entprop.Velocity;
         // DEBUG DEBUG DEBUG -- smooth velocity changes a bit. The simulator seems to be
         //    very sensitive to velocity changes.
-        if (!entprop.Velocity.ApproxEquals(_velocity, 0.1f))
+        if (entprop.Velocity == OMV.Vector3.Zero || !entprop.Velocity.ApproxEquals(_velocity, 0.1f))
             _velocity = entprop.Velocity;
         _acceleration = entprop.Acceleration;
         _rotationalVelocity = entprop.RotationalVelocity;