mirror of
https://github.com/opensim/opensim.git
synced 2026-08-11 11:57:03 +08:00
Major renaming of Physics dlls / folders. No functional changes, just renames.
This commit is contained in:
Diva Canto
451
OpenSim/Region/PhysicsModules/BulletS/BSMotors.cs
Executable file
451
OpenSim/Region/PhysicsModules/BulletS/BSMotors.cs
Executable file
@@ -0,0 +1,451 @@
|
||||
/*
|
||||
* Copyright (c) Contributors, http://opensimulator.org/
|
||||
* See CONTRIBUTORS.TXT for a full list of copyright holders.
|
||||
*
|
||||
* Redistribution and use in source and binary forms, with or without
|
||||
* modification, are permitted provided that the following conditions are met:
|
||||
* * Redistributions of source code must retain the above copyright
|
||||
* notice, this list of conditions and the following disclaimer.
|
||||
* * Redistributions in binary form must reproduce the above copyright
|
||||
* notice, this list of conditions and the following disclaimer in the
|
||||
* documentation and/or other materials provided with the distribution.
|
||||
* * Neither the name of the OpenSimulator Project nor the
|
||||
* names of its contributors may be used to endorse or promote products
|
||||
* derived from this software without specific prior written permission.
|
||||
*
|
||||
* THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
|
||||
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
||||
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
||||
* DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
|
||||
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
||||
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
||||
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
||||
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
||||
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
||||
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
||||
*
|
||||
*/
|
||||
using System;
|
||||
using System.Collections.Generic;
|
||||
using System.Text;
|
||||
using OpenMetaverse;
|
||||
using OpenSim.Framework;
|
||||
|
||||
namespace OpenSim.Region.Physics.BulletSPlugin
|
||||
{
|
||||
public abstract class BSMotor
|
||||
{
|
||||
// Timescales and other things can be turned off by setting them to 'infinite'.
|
||||
public const float Infinite = 12345.6f;
|
||||
public readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite);
|
||||
|
||||
public BSMotor(string useName)
|
||||
{
|
||||
UseName = useName;
|
||||
PhysicsScene = null;
|
||||
Enabled = true;
|
||||
}
|
||||
public virtual bool Enabled { get; set; }
|
||||
public virtual void Reset() { }
|
||||
public virtual void Zero() { }
|
||||
public virtual void GenerateTestOutput(float timeStep) { }
|
||||
|
||||
// A name passed at motor creation for easily identifyable debugging messages.
|
||||
public string UseName { get; private set; }
|
||||
|
||||
// Used only for outputting debug information. Might not be set so check for null.
|
||||
public BSScene PhysicsScene { get; set; }
|
||||
protected void MDetailLog(string msg, params Object[] parms)
|
||||
{
|
||||
if (PhysicsScene != null)
|
||||
{
|
||||
PhysicsScene.DetailLog(msg, parms);
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
// Motor which moves CurrentValue to TargetValue over TimeScale seconds.
|
||||
// The TargetValue decays in TargetValueDecayTimeScale.
|
||||
// This motor will "zero itself" over time in that the targetValue will
|
||||
// decay to zero and the currentValue will follow it to that zero.
|
||||
// The overall effect is for the returned correction value to go from large
|
||||
// values to small and eventually zero values.
|
||||
// TimeScale and TargetDelayTimeScale may be 'infinite' which means no decay.
|
||||
|
||||
// For instance, if something is moving at speed X and the desired speed is Y,
|
||||
// CurrentValue is X and TargetValue is Y. As the motor is stepped, new
|
||||
// values of CurrentValue are returned that approach the TargetValue.
|
||||
// The feature of decaying TargetValue is so vehicles will eventually
|
||||
// come to a stop rather than run forever. This can be disabled by
|
||||
// setting TargetValueDecayTimescale to 'infinite'.
|
||||
// The change from CurrentValue to TargetValue is linear over TimeScale seconds.
|
||||
public class BSVMotor : BSMotor
|
||||
{
|
||||
// public Vector3 FrameOfReference { get; set; }
|
||||
// public Vector3 Offset { get; set; }
|
||||
|
||||
public virtual float TimeScale { get; set; }
|
||||
public virtual float TargetValueDecayTimeScale { get; set; }
|
||||
public virtual float Efficiency { get; set; }
|
||||
|
||||
public virtual float ErrorZeroThreshold { get; set; }
|
||||
|
||||
public virtual Vector3 TargetValue { get; protected set; }
|
||||
public virtual Vector3 CurrentValue { get; protected set; }
|
||||
public virtual Vector3 LastError { get; protected set; }
|
||||
|
||||
public virtual bool ErrorIsZero()
|
||||
{
|
||||
return ErrorIsZero(LastError);
|
||||
}
|
||||
public virtual bool ErrorIsZero(Vector3 err)
|
||||
{
|
||||
return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold));
|
||||
}
|
||||
|
||||
public BSVMotor(string useName)
|
||||
: base(useName)
|
||||
{
|
||||
TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
|
||||
Efficiency = 1f;
|
||||
CurrentValue = TargetValue = Vector3.Zero;
|
||||
ErrorZeroThreshold = 0.001f;
|
||||
}
|
||||
public BSVMotor(string useName, float timeScale, float decayTimeScale, float efficiency)
|
||||
: this(useName)
|
||||
{
|
||||
TimeScale = timeScale;
|
||||
TargetValueDecayTimeScale = decayTimeScale;
|
||||
Efficiency = efficiency;
|
||||
CurrentValue = TargetValue = Vector3.Zero;
|
||||
}
|
||||
public void SetCurrent(Vector3 current)
|
||||
{
|
||||
CurrentValue = current;
|
||||
}
|
||||
public void SetTarget(Vector3 target)
|
||||
{
|
||||
TargetValue = target;
|
||||
}
|
||||
public override void Zero()
|
||||
{
|
||||
base.Zero();
|
||||
CurrentValue = TargetValue = Vector3.Zero;
|
||||
}
|
||||
|
||||
// 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;
|
||||
}
|
||||
// version of step that sets the current value before doing the step
|
||||
public virtual Vector3 Step(float timeStep, Vector3 current)
|
||||
{
|
||||
CurrentValue = current;
|
||||
return Step(timeStep);
|
||||
}
|
||||
// Given and error, computer a correction for this step.
|
||||
// Simple scaling of the error by the timestep.
|
||||
public virtual Vector3 StepError(float timeStep, Vector3 error)
|
||||
{
|
||||
if (!Enabled) return Vector3.Zero;
|
||||
|
||||
Vector3 returnCorrection = Vector3.Zero;
|
||||
if (!ErrorIsZero(error))
|
||||
{
|
||||
// correction = error / secondsItShouldTakeToCorrect
|
||||
Vector3 correctionAmount;
|
||||
if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
|
||||
correctionAmount = error * timeStep;
|
||||
else
|
||||
correctionAmount = error / TimeScale * timeStep;
|
||||
|
||||
returnCorrection = correctionAmount;
|
||||
MDetailLog("{0}, BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
|
||||
BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
|
||||
}
|
||||
return returnCorrection;
|
||||
}
|
||||
|
||||
// The user sets all the parameters and calls this which outputs values until error is zero.
|
||||
public override void GenerateTestOutput(float timeStep)
|
||||
{
|
||||
// maximum number of outputs to generate.
|
||||
int maxOutput = 50;
|
||||
MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName);
|
||||
MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},eff={4},curr={5},tgt={6}",
|
||||
BSScene.DetailLogZero, UseName,
|
||||
TimeScale, TargetValueDecayTimeScale, Efficiency,
|
||||
CurrentValue, TargetValue);
|
||||
|
||||
LastError = BSMotor.InfiniteVector;
|
||||
while (maxOutput-- > 0 && !ErrorIsZero())
|
||||
{
|
||||
Vector3 lastStep = Step(timeStep);
|
||||
MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}",
|
||||
BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep);
|
||||
}
|
||||
MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName);
|
||||
|
||||
|
||||
}
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4}>",
|
||||
UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale);
|
||||
}
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// ============================================================================
|
||||
public class BSFMotor : BSMotor
|
||||
{
|
||||
public virtual float TimeScale { get; set; }
|
||||
public virtual float TargetValueDecayTimeScale { get; set; }
|
||||
public virtual float Efficiency { get; set; }
|
||||
|
||||
public virtual float ErrorZeroThreshold { get; set; }
|
||||
|
||||
public virtual float TargetValue { get; protected set; }
|
||||
public virtual float CurrentValue { get; protected set; }
|
||||
public virtual float LastError { get; protected set; }
|
||||
|
||||
public virtual bool ErrorIsZero()
|
||||
{
|
||||
return ErrorIsZero(LastError);
|
||||
}
|
||||
public virtual bool ErrorIsZero(float err)
|
||||
{
|
||||
return (err >= -ErrorZeroThreshold && err <= ErrorZeroThreshold);
|
||||
}
|
||||
|
||||
public BSFMotor(string useName, float timeScale, float decayTimescale, float efficiency)
|
||||
: base(useName)
|
||||
{
|
||||
TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
|
||||
Efficiency = 1f;
|
||||
CurrentValue = TargetValue = 0f;
|
||||
ErrorZeroThreshold = 0.01f;
|
||||
}
|
||||
public void SetCurrent(float current)
|
||||
{
|
||||
CurrentValue = current;
|
||||
}
|
||||
public void SetTarget(float target)
|
||||
{
|
||||
TargetValue = target;
|
||||
}
|
||||
public override void Zero()
|
||||
{
|
||||
base.Zero();
|
||||
CurrentValue = TargetValue = 0f;
|
||||
}
|
||||
|
||||
public virtual float Step(float timeStep)
|
||||
{
|
||||
if (!Enabled) return TargetValue;
|
||||
|
||||
float origTarget = TargetValue; // DEBUG
|
||||
float origCurrVal = CurrentValue; // DEBUG
|
||||
|
||||
float correction = 0f;
|
||||
float 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}, BSFMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
|
||||
BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
|
||||
timeStep, error, correction);
|
||||
MDetailLog("{0}, BSFMotor.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 (Util.InRange<float>(TargetValue, -ErrorZeroThreshold, ErrorZeroThreshold))
|
||||
{
|
||||
// The target can step down to nearly zero but not get there. If close to zero
|
||||
// it is really zero.
|
||||
TargetValue = 0f;
|
||||
}
|
||||
CurrentValue = TargetValue;
|
||||
MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
|
||||
BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
|
||||
}
|
||||
LastError = error;
|
||||
|
||||
return CurrentValue;
|
||||
}
|
||||
|
||||
public virtual float StepError(float timeStep, float error)
|
||||
{
|
||||
if (!Enabled) return 0f;
|
||||
|
||||
float returnCorrection = 0f;
|
||||
if (!ErrorIsZero(error))
|
||||
{
|
||||
// correction = error / secondsItShouldTakeToCorrect
|
||||
float correctionAmount;
|
||||
if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
|
||||
correctionAmount = error * timeStep;
|
||||
else
|
||||
correctionAmount = error / TimeScale * timeStep;
|
||||
|
||||
returnCorrection = correctionAmount;
|
||||
MDetailLog("{0}, BSFMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
|
||||
BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
|
||||
}
|
||||
return returnCorrection;
|
||||
}
|
||||
|
||||
public override string ToString()
|
||||
{
|
||||
return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4}>",
|
||||
UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale);
|
||||
}
|
||||
|
||||
}
|
||||
|
||||
// ============================================================================
|
||||
// ============================================================================
|
||||
// Proportional, Integral, Derivitive ("PID") Motor
|
||||
// Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
|
||||
public class BSPIDVMotor : BSVMotor
|
||||
{
|
||||
// Larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10.
|
||||
public Vector3 proportionFactor { get; set; }
|
||||
public Vector3 integralFactor { get; set; }
|
||||
public Vector3 derivFactor { get; set; }
|
||||
|
||||
// The factors are vectors for the three dimensions. This is the proportional of each
|
||||
// that is applied. This could be multiplied through the actual factors but it
|
||||
// is sometimes easier to manipulate the factors and their mix separately.
|
||||
public Vector3 FactorMix;
|
||||
|
||||
// Arbritrary factor range.
|
||||
// EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct.
|
||||
public float EfficiencyHigh = 0.4f;
|
||||
public float EfficiencyLow = 4.0f;
|
||||
|
||||
// Running integration of the error
|
||||
Vector3 RunningIntegration { get; set; }
|
||||
|
||||
public BSPIDVMotor(string useName)
|
||||
: base(useName)
|
||||
{
|
||||
proportionFactor = new Vector3(1.00f, 1.00f, 1.00f);
|
||||
integralFactor = new Vector3(1.00f, 1.00f, 1.00f);
|
||||
derivFactor = new Vector3(1.00f, 1.00f, 1.00f);
|
||||
FactorMix = new Vector3(0.5f, 0.25f, 0.25f);
|
||||
RunningIntegration = Vector3.Zero;
|
||||
LastError = Vector3.Zero;
|
||||
}
|
||||
|
||||
public override void Zero()
|
||||
{
|
||||
base.Zero();
|
||||
}
|
||||
|
||||
public override float Efficiency
|
||||
{
|
||||
get { return base.Efficiency; }
|
||||
set
|
||||
{
|
||||
base.Efficiency = Util.Clamp(value, 0f, 1f);
|
||||
|
||||
// Compute factors based on efficiency.
|
||||
// If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot.
|
||||
// If efficiency is low (0f), use a factor value that overcorrects.
|
||||
// TODO: might want to vary contribution of different factor depending on efficiency.
|
||||
// float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f;
|
||||
float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow;
|
||||
|
||||
proportionFactor = new Vector3(factor, factor, factor);
|
||||
integralFactor = new Vector3(factor, factor, factor);
|
||||
derivFactor = new Vector3(factor, factor, factor);
|
||||
|
||||
MDetailLog("{0}, BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor);
|
||||
}
|
||||
}
|
||||
|
||||
// Advance the PID computation on this error.
|
||||
public override Vector3 StepError(float timeStep, Vector3 error)
|
||||
{
|
||||
if (!Enabled) return Vector3.Zero;
|
||||
|
||||
// Add up the error so we can integrate over the accumulated errors
|
||||
RunningIntegration += error * timeStep;
|
||||
|
||||
// A simple derivitive is the rate of change from the last error.
|
||||
Vector3 derivitive = (error - LastError) * timeStep;
|
||||
|
||||
// Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError)
|
||||
Vector3 ret = error / TimeScale * timeStep * proportionFactor * FactorMix.X
|
||||
+ RunningIntegration / TimeScale * integralFactor * FactorMix.Y
|
||||
+ derivitive / TimeScale * derivFactor * FactorMix.Z
|
||||
;
|
||||
|
||||
MDetailLog("{0}, BSPIDVMotor.step,ts={1},err={2},lerr={3},runnInt={4},deriv={5},ret={6}",
|
||||
BSScene.DetailLogZero, timeStep, error, LastError, RunningIntegration, derivitive, ret);
|
||||
|
||||
return ret;
|
||||
}
|
||||
}
|
||||
}
|
||||
Reference in New Issue
Block a user