4 files changed, 82 insertions, 28 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/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/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();