BulletSim: reorganize angular movement routine into separate subroutines enabling external calibration routines and unit testing.

1 files changed, 114 insertions, 95 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index d94abf4..eb4d06a 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -317,7 +317,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_VhoverEfficiency = 0;
                     m_VhoverTimescale = 0;
                     m_VehicleBuoyancy = 0;
-                    
+
                     m_linearDeflectionEfficiency = 1;
                     m_linearDeflectionTimescale = 1;
 
@@ -366,8 +366,8 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                     m_bankingMix = 1;
 
                     m_referenceFrame = Quaternion.Identity;
-                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY 
-                                | VehicleFlag.HOVER_TERRAIN_ONLY 
+                    m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
+                                | VehicleFlag.HOVER_TERRAIN_ONLY
                                 | VehicleFlag.HOVER_GLOBAL_HEIGHT
                                 | VehicleFlag.HOVER_UP_ONLY);
                     m_flags |= (VehicleFlag.NO_DEFLECTION_UP
@@ -575,7 +575,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 Vector3 localInertia = new Vector3(m_vehicleMass, m_vehicleMass, m_vehicleMass);
                 BulletSimAPI.SetMassProps2(Prim.PhysBody.ptr, m_vehicleMass, localInertia);
 
-                VDetailLog("{0},BSDynamics.Refresh,frict={1},inert={2},aDamp={3}", 
+                VDetailLog("{0},BSDynamics.Refresh,frict={1},inert={2},aDamp={3}",
                                 Prim.LocalID, friction, localInertia, angularDamping);
             }
         }
@@ -759,13 +759,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 //     (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}", 
+                VDetailLog("{0},MoveLinear,limitMotorUp,distAbove={1},downForce={2}",
                                     Prim.LocalID, distanceAboveGround, limitMotorUpContribution);
             }
 
             // ==================================================================
-            Vector3 newVelocity = linearMotorContribution 
-                            + terrainHeightContribution 
+            Vector3 newVelocity = linearMotorContribution
+                            + terrainHeightContribution
                             + hoverContribution
                             + limitMotorUpContribution;
 
@@ -801,7 +801,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             }
 
             VDetailLog("{0},MoveLinear,done,lmDir={1},lmVel={2},newVel={3},primVel={4},totalDown={5}",
-                                Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector, 
+                                Prim.LocalID, m_linearMotorDirection, m_lastLinearVelocityVector,
                                 newVelocity, Prim.Velocity, totalDownForce);
 
         } // end MoveLinear()
@@ -850,8 +850,84 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 VDetailLog("{0},MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
             }
 
+            Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction(pTimestep);
+
+            Vector3 deflectionContribution = ComputeAngularDeflection(pTimestep);
+
+            Vector3 bankingContribution = ComputeAngularBanking(pTimestep);
+
+            // ==================================================================
+            m_lastVertAttractor = verticalAttractionContribution;
+
+            // Sum velocities
+            m_lastAngularVelocity = angularMotorContribution
+                                    + verticalAttractionContribution
+                                    + bankingContribution
+                                    + deflectionContribution;
+
+            // ==================================================================
+            //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
+
+                // TODO: this computation should be in the linear section
+                //    because that is where 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);
+            }
+
             // ==================================================================
-            Vector3 verticalAttractionContribution = Vector3.Zero;
+            if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
+            {
+                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
+                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.
+                // Also remove any motion that is on the object so added motion is only from vehicle.
+                Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep)
+                                                - Prim.ForceRotationalVelocity);
+                // Unscale the force by the angular factor so it overwhelmes the Bullet additions.
+                Prim.ForceRotationalVelocity = applyAngularForce;
+
+                VDetailLog("{0},MoveAngular,done,angMotor={1},vertAttr={2},bank={3},deflect={4},newAngForce={5},lastAngular={6}",
+                                    Prim.LocalID,
+                                    angularMotorContribution, verticalAttractionContribution,
+                                    bankingContribution, deflectionContribution,
+                                    applyAngularForce, m_lastAngularVelocity
+                                    );
+            }
+        }
+
+        public Vector3 ComputeAngularVerticalAttraction(float pTimestep)
+        {
+            Vector3 ret = Vector3.Zero;
+
             // If vertical attaction timescale is reasonable and we applied an angular force last time...
             if (m_verticalAttractionTimescale < 500)
             {
@@ -859,7 +935,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 verticalError.Normalize();
                 m_verticalAttractionMotor.SetCurrent(verticalError);
                 m_verticalAttractionMotor.SetTarget(Vector3.UnitZ);
-                verticalAttractionContribution = m_verticalAttractionMotor.Step(pTimestep);
+                ret = m_verticalAttractionMotor.Step(pTimestep);
                 /*
                 // Take a vector pointing up and convert it from world to vehicle relative coords.
                 Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
@@ -895,15 +971,19 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 verticalAttractionContribution *= (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
 
                 VDetailLog("{0},MoveAngular,verticalAttraction,,verticalError={1},unscaled={2},preEff={3},eff={4},effSq={5},vertAttr={6}",
-                                            Prim.LocalID, verticalError, unscaledContrib, preEfficiencyContrib, 
+                                            Prim.LocalID, verticalError, unscaledContrib, preEfficiencyContrib,
                                             m_verticalAttractionEfficiency, efficencySquared,
                                             verticalAttractionContribution);
                  */
 
             }
+            return ret;
+        }
+
+        public Vector3 ComputeAngularDeflection(float pTimestep)
+        {
+            Vector3 ret = Vector3.Zero;
 
-            // ==================================================================
-            Vector3 deflectionContribution = Vector3.Zero;
             if (m_angularDeflectionEfficiency != 0)
             {
                 // Compute a scaled vector that points in the preferred axis (X direction)
@@ -914,24 +994,28 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 Vector3 preferredAxisOfMotion = scaledDefaultDirection * Quaternion.Add(Prim.ForceOrientation, m_referenceFrame);
 
                 // Scale by efficiency and timescale
-                deflectionContribution = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
+                ret = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
+
+                VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", Prim.LocalID, preferredAxisOfMotion, ret);
 
-                VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", 
-                                        Prim.LocalID, preferredAxisOfMotion, deflectionContribution);
                 // This deflection computation is not correct.
-                deflectionContribution = Vector3.Zero;
+                ret = Vector3.Zero;
             }
+            return ret;
+        }
+
+        public Vector3 ComputeAngularBanking(float pTimestep)
+        {
+            Vector3 ret = Vector3.Zero;
 
-            // ==================================================================
-            Vector3 bankingContribution = Vector3.Zero;
             if (m_bankingEfficiency != 0)
             {
                 Vector3 dir = Vector3.One * Prim.ForceOrientation;
-                float mult = (m_bankingMix*m_bankingMix)*-1*(m_bankingMix < 0 ? -1 : 1);
-                    //Changes which way it banks in and out of turns
+                float mult = (m_bankingMix * m_bankingMix) * -1 * (m_bankingMix < 0 ? -1 : 1);
+                //Changes which way it banks in and out of turns
 
                 //Use the square of the efficiency, as it looks much more how SL banking works
-                float effSquared = (m_bankingEfficiency*m_bankingEfficiency);
+                float effSquared = (m_bankingEfficiency * m_bankingEfficiency);
                 if (m_bankingEfficiency < 0)
                     effSquared *= -1; //Keep the negative!
 
@@ -953,99 +1037,34 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 }
                 else
                 {
-                    bankingContribution.Z += (effSquared * (mult * mix)) * (m_angularMotorVelocity.X) * 4;
+                    ret.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)
                 {
-                    float angVelZ = m_angularMotorVelocity.X*-1;
+                    float angVelZ = m_angularMotorVelocity.X * -1;
                     /*if(angVelZ > mix)
                         angVelZ = mix;
                     else if(angVelZ < -mix)
                         angVelZ = -mix;*/
                     //This controls how fast and how far the banking occurs
-                    Vector3 bankingRot = new Vector3(angVelZ*(effSquared*mult), 0, 0);
+                    Vector3 bankingRot = new Vector3(angVelZ * (effSquared * mult), 0, 0);
                     if (bankingRot.X > 3)
                         bankingRot.X = 3;
                     else if (bankingRot.X < -3)
                         bankingRot.X = -3;
                     bankingRot *= Prim.ForceOrientation;
-                    bankingContribution += bankingRot;
+                    ret += bankingRot;
                 }
                 m_angularMotorVelocity.X *= m_bankingEfficiency == 1 ? 0.0f : 1 - m_bankingEfficiency;
-                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, bankingContribution);
-            }
-
-            // ==================================================================
-            m_lastVertAttractor = verticalAttractionContribution;
-
-            // Sum velocities
-            m_lastAngularVelocity = angularMotorContribution 
-                                    + verticalAttractionContribution
-                                    + bankingContribution
-                                    + deflectionContribution;
-
-            // ==================================================================
-            //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
-
-                // TODO: this computation should be in the linear section
-                //    because that is where 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);
-            }
-
-            // ==================================================================
-            if (m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
-            {
-                m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
-                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.
-                // Also remove any motion that is on the object so added motion is only from vehicle.
-                Vector3 applyAngularForce = ((m_lastAngularVelocity * pTimestep) 
-                                                - Prim.ForceRotationalVelocity);
-                // Unscale the force by the angular factor so it overwhelmes the Bullet additions.
-                Prim.ForceRotationalVelocity = applyAngularForce;
-
-                VDetailLog("{0},MoveAngular,done,angMotor={1},vertAttr={2},bank={3},deflect={4},newAngForce={5},lastAngular={6}",
-                                    Prim.LocalID,
-                                    angularMotorContribution, verticalAttractionContribution,
-                                    bankingContribution, deflectionContribution,
-                                    applyAngularForce, m_lastAngularVelocity
-                                    );
+                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, ret);
             }
+            return ret;
         }
 
+
         // This is from previous instantiations of XXXDynamics.cs.
         // Applies roll reference frame.
         // TODO: is this the right way to separate the code to do this operation?