aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSMotors.cs97
1 files changed, 31 insertions, 66 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index 9501e2d..7693195 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -65,13 +65,11 @@ public abstract class BSMotor
65} 65}
66 66
67// Motor which moves CurrentValue to TargetValue over TimeScale seconds. 67// Motor which moves CurrentValue to TargetValue over TimeScale seconds.
68// The TargetValue decays in TargetValueDecayTimeScale and 68// The TargetValue decays in TargetValueDecayTimeScale.
69// the CurrentValue will be held back by FrictionTimeScale.
70// This motor will "zero itself" over time in that the targetValue will 69// This motor will "zero itself" over time in that the targetValue will
71// decay to zero and the currentValue will follow it to that zero. 70// decay to zero and the currentValue will follow it to that zero.
72// The overall effect is for the returned correction value to go from large 71// The overall effect is for the returned correction value to go from large
73// values (the total difference between current and target minus friction) 72// values to small and eventually zero values.
74// to small and eventually zero values.
75// TimeScale and TargetDelayTimeScale may be 'infinite' which means no decay. 73// TimeScale and TargetDelayTimeScale may be 'infinite' which means no decay.
76 74
77// For instance, if something is moving at speed X and the desired speed is Y, 75// For instance, if something is moving at speed X and the desired speed is Y,
@@ -88,7 +86,6 @@ public class BSVMotor : BSMotor
88 86
89 public virtual float TimeScale { get; set; } 87 public virtual float TimeScale { get; set; }
90 public virtual float TargetValueDecayTimeScale { get; set; } 88 public virtual float TargetValueDecayTimeScale { get; set; }
91 public virtual Vector3 FrictionTimescale { get; set; }
92 public virtual float Efficiency { get; set; } 89 public virtual float Efficiency { get; set; }
93 90
94 public virtual float ErrorZeroThreshold { get; set; } 91 public virtual float ErrorZeroThreshold { get; set; }
@@ -102,7 +99,7 @@ public class BSVMotor : BSMotor
102 return ErrorIsZero(LastError); 99 return ErrorIsZero(LastError);
103 } 100 }
104 public virtual bool ErrorIsZero(Vector3 err) 101 public virtual bool ErrorIsZero(Vector3 err)
105 { 102 {
106 return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)); 103 return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold));
107 } 104 }
108 105
@@ -111,16 +108,14 @@ public class BSVMotor : BSMotor
111 { 108 {
112 TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite; 109 TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
113 Efficiency = 1f; 110 Efficiency = 1f;
114 FrictionTimescale = BSMotor.InfiniteVector;
115 CurrentValue = TargetValue = Vector3.Zero; 111 CurrentValue = TargetValue = Vector3.Zero;
116 ErrorZeroThreshold = 0.001f; 112 ErrorZeroThreshold = 0.001f;
117 } 113 }
118 public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) 114 public BSVMotor(string useName, float timeScale, float decayTimeScale, float efficiency)
119 : this(useName) 115 : this(useName)
120 { 116 {
121 TimeScale = timeScale; 117 TimeScale = timeScale;
122 TargetValueDecayTimeScale = decayTimeScale; 118 TargetValueDecayTimeScale = decayTimeScale;
123 FrictionTimescale = frictionTimeScale;
124 Efficiency = efficiency; 119 Efficiency = efficiency;
125 CurrentValue = TargetValue = Vector3.Zero; 120 CurrentValue = TargetValue = Vector3.Zero;
126 } 121 }
@@ -149,7 +144,6 @@ public class BSVMotor : BSMotor
149 144
150 Vector3 correction = Vector3.Zero; 145 Vector3 correction = Vector3.Zero;
151 Vector3 error = TargetValue - CurrentValue; 146 Vector3 error = TargetValue - CurrentValue;
152 LastError = error;
153 if (!ErrorIsZero(error)) 147 if (!ErrorIsZero(error))
154 { 148 {
155 correction = StepError(timeStep, error); 149 correction = StepError(timeStep, error);
@@ -165,26 +159,11 @@ public class BSVMotor : BSMotor
165 TargetValue *= (1f - decayFactor); 159 TargetValue *= (1f - decayFactor);
166 } 160 }
167 161
168 // The amount we can correct the error is reduced by the friction
169 Vector3 frictionFactor = Vector3.Zero;
170 if (FrictionTimescale != BSMotor.InfiniteVector)
171 {
172 // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
173 // Individual friction components can be 'infinite' so compute each separately.
174 frictionFactor.X = (FrictionTimescale.X == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.X);
175 frictionFactor.Y = (FrictionTimescale.Y == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Y);
176 frictionFactor.Z = (FrictionTimescale.Z == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Z);
177 frictionFactor *= timeStep;
178 CurrentValue *= (Vector3.One - frictionFactor);
179 }
180
181 MDetailLog("{0}, BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}", 162 MDetailLog("{0}, BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
182 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, 163 BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
183 timeStep, error, correction); 164 timeStep, error, correction);
184 MDetailLog("{0}, BSVMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},frictTS={4},frictFact={5},tgt={6},curr={7}", 165 MDetailLog("{0}, BSVMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},tgt={4},curr={5}",
185 BSScene.DetailLogZero, UseName, 166 BSScene.DetailLogZero, UseName, TargetValueDecayTimeScale, decayFactor, TargetValue, CurrentValue);
186 TargetValueDecayTimeScale, decayFactor, FrictionTimescale, frictionFactor,
187 TargetValue, CurrentValue);
188 } 167 }
189 else 168 else
190 { 169 {
@@ -199,6 +178,7 @@ public class BSVMotor : BSMotor
199 MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}", 178 MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}",
200 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue); 179 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue);
201 } 180 }
181 LastError = error;
202 182
203 return correction; 183 return correction;
204 } 184 }
@@ -208,6 +188,8 @@ public class BSVMotor : BSMotor
208 CurrentValue = current; 188 CurrentValue = current;
209 return Step(timeStep); 189 return Step(timeStep);
210 } 190 }
191 // Given and error, computer a correction for this step.
192 // Simple scaling of the error by the timestep.
211 public virtual Vector3 StepError(float timeStep, Vector3 error) 193 public virtual Vector3 StepError(float timeStep, Vector3 error)
212 { 194 {
213 if (!Enabled) return Vector3.Zero; 195 if (!Enabled) return Vector3.Zero;
@@ -235,27 +217,27 @@ public class BSVMotor : BSMotor
235 // maximum number of outputs to generate. 217 // maximum number of outputs to generate.
236 int maxOutput = 50; 218 int maxOutput = 50;
237 MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName); 219 MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName);
238 MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},frictTS={4},eff={5},curr={6},tgt={7}", 220 MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},eff={4},curr={5},tgt={6}",
239 BSScene.DetailLogZero, UseName, 221 BSScene.DetailLogZero, UseName,
240 TimeScale, TargetValueDecayTimeScale, FrictionTimescale, Efficiency, 222 TimeScale, TargetValueDecayTimeScale, Efficiency,
241 CurrentValue, TargetValue); 223 CurrentValue, TargetValue);
242 224
243 LastError = BSMotor.InfiniteVector; 225 LastError = BSMotor.InfiniteVector;
244 while (maxOutput-- > 0 && !LastError.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) 226 while (maxOutput-- > 0 && !ErrorIsZero())
245 { 227 {
246 Vector3 lastStep = Step(timeStep); 228 Vector3 lastStep = Step(timeStep);
247 MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}", 229 MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}",
248 BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep); 230 BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep);
249 } 231 }
250 MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName); 232 MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName);
251 233
252 234
253 } 235 }
254 236
255 public override string ToString() 237 public override string ToString()
256 { 238 {
257 return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4},frictTS={5}>", 239 return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4}>",
258 UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale, FrictionTimescale); 240 UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale);
259 } 241 }
260} 242}
261 243
@@ -265,7 +247,6 @@ public class BSFMotor : BSMotor
265{ 247{
266 public virtual float TimeScale { get; set; } 248 public virtual float TimeScale { get; set; }
267 public virtual float TargetValueDecayTimeScale { get; set; } 249 public virtual float TargetValueDecayTimeScale { get; set; }
268 public virtual float FrictionTimescale { get; set; }
269 public virtual float Efficiency { get; set; } 250 public virtual float Efficiency { get; set; }
270 251
271 public virtual float ErrorZeroThreshold { get; set; } 252 public virtual float ErrorZeroThreshold { get; set; }
@@ -279,16 +260,15 @@ public class BSFMotor : BSMotor
279 return ErrorIsZero(LastError); 260 return ErrorIsZero(LastError);
280 } 261 }
281 public virtual bool ErrorIsZero(float err) 262 public virtual bool ErrorIsZero(float err)
282 { 263 {
283 return (err >= -ErrorZeroThreshold && err <= ErrorZeroThreshold); 264 return (err >= -ErrorZeroThreshold && err <= ErrorZeroThreshold);
284 } 265 }
285 266
286 public BSFMotor(string useName, float timeScale, float decayTimescale, float friction, float efficiency) 267 public BSFMotor(string useName, float timeScale, float decayTimescale, float efficiency)
287 : base(useName) 268 : base(useName)
288 { 269 {
289 TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite; 270 TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
290 Efficiency = 1f; 271 Efficiency = 1f;
291 FrictionTimescale = BSMotor.Infinite;
292 CurrentValue = TargetValue = 0f; 272 CurrentValue = TargetValue = 0f;
293 ErrorZeroThreshold = 0.01f; 273 ErrorZeroThreshold = 0.01f;
294 } 274 }
@@ -315,7 +295,6 @@ public class BSFMotor : BSMotor
315 295
316 float correction = 0f; 296 float correction = 0f;
317 float error = TargetValue - CurrentValue; 297 float error = TargetValue - CurrentValue;
318 LastError = error;
319 if (!ErrorIsZero(error)) 298 if (!ErrorIsZero(error))
320 { 299 {
321 correction = StepError(timeStep, error); 300 correction = StepError(timeStep, error);
@@ -331,24 +310,11 @@ public class BSFMotor : BSMotor
331 TargetValue *= (1f - decayFactor); 310 TargetValue *= (1f - decayFactor);
332 } 311 }
333 312
334 // The amount we can correct the error is reduced by the friction
335 float frictionFactor = 0f;
336 if (FrictionTimescale != BSMotor.Infinite)
337 {
338 // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
339 // Individual friction components can be 'infinite' so compute each separately.
340 frictionFactor = 1f / FrictionTimescale;
341 frictionFactor *= timeStep;
342 CurrentValue *= (1f - frictionFactor);
343 }
344
345 MDetailLog("{0}, BSFMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}", 313 MDetailLog("{0}, BSFMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
346 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, 314 BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
347 timeStep, error, correction); 315 timeStep, error, correction);
348 MDetailLog("{0}, BSFMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},frictTS={4},frictFact={5},tgt={6},curr={7}", 316 MDetailLog("{0}, BSFMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},tgt={4},curr={5}",
349 BSScene.DetailLogZero, UseName, 317 BSScene.DetailLogZero, UseName, TargetValueDecayTimeScale, decayFactor, TargetValue, CurrentValue);
350 TargetValueDecayTimeScale, decayFactor, FrictionTimescale, frictionFactor,
351 TargetValue, CurrentValue);
352 } 318 }
353 else 319 else
354 { 320 {
@@ -363,6 +329,7 @@ public class BSFMotor : BSMotor
363 MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}", 329 MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
364 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue); 330 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
365 } 331 }
332 LastError = error;
366 333
367 return CurrentValue; 334 return CurrentValue;
368 } 335 }
@@ -390,15 +357,15 @@ public class BSFMotor : BSMotor
390 357
391 public override string ToString() 358 public override string ToString()
392 { 359 {
393 return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4},frictTS={5}>", 360 return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4}>",
394 UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale, FrictionTimescale); 361 UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale);
395 } 362 }
396 363
397} 364}
398 365
399// ============================================================================ 366// ============================================================================
400// ============================================================================ 367// ============================================================================
401// Proportional, Integral, Derivitive Motor 368// Proportional, Integral, Derivitive ("PID") Motor
402// Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors. 369// Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
403public class BSPIDVMotor : BSVMotor 370public class BSPIDVMotor : BSVMotor
404{ 371{
@@ -410,7 +377,6 @@ public class BSPIDVMotor : BSVMotor
410 // The factors are vectors for the three dimensions. This is the proportional of each 377 // The factors are vectors for the three dimensions. This is the proportional of each
411 // that is applied. This could be multiplied through the actual factors but it 378 // that is applied. This could be multiplied through the actual factors but it
412 // is sometimes easier to manipulate the factors and their mix separately. 379 // is sometimes easier to manipulate the factors and their mix separately.
413 // to
414 public Vector3 FactorMix; 380 public Vector3 FactorMix;
415 381
416 // Arbritrary factor range. 382 // Arbritrary factor range.
@@ -448,14 +414,14 @@ public class BSPIDVMotor : BSVMotor
448 // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot. 414 // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot.
449 // If efficiency is low (0f), use a factor value that overcorrects. 415 // If efficiency is low (0f), use a factor value that overcorrects.
450 // TODO: might want to vary contribution of different factor depending on efficiency. 416 // TODO: might want to vary contribution of different factor depending on efficiency.
451 float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f; 417 // float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f;
452 // float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow; 418 float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow;
453 419
454 proportionFactor = new Vector3(factor, factor, factor); 420 proportionFactor = new Vector3(factor, factor, factor);
455 integralFactor = new Vector3(factor, factor, factor); 421 integralFactor = new Vector3(factor, factor, factor);
456 derivFactor = new Vector3(factor, factor, factor); 422 derivFactor = new Vector3(factor, factor, factor);
457 423
458 MDetailLog("{0},BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor); 424 MDetailLog("{0}, BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor);
459 } 425 }
460 } 426 }
461 427
@@ -469,16 +435,15 @@ public class BSPIDVMotor : BSVMotor
469 435
470 // A simple derivitive is the rate of change from the last error. 436 // A simple derivitive is the rate of change from the last error.
471 Vector3 derivitive = (error - LastError) * timeStep; 437 Vector3 derivitive = (error - LastError) * timeStep;
472 LastError = error;
473 438
474 // Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError) 439 // Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError)
475 Vector3 ret = error * timeStep * proportionFactor * FactorMix.X 440 Vector3 ret = error / TimeScale * timeStep * proportionFactor * FactorMix.X
476 + RunningIntegration * integralFactor * FactorMix.Y 441 + RunningIntegration / TimeScale * integralFactor * FactorMix.Y
477 + derivitive * derivFactor * FactorMix.Z 442 + derivitive / TimeScale * derivFactor * FactorMix.Z
478 ; 443 ;
479 444
480 MDetailLog("{0},BSPIDVMotor.step,ts={1},err={2},runnInt={3},deriv={4},ret={5}", 445 MDetailLog("{0}, BSPIDVMotor.step,ts={1},err={2},lerr={3},runnInt={4},deriv={5},ret={6}",
481 BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivitive, ret); 446 BSScene.DetailLogZero, timeStep, error, LastError, RunningIntegration, derivitive, ret);
482 447
483 return ret; 448 return ret;
484 } 449 }