1
0

BSDynamics.cs 85 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734
  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. * The quotations from http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial
  28. * are Copyright (c) 2009 Linden Research, Inc and are used under their license
  29. * of Creative Commons Attribution-Share Alike 3.0
  30. * (http://creativecommons.org/licenses/by-sa/3.0/).
  31. */
  32. using System;
  33. using System.Collections.Generic;
  34. using System.Reflection;
  35. using System.Runtime.InteropServices;
  36. using OpenMetaverse;
  37. using OpenSim.Framework;
  38. using OpenSim.Region.Physics.Manager;
  39. namespace OpenSim.Region.Physics.BulletSPlugin
  40. {
  41. public sealed class BSDynamics : BSActor
  42. {
  43. private static string LogHeader = "[BULLETSIM VEHICLE]";
  44. // the prim this dynamic controller belongs to
  45. private BSPrimLinkable ControllingPrim { get; set; }
  46. private bool m_haveRegisteredForSceneEvents;
  47. // mass of the vehicle fetched each time we're calles
  48. private float m_vehicleMass;
  49. // Vehicle properties
  50. public Vehicle Type { get; set; }
  51. // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
  52. private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
  53. // HOVER_TERRAIN_ONLY
  54. // HOVER_GLOBAL_HEIGHT
  55. // NO_DEFLECTION_UP
  56. // HOVER_WATER_ONLY
  57. // HOVER_UP_ONLY
  58. // LIMIT_MOTOR_UP
  59. // LIMIT_ROLL_ONLY
  60. private Vector3 m_BlockingEndPoint = Vector3.Zero;
  61. private Quaternion m_RollreferenceFrame = Quaternion.Identity;
  62. private Quaternion m_referenceFrame = Quaternion.Identity;
  63. // Linear properties
  64. private BSVMotor m_linearMotor = new BSVMotor("LinearMotor");
  65. private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
  66. private Vector3 m_linearMotorOffset = Vector3.Zero; // the point of force can be offset from the center
  67. private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
  68. private Vector3 m_linearFrictionTimescale = Vector3.Zero;
  69. private float m_linearMotorDecayTimescale = 0;
  70. private float m_linearMotorTimescale = 0;
  71. private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
  72. private Vector3 m_lastPositionVector = Vector3.Zero;
  73. // private bool m_LinearMotorSetLastFrame = false;
  74. // private Vector3 m_linearMotorOffset = Vector3.Zero;
  75. //Angular properties
  76. private BSVMotor m_angularMotor = new BSVMotor("AngularMotor");
  77. private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
  78. // private int m_angularMotorApply = 0; // application frame counter
  79. private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
  80. private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
  81. private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
  82. private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
  83. private Vector3 m_lastAngularVelocity = Vector3.Zero;
  84. private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
  85. //Deflection properties
  86. private BSVMotor m_angularDeflectionMotor = new BSVMotor("AngularDeflection");
  87. private float m_angularDeflectionEfficiency = 0;
  88. private float m_angularDeflectionTimescale = 0;
  89. private float m_linearDeflectionEfficiency = 0;
  90. private float m_linearDeflectionTimescale = 0;
  91. //Banking properties
  92. private float m_bankingEfficiency = 0;
  93. private float m_bankingMix = 0;
  94. private float m_bankingTimescale = 0;
  95. //Hover and Buoyancy properties
  96. private BSVMotor m_hoverMotor = new BSVMotor("Hover");
  97. private float m_VhoverHeight = 0f;
  98. private float m_VhoverEfficiency = 0f;
  99. private float m_VhoverTimescale = 0f;
  100. private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
  101. // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
  102. private float m_VehicleBuoyancy = 0f;
  103. private Vector3 m_VehicleGravity = Vector3.Zero; // Gravity computed when buoyancy set
  104. //Attractor properties
  105. private BSVMotor m_verticalAttractionMotor = new BSVMotor("VerticalAttraction");
  106. private float m_verticalAttractionEfficiency = 1.0f; // damped
  107. private float m_verticalAttractionCutoff = 500f; // per the documentation
  108. // Timescale > cutoff means no vert attractor.
  109. private float m_verticalAttractionTimescale = 510f;
  110. // Just some recomputed constants:
  111. static readonly float PIOverFour = ((float)Math.PI) / 4f;
  112. static readonly float PIOverTwo = ((float)Math.PI) / 2f;
  113. public BSDynamics(BSScene myScene, BSPrim myPrim, string actorName)
  114. : base(myScene, myPrim, actorName)
  115. {
  116. Type = Vehicle.TYPE_NONE;
  117. m_haveRegisteredForSceneEvents = false;
  118. ControllingPrim = myPrim as BSPrimLinkable;
  119. if (ControllingPrim == null)
  120. {
  121. // THIS CANNOT HAPPEN!!
  122. }
  123. VDetailLog("{0},Creation", ControllingPrim.LocalID);
  124. }
  125. // Return 'true' if this vehicle is doing vehicle things
  126. public bool IsActive
  127. {
  128. get { return (Type != Vehicle.TYPE_NONE && ControllingPrim.IsPhysicallyActive); }
  129. }
  130. // Return 'true' if this a vehicle that should be sitting on the ground
  131. public bool IsGroundVehicle
  132. {
  133. get { return (Type == Vehicle.TYPE_CAR || Type == Vehicle.TYPE_SLED); }
  134. }
  135. #region Vehicle parameter setting
  136. public void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
  137. {
  138. VDetailLog("{0},ProcessFloatVehicleParam,param={1},val={2}", ControllingPrim.LocalID, pParam, pValue);
  139. switch (pParam)
  140. {
  141. case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
  142. m_angularDeflectionEfficiency = ClampInRange(0f, pValue, 1f);
  143. break;
  144. case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
  145. m_angularDeflectionTimescale = Math.Max(pValue, 0.01f);
  146. break;
  147. case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
  148. m_angularMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
  149. m_angularMotor.TargetValueDecayTimeScale = m_angularMotorDecayTimescale;
  150. break;
  151. case Vehicle.ANGULAR_MOTOR_TIMESCALE:
  152. m_angularMotorTimescale = Math.Max(pValue, 0.01f);
  153. m_angularMotor.TimeScale = m_angularMotorTimescale;
  154. break;
  155. case Vehicle.BANKING_EFFICIENCY:
  156. m_bankingEfficiency = ClampInRange(-1f, pValue, 1f);
  157. break;
  158. case Vehicle.BANKING_MIX:
  159. m_bankingMix = Math.Max(pValue, 0.01f);
  160. break;
  161. case Vehicle.BANKING_TIMESCALE:
  162. m_bankingTimescale = Math.Max(pValue, 0.01f);
  163. break;
  164. case Vehicle.BUOYANCY:
  165. m_VehicleBuoyancy = ClampInRange(-1f, pValue, 1f);
  166. m_VehicleGravity = ControllingPrim.ComputeGravity(m_VehicleBuoyancy);
  167. break;
  168. case Vehicle.HOVER_EFFICIENCY:
  169. m_VhoverEfficiency = ClampInRange(0f, pValue, 1f);
  170. break;
  171. case Vehicle.HOVER_HEIGHT:
  172. m_VhoverHeight = pValue;
  173. break;
  174. case Vehicle.HOVER_TIMESCALE:
  175. m_VhoverTimescale = Math.Max(pValue, 0.01f);
  176. break;
  177. case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
  178. m_linearDeflectionEfficiency = ClampInRange(0f, pValue, 1f);
  179. break;
  180. case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
  181. m_linearDeflectionTimescale = Math.Max(pValue, 0.01f);
  182. break;
  183. case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
  184. m_linearMotorDecayTimescale = ClampInRange(0.01f, pValue, 120);
  185. m_linearMotor.TargetValueDecayTimeScale = m_linearMotorDecayTimescale;
  186. break;
  187. case Vehicle.LINEAR_MOTOR_TIMESCALE:
  188. m_linearMotorTimescale = Math.Max(pValue, 0.01f);
  189. m_linearMotor.TimeScale = m_linearMotorTimescale;
  190. break;
  191. case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
  192. m_verticalAttractionEfficiency = ClampInRange(0.1f, pValue, 1f);
  193. m_verticalAttractionMotor.Efficiency = m_verticalAttractionEfficiency;
  194. break;
  195. case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
  196. m_verticalAttractionTimescale = Math.Max(pValue, 0.01f);
  197. m_verticalAttractionMotor.TimeScale = m_verticalAttractionTimescale;
  198. break;
  199. // These are vector properties but the engine lets you use a single float value to
  200. // set all of the components to the same value
  201. case Vehicle.ANGULAR_FRICTION_TIMESCALE:
  202. m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
  203. break;
  204. case Vehicle.ANGULAR_MOTOR_DIRECTION:
  205. m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
  206. m_angularMotor.Zero();
  207. m_angularMotor.SetTarget(m_angularMotorDirection);
  208. break;
  209. case Vehicle.LINEAR_FRICTION_TIMESCALE:
  210. m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
  211. break;
  212. case Vehicle.LINEAR_MOTOR_DIRECTION:
  213. m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
  214. m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
  215. m_linearMotor.SetTarget(m_linearMotorDirection);
  216. break;
  217. case Vehicle.LINEAR_MOTOR_OFFSET:
  218. m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
  219. break;
  220. }
  221. }//end ProcessFloatVehicleParam
  222. internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
  223. {
  224. VDetailLog("{0},ProcessVectorVehicleParam,param={1},val={2}", ControllingPrim.LocalID, pParam, pValue);
  225. switch (pParam)
  226. {
  227. case Vehicle.ANGULAR_FRICTION_TIMESCALE:
  228. m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
  229. break;
  230. case Vehicle.ANGULAR_MOTOR_DIRECTION:
  231. // Limit requested angular speed to 2 rps= 4 pi rads/sec
  232. pValue.X = ClampInRange(-12.56f, pValue.X, 12.56f);
  233. pValue.Y = ClampInRange(-12.56f, pValue.Y, 12.56f);
  234. pValue.Z = ClampInRange(-12.56f, pValue.Z, 12.56f);
  235. m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
  236. m_angularMotor.Zero();
  237. m_angularMotor.SetTarget(m_angularMotorDirection);
  238. break;
  239. case Vehicle.LINEAR_FRICTION_TIMESCALE:
  240. m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
  241. break;
  242. case Vehicle.LINEAR_MOTOR_DIRECTION:
  243. m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
  244. m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
  245. m_linearMotor.SetTarget(m_linearMotorDirection);
  246. break;
  247. case Vehicle.LINEAR_MOTOR_OFFSET:
  248. m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
  249. break;
  250. case Vehicle.BLOCK_EXIT:
  251. m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
  252. break;
  253. }
  254. }//end ProcessVectorVehicleParam
  255. internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
  256. {
  257. VDetailLog("{0},ProcessRotationalVehicleParam,param={1},val={2}", ControllingPrim.LocalID, pParam, pValue);
  258. switch (pParam)
  259. {
  260. case Vehicle.REFERENCE_FRAME:
  261. m_referenceFrame = pValue;
  262. break;
  263. case Vehicle.ROLL_FRAME:
  264. m_RollreferenceFrame = pValue;
  265. break;
  266. }
  267. }//end ProcessRotationVehicleParam
  268. internal void ProcessVehicleFlags(int pParam, bool remove)
  269. {
  270. VDetailLog("{0},ProcessVehicleFlags,param={1},remove={2}", ControllingPrim.LocalID, pParam, remove);
  271. VehicleFlag parm = (VehicleFlag)pParam;
  272. if (pParam == -1)
  273. m_flags = (VehicleFlag)0;
  274. else
  275. {
  276. if (remove)
  277. m_flags &= ~parm;
  278. else
  279. m_flags |= parm;
  280. }
  281. }
  282. public void ProcessTypeChange(Vehicle pType)
  283. {
  284. VDetailLog("{0},ProcessTypeChange,type={1}", ControllingPrim.LocalID, pType);
  285. // Set Defaults For Type
  286. Type = pType;
  287. switch (pType)
  288. {
  289. case Vehicle.TYPE_NONE:
  290. m_linearMotorDirection = Vector3.Zero;
  291. m_linearMotorTimescale = 0;
  292. m_linearMotorDecayTimescale = 0;
  293. m_linearFrictionTimescale = new Vector3(0, 0, 0);
  294. m_angularMotorDirection = Vector3.Zero;
  295. m_angularMotorDecayTimescale = 0;
  296. m_angularMotorTimescale = 0;
  297. m_angularFrictionTimescale = new Vector3(0, 0, 0);
  298. m_VhoverHeight = 0;
  299. m_VhoverEfficiency = 0;
  300. m_VhoverTimescale = 0;
  301. m_VehicleBuoyancy = 0;
  302. m_linearDeflectionEfficiency = 1;
  303. m_linearDeflectionTimescale = 1;
  304. m_angularDeflectionEfficiency = 0;
  305. m_angularDeflectionTimescale = 1000;
  306. m_verticalAttractionEfficiency = 0;
  307. m_verticalAttractionTimescale = 0;
  308. m_bankingEfficiency = 0;
  309. m_bankingTimescale = 1000;
  310. m_bankingMix = 1;
  311. m_referenceFrame = Quaternion.Identity;
  312. m_flags = (VehicleFlag)0;
  313. break;
  314. case Vehicle.TYPE_SLED:
  315. m_linearMotorDirection = Vector3.Zero;
  316. m_linearMotorTimescale = 1000;
  317. m_linearMotorDecayTimescale = 120;
  318. m_linearFrictionTimescale = new Vector3(30, 1, 1000);
  319. m_angularMotorDirection = Vector3.Zero;
  320. m_angularMotorTimescale = 1000;
  321. m_angularMotorDecayTimescale = 120;
  322. m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
  323. m_VhoverHeight = 0;
  324. m_VhoverEfficiency = 10; // TODO: this looks wrong!!
  325. m_VhoverTimescale = 10;
  326. m_VehicleBuoyancy = 0;
  327. m_linearDeflectionEfficiency = 1;
  328. m_linearDeflectionTimescale = 1;
  329. m_angularDeflectionEfficiency = 1;
  330. m_angularDeflectionTimescale = 1000;
  331. m_verticalAttractionEfficiency = 0;
  332. m_verticalAttractionTimescale = 0;
  333. m_bankingEfficiency = 0;
  334. m_bankingTimescale = 10;
  335. m_bankingMix = 1;
  336. m_referenceFrame = Quaternion.Identity;
  337. m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
  338. | VehicleFlag.HOVER_TERRAIN_ONLY
  339. | VehicleFlag.HOVER_GLOBAL_HEIGHT
  340. | VehicleFlag.HOVER_UP_ONLY);
  341. m_flags |= (VehicleFlag.NO_DEFLECTION_UP
  342. | VehicleFlag.LIMIT_ROLL_ONLY
  343. | VehicleFlag.LIMIT_MOTOR_UP);
  344. break;
  345. case Vehicle.TYPE_CAR:
  346. m_linearMotorDirection = Vector3.Zero;
  347. m_linearMotorTimescale = 1;
  348. m_linearMotorDecayTimescale = 60;
  349. m_linearFrictionTimescale = new Vector3(100, 2, 1000);
  350. m_angularMotorDirection = Vector3.Zero;
  351. m_angularMotorTimescale = 1;
  352. m_angularMotorDecayTimescale = 0.8f;
  353. m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
  354. m_VhoverHeight = 0;
  355. m_VhoverEfficiency = 0;
  356. m_VhoverTimescale = 1000;
  357. m_VehicleBuoyancy = 0;
  358. m_linearDeflectionEfficiency = 1;
  359. m_linearDeflectionTimescale = 2;
  360. m_angularDeflectionEfficiency = 0;
  361. m_angularDeflectionTimescale = 10;
  362. m_verticalAttractionEfficiency = 1f;
  363. m_verticalAttractionTimescale = 10f;
  364. m_bankingEfficiency = -0.2f;
  365. m_bankingMix = 1;
  366. m_bankingTimescale = 1;
  367. m_referenceFrame = Quaternion.Identity;
  368. m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
  369. | VehicleFlag.HOVER_TERRAIN_ONLY
  370. | VehicleFlag.HOVER_GLOBAL_HEIGHT);
  371. m_flags |= (VehicleFlag.NO_DEFLECTION_UP
  372. | VehicleFlag.LIMIT_ROLL_ONLY
  373. | VehicleFlag.LIMIT_MOTOR_UP
  374. | VehicleFlag.HOVER_UP_ONLY);
  375. break;
  376. case Vehicle.TYPE_BOAT:
  377. m_linearMotorDirection = Vector3.Zero;
  378. m_linearMotorTimescale = 5;
  379. m_linearMotorDecayTimescale = 60;
  380. m_linearFrictionTimescale = new Vector3(10, 3, 2);
  381. m_angularMotorDirection = Vector3.Zero;
  382. m_angularMotorTimescale = 4;
  383. m_angularMotorDecayTimescale = 4;
  384. m_angularFrictionTimescale = new Vector3(10,10,10);
  385. m_VhoverHeight = 0;
  386. m_VhoverEfficiency = 0.5f;
  387. m_VhoverTimescale = 2;
  388. m_VehicleBuoyancy = 1;
  389. m_linearDeflectionEfficiency = 0.5f;
  390. m_linearDeflectionTimescale = 3;
  391. m_angularDeflectionEfficiency = 0.5f;
  392. m_angularDeflectionTimescale = 5;
  393. m_verticalAttractionEfficiency = 0.5f;
  394. m_verticalAttractionTimescale = 5f;
  395. m_bankingEfficiency = -0.3f;
  396. m_bankingMix = 0.8f;
  397. m_bankingTimescale = 1;
  398. m_referenceFrame = Quaternion.Identity;
  399. m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY
  400. | VehicleFlag.HOVER_GLOBAL_HEIGHT
  401. | VehicleFlag.LIMIT_ROLL_ONLY
  402. | VehicleFlag.HOVER_UP_ONLY);
  403. m_flags |= (VehicleFlag.NO_DEFLECTION_UP
  404. | VehicleFlag.LIMIT_MOTOR_UP
  405. | VehicleFlag.HOVER_WATER_ONLY);
  406. break;
  407. case Vehicle.TYPE_AIRPLANE:
  408. m_linearMotorDirection = Vector3.Zero;
  409. m_linearMotorTimescale = 2;
  410. m_linearMotorDecayTimescale = 60;
  411. m_linearFrictionTimescale = new Vector3(200, 10, 5);
  412. m_angularMotorDirection = Vector3.Zero;
  413. m_angularMotorTimescale = 4;
  414. m_angularMotorDecayTimescale = 4;
  415. m_angularFrictionTimescale = new Vector3(20, 20, 20);
  416. m_VhoverHeight = 0;
  417. m_VhoverEfficiency = 0.5f;
  418. m_VhoverTimescale = 1000;
  419. m_VehicleBuoyancy = 0;
  420. m_linearDeflectionEfficiency = 0.5f;
  421. m_linearDeflectionTimescale = 3;
  422. m_angularDeflectionEfficiency = 1;
  423. m_angularDeflectionTimescale = 2;
  424. m_verticalAttractionEfficiency = 0.9f;
  425. m_verticalAttractionTimescale = 2f;
  426. m_bankingEfficiency = 1;
  427. m_bankingMix = 0.7f;
  428. m_bankingTimescale = 2;
  429. m_referenceFrame = Quaternion.Identity;
  430. m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
  431. | VehicleFlag.HOVER_TERRAIN_ONLY
  432. | VehicleFlag.HOVER_GLOBAL_HEIGHT
  433. | VehicleFlag.HOVER_UP_ONLY
  434. | VehicleFlag.NO_DEFLECTION_UP
  435. | VehicleFlag.LIMIT_MOTOR_UP);
  436. m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
  437. break;
  438. case Vehicle.TYPE_BALLOON:
  439. m_linearMotorDirection = Vector3.Zero;
  440. m_linearMotorTimescale = 5;
  441. m_linearFrictionTimescale = new Vector3(5, 5, 5);
  442. m_linearMotorDecayTimescale = 60;
  443. m_angularMotorDirection = Vector3.Zero;
  444. m_angularMotorTimescale = 6;
  445. m_angularFrictionTimescale = new Vector3(10, 10, 10);
  446. m_angularMotorDecayTimescale = 10;
  447. m_VhoverHeight = 5;
  448. m_VhoverEfficiency = 0.8f;
  449. m_VhoverTimescale = 10;
  450. m_VehicleBuoyancy = 1;
  451. m_linearDeflectionEfficiency = 0;
  452. m_linearDeflectionTimescale = 5;
  453. m_angularDeflectionEfficiency = 0;
  454. m_angularDeflectionTimescale = 5;
  455. m_verticalAttractionEfficiency = 1f;
  456. m_verticalAttractionTimescale = 100f;
  457. m_bankingEfficiency = 0;
  458. m_bankingMix = 0.7f;
  459. m_bankingTimescale = 5;
  460. m_referenceFrame = Quaternion.Identity;
  461. m_referenceFrame = Quaternion.Identity;
  462. m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY
  463. | VehicleFlag.HOVER_TERRAIN_ONLY
  464. | VehicleFlag.HOVER_UP_ONLY
  465. | VehicleFlag.NO_DEFLECTION_UP
  466. | VehicleFlag.LIMIT_MOTOR_UP);
  467. m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY
  468. | VehicleFlag.HOVER_GLOBAL_HEIGHT);
  469. break;
  470. }
  471. m_linearMotor = new BSVMotor("LinearMotor", m_linearMotorTimescale, m_linearMotorDecayTimescale, 1f);
  472. // m_linearMotor.PhysicsScene = m_physicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
  473. m_angularMotor = new BSVMotor("AngularMotor", m_angularMotorTimescale, m_angularMotorDecayTimescale, 1f);
  474. // m_angularMotor.PhysicsScene = m_physicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
  475. /* Not implemented
  476. m_verticalAttractionMotor = new BSVMotor("VerticalAttraction", m_verticalAttractionTimescale,
  477. BSMotor.Infinite, BSMotor.InfiniteVector,
  478. m_verticalAttractionEfficiency);
  479. // Z goes away and we keep X and Y
  480. m_verticalAttractionMotor.PhysicsScene = PhysicsScene; // DEBUG DEBUG DEBUG (enables detail logging)
  481. */
  482. if (this.Type == Vehicle.TYPE_NONE)
  483. {
  484. UnregisterForSceneEvents();
  485. }
  486. else
  487. {
  488. RegisterForSceneEvents();
  489. }
  490. // Update any physical parameters based on this type.
  491. Refresh();
  492. }
  493. #endregion // Vehicle parameter setting
  494. // BSActor.Refresh()
  495. public override void Refresh()
  496. {
  497. // If asking for a refresh, reset the physical parameters before the next simulation step.
  498. // Called whether active or not since the active state may be updated before the next step.
  499. m_physicsScene.PostTaintObject("BSDynamics.Refresh", ControllingPrim.LocalID, delegate()
  500. {
  501. SetPhysicalParameters();
  502. });
  503. }
  504. // Some of the properties of this prim may have changed.
  505. // Do any updating needed for a vehicle
  506. private void SetPhysicalParameters()
  507. {
  508. if (IsActive)
  509. {
  510. // Remember the mass so we don't have to fetch it every step
  511. m_vehicleMass = ControllingPrim.TotalMass;
  512. // Friction affects are handled by this vehicle code
  513. // m_physicsScene.PE.SetFriction(ControllingPrim.PhysBody, BSParam.VehicleFriction);
  514. // m_physicsScene.PE.SetRestitution(ControllingPrim.PhysBody, BSParam.VehicleRestitution);
  515. ControllingPrim.Linkset.SetPhysicalFriction(BSParam.VehicleFriction);
  516. ControllingPrim.Linkset.SetPhysicalRestitution(BSParam.VehicleRestitution);
  517. // Moderate angular movement introduced by Bullet.
  518. // TODO: possibly set AngularFactor and LinearFactor for the type of vehicle.
  519. // Maybe compute linear and angular factor and damping from params.
  520. m_physicsScene.PE.SetAngularDamping(ControllingPrim.PhysBody, BSParam.VehicleAngularDamping);
  521. m_physicsScene.PE.SetLinearFactor(ControllingPrim.PhysBody, BSParam.VehicleLinearFactor);
  522. m_physicsScene.PE.SetAngularFactorV(ControllingPrim.PhysBody, BSParam.VehicleAngularFactor);
  523. // Vehicles report collision events so we know when it's on the ground
  524. // m_physicsScene.PE.AddToCollisionFlags(ControllingPrim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
  525. ControllingPrim.Linkset.AddToPhysicalCollisionFlags(CollisionFlags.BS_VEHICLE_COLLISIONS);
  526. // Vector3 inertia = m_physicsScene.PE.CalculateLocalInertia(ControllingPrim.PhysShape.physShapeInfo, m_vehicleMass);
  527. // ControllingPrim.Inertia = inertia * BSParam.VehicleInertiaFactor;
  528. // m_physicsScene.PE.SetMassProps(ControllingPrim.PhysBody, m_vehicleMass, ControllingPrim.Inertia);
  529. // m_physicsScene.PE.UpdateInertiaTensor(ControllingPrim.PhysBody);
  530. ControllingPrim.Linkset.ComputeAndSetLocalInertia(BSParam.VehicleInertiaFactor, m_vehicleMass);
  531. // Set the gravity for the vehicle depending on the buoyancy
  532. // TODO: what should be done if prim and vehicle buoyancy differ?
  533. m_VehicleGravity = ControllingPrim.ComputeGravity(m_VehicleBuoyancy);
  534. // The actual vehicle gravity is set to zero in Bullet so we can do all the application of same.
  535. // m_physicsScene.PE.SetGravity(ControllingPrim.PhysBody, Vector3.Zero);
  536. ControllingPrim.Linkset.SetPhysicalGravity(Vector3.Zero);
  537. VDetailLog("{0},BSDynamics.SetPhysicalParameters,mass={1},inert={2},vehGrav={3},aDamp={4},frict={5},rest={6},lFact={7},aFact={8}",
  538. ControllingPrim.LocalID, m_vehicleMass, ControllingPrim.Inertia, m_VehicleGravity,
  539. BSParam.VehicleAngularDamping, BSParam.VehicleFriction, BSParam.VehicleRestitution,
  540. BSParam.VehicleLinearFactor, BSParam.VehicleAngularFactor
  541. );
  542. }
  543. else
  544. {
  545. if (ControllingPrim.PhysBody.HasPhysicalBody)
  546. m_physicsScene.PE.RemoveFromCollisionFlags(ControllingPrim.PhysBody, CollisionFlags.BS_VEHICLE_COLLISIONS);
  547. // ControllingPrim.Linkset.RemoveFromPhysicalCollisionFlags(CollisionFlags.BS_VEHICLE_COLLISIONS);
  548. }
  549. }
  550. // BSActor.RemoveBodyDependencies
  551. public override void RemoveDependencies()
  552. {
  553. Refresh();
  554. }
  555. // BSActor.Release()
  556. public override void Dispose()
  557. {
  558. VDetailLog("{0},Dispose", ControllingPrim.LocalID);
  559. UnregisterForSceneEvents();
  560. Type = Vehicle.TYPE_NONE;
  561. Enabled = false;
  562. return;
  563. }
  564. private void RegisterForSceneEvents()
  565. {
  566. if (!m_haveRegisteredForSceneEvents)
  567. {
  568. m_physicsScene.BeforeStep += this.Step;
  569. m_physicsScene.AfterStep += this.PostStep;
  570. ControllingPrim.OnPreUpdateProperty += this.PreUpdateProperty;
  571. m_haveRegisteredForSceneEvents = true;
  572. }
  573. }
  574. private void UnregisterForSceneEvents()
  575. {
  576. if (m_haveRegisteredForSceneEvents)
  577. {
  578. m_physicsScene.BeforeStep -= this.Step;
  579. m_physicsScene.AfterStep -= this.PostStep;
  580. ControllingPrim.OnPreUpdateProperty -= this.PreUpdateProperty;
  581. m_haveRegisteredForSceneEvents = false;
  582. }
  583. }
  584. private void PreUpdateProperty(ref EntityProperties entprop)
  585. {
  586. // A temporary kludge to suppress the rotational effects introduced on vehicles by Bullet
  587. // TODO: handle physics introduced by Bullet with computed vehicle physics.
  588. if (IsActive)
  589. {
  590. entprop.RotationalVelocity = Vector3.Zero;
  591. }
  592. }
  593. #region Known vehicle value functions
  594. // Vehicle physical parameters that we buffer from constant getting and setting.
  595. // The "m_known*" values are unknown until they are fetched and the m_knownHas flag is set.
  596. // Changing is remembered and the parameter is stored back into the physics engine only if updated.
  597. // This does two things: 1) saves continuious calls into unmanaged code, and
  598. // 2) signals when a physics property update must happen back to the simulator
  599. // to update values modified for the vehicle.
  600. private int m_knownChanged;
  601. private int m_knownHas;
  602. private float m_knownTerrainHeight;
  603. private float m_knownWaterLevel;
  604. private Vector3 m_knownPosition;
  605. private Vector3 m_knownVelocity;
  606. private Vector3 m_knownForce;
  607. private Vector3 m_knownForceImpulse;
  608. private Quaternion m_knownOrientation;
  609. private Vector3 m_knownRotationalVelocity;
  610. private Vector3 m_knownRotationalForce;
  611. private Vector3 m_knownRotationalImpulse;
  612. private const int m_knownChangedPosition = 1 << 0;
  613. private const int m_knownChangedVelocity = 1 << 1;
  614. private const int m_knownChangedForce = 1 << 2;
  615. private const int m_knownChangedForceImpulse = 1 << 3;
  616. private const int m_knownChangedOrientation = 1 << 4;
  617. private const int m_knownChangedRotationalVelocity = 1 << 5;
  618. private const int m_knownChangedRotationalForce = 1 << 6;
  619. private const int m_knownChangedRotationalImpulse = 1 << 7;
  620. private const int m_knownChangedTerrainHeight = 1 << 8;
  621. private const int m_knownChangedWaterLevel = 1 << 9;
  622. public void ForgetKnownVehicleProperties()
  623. {
  624. m_knownHas = 0;
  625. m_knownChanged = 0;
  626. }
  627. // Push all the changed values back into the physics engine
  628. public void PushKnownChanged()
  629. {
  630. if (m_knownChanged != 0)
  631. {
  632. if ((m_knownChanged & m_knownChangedPosition) != 0)
  633. ControllingPrim.ForcePosition = m_knownPosition;
  634. if ((m_knownChanged & m_knownChangedOrientation) != 0)
  635. ControllingPrim.ForceOrientation = m_knownOrientation;
  636. if ((m_knownChanged & m_knownChangedVelocity) != 0)
  637. {
  638. ControllingPrim.ForceVelocity = m_knownVelocity;
  639. // Fake out Bullet by making it think the velocity is the same as last time.
  640. // Bullet does a bunch of smoothing for changing parameters.
  641. // Since the vehicle is demanding this setting, we override Bullet's smoothing
  642. // by telling Bullet the value was the same last time.
  643. // PhysicsScene.PE.SetInterpolationLinearVelocity(Prim.PhysBody, m_knownVelocity);
  644. }
  645. if ((m_knownChanged & m_knownChangedForce) != 0)
  646. ControllingPrim.AddForce((Vector3)m_knownForce, false /*pushForce*/, true /*inTaintTime*/);
  647. if ((m_knownChanged & m_knownChangedForceImpulse) != 0)
  648. ControllingPrim.AddForceImpulse((Vector3)m_knownForceImpulse, false /*pushforce*/, true /*inTaintTime*/);
  649. if ((m_knownChanged & m_knownChangedRotationalVelocity) != 0)
  650. {
  651. ControllingPrim.ForceRotationalVelocity = m_knownRotationalVelocity;
  652. // PhysicsScene.PE.SetInterpolationAngularVelocity(Prim.PhysBody, m_knownRotationalVelocity);
  653. }
  654. if ((m_knownChanged & m_knownChangedRotationalImpulse) != 0)
  655. ControllingPrim.ApplyTorqueImpulse((Vector3)m_knownRotationalImpulse, true /*inTaintTime*/);
  656. if ((m_knownChanged & m_knownChangedRotationalForce) != 0)
  657. {
  658. ControllingPrim.AddAngularForce((Vector3)m_knownRotationalForce, false /*pushForce*/, true /*inTaintTime*/);
  659. }
  660. // If we set one of the values (ie, the physics engine didn't do it) we must force
  661. // an UpdateProperties event to send the changes up to the simulator.
  662. m_physicsScene.PE.PushUpdate(ControllingPrim.PhysBody);
  663. }
  664. m_knownChanged = 0;
  665. }
  666. // Since the computation of terrain height can be a little involved, this routine
  667. // is used to fetch the height only once for each vehicle simulation step.
  668. Vector3 lastRememberedHeightPos = new Vector3(-1, -1, -1);
  669. private float GetTerrainHeight(Vector3 pos)
  670. {
  671. if ((m_knownHas & m_knownChangedTerrainHeight) == 0 || pos != lastRememberedHeightPos)
  672. {
  673. lastRememberedHeightPos = pos;
  674. m_knownTerrainHeight = ControllingPrim.PhysScene.TerrainManager.GetTerrainHeightAtXYZ(pos);
  675. m_knownHas |= m_knownChangedTerrainHeight;
  676. }
  677. return m_knownTerrainHeight;
  678. }
  679. // Since the computation of water level can be a little involved, this routine
  680. // is used ot fetch the level only once for each vehicle simulation step.
  681. Vector3 lastRememberedWaterHeightPos = new Vector3(-1, -1, -1);
  682. private float GetWaterLevel(Vector3 pos)
  683. {
  684. if ((m_knownHas & m_knownChangedWaterLevel) == 0 || pos != lastRememberedWaterHeightPos)
  685. {
  686. lastRememberedWaterHeightPos = pos;
  687. m_knownWaterLevel = ControllingPrim.PhysScene.TerrainManager.GetWaterLevelAtXYZ(pos);
  688. m_knownHas |= m_knownChangedWaterLevel;
  689. }
  690. return m_knownWaterLevel;
  691. }
  692. private Vector3 VehiclePosition
  693. {
  694. get
  695. {
  696. if ((m_knownHas & m_knownChangedPosition) == 0)
  697. {
  698. m_knownPosition = ControllingPrim.ForcePosition;
  699. m_knownHas |= m_knownChangedPosition;
  700. }
  701. return m_knownPosition;
  702. }
  703. set
  704. {
  705. m_knownPosition = value;
  706. m_knownChanged |= m_knownChangedPosition;
  707. m_knownHas |= m_knownChangedPosition;
  708. }
  709. }
  710. private Quaternion VehicleOrientation
  711. {
  712. get
  713. {
  714. if ((m_knownHas & m_knownChangedOrientation) == 0)
  715. {
  716. m_knownOrientation = ControllingPrim.ForceOrientation;
  717. m_knownHas |= m_knownChangedOrientation;
  718. }
  719. return m_knownOrientation;
  720. }
  721. set
  722. {
  723. m_knownOrientation = value;
  724. m_knownChanged |= m_knownChangedOrientation;
  725. m_knownHas |= m_knownChangedOrientation;
  726. }
  727. }
  728. private Vector3 VehicleVelocity
  729. {
  730. get
  731. {
  732. if ((m_knownHas & m_knownChangedVelocity) == 0)
  733. {
  734. m_knownVelocity = ControllingPrim.ForceVelocity;
  735. m_knownHas |= m_knownChangedVelocity;
  736. }
  737. return m_knownVelocity;
  738. }
  739. set
  740. {
  741. m_knownVelocity = value;
  742. m_knownChanged |= m_knownChangedVelocity;
  743. m_knownHas |= m_knownChangedVelocity;
  744. }
  745. }
  746. private void VehicleAddForce(Vector3 pForce)
  747. {
  748. if ((m_knownHas & m_knownChangedForce) == 0)
  749. {
  750. m_knownForce = Vector3.Zero;
  751. m_knownHas |= m_knownChangedForce;
  752. }
  753. m_knownForce += pForce;
  754. m_knownChanged |= m_knownChangedForce;
  755. }
  756. private void VehicleAddForceImpulse(Vector3 pImpulse)
  757. {
  758. if ((m_knownHas & m_knownChangedForceImpulse) == 0)
  759. {
  760. m_knownForceImpulse = Vector3.Zero;
  761. m_knownHas |= m_knownChangedForceImpulse;
  762. }
  763. m_knownForceImpulse += pImpulse;
  764. m_knownChanged |= m_knownChangedForceImpulse;
  765. }
  766. private Vector3 VehicleRotationalVelocity
  767. {
  768. get
  769. {
  770. if ((m_knownHas & m_knownChangedRotationalVelocity) == 0)
  771. {
  772. m_knownRotationalVelocity = ControllingPrim.ForceRotationalVelocity;
  773. m_knownHas |= m_knownChangedRotationalVelocity;
  774. }
  775. return (Vector3)m_knownRotationalVelocity;
  776. }
  777. set
  778. {
  779. m_knownRotationalVelocity = value;
  780. m_knownChanged |= m_knownChangedRotationalVelocity;
  781. m_knownHas |= m_knownChangedRotationalVelocity;
  782. }
  783. }
  784. private void VehicleAddAngularForce(Vector3 aForce)
  785. {
  786. if ((m_knownHas & m_knownChangedRotationalForce) == 0)
  787. {
  788. m_knownRotationalForce = Vector3.Zero;
  789. }
  790. m_knownRotationalForce += aForce;
  791. m_knownChanged |= m_knownChangedRotationalForce;
  792. m_knownHas |= m_knownChangedRotationalForce;
  793. }
  794. private void VehicleAddRotationalImpulse(Vector3 pImpulse)
  795. {
  796. if ((m_knownHas & m_knownChangedRotationalImpulse) == 0)
  797. {
  798. m_knownRotationalImpulse = Vector3.Zero;
  799. m_knownHas |= m_knownChangedRotationalImpulse;
  800. }
  801. m_knownRotationalImpulse += pImpulse;
  802. m_knownChanged |= m_knownChangedRotationalImpulse;
  803. }
  804. // Vehicle relative forward velocity
  805. private Vector3 VehicleForwardVelocity
  806. {
  807. get
  808. {
  809. return VehicleVelocity * Quaternion.Inverse(Quaternion.Normalize(VehicleOrientation));
  810. }
  811. }
  812. private float VehicleForwardSpeed
  813. {
  814. get
  815. {
  816. return VehicleForwardVelocity.X;
  817. }
  818. }
  819. #endregion // Known vehicle value functions
  820. // One step of the vehicle properties for the next 'pTimestep' seconds.
  821. internal void Step(float pTimestep)
  822. {
  823. if (!IsActive) return;
  824. ForgetKnownVehicleProperties();
  825. MoveLinear(pTimestep);
  826. MoveAngular(pTimestep);
  827. LimitRotation(pTimestep);
  828. // remember the position so next step we can limit absolute movement effects
  829. m_lastPositionVector = VehiclePosition;
  830. // If we forced the changing of some vehicle parameters, update the values and
  831. // for the physics engine to note the changes so an UpdateProperties event will happen.
  832. PushKnownChanged();
  833. if (m_physicsScene.VehiclePhysicalLoggingEnabled)
  834. m_physicsScene.PE.DumpRigidBody(m_physicsScene.World, ControllingPrim.PhysBody);
  835. VDetailLog("{0},BSDynamics.Step,done,pos={1}, force={2},velocity={3},angvel={4}",
  836. ControllingPrim.LocalID, VehiclePosition, m_knownForce, VehicleVelocity, VehicleRotationalVelocity);
  837. }
  838. // Called after the simulation step
  839. internal void PostStep(float pTimestep)
  840. {
  841. if (!IsActive) return;
  842. if (m_physicsScene.VehiclePhysicalLoggingEnabled)
  843. m_physicsScene.PE.DumpRigidBody(m_physicsScene.World, ControllingPrim.PhysBody);
  844. }
  845. // Apply the effect of the linear motor and other linear motions (like hover and float).
  846. private void MoveLinear(float pTimestep)
  847. {
  848. ComputeLinearVelocity(pTimestep);
  849. ComputeLinearDeflection(pTimestep);
  850. ComputeLinearTerrainHeightCorrection(pTimestep);
  851. ComputeLinearHover(pTimestep);
  852. ComputeLinearBlockingEndPoint(pTimestep);
  853. ComputeLinearMotorUp(pTimestep);
  854. ApplyGravity(pTimestep);
  855. // If not changing some axis, reduce out velocity
  856. if ((m_flags & (VehicleFlag.NO_X | VehicleFlag.NO_Y | VehicleFlag.NO_Z)) != 0)
  857. {
  858. Vector3 vel = VehicleVelocity;
  859. if ((m_flags & (VehicleFlag.NO_X)) != 0)
  860. {
  861. vel.X = 0;
  862. }
  863. if ((m_flags & (VehicleFlag.NO_Y)) != 0)
  864. {
  865. vel.Y = 0;
  866. }
  867. if ((m_flags & (VehicleFlag.NO_Z)) != 0)
  868. {
  869. vel.Z = 0;
  870. }
  871. VehicleVelocity = vel;
  872. }
  873. // ==================================================================
  874. // Clamp high or low velocities
  875. float newVelocityLengthSq = VehicleVelocity.LengthSquared();
  876. if (newVelocityLengthSq > BSParam.VehicleMaxLinearVelocitySquared)
  877. {
  878. Vector3 origVelW = VehicleVelocity; // DEBUG DEBUG
  879. VehicleVelocity /= VehicleVelocity.Length();
  880. VehicleVelocity *= BSParam.VehicleMaxLinearVelocity;
  881. VDetailLog("{0}, MoveLinear,clampMax,origVelW={1},lenSq={2},maxVelSq={3},,newVelW={4}",
  882. ControllingPrim.LocalID, origVelW, newVelocityLengthSq, BSParam.VehicleMaxLinearVelocitySquared, VehicleVelocity);
  883. }
  884. else if (newVelocityLengthSq < 0.001f)
  885. VehicleVelocity = Vector3.Zero;
  886. VDetailLog("{0}, MoveLinear,done,isColl={1},newVel={2}", ControllingPrim.LocalID, ControllingPrim.HasSomeCollision, VehicleVelocity );
  887. } // end MoveLinear()
  888. public void ComputeLinearVelocity(float pTimestep)
  889. {
  890. // Step the motor from the current value. Get the correction needed this step.
  891. Vector3 origVelW = VehicleVelocity; // DEBUG
  892. Vector3 currentVelV = VehicleForwardVelocity;
  893. Vector3 linearMotorCorrectionV = m_linearMotor.Step(pTimestep, currentVelV);
  894. // Friction reduces vehicle motion based on absolute speed. Slow vehicle down by friction.
  895. Vector3 frictionFactorV = ComputeFrictionFactor(m_linearFrictionTimescale, pTimestep);
  896. linearMotorCorrectionV -= (currentVelV * frictionFactorV);
  897. // Motor is vehicle coordinates. Rotate it to world coordinates
  898. Vector3 linearMotorVelocityW = linearMotorCorrectionV * VehicleOrientation;
  899. // If we're a ground vehicle, don't add any upward Z movement
  900. if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != 0)
  901. {
  902. if (linearMotorVelocityW.Z > 0f)
  903. linearMotorVelocityW.Z = 0f;
  904. }
  905. // Add this correction to the velocity to make it faster/slower.
  906. VehicleVelocity += linearMotorVelocityW;
  907. VDetailLog("{0}, MoveLinear,velocity,origVelW={1},velV={2},tgt={3},correctV={4},correctW={5},newVelW={6},fricFact={7}",
  908. ControllingPrim.LocalID, origVelW, currentVelV, m_linearMotor.TargetValue, linearMotorCorrectionV,
  909. linearMotorVelocityW, VehicleVelocity, frictionFactorV);
  910. }
  911. //Given a Deflection Effiency and a Velocity, Returns a Velocity that is Partially Deflected onto the X Axis
  912. //Clamped so that a DeflectionTimescale of less then 1 does not increase force over original velocity
  913. private void ComputeLinearDeflection(float pTimestep)
  914. {
  915. Vector3 linearDeflectionV = Vector3.Zero;
  916. Vector3 velocityV = VehicleForwardVelocity;
  917. if (BSParam.VehicleEnableLinearDeflection)
  918. {
  919. // Velocity in Y and Z dimensions is movement to the side or turning.
  920. // Compute deflection factor from the to the side and rotational velocity
  921. linearDeflectionV.Y = SortedClampInRange(0, (velocityV.Y * m_linearDeflectionEfficiency) / m_linearDeflectionTimescale, velocityV.Y);
  922. linearDeflectionV.Z = SortedClampInRange(0, (velocityV.Z * m_linearDeflectionEfficiency) / m_linearDeflectionTimescale, velocityV.Z);
  923. // Velocity to the side and around is corrected and moved into the forward direction
  924. linearDeflectionV.X += Math.Abs(linearDeflectionV.Y);
  925. linearDeflectionV.X += Math.Abs(linearDeflectionV.Z);
  926. // Scale the deflection to the fractional simulation time
  927. linearDeflectionV *= pTimestep;
  928. // Subtract the sideways and rotational velocity deflection factors while adding the correction forward
  929. linearDeflectionV *= new Vector3(1, -1, -1);
  930. // Correction is vehicle relative. Convert to world coordinates.
  931. Vector3 linearDeflectionW = linearDeflectionV * VehicleOrientation;
  932. // Optionally, if not colliding, don't effect world downward velocity. Let falling things fall.
  933. if (BSParam.VehicleLinearDeflectionNotCollidingNoZ && !m_controllingPrim.HasSomeCollision)
  934. {
  935. linearDeflectionW.Z = 0f;
  936. }
  937. VehicleVelocity += linearDeflectionW;
  938. VDetailLog("{0}, MoveLinear,LinearDeflection,linDefEff={1},linDefTS={2},linDeflectionV={3}",
  939. ControllingPrim.LocalID, m_linearDeflectionEfficiency, m_linearDeflectionTimescale, linearDeflectionV);
  940. }
  941. }
  942. public void ComputeLinearTerrainHeightCorrection(float pTimestep)
  943. {
  944. // If below the terrain, move us above the ground a little.
  945. // TODO: Consider taking the rotated size of the object or possibly casting a ray.
  946. if (VehiclePosition.Z < GetTerrainHeight(VehiclePosition))
  947. {
  948. // Force position because applying force won't get the vehicle through the terrain
  949. Vector3 newPosition = VehiclePosition;
  950. newPosition.Z = GetTerrainHeight(VehiclePosition) + 1f;
  951. VehiclePosition = newPosition;
  952. VDetailLog("{0}, MoveLinear,terrainHeight,terrainHeight={1},pos={2}",
  953. ControllingPrim.LocalID, GetTerrainHeight(VehiclePosition), VehiclePosition);
  954. }
  955. }
  956. public void ComputeLinearHover(float pTimestep)
  957. {
  958. // m_VhoverEfficiency: 0=bouncy, 1=totally damped
  959. // m_VhoverTimescale: time to achieve height
  960. if ((m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
  961. {
  962. // We should hover, get the target height
  963. if ((m_flags & VehicleFlag.HOVER_WATER_ONLY) != 0)
  964. {
  965. m_VhoverTargetHeight = GetWaterLevel(VehiclePosition) + m_VhoverHeight;
  966. }
  967. if ((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
  968. {
  969. m_VhoverTargetHeight = GetTerrainHeight(VehiclePosition) + m_VhoverHeight;
  970. }
  971. if ((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
  972. {
  973. m_VhoverTargetHeight = m_VhoverHeight;
  974. }
  975. if ((m_flags & VehicleFlag.HOVER_UP_ONLY) != 0)
  976. {
  977. // If body is already heigher, use its height as target height
  978. if (VehiclePosition.Z > m_VhoverTargetHeight)
  979. m_VhoverTargetHeight = VehiclePosition.Z;
  980. }
  981. if ((m_flags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
  982. {
  983. if (Math.Abs(VehiclePosition.Z - m_VhoverTargetHeight) > 0.2f)
  984. {
  985. Vector3 pos = VehiclePosition;
  986. pos.Z = m_VhoverTargetHeight;
  987. VehiclePosition = pos;
  988. VDetailLog("{0}, MoveLinear,hover,pos={1},lockHoverHeight", ControllingPrim.LocalID, pos);
  989. }
  990. }
  991. else
  992. {
  993. // Error is positive if below the target and negative if above.
  994. Vector3 hpos = VehiclePosition;
  995. float verticalError = m_VhoverTargetHeight - hpos.Z;
  996. float verticalCorrection = verticalError / m_VhoverTimescale;
  997. verticalCorrection *= m_VhoverEfficiency;
  998. hpos.Z += verticalCorrection;
  999. VehiclePosition = hpos;
  1000. // Since we are hovering, we need to do the opposite of falling -- get rid of world Z
  1001. Vector3 vel = VehicleVelocity;
  1002. vel.Z = 0f;
  1003. VehicleVelocity = vel;
  1004. /*
  1005. float verticalCorrectionVelocity = verticalError / m_VhoverTimescale;
  1006. Vector3 verticalCorrection = new Vector3(0f, 0f, verticalCorrectionVelocity);
  1007. verticalCorrection *= m_vehicleMass;
  1008. // TODO: implement m_VhoverEfficiency correctly
  1009. VehicleAddForceImpulse(verticalCorrection);
  1010. */
  1011. VDetailLog("{0}, MoveLinear,hover,pos={1},eff={2},hoverTS={3},height={4},target={5},err={6},corr={7}",
  1012. ControllingPrim.LocalID, VehiclePosition, m_VhoverEfficiency,
  1013. m_VhoverTimescale, m_VhoverHeight, m_VhoverTargetHeight,
  1014. verticalError, verticalCorrection);
  1015. }
  1016. }
  1017. }
  1018. public bool ComputeLinearBlockingEndPoint(float pTimestep)
  1019. {
  1020. bool changed = false;
  1021. Vector3 pos = VehiclePosition;
  1022. Vector3 posChange = pos - m_lastPositionVector;
  1023. if (m_BlockingEndPoint != Vector3.Zero)
  1024. {
  1025. if (pos.X >= (m_BlockingEndPoint.X - (float)1))
  1026. {
  1027. pos.X -= posChange.X + 1;
  1028. changed = true;
  1029. }
  1030. if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
  1031. {
  1032. pos.Y -= posChange.Y + 1;
  1033. changed = true;
  1034. }
  1035. if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
  1036. {
  1037. pos.Z -= posChange.Z + 1;
  1038. changed = true;
  1039. }
  1040. if (pos.X <= 0)
  1041. {
  1042. pos.X += posChange.X + 1;
  1043. changed = true;
  1044. }
  1045. if (pos.Y <= 0)
  1046. {
  1047. pos.Y += posChange.Y + 1;
  1048. changed = true;
  1049. }
  1050. if (changed)
  1051. {
  1052. VehiclePosition = pos;
  1053. VDetailLog("{0}, MoveLinear,blockingEndPoint,block={1},origPos={2},pos={3}",
  1054. ControllingPrim.LocalID, m_BlockingEndPoint, posChange, pos);
  1055. }
  1056. }
  1057. return changed;
  1058. }
  1059. // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
  1060. // Prevent ground vehicles from motoring into the sky. This flag has a subtle effect when
  1061. // used with conjunction with banking: the strength of the banking will decay when the
  1062. // vehicle no longer experiences collisions. The decay timescale is the same as
  1063. // VEHICLE_BANKING_TIMESCALE. This is to help prevent ground vehicles from steering
  1064. // when they are in mid jump.
  1065. // TODO: this code is wrong. Also, what should it do for boats (height from water)?
  1066. // This is just using the ground and a general collision check. Should really be using
  1067. // a downward raycast to find what is below.
  1068. public void ComputeLinearMotorUp(float pTimestep)
  1069. {
  1070. if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
  1071. {
  1072. // This code tries to decide if the object is not on the ground and then pushing down
  1073. /*
  1074. float targetHeight = Type == Vehicle.TYPE_BOAT ? GetWaterLevel(VehiclePosition) : GetTerrainHeight(VehiclePosition);
  1075. distanceAboveGround = VehiclePosition.Z - targetHeight;
  1076. // Not colliding if the vehicle is off the ground
  1077. if (!Prim.HasSomeCollision)
  1078. {
  1079. // downForce = new Vector3(0, 0, -distanceAboveGround / m_bankingTimescale);
  1080. VehicleVelocity += new Vector3(0, 0, -distanceAboveGround);
  1081. }
  1082. // TODO: this calculation is wrong. From the description at
  1083. // (http://wiki.secondlife.com/wiki/Category:LSL_Vehicle), the downForce
  1084. // has a decay factor. This says this force should
  1085. // be computed with a motor.
  1086. // TODO: add interaction with banking.
  1087. VDetailLog("{0}, MoveLinear,limitMotorUp,distAbove={1},colliding={2},ret={3}",
  1088. Prim.LocalID, distanceAboveGround, Prim.HasSomeCollision, ret);
  1089. */
  1090. // Another approach is to measure if we're going up. If going up and not colliding,
  1091. // the vehicle is in the air. Fix that by pushing down.
  1092. if (!ControllingPrim.HasSomeCollision && VehicleVelocity.Z > 0.1)
  1093. {
  1094. // Get rid of any of the velocity vector that is pushing us up.
  1095. float upVelocity = VehicleVelocity.Z;
  1096. VehicleVelocity += new Vector3(0, 0, -upVelocity);
  1097. /*
  1098. // If we're pointed up into the air, we should nose down
  1099. Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
  1100. // The rotation around the Y axis is pitch up or down
  1101. if (pointingDirection.Y > 0.01f)
  1102. {
  1103. float angularCorrectionForce = -(float)Math.Asin(pointingDirection.Y);
  1104. Vector3 angularCorrectionVector = new Vector3(0f, angularCorrectionForce, 0f);
  1105. // Rotate into world coordinates and apply to vehicle
  1106. angularCorrectionVector *= VehicleOrientation;
  1107. VehicleAddAngularForce(angularCorrectionVector);
  1108. VDetailLog("{0}, MoveLinear,limitMotorUp,newVel={1},pntDir={2},corrFrc={3},aCorr={4}",
  1109. Prim.LocalID, VehicleVelocity, pointingDirection, angularCorrectionForce, angularCorrectionVector);
  1110. }
  1111. */
  1112. VDetailLog("{0}, MoveLinear,limitMotorUp,collide={1},upVel={2},newVel={3}",
  1113. ControllingPrim.LocalID, ControllingPrim.HasSomeCollision, upVelocity, VehicleVelocity);
  1114. }
  1115. }
  1116. }
  1117. private void ApplyGravity(float pTimeStep)
  1118. {
  1119. Vector3 appliedGravity = m_VehicleGravity * m_vehicleMass;
  1120. // Hack to reduce downward force if the vehicle is probably sitting on the ground
  1121. if (ControllingPrim.HasSomeCollision && IsGroundVehicle)
  1122. appliedGravity *= BSParam.VehicleGroundGravityFudge;
  1123. VehicleAddForce(appliedGravity);
  1124. VDetailLog("{0}, MoveLinear,applyGravity,vehGrav={1},collid={2},fudge={3},mass={4},appliedForce={5}",
  1125. ControllingPrim.LocalID, m_VehicleGravity,
  1126. ControllingPrim.HasSomeCollision, BSParam.VehicleGroundGravityFudge, m_vehicleMass, appliedGravity);
  1127. }
  1128. // =======================================================================
  1129. // =======================================================================
  1130. // Apply the effect of the angular motor.
  1131. // The 'contribution' is how much angular correction velocity each function wants.
  1132. // All the contributions are added together and the resulting velocity is
  1133. // set directly on the vehicle.
  1134. private void MoveAngular(float pTimestep)
  1135. {
  1136. ComputeAngularTurning(pTimestep);
  1137. ComputeAngularVerticalAttraction();
  1138. ComputeAngularDeflection();
  1139. ComputeAngularBanking();
  1140. // ==================================================================
  1141. if (VehicleRotationalVelocity.ApproxEquals(Vector3.Zero, 0.0001f))
  1142. {
  1143. // The vehicle is not adding anything angular wise.
  1144. VehicleRotationalVelocity = Vector3.Zero;
  1145. VDetailLog("{0}, MoveAngular,done,zero", ControllingPrim.LocalID);
  1146. }
  1147. else
  1148. {
  1149. VDetailLog("{0}, MoveAngular,done,nonZero,angVel={1}", ControllingPrim.LocalID, VehicleRotationalVelocity);
  1150. }
  1151. // ==================================================================
  1152. //Offset section
  1153. if (m_linearMotorOffset != Vector3.Zero)
  1154. {
  1155. //Offset of linear velocity doesn't change the linear velocity,
  1156. // but causes a torque to be applied, for example...
  1157. //
  1158. // IIIII >>> IIIII
  1159. // IIIII >>> IIIII
  1160. // IIIII >>> IIIII
  1161. // ^
  1162. // | Applying a force at the arrow will cause the object to move forward, but also rotate
  1163. //
  1164. //
  1165. // The torque created is the linear velocity crossed with the offset
  1166. // TODO: this computation should be in the linear section
  1167. // because that is where we know the impulse being applied.
  1168. Vector3 torqueFromOffset = Vector3.Zero;
  1169. // torqueFromOffset = Vector3.Cross(m_linearMotorOffset, appliedImpulse);
  1170. if (float.IsNaN(torqueFromOffset.X))
  1171. torqueFromOffset.X = 0;
  1172. if (float.IsNaN(torqueFromOffset.Y))
  1173. torqueFromOffset.Y = 0;
  1174. if (float.IsNaN(torqueFromOffset.Z))
  1175. torqueFromOffset.Z = 0;
  1176. VehicleAddAngularForce(torqueFromOffset * m_vehicleMass);
  1177. VDetailLog("{0}, BSDynamic.MoveAngular,motorOffset,applyTorqueImpulse={1}", ControllingPrim.LocalID, torqueFromOffset);
  1178. }
  1179. }
  1180. private void ComputeAngularTurning(float pTimestep)
  1181. {
  1182. // The user wants this many radians per second angular change?
  1183. Vector3 origVehicleRotationalVelocity = VehicleRotationalVelocity; // DEBUG DEBUG
  1184. Vector3 currentAngularV = VehicleRotationalVelocity * Quaternion.Inverse(VehicleOrientation);
  1185. Vector3 angularMotorContributionV = m_angularMotor.Step(pTimestep, currentAngularV);
  1186. // ==================================================================
  1187. // From http://wiki.secondlife.com/wiki/LlSetVehicleFlags :
  1188. // This flag prevents linear deflection parallel to world z-axis. This is useful
  1189. // for preventing ground vehicles with large linear deflection, like bumper cars,
  1190. // from climbing their linear deflection into the sky.
  1191. // That is, NO_DEFLECTION_UP says angular motion should not add any pitch or roll movement
  1192. // TODO: This is here because this is where ODE put it but documentation says it
  1193. // is a linear effect. Where should this check go?
  1194. //if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
  1195. // {
  1196. // angularMotorContributionV.X = 0f;
  1197. // angularMotorContributionV.Y = 0f;
  1198. // }
  1199. // Reduce any velocity by friction.
  1200. Vector3 frictionFactorW = ComputeFrictionFactor(m_angularFrictionTimescale, pTimestep);
  1201. angularMotorContributionV -= (currentAngularV * frictionFactorW);
  1202. Vector3 angularMotorContributionW = angularMotorContributionV * VehicleOrientation;
  1203. VehicleRotationalVelocity += angularMotorContributionW;
  1204. VDetailLog("{0}, MoveAngular,angularTurning,curAngVelV={1},origVehRotVel={2},vehRotVel={3},frictFact={4}, angContribV={5},angContribW={6}",
  1205. ControllingPrim.LocalID, currentAngularV, origVehicleRotationalVelocity, VehicleRotationalVelocity, frictionFactorW, angularMotorContributionV, angularMotorContributionW);
  1206. }
  1207. // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
  1208. // Some vehicles, like boats, should always keep their up-side up. This can be done by
  1209. // enabling the "vertical attractor" behavior that springs the vehicle's local z-axis to
  1210. // the world z-axis (a.k.a. "up"). To take advantage of this feature you would set the
  1211. // VEHICLE_VERTICAL_ATTRACTION_TIMESCALE to control the period of the spring frequency,
  1212. // and then set the VEHICLE_VERTICAL_ATTRACTION_EFFICIENCY to control the damping. An
  1213. // efficiency of 0.0 will cause the spring to wobble around its equilibrium, while an
  1214. // efficiency of 1.0 will cause the spring to reach its equilibrium with exponential decay.
  1215. public void ComputeAngularVerticalAttraction()
  1216. {
  1217. // If vertical attaction timescale is reasonable
  1218. if (BSParam.VehicleEnableAngularVerticalAttraction && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
  1219. {
  1220. Vector3 vehicleUpAxis = Vector3.UnitZ * VehicleOrientation;
  1221. switch (BSParam.VehicleAngularVerticalAttractionAlgorithm)
  1222. {
  1223. case 0:
  1224. {
  1225. //Another formula to try got from :
  1226. //http://answers.unity3d.com/questions/10425/how-to-stabilize-angular-motion-alignment-of-hover.html
  1227. // Flipping what was originally a timescale into a speed variable and then multiplying it by 2
  1228. // since only computing half the distance between the angles.
  1229. float verticalAttractionSpeed = (1 / m_verticalAttractionTimescale) * 2.0f;
  1230. // Make a prediction of where the up axis will be when this is applied rather then where it is now as
  1231. // this makes for a smoother adjustment and less fighting between the various forces.
  1232. Vector3 predictedUp = vehicleUpAxis * Quaternion.CreateFromAxisAngle(VehicleRotationalVelocity, 0f);
  1233. // This is only half the distance to the target so it will take 2 seconds to complete the turn.
  1234. Vector3 torqueVector = Vector3.Cross(predictedUp, Vector3.UnitZ);
  1235. // Scale vector by our timescale since it is an acceleration it is r/s^2 or radians a timescale squared
  1236. Vector3 vertContributionV = torqueVector * verticalAttractionSpeed * verticalAttractionSpeed;
  1237. VehicleRotationalVelocity += vertContributionV;
  1238. VDetailLog("{0}, MoveAngular,verticalAttraction,vertAttrSpeed={1},upAxis={2},PredictedUp={3},torqueVector={4},contrib={5}",
  1239. ControllingPrim.LocalID,
  1240. verticalAttractionSpeed,
  1241. vehicleUpAxis,
  1242. predictedUp,
  1243. torqueVector,
  1244. vertContributionV);
  1245. break;
  1246. }
  1247. case 1:
  1248. {
  1249. // Possible solution derived from a discussion at:
  1250. // http://stackoverflow.com/questions/14939657/computing-vector-from-quaternion-works-computing-quaternion-from-vector-does-no
  1251. // Create a rotation that is only the vehicle's rotation around Z
  1252. Vector3 currentEulerW = Vector3.Zero;
  1253. VehicleOrientation.GetEulerAngles(out currentEulerW.X, out currentEulerW.Y, out currentEulerW.Z);
  1254. Quaternion justZOrientation = Quaternion.CreateFromAxisAngle(Vector3.UnitZ, currentEulerW.Z);
  1255. // Create the axis that is perpendicular to the up vector and the rotated up vector.
  1256. Vector3 differenceAxisW = Vector3.Cross(Vector3.UnitZ * justZOrientation, Vector3.UnitZ * VehicleOrientation);
  1257. // Compute the angle between those to vectors.
  1258. double differenceAngle = Math.Acos((double)Vector3.Dot(Vector3.UnitZ, Vector3.Normalize(Vector3.UnitZ * VehicleOrientation)));
  1259. // 'differenceAngle' is the angle to rotate and 'differenceAxis' is the plane to rotate in to get the vehicle vertical
  1260. // Reduce the change by the time period it is to change in. Timestep is handled when velocity is applied.
  1261. // TODO: add 'efficiency'.
  1262. // differenceAngle /= m_verticalAttractionTimescale;
  1263. // Create the quaterian representing the correction angle
  1264. Quaternion correctionRotationW = Quaternion.CreateFromAxisAngle(differenceAxisW, (float)differenceAngle);
  1265. // Turn that quaternion into Euler values to make it into velocities to apply.
  1266. Vector3 vertContributionW = Vector3.Zero;
  1267. correctionRotationW.GetEulerAngles(out vertContributionW.X, out vertContributionW.Y, out vertContributionW.Z);
  1268. vertContributionW *= -1f;
  1269. vertContributionW /= m_verticalAttractionTimescale;
  1270. VehicleRotationalVelocity += vertContributionW;
  1271. VDetailLog("{0}, MoveAngular,verticalAttraction,upAxis={1},diffAxis={2},diffAng={3},corrRot={4},contrib={5}",
  1272. ControllingPrim.LocalID,
  1273. vehicleUpAxis,
  1274. differenceAxisW,
  1275. differenceAngle,
  1276. correctionRotationW,
  1277. vertContributionW);
  1278. break;
  1279. }
  1280. case 2:
  1281. {
  1282. Vector3 vertContributionV = Vector3.Zero;
  1283. Vector3 origRotVelW = VehicleRotationalVelocity; // DEBUG DEBUG
  1284. // Take a vector pointing up and convert it from world to vehicle relative coords.
  1285. Vector3 verticalError = Vector3.Normalize(Vector3.UnitZ * VehicleOrientation);
  1286. // If vertical attraction correction is needed, the vector that was pointing up (UnitZ)
  1287. // is now:
  1288. // leaning to one side: rotated around the X axis with the Y value going
  1289. // from zero (nearly straight up) to one (completely to the side)) or
  1290. // leaning front-to-back: rotated around the Y axis with the value of X being between
  1291. // zero and one.
  1292. // The value of Z is how far the rotation is off with 1 meaning none and 0 being 90 degrees.
  1293. // Y error means needed rotation around X axis and visa versa.
  1294. // Since the error goes from zero to one, the asin is the corresponding angle.
  1295. vertContributionV.X = (float)Math.Asin(verticalError.Y);
  1296. // (Tilt forward (positive X) needs to tilt back (rotate negative) around Y axis.)
  1297. vertContributionV.Y = -(float)Math.Asin(verticalError.X);
  1298. // If verticalError.Z is negative, the vehicle is upside down. Add additional push.
  1299. if (verticalError.Z < 0f)
  1300. {
  1301. vertContributionV.X += Math.Sign(vertContributionV.X) * PIOverFour;
  1302. // vertContribution.Y -= PIOverFour;
  1303. }
  1304. // 'vertContrbution' is now the necessary angular correction to correct tilt in one second.
  1305. // Correction happens over a number of seconds.
  1306. Vector3 unscaledContribVerticalErrorV = vertContributionV; // DEBUG DEBUG
  1307. // The correction happens over the user's time period
  1308. vertContributionV /= m_verticalAttractionTimescale;
  1309. // Rotate the vehicle rotation to the world coordinates.
  1310. VehicleRotationalVelocity += (vertContributionV * VehicleOrientation);
  1311. VDetailLog("{0}, MoveAngular,verticalAttraction,,upAxis={1},origRotVW={2},vertError={3},unscaledV={4},eff={5},ts={6},vertContribV={7}",
  1312. ControllingPrim.LocalID,
  1313. vehicleUpAxis,
  1314. origRotVelW,
  1315. verticalError,
  1316. unscaledContribVerticalErrorV,
  1317. m_verticalAttractionEfficiency,
  1318. m_verticalAttractionTimescale,
  1319. vertContributionV);
  1320. break;
  1321. }
  1322. default:
  1323. {
  1324. break;
  1325. }
  1326. }
  1327. }
  1328. }
  1329. // Angular correction to correct the direction the vehicle is pointing to be
  1330. // the direction is should want to be pointing.
  1331. // The vehicle is moving in some direction and correct its orientation to it is pointing
  1332. // in that direction.
  1333. // TODO: implement reference frame.
  1334. public void ComputeAngularDeflection()
  1335. {
  1336. if (BSParam.VehicleEnableAngularDeflection && m_angularDeflectionEfficiency != 0 && VehicleForwardSpeed > 0.2)
  1337. {
  1338. Vector3 deflectContributionV = Vector3.Zero;
  1339. // The direction the vehicle is moving
  1340. Vector3 movingDirection = VehicleVelocity;
  1341. movingDirection.Normalize();
  1342. // If the vehicle is going backward, it is still pointing forward
  1343. movingDirection *= Math.Sign(VehicleForwardSpeed);
  1344. // The direction the vehicle is pointing
  1345. Vector3 pointingDirection = Vector3.UnitX * VehicleOrientation;
  1346. //Predict where the Vehicle will be pointing after AngularVelocity change is applied. This will keep
  1347. // from overshooting and allow this correction to merge with the Vertical Attraction peacefully.
  1348. Vector3 predictedPointingDirection = pointingDirection * Quaternion.CreateFromAxisAngle(VehicleRotationalVelocity, 0f);
  1349. predictedPointingDirection.Normalize();
  1350. // The difference between what is and what should be.
  1351. // Vector3 deflectionError = movingDirection - predictedPointingDirection;
  1352. Vector3 deflectionError = Vector3.Cross(movingDirection, predictedPointingDirection);
  1353. // Don't try to correct very large errors (not our job)
  1354. // if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = PIOverTwo * Math.Sign(deflectionError.X);
  1355. // if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = PIOverTwo * Math.Sign(deflectionError.Y);
  1356. // if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = PIOverTwo * Math.Sign(deflectionError.Z);
  1357. if (Math.Abs(deflectionError.X) > PIOverFour) deflectionError.X = 0f;
  1358. if (Math.Abs(deflectionError.Y) > PIOverFour) deflectionError.Y = 0f;
  1359. if (Math.Abs(deflectionError.Z) > PIOverFour) deflectionError.Z = 0f;
  1360. // ret = m_angularDeflectionCorrectionMotor(1f, deflectionError);
  1361. // Scale the correction by recovery timescale and efficiency
  1362. // Not modeling a spring so clamp the scale to no more then the arc
  1363. deflectContributionV = (-deflectionError) * ClampInRange(0, m_angularDeflectionEfficiency/m_angularDeflectionTimescale,1f);
  1364. //deflectContributionV /= m_angularDeflectionTimescale;
  1365. // VehicleRotationalVelocity += deflectContributionV * VehicleOrientation;
  1366. VehicleRotationalVelocity += deflectContributionV;
  1367. VDetailLog("{0}, MoveAngular,Deflection,movingDir={1},pointingDir={2},deflectError={3},ret={4}",
  1368. ControllingPrim.LocalID, movingDirection, pointingDirection, deflectionError, deflectContributionV);
  1369. VDetailLog("{0}, MoveAngular,Deflection,fwdSpd={1},defEff={2},defTS={3},PredictedPointingDir={4}",
  1370. ControllingPrim.LocalID, VehicleForwardSpeed, m_angularDeflectionEfficiency, m_angularDeflectionTimescale, predictedPointingDirection);
  1371. }
  1372. }
  1373. // Angular change to rotate the vehicle around the Z axis when the vehicle
  1374. // is tipped around the X axis.
  1375. // From http://wiki.secondlife.com/wiki/Linden_Vehicle_Tutorial:
  1376. // The vertical attractor feature must be enabled in order for the banking behavior to
  1377. // function. The way banking works is this: a rotation around the vehicle's roll-axis will
  1378. // produce a angular velocity around the yaw-axis, causing the vehicle to turn. The magnitude
  1379. // of the yaw effect will be proportional to the
  1380. // VEHICLE_BANKING_EFFICIENCY, the angle of the roll rotation, and sometimes the vehicle's
  1381. // velocity along its preferred axis of motion.
  1382. // The VEHICLE_BANKING_EFFICIENCY can vary between -1 and +1. When it is positive then any
  1383. // positive rotation (by the right-hand rule) about the roll-axis will effect a
  1384. // (negative) torque around the yaw-axis, making it turn to the right--that is the
  1385. // vehicle will lean into the turn, which is how real airplanes and motorcycle's work.
  1386. // Negating the banking coefficient will make it so that the vehicle leans to the
  1387. // outside of the turn (not very "physical" but might allow interesting vehicles so why not?).
  1388. // The VEHICLE_BANKING_MIX is a fake (i.e. non-physical) parameter that is useful for making
  1389. // banking vehicles do what you want rather than what the laws of physics allow.
  1390. // For example, consider a real motorcycle...it must be moving forward in order for
  1391. // it to turn while banking, however video-game motorcycles are often configured
  1392. // to turn in place when at a dead stop--because they are often easier to control
  1393. // that way using the limited interface of the keyboard or game controller. The
  1394. // VEHICLE_BANKING_MIX enables combinations of both realistic and non-realistic
  1395. // banking by functioning as a slider between a banking that is correspondingly
  1396. // totally static (0.0) and totally dynamic (1.0). By "static" we mean that the
  1397. // banking effect depends only on the vehicle's rotation about its roll-axis compared
  1398. // to "dynamic" where the banking is also proportional to its velocity along its
  1399. // roll-axis. Finding the best value of the "mixture" will probably require trial and error.
  1400. // The time it takes for the banking behavior to defeat a preexisting angular velocity about the
  1401. // world z-axis is determined by the VEHICLE_BANKING_TIMESCALE. So if you want the vehicle to
  1402. // bank quickly then give it a banking timescale of about a second or less, otherwise you can
  1403. // make a sluggish vehicle by giving it a timescale of several seconds.
  1404. public void ComputeAngularBanking()
  1405. {
  1406. if (BSParam.VehicleEnableAngularBanking && m_bankingEfficiency != 0 && m_verticalAttractionTimescale < m_verticalAttractionCutoff)
  1407. {
  1408. Vector3 bankingContributionV = Vector3.Zero;
  1409. // Rotate a UnitZ vector (pointing up) to how the vehicle is oriented.
  1410. // As the vehicle rolls to the right or left, the Y value will increase from
  1411. // zero (straight up) to 1 or -1 (full tilt right or left)
  1412. Vector3 rollComponents = Vector3.UnitZ * VehicleOrientation;
  1413. // Figure out the yaw value for this much roll.
  1414. float yawAngle = m_angularMotorDirection.X * m_bankingEfficiency;
  1415. // actual error = static turn error + dynamic turn error
  1416. float mixedYawAngle =(yawAngle * (1f - m_bankingMix)) + ((yawAngle * m_bankingMix) * VehicleForwardSpeed);
  1417. // TODO: the banking effect should not go to infinity but what to limit it to?
  1418. // And what should happen when this is being added to a user defined yaw that is already PI*4?
  1419. mixedYawAngle = ClampInRange(-12, mixedYawAngle, 12);
  1420. // Build the force vector to change rotation from what it is to what it should be
  1421. bankingContributionV.Z = -mixedYawAngle;
  1422. // Don't do it all at once. Fudge because 1 second is too fast with most user defined roll as PI*4.
  1423. bankingContributionV /= m_bankingTimescale * BSParam.VehicleAngularBankingTimescaleFudge;
  1424. //VehicleRotationalVelocity += bankingContributionV * VehicleOrientation;
  1425. VehicleRotationalVelocity += bankingContributionV;
  1426. VDetailLog("{0}, MoveAngular,Banking,rollComp={1},speed={2},rollComp={3},yAng={4},mYAng={5},ret={6}",
  1427. ControllingPrim.LocalID, rollComponents, VehicleForwardSpeed, rollComponents, yawAngle, mixedYawAngle, bankingContributionV);
  1428. }
  1429. }
  1430. // This is from previous instantiations of XXXDynamics.cs.
  1431. // Applies roll reference frame.
  1432. // TODO: is this the right way to separate the code to do this operation?
  1433. // Should this be in MoveAngular()?
  1434. internal void LimitRotation(float timestep)
  1435. {
  1436. Quaternion rotq = VehicleOrientation;
  1437. Quaternion m_rot = rotq;
  1438. if (m_RollreferenceFrame != Quaternion.Identity)
  1439. {
  1440. if (rotq.X >= m_RollreferenceFrame.X)
  1441. {
  1442. m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
  1443. }
  1444. if (rotq.Y >= m_RollreferenceFrame.Y)
  1445. {
  1446. m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
  1447. }
  1448. if (rotq.X <= -m_RollreferenceFrame.X)
  1449. {
  1450. m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
  1451. }
  1452. if (rotq.Y <= -m_RollreferenceFrame.Y)
  1453. {
  1454. m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
  1455. }
  1456. }
  1457. if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
  1458. {
  1459. m_rot.X = 0;
  1460. m_rot.Y = 0;
  1461. }
  1462. if (rotq != m_rot)
  1463. {
  1464. VehicleOrientation = m_rot;
  1465. VDetailLog("{0}, LimitRotation,done,orig={1},new={2}", ControllingPrim.LocalID, rotq, m_rot);
  1466. }
  1467. }
  1468. // Given a friction vector (reduction in seconds) and a timestep, return the factor to reduce
  1469. // some value by to apply this friction.
  1470. private Vector3 ComputeFrictionFactor(Vector3 friction, float pTimestep)
  1471. {
  1472. Vector3 frictionFactor = Vector3.Zero;
  1473. if (friction != BSMotor.InfiniteVector)
  1474. {
  1475. // frictionFactor = (Vector3.One / FrictionTimescale) * timeStep;
  1476. // Individual friction components can be 'infinite' so compute each separately.
  1477. frictionFactor.X = (friction.X == BSMotor.Infinite) ? 0f : (1f / friction.X);
  1478. frictionFactor.Y = (friction.Y == BSMotor.Infinite) ? 0f : (1f / friction.Y);
  1479. frictionFactor.Z = (friction.Z == BSMotor.Infinite) ? 0f : (1f / friction.Z);
  1480. frictionFactor *= pTimestep;
  1481. }
  1482. return frictionFactor;
  1483. }
  1484. private float SortedClampInRange(float clampa, float val, float clampb)
  1485. {
  1486. if (clampa > clampb)
  1487. {
  1488. float temp = clampa;
  1489. clampa = clampb;
  1490. clampb = temp;
  1491. }
  1492. return ClampInRange(clampa, val, clampb);
  1493. }
  1494. private float ClampInRange(float low, float val, float high)
  1495. {
  1496. return Math.Max(low, Math.Min(val, high));
  1497. // return Utils.Clamp(val, low, high);
  1498. }
  1499. // Invoke the detailed logger and output something if it's enabled.
  1500. private void VDetailLog(string msg, params Object[] args)
  1501. {
  1502. if (ControllingPrim.PhysScene.VehicleLoggingEnabled)
  1503. ControllingPrim.PhysScene.DetailLog(msg, args);
  1504. }
  1505. }
  1506. }