1 files changed, 161 insertions, 63 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index 819635a..dbc9039 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -54,10 +54,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 {
     public sealed class BSDynamics
     {
+        private static string LogHeader = "[BULLETSIM VEHICLE]";
+
         private BSScene PhysicsScene { get; set; }
         // the prim this dynamic controller belongs to
         private BSPrim Prim { get; set; }
 
+        // mass of the vehicle fetched each time we're calles
+        private float m_vehicleMass;
+
         // Vehicle properties
         public Vehicle Type { get; set; }
 
@@ -516,7 +521,29 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // 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);
+
+                VDetailLog("{0},BSDynamics.Refresh,zeroingFriction and adding damping", Prim.LocalID);
+            }
+        }
+
+        public bool RemoveBodyDependencies(BSPhysObject prim)
+        {
+            // If active, we need to add our properties back when the body is rebuilt.
+            return IsActive;
+        }
+
+        public void RestoreBodyDependencies(BSPhysObject prim)
+        {
+            if (Prim.LocalID != prim.LocalID)
+            {
+                // The call should be on us by our prim. Error if not.
+                PhysicsScene.Logger.ErrorFormat("{0} RestoreBodyDependencies: called by not my prim. passedLocalID={1}, vehiclePrimLocalID={2}",
+                                LogHeader, prim.LocalID, Prim.LocalID);
+                return;
             }
+            Refresh();
         }
 
         // One step of the vehicle properties for the next 'pTimestep' seconds.
@@ -533,16 +560,26 @@ 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);
-            LimitRotation(pTimestep);
+            // Prim.ApplyTorqueImpulse(-Prim.RotationalVelocity * m_vehicleMass, false);    // DEBUG DEBUG
+            // Prim.ForceRotationalVelocity = -Prim.RotationalVelocity;    // DEBUG DEBUG
 
-            // DEBUG: Trying to figure out why Bullet goes crazy when the root prim is moved.
-            // BulletSimAPI.SetInterpolationVelocity2(Prim.BSBody.ptr, m_newVelocity, m_lastAngularVelocity); // DEBUG DEBUG DEBUG
+            LimitRotation(pTimestep);
 
             // remember the position so next step we can limit absolute movement effects
             m_lastPositionVector = Prim.ForcePosition;
 
+            VDetailLog("{0},BSDynamics.Step,frict={1},grav={2},inertia={3},mass={4}",        // DEBUG DEBUG
+                                    Prim.LocalID, 
+                                    BulletSimAPI.GetFriction2(Prim.PhysBody.ptr),
+                                    BulletSimAPI.GetGravity2(Prim.PhysBody.ptr),
+                                    Prim.Inertia,
+                                    m_vehicleMass
+                                    );
             VDetailLog("{0},BSDynamics.Step,done,pos={1},force={2},velocity={3},angvel={4}",
                     Prim.LocalID, Prim.ForcePosition, Prim.Force, Prim.ForceVelocity, Prim.RotationalVelocity);
         }// end Step
@@ -555,25 +592,26 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // m_lastLinearVelocityVector is the current speed we are moving in that direction
             if (m_linearMotorDirection.LengthSquared() > 0.001f)
             {
-                Vector3 origDir = m_linearMotorDirection;
-                Vector3 origVel = m_lastLinearVelocityVector;
+                Vector3 origDir = m_linearMotorDirection;       // DEBUG
+                Vector3 origVel = m_lastLinearVelocityVector;   // DEBUG
+                // DEBUG: the vehicle velocity rotated to be relative to vehicle coordinates for comparison
                 Vector3 vehicleVelocity = Prim.ForceVelocity * Quaternion.Inverse(Prim.ForceOrientation);   // DEBUG
 
-                // add drive to body
+                // Add (desiredVelocity - lastAppliedVelocity) / howLongItShouldTakeToComplete
                 Vector3 addAmount = (m_linearMotorDirection - m_lastLinearVelocityVector)/(m_linearMotorTimescale) * pTimestep;
-                // lastLinearVelocityVector is the current body velocity vector
                 m_lastLinearVelocityVector += addAmount;
 
                 float decayFactor = (1.0f / m_linearMotorDecayTimescale) * pTimestep;
                 m_linearMotorDirection *= (1f - decayFactor);
 
-                Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep;
-                m_lastLinearVelocityVector *= (Vector3.One - frictionFactor);
-
                 // Rotate new object velocity from vehicle relative to world coordinates
                 m_newVelocity = m_lastLinearVelocityVector * Prim.ForceOrientation;
 
-                VDetailLog("{0},MoveLinear,nonZero,origdir={1},origvel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lmVel={8},newVel={9}",
+                // Apply friction for next time
+                Vector3 frictionFactor = (Vector3.One / m_linearFrictionTimescale) * pTimestep;
+                m_lastLinearVelocityVector *= (Vector3.One - frictionFactor);
+
+                VDetailLog("{0},MoveLinear,nonZero,origlmDir={1},origlvVel={2},vehVel={3},add={4},decay={5},frict={6},lmDir={7},lvVec={8},newVel={9}",
                                 Prim.LocalID, origDir, origVel, vehicleVelocity, addAmount, decayFactor, frictionFactor, 
                                 m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity);
             }
@@ -607,7 +645,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // If below the terrain, move us above the ground a little.
             float terrainHeight = Prim.PhysicsScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
             // Taking the rotated size doesn't work here because m_prim.Size is the size of the root prim and not the linkset.
-            //     Need to add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
+            //     TODO: Add a m_prim.LinkSet.Size similar to m_prim.LinkSet.Mass.
             // Vector3 rotatedSize = m_prim.Size * m_prim.ForceOrientation;
             // if (rotatedSize.Z < terrainHeight)
             if (pos.Z < terrainHeight)
@@ -638,13 +676,15 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                 if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
                 {
-                    // If body is aready heigher, use its height as target height
-                    if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
+                    // If body is already heigher, use its height as target height
+                    if (pos.Z > m_VhoverTargetHeight)
+                        m_VhoverTargetHeight = pos.Z;
                 }
                 if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
                 {
-                    if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
+                    if (Math.Abs(pos.Z - m_VhoverTargetHeight) > 0.2f)
                     {
+                        pos.Z = m_VhoverTargetHeight;
                         Prim.ForcePosition = pos;
                     }
                 }
@@ -709,25 +749,28 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
             }
 
-            // Limit absolute vertical change
-            float Zchange = Math.Abs(posChange.Z);
+            #region downForce
+            Vector3 downForce = Vector3.Zero;
+
             if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
             {
-                if (Zchange > .3)
-                    grav.Z = (float)(grav.Z * 3);
-                if (Zchange > .15)
-                    grav.Z = (float)(grav.Z * 2);
-                if (Zchange > .75)
-                    grav.Z = (float)(grav.Z * 1.5);
-                if (Zchange > .05)
-                    grav.Z = (float)(grav.Z * 1.25);
-                if (Zchange > .025)
-                    grav.Z = (float)(grav.Z * 1.125);
-                float postemp = (pos.Z - terrainHeight);
-                if (postemp > 2.5f)
-                    grav.Z = (float)(grav.Z * 1.037125);
-                VDetailLog("{0},MoveLinear,limitMotorUp,grav={1}", Prim.LocalID, grav);
+                // If the vehicle is motoring into the sky, get it going back down.
+                // Is this an angular force or both linear and angular??
+                float distanceAboveGround = pos.Z - terrainHeight;
+                if (distanceAboveGround > 2f)
+                {
+                    // downForce = new Vector3(0, 0, (-distanceAboveGround / m_bankingTimescale) * pTimestep);
+                    // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
+                    downForce = new Vector3(0, 0, -distanceAboveGround);
+                }
+                // TODO: this calculation is all wrong. From the description at
+                //     (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
+                //     has a decay factor. This says this force should
+                //     be computed with a motor.
+                VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}", 
+                                    Prim.LocalID, distanceAboveGround, downForce);
             }
+            #endregion // downForce
 
             // If not changing some axis, reduce out velocity
             if ((m_flags & (VehicleFlag.NO_X)) != 0)
@@ -737,13 +780,28 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             if ((m_flags & (VehicleFlag.NO_Z)) != 0)
                 m_newVelocity.Z = 0;
 
-            // Apply velocity
+            // Clamp REALLY high or low velocities
+            if (m_newVelocity.LengthSquared() > 1e6f)
+            {
+                m_newVelocity /= m_newVelocity.Length();
+                m_newVelocity *= 1000f;
+            }
+            else if (m_newVelocity.LengthSquared() < 1e-6f)
+                m_newVelocity = Vector3.Zero;
+
+            // Stuff new linear velocity into the vehicle
             Prim.ForceVelocity = m_newVelocity;
-            // Prim.AddForce(m_newVelocity * Prim.Linkset.LinksetMass, false);
-            Prim.AddForce(grav * Prim.Linkset.LinksetMass, false);
+            // Prim.ApplyForceImpulse((m_newVelocity - Prim.Velocity) * m_vehicleMass, false);    // DEBUG DEBUG
 
-            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},grav={4}",
-                    Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, grav);
+            Vector3 totalDownForce = downForce + grav;
+            if (totalDownForce != Vector3.Zero)
+            {
+                Prim.AddForce(totalDownForce * m_vehicleMass, false);
+                // Prim.ApplyForceImpulse(totalDownForce * m_vehicleMass, false);
+            }
+
+            VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}",
+                    Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, m_newVelocity, Prim.Velocity, totalDownForce);
 
         } // end MoveLinear()
 
@@ -765,7 +823,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 Vector3 origDir = m_angularMotorDirection;
 
                 //       new velocity    +=                      error                       /  (  time to get there   / step interval)
-                //                             requested speed     -    last motor speed
+                //                           requested direction   - current vehicle direction
                 m_angularMotorVelocity += (m_angularMotorDirection - m_angularMotorVelocity) /  (m_angularMotorTimescale / pTimestep);
                 // decay requested direction
                 m_angularMotorDirection *= (1.0f - (pTimestep * 1.0f/m_angularMotorDecayTimescale));
@@ -784,10 +842,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             Vector3 deflection = Vector3.Zero;
             Vector3 banking = Vector3.Zero;
 
+            // If vertical attaction timescale is reasonable and we applied an angular force last time...
             if (m_verticalAttractionTimescale < 300 && m_lastAngularVelocity != Vector3.Zero)
             {
                 float VAservo = pTimestep * 0.2f / m_verticalAttractionTimescale;
-                if (Prim.Linkset.LinksetIsColliding)
+                if (Prim.IsColliding)
                     VAservo = pTimestep * 0.05f / (m_verticalAttractionTimescale);
 
                 VAservo *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
@@ -806,7 +865,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     verticalError.X = 2.0f - verticalError.X;
                     verticalError.Y = 2.0f - verticalError.Y;
                 }
-                // scale it by VAservo
+                // scale it by VAservo (timestep and timescale)
                 verticalError = verticalError * VAservo;
 
                 // As the body rotates around the X axis, then verticalError.Y increases; Rotated around Y
@@ -822,25 +881,30 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 vertattr.X += bounce * angularVelocity.X;
                 vertattr.Y += bounce * angularVelocity.Y;
 
-                VDetailLog("{0},MoveAngular,verticalAttraction,verticalError={1},bounce={2},vertattr={3}",
-                            Prim.LocalID, verticalError, bounce, vertattr);
+                VDetailLog("{0},MoveAngular,verticalAttraction,VAservo={1},effic={2},verticalError={3},bounce={4},vertattr={5}",
+                            Prim.LocalID, VAservo, m_verticalAttractionEfficiency, verticalError, bounce, vertattr);
 
             }
             #endregion // Vertical attactor
 
             #region Deflection
 
-            //Forward is the prefered direction, but if the reference frame has changed, we need to take this into account as well
             if (m_angularDeflectionEfficiency != 0)
             {
-                Vector3 preferredAxisOfMotion =
+                // Compute a scaled vector that points in the preferred axis (X direction)
+                Vector3 scaledDefaultDirection =
                     new Vector3((pTimestep * 10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale)), 0, 0);
-                preferredAxisOfMotion *= Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
+                // Adding the current vehicle orientation and reference frame displaces the orientation to the frame.
+                // Rotate the scaled default axix relative to the actual vehicle direction giving where it should point.
+                Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
 
+                // Scale by efficiency and timescale
                 deflection = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
 
                 VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", 
                                         Prim.LocalID, preferredAxisOfMotion, deflection);
+                // This deflection computation is not correct.
+                deflection = Vector3.Zero;
             }
 
             #endregion
@@ -875,7 +939,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
                 else
                     banking.Z += (effSquared*(mult*mix))*(m_angularMotorVelocity.X) * 4;
-                if (!Prim.Linkset.LinksetIsColliding && Math.Abs(m_angularMotorVelocity.X) > mix)
+                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;
@@ -904,6 +968,40 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // Sum velocities
             m_lastAngularVelocity = m_angularMotorVelocity + vertattr + banking + deflection;
 
+            #region Linear Motor Offset
+
+            //Offset section
+            if (m_linearMotorOffset != Vector3.Zero)
+            {
+                //Offset of linear velocity doesn't change the linear velocity,
+                //   but causes a torque to be applied, for example...
+                //
+                //      IIIII     >>>   IIIII
+                //      IIIII     >>>    IIIII
+                //      IIIII     >>>     IIIII
+                //          ^
+                //          |  Applying a force at the arrow will cause the object to move forward, but also rotate
+                //
+                //
+                // The torque created is the linear velocity crossed with the offset
+
+                // NOTE: this computation does should be in the linear section
+                //    because there we know the impulse being applied.
+                Vector3 torqueFromOffset = Vector3.Zero;
+                // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
+                if (float.IsNaN(torqueFromOffset.X))
+                    torqueFromOffset.X = 0;
+                if (float.IsNaN(torqueFromOffset.Y))
+                    torqueFromOffset.Y = 0;
+                if (float.IsNaN(torqueFromOffset.Z))
+                    torqueFromOffset.Z = 0;
+                torqueFromOffset *= m_vehicleMass;
+                Prim.ApplyTorqueImpulse(torqueFromOffset, true);
+                VDetailLog("{0},BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", Prim.LocalID, torqueFromOffset);
+            }
+
+            #endregion
+
             if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
             {
                 m_lastAngularVelocity.X = 0;
@@ -914,25 +1012,25 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
             {
                 m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
-                VDetailLog("{0},MoveAngular,zeroSmallValues,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
+                Prim.ZeroAngularMotion(true);
+                VDetailLog("{0},MoveAngular,zeroAngularMotion,lastAngular={1}", Prim.LocalID, m_lastAngularVelocity);
+            }
+            else
+            {
+                // Apply to the body.
+                // The above calculates the absolute angular velocity needed. Angular velocity is massless.
+                // Since we are stuffing the angular velocity directly into the object, the computed
+                //     velocity needs to be scaled by the timestep.
+                Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep) - Prim.ForceRotationalVelocity);
+                Prim.ForceRotationalVelocity = applyAngularForce;
+
+                // Decay the angular movement for next time
+                Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep;
+                m_lastAngularVelocity *= Vector3.One - decayamount;
+
+                VDetailLog("{0},MoveAngular,done,newRotVel={1},decay={2},lastAngular={3}", 
+                                    Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity);
             }
-
-            // Apply to the body
-            // The above calculates the absolute angular velocity needed
-            // Prim.ForceRotationalVelocity = m_lastAngularVelocity;
-
-            // Apply a force to overcome current angular velocity
-            Vector3 applyAngularForce = (m_lastAngularVelocity - Prim.ForceRotationalVelocity) * Prim.Linkset.LinksetMass;
-            // Vector3 applyAngularForce = (m_lastAngularVelocity - Prim.ForceRotationalVelocity);
-            // Prim.AddAngularForce(applyAngularForce, false);
-            Prim.ApplyTorqueImpulse(applyAngularForce, false);
-
-             // Apply friction for next time
-            Vector3 decayamount = (Vector3.One / m_angularFrictionTimescale) * pTimestep;
-            m_lastAngularVelocity *= Vector3.One - decayamount;
-
-            VDetailLog("{0},MoveAngular,done,applyAForce={1},decay={2},lastAngular={3}", 
-                                Prim.LocalID, applyAngularForce, decayamount, m_lastAngularVelocity);
         } //end MoveAngular
 
         internal void LimitRotation(float timestep)