BulletSim: implement VEHICLE_REFERENCE_FRAME. Thanks Vegaslon!.

inv-download
Robert Adams 2015-04-05 20:20:37 -07:00
parent b51e46ceb8
commit 8d66284841
1 changed files with 21 additions and 14 deletions

View File

@ -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;