aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
blob: 0128d8dbdde90addc04fa966ae7e24390c6e2fcc (plain)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
402
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
442
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
481
482
483
484
485
486
/*
 * Copyright (c) Contributors, http://opensimulator.org/
 * See CONTRIBUTORS.TXT for a full list of copyright holders.
 *
 * Redistribution and use in source and binary forms, with or without
 * modification, are permitted provided that the following conditions are met:
 *     * Redistributions of source code must retain the above copyright
 *       notice, this list of conditions and the following disclaimer.
 *     * Redistributions in binary form must reproduce the above copyright
 *       notice, this list of conditions and the following disclaimer in the
 *       documentation and/or other materials provided with the distribution.
 *     * Neither the name of the OpenSimulator Project nor the
 *       names of its contributors may be used to endorse or promote products
 *       derived from this software without specific prior written permission.
 *
 * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
 * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
 * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
 * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
 * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
 * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
 * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
 * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
 * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
 *
 */
using System;
using System.Collections.Generic;
using System.Text;
using OpenMetaverse;
using OpenSim.Framework;

namespace OpenSim.Region.Physics.BulletSPlugin
{
public abstract class BSMotor
{
    // Timescales and other things can be turned off by setting them to 'infinite'.
    public const float Infinite = 12345.6f;
    public readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite);

    public BSMotor(string useName)
    {
        UseName = useName;
        PhysicsScene = null;
        Enabled = true;
    }
    public virtual bool Enabled { get; set; }
    public virtual void Reset() { }
    public virtual void Zero() { }
    public virtual void GenerateTestOutput(float timeStep) { }

    // A name passed at motor creation for easily identifyable debugging messages.
    public string UseName { get; private set; }

    // Used only for outputting debug information. Might not be set so check for null.
    public BSScene PhysicsScene { get; set; }
    protected void MDetailLog(string msg, params Object[] parms)
    {
        if (PhysicsScene != null)
        {
            PhysicsScene.DetailLog(msg, parms);
        }
    }
}

// Motor which moves CurrentValue to TargetValue over TimeScale seconds.
// The TargetValue decays in TargetValueDecayTimeScale and
//     the CurrentValue will be held back by FrictionTimeScale.
// This motor will "zero itself" over time in that the targetValue will
//    decay to zero and the currentValue will follow it to that zero.
//    The overall effect is for the returned correction value to go from large
//    values (the total difference between current and target minus friction)
//    to small and eventually zero values.
// TimeScale and TargetDelayTimeScale may be 'infinite' which means no decay.

// For instance, if something is moving at speed X and the desired speed is Y,
//    CurrentValue is X and TargetValue is Y. As the motor is stepped, new
//    values of CurrentValue are returned that approach the TargetValue.
// The feature of decaying TargetValue is so vehicles will eventually
//    come to a stop rather than run forever. This can be disabled by
//    setting TargetValueDecayTimescale to 'infinite'.
// The change from CurrentValue to TargetValue is linear over TimeScale seconds.
public class BSVMotor : BSMotor
{
    // public Vector3 FrameOfReference { get; set; }
    // public Vector3 Offset { get; set; }

    public virtual float TimeScale { get; set; }
    public virtual float TargetValueDecayTimeScale { get; set; }
    public virtual Vector3 FrictionTimescale { get; set; }
    public virtual float Efficiency { get; set; }

    public virtual float ErrorZeroThreshold { get; set; }

    public virtual Vector3 TargetValue { get; protected set; }
    public virtual Vector3 CurrentValue { get; protected set; }
    public virtual Vector3 LastError { get; protected set; }

    public virtual bool ErrorIsZero()
    {
        return ErrorIsZero(LastError);
    }
    public virtual bool ErrorIsZero(Vector3 err)
    {
        return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold));
    }

    public BSVMotor(string useName)
        : base(useName)
    {
        TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
        Efficiency = 1f;
        FrictionTimescale = BSMotor.InfiniteVector;
        CurrentValue = TargetValue = Vector3.Zero;
        ErrorZeroThreshold = 0.001f;
    }
    public BSVMotor(string useName, float timeScale, float decayTimeScale, Vector3 frictionTimeScale, float efficiency)
        : this(useName)
    {
        TimeScale = timeScale;
        TargetValueDecayTimeScale = decayTimeScale;
        FrictionTimescale = frictionTimeScale;
        Efficiency = efficiency;
        CurrentValue = TargetValue = Vector3.Zero;
    }
    public void SetCurrent(Vector3 current)
    {
        CurrentValue = current;
    }
    public void SetTarget(Vector3 target)
    {
        TargetValue = target;
    }
    public override void Zero()
    {
        base.Zero();
        CurrentValue = TargetValue = Vector3.Zero;
    }

    // Compute the next step and return the new current value.
    // Returns the correction needed to move 'current' to 'target'.
    public virtual Vector3 Step(float timeStep)
    {
        if (!Enabled) return TargetValue;

        Vector3 origTarget = TargetValue;       // DEBUG
        Vector3 origCurrVal = CurrentValue;     // DEBUG

        Vector3 correction = Vector3.Zero;
        Vector3 error = TargetValue - CurrentValue;
        LastError = error;
        if (!ErrorIsZero(error))
        {
            correction = StepError(timeStep, error);

            CurrentValue += correction;

            // The desired value reduces to zero which also reduces the difference with current.
            // If the decay time is infinite, don't decay at all.
            float decayFactor = 0f;
            if (TargetValueDecayTimeScale != BSMotor.Infinite)
            {
                decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
                TargetValue *= (1f - decayFactor);
            }

            // The amount we can correct the error is reduced by the friction
            Vector3 frictionFactor = Vector3.Zero;
            if (FrictionTimescale != BSMotor.InfiniteVector)
            {
                // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
                // Individual friction components can be 'infinite' so compute each separately.
                frictionFactor.X = (FrictionTimescale.X == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.X);
                frictionFactor.Y = (FrictionTimescale.Y == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Y);
                frictionFactor.Z = (FrictionTimescale.Z == BSMotor.Infinite) ? 0f : (1f / FrictionTimescale.Z);
                frictionFactor *= timeStep;
                CurrentValue *= (Vector3.One - frictionFactor);
            }

            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
                                BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
                                timeStep, error, correction);
            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},frictTS={4},frictFact={5},tgt={6},curr={7}",
                                BSScene.DetailLogZero, UseName,
                                TargetValueDecayTimeScale, decayFactor, FrictionTimescale, frictionFactor,
                                TargetValue, CurrentValue);
        }
        else
        {
            // Difference between what we have and target is small. Motor is done.
            if (TargetValue.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
            {
                // The target can step down to nearly zero but not get there.  If close to zero
                //     it is really zero.
                TargetValue = Vector3.Zero;
            }
            CurrentValue = TargetValue;
            MDetailLog("{0},  BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}",
                        BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue);
        }

        return correction;
    }
    // version of step that sets the current value before doing the step
    public virtual Vector3 Step(float timeStep, Vector3 current)
    {
        CurrentValue = current;
        return Step(timeStep);
    }
    public virtual Vector3 StepError(float timeStep, Vector3 error)
    {
        if (!Enabled) return Vector3.Zero;

        Vector3 returnCorrection = Vector3.Zero;
        if (!ErrorIsZero(error))
        {
            // correction =  error / secondsItShouldTakeToCorrect
            Vector3 correctionAmount;
            if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
                correctionAmount = error * timeStep;
            else
                correctionAmount = error / TimeScale * timeStep;

            returnCorrection = correctionAmount;
            MDetailLog("{0},  BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
                                    BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
        }
        return returnCorrection;
    }

    // The user sets all the parameters and calls this which outputs values until error is zero.
    public override void GenerateTestOutput(float timeStep)
    {
        // maximum number of outputs to generate.
        int maxOutput = 50;
        MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName);
        MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},frictTS={4},eff={5},curr={6},tgt={7}",
                                BSScene.DetailLogZero, UseName,
                                TimeScale, TargetValueDecayTimeScale, FrictionTimescale, Efficiency,
                                CurrentValue, TargetValue);

        LastError = BSMotor.InfiniteVector;
        while (maxOutput-- > 0 && !LastError.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
        {
            Vector3 lastStep = Step(timeStep);
            MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}",
                            BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep);
        }
        MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName);


    }

    public override string ToString()
    {
        return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4},frictTS={5}>",
            UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale, FrictionTimescale);
    }
}

// ============================================================================
// ============================================================================
public class BSFMotor : BSMotor
{
    public virtual float TimeScale { get; set; }
    public virtual float TargetValueDecayTimeScale { get; set; }
    public virtual float FrictionTimescale { get; set; }
    public virtual float Efficiency { get; set; }

    public virtual float ErrorZeroThreshold { get; set; }

    public virtual float TargetValue { get; protected set; }
    public virtual float CurrentValue { get; protected set; }
    public virtual float LastError { get; protected set; }

    public virtual bool ErrorIsZero()
    {
        return ErrorIsZero(LastError);
    }
    public virtual bool ErrorIsZero(float err)
    {
        return (err >= -ErrorZeroThreshold && err <= ErrorZeroThreshold);
    }

    public BSFMotor(string useName, float timeScale, float decayTimescale, float friction, float efficiency)
        : base(useName)
    {
        TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
        Efficiency = 1f;
        FrictionTimescale = BSMotor.Infinite;
        CurrentValue = TargetValue = 0f;
        ErrorZeroThreshold = 0.01f;
    }
    public void SetCurrent(float current)
    {
        CurrentValue = current;
    }
    public void SetTarget(float target)
    {
        TargetValue = target;
    }
    public override void Zero()
    {
        base.Zero();
        CurrentValue = TargetValue = 0f;
    }

    public virtual float Step(float timeStep)
    {
        if (!Enabled) return TargetValue;

        float origTarget = TargetValue;       // DEBUG
        float origCurrVal = CurrentValue;     // DEBUG

        float correction = 0f;
        float error = TargetValue - CurrentValue;
        LastError = error;
        if (!ErrorIsZero(error))
        {
            correction = StepError(timeStep, error);

            CurrentValue += correction;

            // The desired value reduces to zero which also reduces the difference with current.
            // If the decay time is infinite, don't decay at all.
            float decayFactor = 0f;
            if (TargetValueDecayTimeScale != BSMotor.Infinite)
            {
                decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
                TargetValue *= (1f - decayFactor);
            }

            // The amount we can correct the error is reduced by the friction
            float frictionFactor = 0f;
            if (FrictionTimescale != BSMotor.Infinite)
            {
                // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
                // Individual friction components can be 'infinite' so compute each separately.
                frictionFactor = 1f / FrictionTimescale;
                frictionFactor *= timeStep;
                CurrentValue *= (1f - frictionFactor);
            }

            MDetailLog("{0},  BSFMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
                                BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
                                timeStep, error, correction);
            MDetailLog("{0},  BSFMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},frictTS={4},frictFact={5},tgt={6},curr={7}",
                                BSScene.DetailLogZero, UseName,
                                TargetValueDecayTimeScale, decayFactor, FrictionTimescale, frictionFactor,
                                TargetValue, CurrentValue);
        }
        else
        {
            // Difference between what we have and target is small. Motor is done.
            if (Util.InRange<float>(TargetValue, -ErrorZeroThreshold, ErrorZeroThreshold))
            {
                // The target can step down to nearly zero but not get there.  If close to zero
                //     it is really zero.
                TargetValue = 0f;
            }
            CurrentValue = TargetValue;
            MDetailLog("{0},  BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
                        BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
        }

        return CurrentValue;
    }

    public virtual float StepError(float timeStep, float error)
    {
        if (!Enabled) return 0f;

        float returnCorrection = 0f;
        if (!ErrorIsZero(error))
        {
            // correction =  error / secondsItShouldTakeToCorrect
            float correctionAmount;
            if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
                correctionAmount = error * timeStep;
            else
                correctionAmount = error / TimeScale * timeStep;

            returnCorrection = correctionAmount;
            MDetailLog("{0},  BSFMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
                                    BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
        }
        return returnCorrection;
    }

    public override string ToString()
    {
        return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4},frictTS={5}>",
            UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale, FrictionTimescale);
    }

}

// ============================================================================
// ============================================================================
// Proportional, Integral, Derivitive Motor
// Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
public class BSPIDVMotor : BSVMotor
{
    // Larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10.
    public Vector3 proportionFactor { get; set; }
    public Vector3 integralFactor { get; set; }
    public Vector3 derivFactor { get; set; }

    // The factors are vectors for the three dimensions. This is the proportional of each
    //    that is applied. This could be multiplied through the actual factors but it
    //    is sometimes easier to manipulate the factors and their mix separately.
    //      to
    public Vector3 FactorMix;

    // Arbritrary factor range.
    // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct.
    public float EfficiencyHigh = 0.4f;
    public float EfficiencyLow = 4.0f;

    // Running integration of the error
    Vector3 RunningIntegration { get; set; }

    public BSPIDVMotor(string useName)
        : base(useName)
    {
        proportionFactor = new Vector3(1.00f, 1.00f, 1.00f);
        integralFactor = new Vector3(1.00f, 1.00f, 1.00f);
        derivFactor = new Vector3(1.00f, 1.00f, 1.00f);
        FactorMix = new Vector3(0.5f, 0.25f, 0.25f);
        RunningIntegration = Vector3.Zero;
        LastError = Vector3.Zero;
    }

    public override void Zero()
    {
        base.Zero();
    }

    public override float Efficiency
    {
        get { return base.Efficiency; }
        set
        {
            base.Efficiency = Util.Clamp(value, 0f, 1f);

            // Compute factors based on efficiency.
            // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot.
            // If efficiency is low (0f), use a factor value that overcorrects.
            // TODO: might want to vary contribution of different factor depending on efficiency.
            float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f;
            // float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow;

            proportionFactor = new Vector3(factor, factor, factor);
            integralFactor = new Vector3(factor, factor, factor);
            derivFactor = new Vector3(factor, factor, factor);

            MDetailLog("{0},BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor);
        }
    }

    // Advance the PID computation on this error.
    public override Vector3 StepError(float timeStep, Vector3 error)
    {
        if (!Enabled) return Vector3.Zero;

        // Add up the error so we can integrate over the accumulated errors
        RunningIntegration += error * timeStep;

        // A simple derivitive is the rate of change from the last error.
        Vector3 derivitive = (error - LastError) * timeStep;
        LastError = error;

        // Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError)
        Vector3 ret   =   error * timeStep   * proportionFactor * FactorMix.X
                        + RunningIntegration * integralFactor   * FactorMix.Y
                        + derivitive         * derivFactor      * FactorMix.Z
                        ;

        MDetailLog("{0},BSPIDVMotor.step,ts={1},err={2},runnInt={3},deriv={4},ret={5}",
                        BSScene.DetailLogZero, timeStep, error, RunningIntegration, derivitive, ret);

        return ret;
    }
}
}