BulletSim: small change to add position correction force with AddForce rather than just storing it in the variable

2 files changed, 22 insertions, 16 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index dbc9039..a121c3d 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -518,13 +518,18 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             if (IsActive)
             {
+                VDetailLog("{0},BSDynamics.Refresh", Prim.LocalID);
+                m_vehicleMass = Prim.Linkset.LinksetMass;
+
                 // Friction effects are handled by this vehicle code
                 BulletSimAPI.SetFriction2(Prim.PhysBody.ptr, 0f);
                 BulletSimAPI.SetHitFraction2(Prim.PhysBody.ptr, 0f);
 
-                // BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, 0.8f);
+                BulletSimAPI.SetAngularDamping2(Prim.PhysBody.ptr, 0.8f);
+
+                BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, new Vector3(1f, 1f, 1f));
+
 
-                VDetailLog("{0},BSDynamics.Refresh,zeroingFriction and adding damping", Prim.LocalID);
             }
         }
 
@@ -560,8 +565,6 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // m_lastLinearVelocityVector = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation);        // DEBUG:
             // END DEBUG
 
-            m_vehicleMass = Prim.Linkset.LinksetMass;
-
             MoveLinear(pTimestep);
             // Commented out for debug
             MoveAngular(pTimestep);
@@ -650,6 +653,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // if (rotatedSize.Z < terrainHeight)
             if (pos.Z < terrainHeight)
             {
+                // TODO: correct position by applying force rather than forcing position.
                 pos.Z = terrainHeight + 2;
                 Prim.ForcePosition = pos;
                 VDetailLog("{0},MoveLinear,terrainHeight,terrainHeight={1},pos={2}", Prim.LocalID, terrainHeight, pos);
@@ -810,9 +814,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         private void MoveAngular(float pTimestep)
         {
             // m_angularMotorDirection         // angular velocity requested by LSL motor
-            // m_angularMotorApply             // application frame counter
             // m_angularMotorVelocity          // current angular motor velocity (ramps up and down)
-            // m_angularMotorTimescale         // motor angular velocity ramp up rate
+            // m_angularMotorTimescale         // motor angular velocity ramp up time
             // m_angularMotorDecayTimescale    // motor angular velocity decay rate
             // m_angularFrictionTimescale      // body angular velocity  decay rate
             // m_lastAngularVelocity           // what was last applied to body
@@ -847,7 +850,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             {
                 float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale;
                 if (Prim.IsColliding)
-                    VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale);
+                    VAservo = pTimestep * 0.05f / m_verticalAttractionTimescale;
 
                 VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
 
@@ -925,6 +928,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 float mix = Math.Abs(m_bankingMix);
                 if (m_angularMotorVelocity.X == 0)
                 {
+                    // The vehicle is stopped
                     /*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
                     {
                         Vector3 axisAngle;
@@ -938,9 +942,12 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     }*/
                 }
                 else
-                    banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X) * 4;
+                {
+                    banking.Z += (effSquared * (mult * mix)) * (m_angularMotorVelocity.X) * 4;
+                }
+
+                //If they are colliding, we probably shouldn't shove the prim around... probably
                 if (!Prim.IsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
-                    //If they are colliding, we probably shouldn't shove the prim around... probably
                 {
                     float angVelZ = m_angularMotorVelocity.X*-1;
                     /*if(angVelZ > mix)
@@ -957,8 +964,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     banking += bankingRot;
                 }
                 m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency;
-                VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},banking={3}", 
-                                Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, banking);
+                VDetailLog("{0},MoveAngular,Banking,bEff={1},angMotVel={2},effSq={3},mult={4},mix={5},banking={6}", 
+                                Prim.LocalID, m_bankingEfficiency, m_angularMotorVelocity, effSquared, mult, mix, banking);
             }
 
             #endregion
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
index 2b3fa25..caa6c46 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSPrim.cs
@@ -342,13 +342,12 @@ public sealed class BSPrim : BSPhysObject
         // TODO: check for out of bounds
 
         // The above code computes a force to apply to correct any out-of-bounds problems. Apply same.
+        // TODO: This should be intergrated with a geneal physics action mechanism.
+        // TODO: This should be moderated with PID'ness.
         if (ret)
         {
-            PhysicsScene.TaintedObject(inTaintTime, "BSPrim.PositionSanityCheck:belowTerrain", delegate()
-            {
-                // Apply upforce and overcome gravity.
-                ForceVelocity = ForceVelocity + upForce - PhysicsScene.DefaultGravity;
-            });
+            // Apply upforce and overcome gravity.
+            AddForce(upForce - PhysicsScene.DefaultGravity, false, inTaintTime);
         }
         return ret;
     }