BSMotors.cs 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451
  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. namespace OpenSim.Region.Physics.BulletSPlugin
  34. {
  35. public abstract class BSMotor
  36. {
  37. // Timescales and other things can be turned off by setting them to 'infinite'.
  38. public const float Infinite = 12345.6f;
  39. public readonly static Vector3 InfiniteVector = new Vector3(BSMotor.Infinite, BSMotor.Infinite, BSMotor.Infinite);
  40. public BSMotor(string useName)
  41. {
  42. UseName = useName;
  43. PhysicsScene = null;
  44. Enabled = true;
  45. }
  46. public virtual bool Enabled { get; set; }
  47. public virtual void Reset() { }
  48. public virtual void Zero() { }
  49. public virtual void GenerateTestOutput(float timeStep) { }
  50. // A name passed at motor creation for easily identifyable debugging messages.
  51. public string UseName { get; private set; }
  52. // Used only for outputting debug information. Might not be set so check for null.
  53. public BSScene PhysicsScene { get; set; }
  54. protected void MDetailLog(string msg, params Object[] parms)
  55. {
  56. if (PhysicsScene != null)
  57. {
  58. PhysicsScene.DetailLog(msg, parms);
  59. }
  60. }
  61. }
  62. // Motor which moves CurrentValue to TargetValue over TimeScale seconds.
  63. // The TargetValue decays in TargetValueDecayTimeScale.
  64. // This motor will "zero itself" over time in that the targetValue will
  65. // decay to zero and the currentValue will follow it to that zero.
  66. // The overall effect is for the returned correction value to go from large
  67. // values to small and eventually zero values.
  68. // TimeScale and TargetDelayTimeScale may be 'infinite' which means no decay.
  69. // For instance, if something is moving at speed X and the desired speed is Y,
  70. // CurrentValue is X and TargetValue is Y. As the motor is stepped, new
  71. // values of CurrentValue are returned that approach the TargetValue.
  72. // The feature of decaying TargetValue is so vehicles will eventually
  73. // come to a stop rather than run forever. This can be disabled by
  74. // setting TargetValueDecayTimescale to 'infinite'.
  75. // The change from CurrentValue to TargetValue is linear over TimeScale seconds.
  76. public class BSVMotor : BSMotor
  77. {
  78. // public Vector3 FrameOfReference { get; set; }
  79. // public Vector3 Offset { get; set; }
  80. public virtual float TimeScale { get; set; }
  81. public virtual float TargetValueDecayTimeScale { get; set; }
  82. public virtual float Efficiency { get; set; }
  83. public virtual float ErrorZeroThreshold { get; set; }
  84. public virtual Vector3 TargetValue { get; protected set; }
  85. public virtual Vector3 CurrentValue { get; protected set; }
  86. public virtual Vector3 LastError { get; protected set; }
  87. public virtual bool ErrorIsZero()
  88. {
  89. return ErrorIsZero(LastError);
  90. }
  91. public virtual bool ErrorIsZero(Vector3 err)
  92. {
  93. return (err == Vector3.Zero || err.ApproxEquals(Vector3.Zero, ErrorZeroThreshold));
  94. }
  95. public BSVMotor(string useName)
  96. : base(useName)
  97. {
  98. TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
  99. Efficiency = 1f;
  100. CurrentValue = TargetValue = Vector3.Zero;
  101. ErrorZeroThreshold = 0.001f;
  102. }
  103. public BSVMotor(string useName, float timeScale, float decayTimeScale, float efficiency)
  104. : this(useName)
  105. {
  106. TimeScale = timeScale;
  107. TargetValueDecayTimeScale = decayTimeScale;
  108. Efficiency = efficiency;
  109. CurrentValue = TargetValue = Vector3.Zero;
  110. }
  111. public void SetCurrent(Vector3 current)
  112. {
  113. CurrentValue = current;
  114. }
  115. public void SetTarget(Vector3 target)
  116. {
  117. TargetValue = target;
  118. }
  119. public override void Zero()
  120. {
  121. base.Zero();
  122. CurrentValue = TargetValue = Vector3.Zero;
  123. }
  124. // Compute the next step and return the new current value.
  125. // Returns the correction needed to move 'current' to 'target'.
  126. public virtual Vector3 Step(float timeStep)
  127. {
  128. if (!Enabled) return TargetValue;
  129. Vector3 origTarget = TargetValue; // DEBUG
  130. Vector3 origCurrVal = CurrentValue; // DEBUG
  131. Vector3 correction = Vector3.Zero;
  132. Vector3 error = TargetValue - CurrentValue;
  133. if (!ErrorIsZero(error))
  134. {
  135. correction = StepError(timeStep, error);
  136. CurrentValue += correction;
  137. // The desired value reduces to zero which also reduces the difference with current.
  138. // If the decay time is infinite, don't decay at all.
  139. float decayFactor = 0f;
  140. if (TargetValueDecayTimeScale != BSMotor.Infinite)
  141. {
  142. decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
  143. TargetValue *= (1f - decayFactor);
  144. }
  145. MDetailLog("{0}, BSVMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
  146. BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
  147. timeStep, error, correction);
  148. MDetailLog("{0}, BSVMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},tgt={4},curr={5}",
  149. BSScene.DetailLogZero, UseName, TargetValueDecayTimeScale, decayFactor, TargetValue, CurrentValue);
  150. }
  151. else
  152. {
  153. // Difference between what we have and target is small. Motor is done.
  154. if (TargetValue.ApproxEquals(Vector3.Zero, ErrorZeroThreshold))
  155. {
  156. // The target can step down to nearly zero but not get there. If close to zero
  157. // it is really zero.
  158. TargetValue = Vector3.Zero;
  159. }
  160. CurrentValue = TargetValue;
  161. MDetailLog("{0}, BSVMotor.Step,zero,{1},origTgt={2},origCurr={3},currTgt={4},currCurr={5}",
  162. BSScene.DetailLogZero, UseName, origCurrVal, origTarget, TargetValue, CurrentValue);
  163. }
  164. LastError = error;
  165. return correction;
  166. }
  167. // version of step that sets the current value before doing the step
  168. public virtual Vector3 Step(float timeStep, Vector3 current)
  169. {
  170. CurrentValue = current;
  171. return Step(timeStep);
  172. }
  173. // Given and error, computer a correction for this step.
  174. // Simple scaling of the error by the timestep.
  175. public virtual Vector3 StepError(float timeStep, Vector3 error)
  176. {
  177. if (!Enabled) return Vector3.Zero;
  178. Vector3 returnCorrection = Vector3.Zero;
  179. if (!ErrorIsZero(error))
  180. {
  181. // correction = error / secondsItShouldTakeToCorrect
  182. Vector3 correctionAmount;
  183. if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
  184. correctionAmount = error * timeStep;
  185. else
  186. correctionAmount = error / TimeScale * timeStep;
  187. returnCorrection = correctionAmount;
  188. MDetailLog("{0}, BSVMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
  189. BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
  190. }
  191. return returnCorrection;
  192. }
  193. // The user sets all the parameters and calls this which outputs values until error is zero.
  194. public override void GenerateTestOutput(float timeStep)
  195. {
  196. // maximum number of outputs to generate.
  197. int maxOutput = 50;
  198. MDetailLog("{0},BSVMotor.Test,{1},===================================== BEGIN Test Output", BSScene.DetailLogZero, UseName);
  199. MDetailLog("{0},BSVMotor.Test,{1},timeScale={2},targDlyTS={3},eff={4},curr={5},tgt={6}",
  200. BSScene.DetailLogZero, UseName,
  201. TimeScale, TargetValueDecayTimeScale, Efficiency,
  202. CurrentValue, TargetValue);
  203. LastError = BSMotor.InfiniteVector;
  204. while (maxOutput-- > 0 && !ErrorIsZero())
  205. {
  206. Vector3 lastStep = Step(timeStep);
  207. MDetailLog("{0},BSVMotor.Test,{1},cur={2},tgt={3},lastError={4},lastStep={5}",
  208. BSScene.DetailLogZero, UseName, CurrentValue, TargetValue, LastError, lastStep);
  209. }
  210. MDetailLog("{0},BSVMotor.Test,{1},===================================== END Test Output", BSScene.DetailLogZero, UseName);
  211. }
  212. public override string ToString()
  213. {
  214. return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4}>",
  215. UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale);
  216. }
  217. }
  218. // ============================================================================
  219. // ============================================================================
  220. public class BSFMotor : BSMotor
  221. {
  222. public virtual float TimeScale { get; set; }
  223. public virtual float TargetValueDecayTimeScale { get; set; }
  224. public virtual float Efficiency { get; set; }
  225. public virtual float ErrorZeroThreshold { get; set; }
  226. public virtual float TargetValue { get; protected set; }
  227. public virtual float CurrentValue { get; protected set; }
  228. public virtual float LastError { get; protected set; }
  229. public virtual bool ErrorIsZero()
  230. {
  231. return ErrorIsZero(LastError);
  232. }
  233. public virtual bool ErrorIsZero(float err)
  234. {
  235. return (err >= -ErrorZeroThreshold && err <= ErrorZeroThreshold);
  236. }
  237. public BSFMotor(string useName, float timeScale, float decayTimescale, float efficiency)
  238. : base(useName)
  239. {
  240. TimeScale = TargetValueDecayTimeScale = BSMotor.Infinite;
  241. Efficiency = 1f;
  242. CurrentValue = TargetValue = 0f;
  243. ErrorZeroThreshold = 0.01f;
  244. }
  245. public void SetCurrent(float current)
  246. {
  247. CurrentValue = current;
  248. }
  249. public void SetTarget(float target)
  250. {
  251. TargetValue = target;
  252. }
  253. public override void Zero()
  254. {
  255. base.Zero();
  256. CurrentValue = TargetValue = 0f;
  257. }
  258. public virtual float Step(float timeStep)
  259. {
  260. if (!Enabled) return TargetValue;
  261. float origTarget = TargetValue; // DEBUG
  262. float origCurrVal = CurrentValue; // DEBUG
  263. float correction = 0f;
  264. float error = TargetValue - CurrentValue;
  265. if (!ErrorIsZero(error))
  266. {
  267. correction = StepError(timeStep, error);
  268. CurrentValue += correction;
  269. // The desired value reduces to zero which also reduces the difference with current.
  270. // If the decay time is infinite, don't decay at all.
  271. float decayFactor = 0f;
  272. if (TargetValueDecayTimeScale != BSMotor.Infinite)
  273. {
  274. decayFactor = (1.0f / TargetValueDecayTimeScale) * timeStep;
  275. TargetValue *= (1f - decayFactor);
  276. }
  277. MDetailLog("{0}, BSFMotor.Step,nonZero,{1},origCurr={2},origTarget={3},timeStep={4},err={5},corr={6}",
  278. BSScene.DetailLogZero, UseName, origCurrVal, origTarget,
  279. timeStep, error, correction);
  280. MDetailLog("{0}, BSFMotor.Step,nonZero,{1},tgtDecayTS={2},decayFact={3},tgt={4},curr={5}",
  281. BSScene.DetailLogZero, UseName, TargetValueDecayTimeScale, decayFactor, TargetValue, CurrentValue);
  282. }
  283. else
  284. {
  285. // Difference between what we have and target is small. Motor is done.
  286. if (Util.InRange<float>(TargetValue, -ErrorZeroThreshold, ErrorZeroThreshold))
  287. {
  288. // The target can step down to nearly zero but not get there. If close to zero
  289. // it is really zero.
  290. TargetValue = 0f;
  291. }
  292. CurrentValue = TargetValue;
  293. MDetailLog("{0}, BSFMotor.Step,zero,{1},origTgt={2},origCurr={3},ret={4}",
  294. BSScene.DetailLogZero, UseName, origCurrVal, origTarget, CurrentValue);
  295. }
  296. LastError = error;
  297. return CurrentValue;
  298. }
  299. public virtual float StepError(float timeStep, float error)
  300. {
  301. if (!Enabled) return 0f;
  302. float returnCorrection = 0f;
  303. if (!ErrorIsZero(error))
  304. {
  305. // correction = error / secondsItShouldTakeToCorrect
  306. float correctionAmount;
  307. if (TimeScale == 0f || TimeScale == BSMotor.Infinite)
  308. correctionAmount = error * timeStep;
  309. else
  310. correctionAmount = error / TimeScale * timeStep;
  311. returnCorrection = correctionAmount;
  312. MDetailLog("{0}, BSFMotor.Step,nonZero,{1},timeStep={2},timeScale={3},err={4},corr={5}",
  313. BSScene.DetailLogZero, UseName, timeStep, TimeScale, error, correctionAmount);
  314. }
  315. return returnCorrection;
  316. }
  317. public override string ToString()
  318. {
  319. return String.Format("<{0},curr={1},targ={2},lastErr={3},decayTS={4}>",
  320. UseName, CurrentValue, TargetValue, LastError, TargetValueDecayTimeScale);
  321. }
  322. }
  323. // ============================================================================
  324. // ============================================================================
  325. // Proportional, Integral, Derivitive ("PID") Motor
  326. // Good description at http://www.answers.com/topic/pid-controller . Includes processes for choosing p, i and d factors.
  327. public class BSPIDVMotor : BSVMotor
  328. {
  329. // Larger makes more overshoot, smaller means converge quicker. Range of 0.1 to 10.
  330. public Vector3 proportionFactor { get; set; }
  331. public Vector3 integralFactor { get; set; }
  332. public Vector3 derivFactor { get; set; }
  333. // The factors are vectors for the three dimensions. This is the proportional of each
  334. // that is applied. This could be multiplied through the actual factors but it
  335. // is sometimes easier to manipulate the factors and their mix separately.
  336. public Vector3 FactorMix;
  337. // Arbritrary factor range.
  338. // EfficiencyHigh means move quickly to the correct number. EfficiencyLow means might over correct.
  339. public float EfficiencyHigh = 0.4f;
  340. public float EfficiencyLow = 4.0f;
  341. // Running integration of the error
  342. Vector3 RunningIntegration { get; set; }
  343. public BSPIDVMotor(string useName)
  344. : base(useName)
  345. {
  346. proportionFactor = new Vector3(1.00f, 1.00f, 1.00f);
  347. integralFactor = new Vector3(1.00f, 1.00f, 1.00f);
  348. derivFactor = new Vector3(1.00f, 1.00f, 1.00f);
  349. FactorMix = new Vector3(0.5f, 0.25f, 0.25f);
  350. RunningIntegration = Vector3.Zero;
  351. LastError = Vector3.Zero;
  352. }
  353. public override void Zero()
  354. {
  355. base.Zero();
  356. }
  357. public override float Efficiency
  358. {
  359. get { return base.Efficiency; }
  360. set
  361. {
  362. base.Efficiency = Util.Clamp(value, 0f, 1f);
  363. // Compute factors based on efficiency.
  364. // If efficiency is high (1f), use a factor value that moves the error value to zero with little overshoot.
  365. // If efficiency is low (0f), use a factor value that overcorrects.
  366. // TODO: might want to vary contribution of different factor depending on efficiency.
  367. // float factor = ((1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow) / 3f;
  368. float factor = (1f - this.Efficiency) * EfficiencyHigh + EfficiencyLow;
  369. proportionFactor = new Vector3(factor, factor, factor);
  370. integralFactor = new Vector3(factor, factor, factor);
  371. derivFactor = new Vector3(factor, factor, factor);
  372. MDetailLog("{0}, BSPIDVMotor.setEfficiency,eff={1},factor={2}", BSScene.DetailLogZero, Efficiency, factor);
  373. }
  374. }
  375. // Advance the PID computation on this error.
  376. public override Vector3 StepError(float timeStep, Vector3 error)
  377. {
  378. if (!Enabled) return Vector3.Zero;
  379. // Add up the error so we can integrate over the accumulated errors
  380. RunningIntegration += error * timeStep;
  381. // A simple derivitive is the rate of change from the last error.
  382. Vector3 derivitive = (error - LastError) * timeStep;
  383. // Correction = (proportionOfPresentError + accumulationOfPastError + rateOfChangeOfError)
  384. Vector3 ret = error / TimeScale * timeStep * proportionFactor * FactorMix.X
  385. + RunningIntegration / TimeScale * integralFactor * FactorMix.Y
  386. + derivitive / TimeScale * derivFactor * FactorMix.Z
  387. ;
  388. MDetailLog("{0}, BSPIDVMotor.step,ts={1},err={2},lerr={3},runnInt={4},deriv={5},ret={6}",
  389. BSScene.DetailLogZero, timeStep, error, LastError, RunningIntegration, derivitive, ret);
  390. return ret;
  391. }
  392. }
  393. }