From 8d6628484107e3ebe2c29b2b754e43c164545cf1 Mon Sep 17 00:00:00 2001 From: Robert Adams Date: Sun, 5 Apr 2015 20:20:37 -0700 Subject: BulletSim: implement VEHICLE_REFERENCE_FRAME. Thanks Vegaslon!. --- OpenSim/Region/Physics/BulletSPlugin/BSDynamics.cs | 35 +++++++++++++--------- 1 file changed, 21 insertions(+), 14 deletions(-) (limited to 'OpenSim/Region/Physics/BulletSPlugin') 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; -- cgit v1.1