aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs')
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSMotors.cs52
1 files changed, 43 insertions, 9 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index b57d2c8..a6af40d 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -46,6 +46,7 @@ public abstract class BSMotor
46 } 46 }
47 public virtual void Reset() { } 47 public virtual void Reset() { }
48 public virtual void Zero() { } 48 public virtual void Zero() { }
49 public virtual void GenerateTestOutput(float timeStep) { }
49 50
50 // A name passed at motor creation for easily identifyable debugging messages. 51 // A name passed at motor creation for easily identifyable debugging messages.
51 public string UseName { get; private set; } 52 public string UseName { get; private set; }
@@ -93,8 +94,9 @@ public class BSVMotor : BSMotor
93 94
94 public virtual float ErrorZeroThreshold { get; set; } 95 public virtual float ErrorZeroThreshold { get; set; }
95 96
96 public virtual Vector3 TargetValue { get; private set; } 97 public virtual Vector3 TargetValue { get; protected set; }
97 public virtual Vector3 CurrentValue { get; private set; } 98 public virtual Vector3 CurrentValue { get; protected set; }
99 public virtual Vector3 LastError { get; protected set; }
98 100
99 public BSVMotor(string useName) 101 public BSVMotor(string useName)
100 : base(useName) 102 : base(useName)
@@ -122,6 +124,11 @@ public class BSVMotor : BSMotor
122 { 124 {
123 TargetValue = target; 125 TargetValue = target;
124 } 126 }
127 public override void Zero()
128 {
129 base.Zero();
130 CurrentValue = TargetValue = Vector3.Zero;
131 }
125 132
126 // Compute the next step and return the new current value 133 // Compute the next step and return the new current value
127 public virtual Vector3 Step(float timeStep) 134 public virtual Vector3 Step(float timeStep)
@@ -168,21 +175,25 @@ public class BSVMotor : BSMotor
168 else 175 else
169 { 176 {
170 // Difference between what we have and target is small. Motor is done. 177 // Difference between what we have and target is small. Motor is done.
171 CurrentValue = Vector3.Zero; 178 CurrentValue = TargetValue;
172 TargetValue = Vector3.Zero; 179 MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={2}",
173 MDetailLog("{0}, BSVMotor.Step,zero,{1},ret={2}", 180 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
174 BSScene.DetailLogZero, UseName, CurrentValue);
175 } 181 }
176 182
177 return CurrentValue; 183 return CurrentValue;
178 } 184 }
179 public virtual Vector3 Step(float timeStep, Vector3 error) 185 public virtual Vector3 Step(float timeStep, Vector3 error)
180 { 186 {
187 LastError = error;
181 Vector3 returnCorrection = Vector3.Zero; 188 Vector3 returnCorrection = Vector3.Zero;
182 if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) 189 if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
183 { 190 {
184 // correction = error / secondsItShouldTakeToCorrect 191 // correction = error / secondsItShouldTakeToCorrect
185 Vector3 correctionAmount = error / TimeScale * timeStep; 192 Vector3 correctionAmount;
193 if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
194 correctionAmount = error * timeStep;
195 else
196 correctionAmount = error / TimeScale * timeStep;
186 197
187 returnCorrection = correctionAmount; 198 returnCorrection = correctionAmount;
188 MDetailLog("{0}, BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5},frictTS={6},ret={7}", 199 MDetailLog("{0}, BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5},frictTS={6},ret={7}",
@@ -191,6 +202,30 @@ public class BSVMotor : BSMotor
191 } 202 }
192 return returnCorrection; 203 return returnCorrection;
193 } 204 }
205
206 // The user sets all the parameters and calls this which outputs values until error is zero.
207 public override void GenerateTestOutput(float timeStep)
208 {
209 // maximum number of outputs to generate.
210 int maxOutput = 50;
211 MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName);
212 MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},frictTS={4},eff={5},curr={6},tgt={7}",
213 BSScene.DetailLogZero, UseName,
214 TimeScale, TargetValueDecayTimeScale, FrictionTimescale, Efficiency,
215 CurrentValue, TargetValue);
216
217 LastError = BSMotor.InfiniteVector;
218 while (maxOutput-- > 0 && !LastError.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
219 {
220 Vector3 lastStep = Step(timeStep);
221 MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}",
222 BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep);
223 }
224 MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName);
225
226
227 }
228
194 public override string ToString() 229 public override string ToString()
195 { 230 {
196 return String.Format("<{0},curr={1},targ={2},decayTS={3},frictTS={4}>", 231 return String.Format("<{0},curr={1},targ={2},decayTS={3},frictTS={4}>",
@@ -238,7 +273,6 @@ public class BSPIDVMotor : BSVMotor
238 public float EfficiencyLow = 4.0f; 273 public float EfficiencyLow = 4.0f;
239 274
240 Vector3 IntegralFactor { get; set; } 275 Vector3 IntegralFactor { get; set; }
241 Vector3 LastError { get; set; }
242 276
243 public BSPIDVMotor(string useName) 277 public BSPIDVMotor(string useName)
244 : base(useName) 278 : base(useName)
@@ -274,7 +308,7 @@ public class BSPIDVMotor : BSVMotor
274 } 308 }
275 309
276 // Ignore Current and Target Values and just advance the PID computation on this error. 310 // Ignore Current and Target Values and just advance the PID computation on this error.
277 public Vector3 Step(float timeStep, Vector3 error) 311 public override Vector3 Step(float timeStep, Vector3 error)
278 { 312 {
279 // Add up the error so we can integrate over the accumulated errors 313 // Add up the error so we can integrate over the accumulated errors
280 IntegralFactor += error * timeStep; 314 IntegralFactor += error * timeStep;