aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim')
-rwxr-xr-xOpenSim/Region/Physics/BulletSPlugin/BSMotors.cs44
1 files changed, 38 insertions, 6 deletions
diff --git a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
index e0faf4e..c718228 100755
--- a/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
+++ b/OpenSim/Region/Physics/BulletSPlugin/BSMotors.cs
@@ -117,12 +117,12 @@ public class BSVMotor : BSMotor
117 117
118 // A form of stepping that does not take the time quantum into account. 118 // A form of stepping that does not take the time quantum into account.
119 // The caller must do the right thing later. 119 // The caller must do the right thing later.
120 public Vector3 Step() 120 public virtual Vector3 Step()
121 { 121 {
122 return Step(1f); 122 return Step(1f);
123 } 123 }
124 124
125 public Vector3 Step(float timeStep) 125 public virtual Vector3 Step(float timeStep)
126 { 126 {
127 Vector3 returnCurrent = Vector3.Zero; 127 Vector3 returnCurrent = Vector3.Zero;
128 if (!CurrentValue.ApproxEquals(TargetValue, 0.01f)) 128 if (!CurrentValue.ApproxEquals(TargetValue, 0.01f))
@@ -204,17 +204,49 @@ public class BSFMotor : BSMotor
204 public void SetTarget(float target) 204 public void SetTarget(float target)
205 { 205 {
206 } 206 }
207 public float Step(float timeStep) 207 public virtual float Step(float timeStep)
208 { 208 {
209 return 0f; 209 return 0f;
210 } 210 }
211} 211}
212public class BSPIDMotor : BSMotor 212
213// Proportional, Integral, Derivitive Motor
214// Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
215public class BSPIDVMotor : BSVMotor
213{ 216{
214 // TODO: write and use this one 217 public Vector3 pFactor { get; set; } // Amount of direct correction of an error (sometimes called 'proportional gain')
215 public BSPIDMotor(string useName) 218 public Vector3 iFactor { get; set; } //
219 public Vector3 dFactor { get; set; }
220
221 Vector3 IntegralFactor { get; set; }
222 Vector3 LastError { get; set; }
223
224 public BSPIDVMotor(string useName)
216 : base(useName) 225 : base(useName)
217 { 226 {
227 // larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10.
228 pFactor = new Vector3(1.00f, 1.00f, 1.00f);
229 iFactor = new Vector3(1.00f, 1.00f, 1.00f);
230 dFactor = new Vector3(1.00f, 1.00f, 1.00f);
231 }
232
233 public override Vector3 Step(float timeStep)
234 {
235 // How far are we from where we should be
236 Vector3 error = TargetValue - CurrentValue;
237
238 // Add up the error so we can integrate over the accumulated errors
239 IntegralFactor += error * timeStep;
240
241 // A simple derivitive is the rate of change from the last error.
242 Vector3 derivFactor = (error - LastError) * timeStep;
243 LastError = error;
244
245 // Proportion Integral Derivitive
246 // Correction = proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError
247 Vector3 ret = error * pFactor + IntegralFactor * iFactor + derivFactor * dFactor;
248
249 return ret;
218 } 250 }
219} 251}
220} 252}