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

integration
Robert Adams 2012-11-27 05:37:06 -08:00
parent 59554758b1
commit 68fe7dff20
1 changed files with 154 additions and 135 deletions

View File

@ -850,133 +850,11 @@ namespace OpenSim.Region.Physics.BulletSPlugin
VDetailLog("{0},MoveAngular,noDeflectionUp,angularMotorContrib={1}", Prim.LocalID, angularMotorContribution);
}
// ==================================================================
Vector3 verticalAttractionContribution = Vector3.Zero;
// If vertical attaction timescale is reasonable and we applied an angular force last time...
if (m_verticalAttractionTimescale < 500)
{
Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
verticalError.Normalize();
m_verticalAttractionMotor.SetCurrent(verticalError);
m_verticalAttractionMotor.SetTarget(Vector3.UnitZ);
verticalAttractionContribution = m_verticalAttractionMotor.Step(pTimestep);
/*
// Take a vector pointing up and convert it from world to vehicle relative coords.
Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
verticalError.Normalize();
Vector3 verticalAttractionContribution = ComputeAngularVerticalAttraction(pTimestep);
// If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
// is now leaning to one side (rotated around the X axis) and the Y value will
// go from zero (nearly straight up) to one (completely to the side) or leaning
// front-to-back (rotated around the Y axis) and the value of X will be between
// zero and one.
// The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees.
Vector3 deflectionContribution = ComputeAngularDeflection(pTimestep);
// If verticalError.Z is negative, the vehicle is upside down. Add additional push.
if (verticalError.Z < 0f)
{
verticalError.X = 2f - verticalError.X;
verticalError.Y = 2f - verticalError.Y;
}
// Y error means needed rotation around X axis and visa versa.
verticalAttractionContribution.X = verticalError.Y;
verticalAttractionContribution.Y = - verticalError.X;
verticalAttractionContribution.Z = 0f;
// scale by the time scale and timestep
Vector3 unscaledContrib = verticalAttractionContribution;
verticalAttractionContribution /= m_verticalAttractionTimescale;
verticalAttractionContribution *= pTimestep;
// apply efficiency
Vector3 preEfficiencyContrib = verticalAttractionContribution;
float efficencySquared = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
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,
m_verticalAttractionEfficiency, efficencySquared,
verticalAttractionContribution);
*/
}
// ==================================================================
Vector3 deflectionContribution = Vector3.Zero;
if (m_angularDeflectionEfficiency != 0)
{
// Compute a scaled vector that points in the preferred axis (X direction)
Vector3 scaledDefaultDirection =
new Vector3((pTimestep * 10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale)), 0, 0);
// 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
deflectionContribution = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}",
Prim.LocalID, preferredAxisOfMotion, deflectionContribution);
// This deflection computation is not correct.
deflectionContribution = 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
//Use the square of the efficiency, as it looks much more how SL banking works
float effSquared = (m_bankingEfficiency*m_bankingEfficiency);
if (m_bankingEfficiency < 0)
effSquared *= -1; //Keep the negative!
float mix = Math.Abs(m_bankingMix);
if (m_angularMotorVelocity.X == 0)
{
// The vehicle is stopped
/*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
{
Vector3 axisAngle;
float angle;
parent.Orientation.GetAxisAngle(out axisAngle, out angle);
Vector3 rotatedVel = parent.Velocity * parent.Orientation;
if ((rotatedVel.X < 0 && axisAngle.Y > 0) || (rotatedVel.X > 0 && axisAngle.Y < 0))
m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (1f) * 10;
else
m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (-1f) * 10;
}*/
}
else
{
bankingContribution.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;
/*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);
if (bankingRot.X > 3)
bankingRot.X = 3;
else if (bankingRot.X < -3)
bankingRot.X = -3;
bankingRot *= Prim.ForceOrientation;
bankingContribution += 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);
}
Vector3 bankingContribution = ComputeAngularBanking(pTimestep);
// ==================================================================
m_lastVertAttractor = verticalAttractionContribution;
@ -1046,6 +924,147 @@ namespace OpenSim.Region.Physics.BulletSPlugin
}
}
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)
{
Vector3 verticalError = Vector3.UnitZ * Prim.ForceOrientation;
verticalError.Normalize();
m_verticalAttractionMotor.SetCurrent(verticalError);
m_verticalAttractionMotor.SetTarget(Vector3.UnitZ);
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;
verticalError.Normalize();
// If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
// is now leaning to one side (rotated around the X axis) and the Y value will
// go from zero (nearly straight up) to one (completely to the side) or leaning
// front-to-back (rotated around the Y axis) and the value of X will be between
// zero and one.
// The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees.
// If verticalError.Z is negative, the vehicle is upside down. Add additional push.
if (verticalError.Z < 0f)
{
verticalError.X = 2f - verticalError.X;
verticalError.Y = 2f - verticalError.Y;
}
// Y error means needed rotation around X axis and visa versa.
verticalAttractionContribution.X = verticalError.Y;
verticalAttractionContribution.Y = - verticalError.X;
verticalAttractionContribution.Z = 0f;
// scale by the time scale and timestep
Vector3 unscaledContrib = verticalAttractionContribution;
verticalAttractionContribution /= m_verticalAttractionTimescale;
verticalAttractionContribution *= pTimestep;
// apply efficiency
Vector3 preEfficiencyContrib = verticalAttractionContribution;
float efficencySquared = m_verticalAttractionEfficiency * m_verticalAttractionEfficiency;
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,
m_verticalAttractionEfficiency, efficencySquared,
verticalAttractionContribution);
*/
}
return ret;
}
public Vector3 ComputeAngularDeflection(float pTimestep)
{
Vector3 ret = Vector3.Zero;
if (m_angularDeflectionEfficiency != 0)
{
// Compute a scaled vector that points in the preferred axis (X direction)
Vector3 scaledDefaultDirection =
new Vector3((pTimestep * 10 * (m_angularDeflectionEfficiency / m_angularDeflectionTimescale)), 0, 0);
// 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
ret = (preferredAxisOfMotion * (m_angularDeflectionEfficiency) / m_angularDeflectionTimescale) * pTimestep;
VDetailLog("{0},MoveAngular,Deflection,perfAxis={1},deflection={2}", Prim.LocalID, preferredAxisOfMotion, ret);
// This deflection computation is not correct.
ret = Vector3.Zero;
}
return ret;
}
public Vector3 ComputeAngularBanking(float pTimestep)
{
Vector3 ret = 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
//Use the square of the efficiency, as it looks much more how SL banking works
float effSquared = (m_bankingEfficiency * m_bankingEfficiency);
if (m_bankingEfficiency < 0)
effSquared *= -1; //Keep the negative!
float mix = Math.Abs(m_bankingMix);
if (m_angularMotorVelocity.X == 0)
{
// The vehicle is stopped
/*if (!parent.Orientation.ApproxEquals(this.m_referenceFrame, 0.25f))
{
Vector3 axisAngle;
float angle;
parent.Orientation.GetAxisAngle(out axisAngle, out angle);
Vector3 rotatedVel = parent.Velocity * parent.Orientation;
if ((rotatedVel.X < 0 && axisAngle.Y > 0) || (rotatedVel.X > 0 && axisAngle.Y < 0))
m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (1f) * 10;
else
m_angularMotorVelocity.X += (effSquared * (mult * mix)) * (-1f) * 10;
}*/
}
else
{
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;
/*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);
if (bankingRot.X > 3)
bankingRot.X = 3;
else if (bankingRot.X < -3)
bankingRot.X = -3;
bankingRot *= Prim.ForceOrientation;
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, 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?