opensim-development – Blame information for rev 1

Subversion Repositories:
Rev:
Rev Author Line No. Line
1 eva 1 /*
2 * Copyright (c) Contributors, http://opensimulator.org/
3 * See CONTRIBUTORS.TXT for a full list of copyright holders.
4 *
5 * Redistribution and use in source and binary forms, with or without
6 * modification, are permitted provided that the following conditions are met:
7 * * Redistributions of source code must retain the above copyright
8 * notice, this list of conditions and the following disclaimer.
9 * * Redistributions in binary form must reproduce the above copyright
10 * notice, this list of conditions and the following disclaimer in the
11 * documentation and/or other materials provided with the distribution.
12 * * Neither the name of the OpenSimulator Project nor the
13 * names of its contributors may be used to endorse or promote products
14 * derived from this software without specific prior written permission.
15 *
16 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
17 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
18 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
19 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
20 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
21 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
22 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
23 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
24 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 *
27 */
28 using System;
29 using System.Collections.Generic;
30 using System.Text;
31 using OpenMetaverse;
32 using OpenSim.Framework;
33  
34 namespace OpenSim.Region.Physics.BulletSPlugin
35 {
36 public abstract class BSMotor
37 {
38 // Timescales and other things can be turned off by setting them to 'infinite'.
39 public const float Infinite = 12345.6f;
40 public readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite);
41  
42 public BSMotor(string useName)
43 {
44 UseName = useName;
45 PhysicsScene = null;
46 Enabled = true;
47 }
48 public virtual bool Enabled { get; set; }
49 public virtual void Reset() { }
50 public virtual void Zero() { }
51 public virtual void GenerateTestOutput(float timeStep) { }
52  
53 // A name passed at motor creation for easily identifyable debugging messages.
54 public string UseName { get; private set; }
55  
56 // Used only for outputting debug information. Might not be set so check for null.
57 public BSScene PhysicsScene { get; set; }
58 protected void MDetailLog(string msg, params Object[] parms)
59 {
60 if (PhysicsScene != null)
61 {
62 PhysicsScene.DetailLog(msg, parms);
63 }
64 }
65 }
66  
67 // Motor which moves CurrentValue to TargetValue over TimeScale seconds.
68 // The TargetValue decays in TargetValueDecayTimeScale.
69 // This motor will "zero itself" over time in that the targetValue will
70 // decay to zero and the currentValue will follow it to that zero.
71 // The overall effect is for the returned correction value to go from large
72 // values to small and eventually zero values.
73 // TimeScale and TargetDelayTimeScale may be 'infinite' which means no decay.
74  
75 // For instance, if something is moving at speed X and the desired speed is Y,
76 // CurrentValue is X and TargetValue is Y. As the motor is stepped, new
77 // values of CurrentValue are returned that approach the TargetValue.
78 // The feature of decaying TargetValue is so vehicles will eventually
79 // come to a stop rather than run forever. This can be disabled by
80 // setting TargetValueDecayTimescale to 'infinite'.
81 // The change from CurrentValue to TargetValue is linear over TimeScale seconds.
82 public class BSVMotor : BSMotor
83 {
84 // public Vector3 FrameOfReference { get; set; }
85 // public Vector3 Offset { get; set; }
86  
87 public virtual float TimeScale { get; set; }
88 public virtual float TargetValueDecayTimeScale { get; set; }
89 public virtual float Efficiency { get; set; }
90  
91 public virtual float ErrorZeroThreshold { get; set; }
92  
93 public virtual Vector3 TargetValue { get; protected set; }
94 public virtual Vector3 CurrentValue { get; protected set; }
95 public virtual Vector3 LastError { get; protected set; }
96  
97 public virtual bool ErrorIsZero()
98 {
99 return ErrorIsZero(LastError);
100 }
101 public virtual bool ErrorIsZero(Vector3 err)
102 {
103 return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold));
104 }
105  
106 public BSVMotor(string useName)
107 : base(useName)
108 {
109 TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
110 Efficiency = 1f;
111 CurrentValue = TargetValue = Vector3.Zero;
112 ErrorZeroThreshold = 0.001f;
113 }
114 public BSVMotor(string useName, float timeScale, float decayTimeScale, float efficiency)
115 : this(useName)
116 {
117 TimeScale = timeScale;
118 TargetValueDecayTimeScale = decayTimeScale;
119 Efficiency = efficiency;
120 CurrentValue = TargetValue = Vector3.Zero;
121 }
122 public void SetCurrent(Vector3 current)
123 {
124 CurrentValue = current;
125 }
126 public void SetTarget(Vector3 target)
127 {
128 TargetValue = target;
129 }
130 public override void Zero()
131 {
132 base.Zero();
133 CurrentValue = TargetValue = Vector3.Zero;
134 }
135  
136 // Compute the next step and return the new current value.
137 // Returns the correction needed to move 'current' to 'target'.
138 public virtual Vector3 Step(float timeStep)
139 {
140 if (!Enabled) return TargetValue;
141  
142 Vector3 origTarget = TargetValue; // DEBUG
143 Vector3 origCurrVal = CurrentValue; // DEBUG
144  
145 Vector3 correction = Vector3.Zero;
146 Vector3 error = TargetValue - CurrentValue;
147 if (!ErrorIsZero(error))
148 {
149 correction = StepError(timeStep, error);
150  
151 CurrentValue += correction;
152  
153 // The desired value reduces to zero which also reduces the difference with current.
154 // If the decay time is infinite, don't decay at all.
155 float decayFactor = 0f;
156 if (TargetValueDecayTimeScale != BSMotor.Infinite)
157 {
158 decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
159 TargetValue *= (1f - decayFactor);
160 }
161  
162 MDetailLog("{0}, BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
163 BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
164 timeStep, error, correction);
165 MDetailLog("{0}, BSVMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},tgt={4},curr={5}",
166 BSScene.DetailLogZero, UseName, TargetValueDecayTimeScale, decayFactor, TargetValue, CurrentValue);
167 }
168 else
169 {
170 // Difference between what we have and target is small. Motor is done.
171 if (TargetValue.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
172 {
173 // The target can step down to nearly zero but not get there. If close to zero
174 // it is really zero.
175 TargetValue = Vector3.Zero;
176 }
177 CurrentValue = TargetValue;
178 MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}",
179 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue);
180 }
181 LastError = error;
182  
183 return correction;
184 }
185 // version of step that sets the current value before doing the step
186 public virtual Vector3 Step(float timeStep, Vector3 current)
187 {
188 CurrentValue = current;
189 return Step(timeStep);
190 }
191 // Given and error, computer a correction for this step.
192 // Simple scaling of the error by the timestep.
193 public virtual Vector3 StepError(float timeStep, Vector3 error)
194 {
195 if (!Enabled) return Vector3.Zero;
196  
197 Vector3 returnCorrection = Vector3.Zero;
198 if (!ErrorIsZero(error))
199 {
200 // correction = error / secondsItShouldTakeToCorrect
201 Vector3 correctionAmount;
202 if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
203 correctionAmount = error * timeStep;
204 else
205 correctionAmount = error / TimeScale * timeStep;
206  
207 returnCorrection = correctionAmount;
208 MDetailLog("{0}, BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
209 BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
210 }
211 return returnCorrection;
212 }
213  
214 // The user sets all the parameters and calls this which outputs values until error is zero.
215 public override void GenerateTestOutput(float timeStep)
216 {
217 // maximum number of outputs to generate.
218 int maxOutput = 50;
219 MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName);
220 MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},eff={4},curr={5},tgt={6}",
221 BSScene.DetailLogZero, UseName,
222 TimeScale, TargetValueDecayTimeScale, Efficiency,
223 CurrentValue, TargetValue);
224  
225 LastError = BSMotor.InfiniteVector;
226 while (maxOutput-- > 0 && !ErrorIsZero())
227 {
228 Vector3 lastStep = Step(timeStep);
229 MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}",
230 BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep);
231 }
232 MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName);
233  
234  
235 }
236  
237 public override string ToString()
238 {
239 return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4}>",
240 UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale);
241 }
242 }
243  
244 // ============================================================================
245 // ============================================================================
246 public class BSFMotor : BSMotor
247 {
248 public virtual float TimeScale { get; set; }
249 public virtual float TargetValueDecayTimeScale { get; set; }
250 public virtual float Efficiency { get; set; }
251  
252 public virtual float ErrorZeroThreshold { get; set; }
253  
254 public virtual float TargetValue { get; protected set; }
255 public virtual float CurrentValue { get; protected set; }
256 public virtual float LastError { get; protected set; }
257  
258 public virtual bool ErrorIsZero()
259 {
260 return ErrorIsZero(LastError);
261 }
262 public virtual bool ErrorIsZero(float err)
263 {
264 return (err >= -ErrorZeroThreshold && err <= ErrorZeroThreshold);
265 }
266  
267 public BSFMotor(string useName, float timeScale, float decayTimescale, float efficiency)
268 : base(useName)
269 {
270 TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
271 Efficiency = 1f;
272 CurrentValue = TargetValue = 0f;
273 ErrorZeroThreshold = 0.01f;
274 }
275 public void SetCurrent(float current)
276 {
277 CurrentValue = current;
278 }
279 public void SetTarget(float target)
280 {
281 TargetValue = target;
282 }
283 public override void Zero()
284 {
285 base.Zero();
286 CurrentValue = TargetValue = 0f;
287 }
288  
289 public virtual float Step(float timeStep)
290 {
291 if (!Enabled) return TargetValue;
292  
293 float origTarget = TargetValue; // DEBUG
294 float origCurrVal = CurrentValue; // DEBUG
295  
296 float correction = 0f;
297 float error = TargetValue - CurrentValue;
298 if (!ErrorIsZero(error))
299 {
300 correction = StepError(timeStep, error);
301  
302 CurrentValue += correction;
303  
304 // The desired value reduces to zero which also reduces the difference with current.
305 // If the decay time is infinite, don't decay at all.
306 float decayFactor = 0f;
307 if (TargetValueDecayTimeScale != BSMotor.Infinite)
308 {
309 decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
310 TargetValue *= (1f - decayFactor);
311 }
312  
313 MDetailLog("{0}, BSFMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
314 BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
315 timeStep, error, correction);
316 MDetailLog("{0}, BSFMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},tgt={4},curr={5}",
317 BSScene.DetailLogZero, UseName, TargetValueDecayTimeScale, decayFactor, TargetValue, CurrentValue);
318 }
319 else
320 {
321 // Difference between what we have and target is small. Motor is done.
322 if (Util.InRange<float>(TargetValue, -ErrorZeroThreshold, ErrorZeroThreshold))
323 {
324 // The target can step down to nearly zero but not get there. If close to zero
325 // it is really zero.
326 TargetValue = 0f;
327 }
328 CurrentValue = TargetValue;
329 MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
330 BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
331 }
332 LastError = error;
333  
334 return CurrentValue;
335 }
336  
337 public virtual float StepError(float timeStep, float error)
338 {
339 if (!Enabled) return 0f;
340  
341 float returnCorrection = 0f;
342 if (!ErrorIsZero(error))
343 {
344 // correction = error / secondsItShouldTakeToCorrect
345 float correctionAmount;
346 if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
347 correctionAmount = error * timeStep;
348 else
349 correctionAmount = error / TimeScale * timeStep;
350  
351 returnCorrection = correctionAmount;
352 MDetailLog("{0}, BSFMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
353 BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
354 }
355 return returnCorrection;
356 }
357  
358 public override string ToString()
359 {
360 return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4}>",
361 UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale);
362 }
363  
364 }
365  
366 // ============================================================================
367 // ============================================================================
368 // Proportional, Integral, Derivitive ("PID") Motor
369 // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
370 public class BSPIDVMotor : BSVMotor
371 {
372 // Larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10.
373 public Vector3 proportionFactor { get; set; }
374 public Vector3 integralFactor { get; set; }
375 public Vector3 derivFactor { get; set; }
376  
377 // The factors are vectors for the three dimensions. This is the proportional of each
378 // that is applied. This could be multiplied through the actual factors but it
379 // is sometimes easier to manipulate the factors and their mix separately.
380 public Vector3 FactorMix;
381  
382 // Arbritrary factor range.
383 // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct.
384 public float EfficiencyHigh = 0.4f;
385 public float EfficiencyLow = 4.0f;
386  
387 // Running integration of the error
388 Vector3 RunningIntegration { get; set; }
389  
390 public BSPIDVMotor(string useName)
391 : base(useName)
392 {
393 proportionFactor = new Vector3(1.00f, 1.00f, 1.00f);
394 integralFactor = new Vector3(1.00f, 1.00f, 1.00f);
395 derivFactor = new Vector3(1.00f, 1.00f, 1.00f);
396 FactorMix = new Vector3(0.5f, 0.25f, 0.25f);
397 RunningIntegration = Vector3.Zero;
398 LastError = Vector3.Zero;
399 }
400  
401 public override void Zero()
402 {
403 base.Zero();
404 }
405  
406 public override float Efficiency
407 {
408 get { return base.Efficiency; }
409 set
410 {
411 base.Efficiency = Util.Clamp(value, 0f, 1f);
412  
413 // Compute factors based on efficiency.
414 // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot.
415 // If efficiency is low (0f), use a factor value that overcorrects.
416 // TODO: might want to vary contribution of different factor depending on efficiency.
417 // float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f;
418 float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow;
419  
420 proportionFactor = new Vector3(factor, factor, factor);
421 integralFactor = new Vector3(factor, factor, factor);
422 derivFactor = new Vector3(factor, factor, factor);
423  
424 MDetailLog("{0}, BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor);
425 }
426 }
427  
428 // Advance the PID computation on this error.
429 public override Vector3 StepError(float timeStep, Vector3 error)
430 {
431 if (!Enabled) return Vector3.Zero;
432  
433 // Add up the error so we can integrate over the accumulated errors
434 RunningIntegration += error * timeStep;
435  
436 // A simple derivitive is the rate of change from the last error.
437 Vector3 derivitive = (error - LastError) * timeStep;
438  
439 // Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError)
440 Vector3 ret = error / TimeScale * timeStep * proportionFactor * FactorMix.X
441 + RunningIntegration / TimeScale * integralFactor * FactorMix.Y
442 + derivitive / TimeScale * derivFactor * FactorMix.Z
443 ;
444  
445 MDetailLog("{0}, BSPIDVMotor.step,ts={1},err={2},lerr={3},runnInt={4},deriv={5},ret={6}",
446 BSScene.DetailLogZero, timeStep, error, LastError, RunningIntegration, derivitive, ret);
447  
448 return ret;
449 }
450 }
451 }