BulletSim: implement VEHICLE_REFERENCE_FRAME. Thanks Vegaslon!.

1 files changed, 21 insertions, 14 deletions

diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
index a4ef709..c6d6331 100644
--- a/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs
@@ -945,7 +945,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             get
             {
-                return VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleOrientation));
+                return VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleFrameOrientation));
             }
         }
 
@@ -956,6 +956,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 return VehicleForwardVelocity.X;
             }
         }
+        private Quaternion VehicleFrameOrientation
+        {
+            get
+            {
+                return VehicleOrientation * m_referenceFrame;
+            }
+        }
 
         #endregion // Known vehicle value functions
 
@@ -1065,7 +1072,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             linearMotorCorrectionV -= (currentVelV * frictionFactorV);
 
             // Motor is vehicle coordinates. Rotate it to world coordinates
-            Vector3 linearMotorVelocityW = linearMotorCorrectionV * VehicleOrientation;
+            Vector3 linearMotorVelocityW = linearMotorCorrectionV * VehicleFrameOrientation;
 
             // If we're a ground vehicle, don't add any upward Z movement
             if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
@@ -1107,7 +1114,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 linearDeflectionV *= new Vector3(1, -1, -1);
 
                 // Correction is vehicle relative. Convert to world coordinates.
-                Vector3 linearDeflectionW = linearDeflectionV * VehicleOrientation;
+                Vector3 linearDeflectionW = linearDeflectionV * VehicleFrameOrientation;
 
                 // Optionally, if not colliding, don't effect world downward velocity. Let falling things fall.
                 if (BSParam.VehicleLinearDeflectionNotCollidingNoZ && !m_controllingPrim.HasSomeCollision)
@@ -1403,7 +1410,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
         {
             // The user wants this many radians per second angular change?
             Vector3 origVehicleRotationalVelocity = VehicleRotationalVelocity;      // DEBUG DEBUG
-            Vector3 currentAngularV = VehicleRotationalVelocity * Quaternion.Inverse(VehicleOrientation);
+            Vector3 currentAngularV = VehicleRotationalVelocity * Quaternion.Inverse(VehicleFrameOrientation);
             Vector3 angularMotorContributionV = m_angularMotor.Step(pTimestep, currentAngularV);
 
             // ==================================================================
@@ -1424,7 +1431,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             Vector3 frictionFactorW = ComputeFrictionFactor(m_angularFrictionTimescale, pTimestep);
             angularMotorContributionV -= (currentAngularV * frictionFactorW);
 
-            Vector3 angularMotorContributionW = angularMotorContributionV * VehicleOrientation;
+            Vector3 angularMotorContributionW = angularMotorContributionV * VehicleFrameOrientation;
             VehicleRotationalVelocity += angularMotorContributionW;
 
             VDetailLog("{0},  MoveAngular,angularTurning,curAngVelV={1},origVehRotVel={2},vehRotVel={3},frictFact={4}, angContribV={5},angContribW={6}",
@@ -1445,7 +1452,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
             // If vertical attaction timescale is reasonable
             if (BSParam.VehicleEnableAngularVerticalAttraction && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
             {
-                Vector3 vehicleUpAxis = Vector3.UnitZ * VehicleOrientation;
+                Vector3 vehicleUpAxis = Vector3.UnitZ * VehicleFrameOrientation;
                 switch (BSParam.VehicleAngularVerticalAttractionAlgorithm)
                 {
                     case 0:
@@ -1466,7 +1473,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                             if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) != 0)
                             {
-                                Vector3 vehicleForwardAxis = Vector3.UnitX * VehicleOrientation;
+                                Vector3 vehicleForwardAxis = Vector3.UnitX * VehicleFrameOrientation;
                                 torqueVector = ProjectVector(torqueVector, vehicleForwardAxis);
                             }
 
@@ -1491,13 +1498,13 @@ namespace OpenSim.Region.Physics.BulletSPlugin
 
                             // Create a rotation that is only the vehicle's rotation around Z
                             Vector3 currentEulerW = Vector3.Zero;
-                            VehicleOrientation.GetEulerAngles(out currentEulerW.X, out currentEulerW.Y, out currentEulerW.Z);
+                            VehicleFrameOrientation.GetEulerAngles(out currentEulerW.X, out currentEulerW.Y, out currentEulerW.Z);
                             Quaternion justZOrientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, currentEulerW.Z);
 
                             // Create the axis that is perpendicular to the up vector and the rotated up vector.
-                            Vector3 differenceAxisW = Vector3.Cross(Vector3.UnitZ * justZOrientation, Vector3.UnitZ * VehicleOrientation);
+                            Vector3 differenceAxisW = Vector3.Cross(Vector3.UnitZ * justZOrientation, Vector3.UnitZ * VehicleFrameOrientation);
                             // Compute the angle between those to vectors.
-                            double differenceAngle = Math.Acos((double)Vector3.Dot(Vector3.UnitZ, Vector3.Normalize(Vector3.UnitZ * VehicleOrientation)));
+                            double differenceAngle = Math.Acos((double)Vector3.Dot(Vector3.UnitZ, Vector3.Normalize(Vector3.UnitZ * VehicleFrameOrientation)));
                             // 'differenceAngle' is the angle to rotate and 'differenceAxis' is the plane to rotate in to get the vehicle vertical
 
                             // Reduce the change by the time period it is to change in. Timestep is handled when velocity is applied.
@@ -1530,7 +1537,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                             Vector3 origRotVelW = VehicleRotationalVelocity;        // DEBUG DEBUG
 
                             // Take a vector pointing up and convert it from world to vehicle relative coords.
-                            Vector3 verticalError = Vector3.Normalize(Vector3.UnitZ * VehicleOrientation);
+                            Vector3 verticalError = Vector3.Normalize(Vector3.UnitZ * VehicleFrameOrientation);
 
                             // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
                             //    is now:
@@ -1561,7 +1568,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                             vertContributionV /= m_verticalAttractionTimescale;
 
                             // Rotate the vehicle rotation to the world coordinates.
-                            VehicleRotationalVelocity += (vertContributionV * VehicleOrientation);
+                            VehicleRotationalVelocity += (vertContributionV * VehicleFrameOrientation);
 
                             VDetailLog("{0},  MoveAngular,verticalAttraction,,upAxis={1},origRotVW={2},vertError={3},unscaledV={4},eff={5},ts={6},vertContribV={7}",
                                             ControllingPrim.LocalID,
@@ -1602,7 +1609,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 movingDirection *= Math.Sign(VehicleForwardSpeed);
 
                 // The direction the vehicle is pointing
-                Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
+                Vector3 pointingDirection = Vector3.UnitX * VehicleFrameOrientation;
                 //Predict where the Vehicle will be pointing after AngularVelocity change is applied. This will keep
                 //   from overshooting and allow this correction to merge with the Vertical Attraction peacefully.
                 Vector3 predictedPointingDirection = pointingDirection * Quaternion.CreateFromAxisAngle(VehicleRotationalVelocity, 0f);
@@ -1675,7 +1682,7 @@ namespace OpenSim.Region.Physics.BulletSPlugin
                 // Rotate a UnitZ vector (pointing up) to how the vehicle is oriented.
                 // As the vehicle rolls to the right or left, the Y value will increase from
                 //     zero (straight up) to 1 or -1 (full tilt right  or left)
-                Vector3 rollComponents = Vector3.UnitZ * VehicleOrientation;
+                Vector3 rollComponents = Vector3.UnitZ * VehicleFrameOrientation;
 
                 // Figure out the yaw value for this much roll.
                 float yawAngle = m_angularMotorDirection.X * m_bankingEfficiency;