opensim – Blame information for rev 1
?pathlinks?
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 | } |