/// <summary>
/// Step the motor and return the resulting current value.
/// </summary>
/// <param name="timeStep"> The current timestep of the scene </param>
public virtual Vector3 Step(float timeStep)
{
Vector3 errorValue, correctionValue;
float decayFactor;
// If the motor is not enabled, we assume that we
// have reached the target value, so simply just
// return the target value that we have
if (!Enabled)
{
return(TargetValue);
}
// Calculate the difference in the current and target values
errorValue = TargetValue - CurrentValue;
correctionValue = Vector3.Zero;
// If the calculated error value is not zero
if (!ErrorIsZero(errorValue))
{
// Calculate the error correction value
// and add it to the current value
correctionValue = StepError(timeStep, errorValue);
CurrentValue += correctionValue;
// The desired value reduces to zero which also reduces the
// difference with current
// If the decay timescale is not infinite, we decay
if (TargetValueDecayTimeScale != PxMotor.Infinite)
{
decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
TargetValue *= (1f - decayFactor);
}
}
else
{
// Difference between what we have and target is small, Motor
// is done
if (TargetValue.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
{
// The target can step down to nearly zero but not get
// there If close to zero it is really zero
TargetValue = Vector3.Zero;
}
// If the motor is done, set the current value to the target value
CurrentValue = TargetValue;
}
// Update the last error as the most recently calculated error
LastError = errorValue;
return(correctionValue);
}
// Compute the next step and return the new current value.
// Returns the correction needed to move 'current' to 'target'.
public virtual Vector3 Step(float timeStep)
{
if (!Enabled)
{
return(TargetValue);
}
Vector3 origTarget = TargetValue; // DEBUG
Vector3 origCurrVal = CurrentValue; // DEBUG
Vector3 correction = Vector3.Zero;
Vector3 error = TargetValue - CurrentValue;
if (!ErrorIsZero(error))
{
correction = StepError(timeStep, error);
CurrentValue += correction;
// The desired value reduces to zero which also reduces the difference with current.
// If the decay time is infinite, don't decay at all.
float decayFactor = 0f;
if (TargetValueDecayTimeScale != BSMotor.Infinite)
{
decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
TargetValue *= (1f - decayFactor);
}
MDetailLog("{0}, BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
timeStep, error, correction);
MDetailLog("{0}, BSVMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},tgt={4},curr={5}",
BSScene.DetailLogZero, UseName, TargetValueDecayTimeScale, decayFactor, TargetValue, CurrentValue);
}
else
{
// Difference between what we have and target is small. Motor is done.
if (TargetValue.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
{
// The target can step down to nearly zero but not get there. If close to zero
// it is really zero.
TargetValue = Vector3.Zero;
}
CurrentValue = TargetValue;
MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}",
BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue);
}
LastError = error;
return(correction);
}
// Compute the next step and return the new current value.
// Returns the correction needed to move 'current' to 'target'.
public virtual Vector3 Step(float timeStep)
{
if (!Enabled)
{
return(TargetValue);
}
Vector3 origTarget = TargetValue; // DEBUG
Vector3 origCurrVal = CurrentValue; // DEBUG
Vector3 correction = Vector3.Zero;
Vector3 error = TargetValue - CurrentValue;
LastError = error;
if (!ErrorIsZero(error))
{
correction = StepError(timeStep, error);
CurrentValue += correction;
// The desired value reduces to zero which also reduces the difference with current.
// If the decay time is infinite, don't decay at all.
float decayFactor = 0f;
if (TargetValueDecayTimeScale != BSMotor.Infinite)
{
decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
TargetValue *= (1f - decayFactor);
}
// The amount we can correct the error is reduced by the friction
Vector3 frictionFactor = Vector3.Zero;
if (FrictionTimescale != BSMotor.InfiniteVector)
{
// Individual friction components can be 'infinite' so compute each separately.
frictionFactor.X = (FrictionTimescale.X == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.X);
frictionFactor.Y = (FrictionTimescale.Y == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Y);
frictionFactor.Z = (FrictionTimescale.Z == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Z);
frictionFactor *= timeStep;
CurrentValue *= (Vector3.One - frictionFactor);
}
MDetailLog("{0}, BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
timeStep, error, correction);
MDetailLog("{0}, BSVMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},frictTS={4},frictFact={5},tgt={6},curr={7}",
BSScene.DetailLogZero, UseName,
TargetValueDecayTimeScale, decayFactor, FrictionTimescale, frictionFactor,
TargetValue, CurrentValue);
}
else
{
// Difference between what we have and target is small. Motor is done.
if (TargetValue.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
{
// The target can step down to nearly zero but not get there. If close to zero
// it is really zero.
TargetValue = Vector3.Zero;
}
CurrentValue = TargetValue;
MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}",
BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue);
}
return(correction);
}
