diff options
author | David Walter Seikel | 2016-11-03 21:44:39 +1000 |
---|---|---|
committer | David Walter Seikel | 2016-11-03 21:44:39 +1000 |
commit | 134f86e8d5c414409631b25b8c6f0ee45fbd8631 (patch) | |
tree | 216b89d3fb89acfb81be1e440c25c41ab09fa96d /OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs | |
parent | More changing to production grid. Double oops. (diff) | |
download | opensim-SC_OLD-134f86e8d5c414409631b25b8c6f0ee45fbd8631.zip opensim-SC_OLD-134f86e8d5c414409631b25b8c6f0ee45fbd8631.tar.gz opensim-SC_OLD-134f86e8d5c414409631b25b8c6f0ee45fbd8631.tar.bz2 opensim-SC_OLD-134f86e8d5c414409631b25b8c6f0ee45fbd8631.tar.xz |
Initial update to OpenSim 0.8.2.1 source code.
Diffstat (limited to 'OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs')
-rwxr-xr-x | OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs | 347 |
1 files changed, 0 insertions, 347 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs deleted file mode 100755 index 817a5f7..0000000 --- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs +++ /dev/null | |||
@@ -1,347 +0,0 @@ | |||
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 | if (PhysicsScene.VehicleLoggingEnabled) | ||
63 | { | ||
64 | PhysicsScene.DetailLog(msg, parms); | ||
65 | } | ||
66 | } | ||
67 | } | ||
68 | } | ||
69 | |||
70 | // Motor which moves CurrentValue to TargetValue over TimeScale seconds. | ||
71 | // The TargetValue decays in TargetValueDecayTimeScale and | ||
72 | // the CurrentValue will be held back by FrictionTimeScale. | ||
73 | // This motor will "zero itself" over time in that the targetValue will | ||
74 | // decay to zero and the currentValue will follow it to that zero. | ||
75 | // The overall effect is for the returned correction value to go from large | ||
76 | // values (the total difference between current and target minus friction) | ||
77 | // to small and eventually zero values. | ||
78 | // TimeScale and TargetDelayTimeScale may be 'infinite' which means no decay. | ||
79 | |||
80 | // For instance, if something is moving at speed X and the desired speed is Y, | ||
81 | // CurrentValue is X and TargetValue is Y. As the motor is stepped, new | ||
82 | // values of CurrentValue are returned that approach the TargetValue. | ||
83 | // The feature of decaying TargetValue is so vehicles will eventually | ||
84 | // come to a stop rather than run forever. This can be disabled by | ||
85 | // setting TargetValueDecayTimescale to 'infinite'. | ||
86 | // The change from CurrentValue to TargetValue is linear over TimeScale seconds. | ||
87 | public class BSVMotor : BSMotor | ||
88 | { | ||
89 | // public Vector3 FrameOfReference { get; set; } | ||
90 | // public Vector3 Offset { get; set; } | ||
91 | |||
92 | public virtual float TimeScale { get; set; } | ||
93 | public virtual float TargetValueDecayTimeScale { get; set; } | ||
94 | public virtual Vector3 FrictionTimescale { get; set; } | ||
95 | public virtual float Efficiency { get; set; } | ||
96 | |||
97 | public virtual float ErrorZeroThreshold { get; set; } | ||
98 | |||
99 | public virtual Vector3 TargetValue { get; protected set; } | ||
100 | public virtual Vector3 CurrentValue { get; protected set; } | ||
101 | public virtual Vector3 LastError { get; protected set; } | ||
102 | |||
103 | public virtual bool ErrorIsZero | ||
104 | { get { | ||
105 | return (LastError == Vector3.Zero || LastError.LengthSquared() <= ErrorZeroThreshold); | ||
106 | } | ||
107 | } | ||
108 | |||
109 | public BSVMotor(string useName) | ||
110 | : base(useName) | ||
111 | { | ||
112 | TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite; | ||
113 | Efficiency = 1f; | ||
114 | FrictionTimescale = BSMotor.InfiniteVector; | ||
115 | CurrentValue = TargetValue = Vector3.Zero; | ||
116 | ErrorZeroThreshold = 0.001f; | ||
117 | } | ||
118 | public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency) | ||
119 | : this(useName) | ||
120 | { | ||
121 | TimeScale = timeScale; | ||
122 | TargetValueDecayTimeScale = decayTimeScale; | ||
123 | FrictionTimescale = frictionTimeScale; | ||
124 | Efficiency = efficiency; | ||
125 | CurrentValue = TargetValue = Vector3.Zero; | ||
126 | } | ||
127 | public void SetCurrent(Vector3 current) | ||
128 | { | ||
129 | CurrentValue = current; | ||
130 | } | ||
131 | public void SetTarget(Vector3 target) | ||
132 | { | ||
133 | TargetValue = target; | ||
134 | } | ||
135 | public override void Zero() | ||
136 | { | ||
137 | base.Zero(); | ||
138 | CurrentValue = TargetValue = Vector3.Zero; | ||
139 | } | ||
140 | |||
141 | // Compute the next step and return the new current value | ||
142 | public virtual Vector3 Step(float timeStep) | ||
143 | { | ||
144 | if (!Enabled) return TargetValue; | ||
145 | |||
146 | Vector3 origTarget = TargetValue; // DEBUG | ||
147 | Vector3 origCurrVal = CurrentValue; // DEBUG | ||
148 | |||
149 | Vector3 correction = Vector3.Zero; | ||
150 | Vector3 error = TargetValue - CurrentValue; | ||
151 | if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) | ||
152 | { | ||
153 | correction = Step(timeStep, error); | ||
154 | |||
155 | CurrentValue += correction; | ||
156 | |||
157 | // The desired value reduces to zero which also reduces the difference with current. | ||
158 | // If the decay time is infinite, don't decay at all. | ||
159 | float decayFactor = 0f; | ||
160 | if (TargetValueDecayTimeScale != BSMotor.Infinite) | ||
161 | { | ||
162 | decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep; | ||
163 | TargetValue *= (1f - decayFactor); | ||
164 | } | ||
165 | |||
166 | // The amount we can correct the error is reduced by the friction | ||
167 | Vector3 frictionFactor = Vector3.Zero; | ||
168 | if (FrictionTimescale != BSMotor.InfiniteVector) | ||
169 | { | ||
170 | // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep; | ||
171 | // Individual friction components can be 'infinite' so compute each separately. | ||
172 | frictionFactor.X = (FrictionTimescale.X == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.X); | ||
173 | frictionFactor.Y = (FrictionTimescale.Y == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Y); | ||
174 | frictionFactor.Z = (FrictionTimescale.Z == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Z); | ||
175 | frictionFactor *= timeStep; | ||
176 | CurrentValue *= (Vector3.One - frictionFactor); | ||
177 | } | ||
178 | |||
179 | MDetailLog("{0}, BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}", | ||
180 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, | ||
181 | timeStep, error, correction); | ||
182 | MDetailLog("{0}, BSVMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},frictTS={4},frictFact={5},tgt={6},curr={7}", | ||
183 | BSScene.DetailLogZero, UseName, | ||
184 | TargetValueDecayTimeScale, decayFactor, FrictionTimescale, frictionFactor, | ||
185 | TargetValue, CurrentValue); | ||
186 | } | ||
187 | else | ||
188 | { | ||
189 | // Difference between what we have and target is small. Motor is done. | ||
190 | CurrentValue = TargetValue; | ||
191 | MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}", | ||
192 | BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue); | ||
193 | } | ||
194 | |||
195 | return CurrentValue; | ||
196 | } | ||
197 | public virtual Vector3 Step(float timeStep, Vector3 error) | ||
198 | { | ||
199 | if (!Enabled) return Vector3.Zero; | ||
200 | |||
201 | LastError = error; | ||
202 | Vector3 returnCorrection = Vector3.Zero; | ||
203 | if (!error.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) | ||
204 | { | ||
205 | // correction = error / secondsItShouldTakeToCorrect | ||
206 | Vector3 correctionAmount; | ||
207 | if (TimeScale == 0f || TimeScale == BSMotor.Infinite) | ||
208 | correctionAmount = error * timeStep; | ||
209 | else | ||
210 | correctionAmount = error / TimeScale * timeStep; | ||
211 | |||
212 | returnCorrection = correctionAmount; | ||
213 | MDetailLog("{0}, BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}", | ||
214 | BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount); | ||
215 | } | ||
216 | return returnCorrection; | ||
217 | } | ||
218 | |||
219 | // The user sets all the parameters and calls this which outputs values until error is zero. | ||
220 | public override void GenerateTestOutput(float timeStep) | ||
221 | { | ||
222 | // maximum number of outputs to generate. | ||
223 | int maxOutput = 50; | ||
224 | MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName); | ||
225 | MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},frictTS={4},eff={5},curr={6},tgt={7}", | ||
226 | BSScene.DetailLogZero, UseName, | ||
227 | TimeScale, TargetValueDecayTimeScale, FrictionTimescale, Efficiency, | ||
228 | CurrentValue, TargetValue); | ||
229 | |||
230 | LastError = BSMotor.InfiniteVector; | ||
231 | while (maxOutput-- > 0 && !LastError.ApproxEquals(Vector3.Zero, ErrorZeroThreshold)) | ||
232 | { | ||
233 | Vector3 lastStep = Step(timeStep); | ||
234 | MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}", | ||
235 | BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep); | ||
236 | } | ||
237 | MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName); | ||
238 | |||
239 | |||
240 | } | ||
241 | |||
242 | public override string ToString() | ||
243 | { | ||
244 | return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4},frictTS={5}>", | ||
245 | UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale, FrictionTimescale); | ||
246 | } | ||
247 | } | ||
248 | |||
249 | public class BSFMotor : BSMotor | ||
250 | { | ||
251 | public float TimeScale { get; set; } | ||
252 | public float DecayTimeScale { get; set; } | ||
253 | public float Friction { get; set; } | ||
254 | public float Efficiency { get; set; } | ||
255 | |||
256 | public float Target { get; private set; } | ||
257 | public float CurrentValue { get; private set; } | ||
258 | |||
259 | public BSFMotor(string useName, float timeScale, float decayTimescale, float friction, float efficiency) | ||
260 | : base(useName) | ||
261 | { | ||
262 | } | ||
263 | public void SetCurrent(float target) | ||
264 | { | ||
265 | } | ||
266 | public void SetTarget(float target) | ||
267 | { | ||
268 | } | ||
269 | public virtual float Step(float timeStep) | ||
270 | { | ||
271 | return 0f; | ||
272 | } | ||
273 | } | ||
274 | |||
275 | // Proportional, Integral, Derivitive Motor | ||
276 | // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors. | ||
277 | public class BSPIDVMotor : BSVMotor | ||
278 | { | ||
279 | // Larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10. | ||
280 | public Vector3 proportionFactor { get; set; } | ||
281 | public Vector3 integralFactor { get; set; } | ||
282 | public Vector3 derivFactor { get; set; } | ||
283 | |||
284 | // Arbritrary factor range. | ||
285 | // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct. | ||
286 | public float EfficiencyHigh = 0.4f; | ||
287 | public float EfficiencyLow = 4.0f; | ||
288 | |||
289 | // Running integration of the error | ||
290 | Vector3 RunningIntegration { get; set; } | ||
291 | |||
292 | public BSPIDVMotor(string useName) | ||
293 | : base(useName) | ||
294 | { | ||
295 | proportionFactor = new Vector3(1.00f, 1.00f, 1.00f); | ||
296 | integralFactor = new Vector3(1.00f, 1.00f, 1.00f); | ||
297 | derivFactor = new Vector3(1.00f, 1.00f, 1.00f); | ||
298 | RunningIntegration = Vector3.Zero; | ||
299 | LastError = Vector3.Zero; | ||
300 | } | ||
301 | |||
302 | public override void Zero() | ||
303 | { | ||
304 | base.Zero(); | ||
305 | } | ||
306 | |||
307 | public override float Efficiency | ||
308 | { | ||
309 | get { return base.Efficiency; } | ||
310 | set | ||
311 | { | ||
312 | base.Efficiency = Util.Clamp(value, 0f, 1f); | ||
313 | // Compute factors based on efficiency. | ||
314 | // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot. | ||
315 | // If efficiency is low (0f), use a factor value that overcorrects. | ||
316 | // TODO: might want to vary contribution of different factor depending on efficiency. | ||
317 | float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f; | ||
318 | // float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow; | ||
319 | proportionFactor = new Vector3(factor, factor, factor); | ||
320 | integralFactor = new Vector3(factor, factor, factor); | ||
321 | derivFactor = new Vector3(factor, factor, factor); | ||
322 | } | ||
323 | } | ||
324 | |||
325 | // Ignore Current and Target Values and just advance the PID computation on this error. | ||
326 | public override Vector3 Step(float timeStep, Vector3 error) | ||
327 | { | ||
328 | if (!Enabled) return Vector3.Zero; | ||
329 | |||
330 | // Add up the error so we can integrate over the accumulated errors | ||
331 | RunningIntegration += error * timeStep; | ||
332 | |||
333 | // A simple derivitive is the rate of change from the last error. | ||
334 | Vector3 derivFactor = (error - LastError) * timeStep; | ||
335 | LastError = error; | ||
336 | |||
337 | // Correction = -(proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError) | ||
338 | Vector3 ret = -( | ||
339 | error * proportionFactor | ||
340 | + RunningIntegration * integralFactor | ||
341 | + derivFactor * derivFactor | ||
342 | ); | ||
343 | |||
344 | return ret; | ||
345 | } | ||
346 | } | ||
347 | } | ||