/// <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); }