ODEDynamics.cs 48 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981
  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. /* Revised Aug, Sept 2009 by Kitto Flora. ODEDynamics.cs replaces
  28. * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
  29. * ODEPrim.cs contains methods dealing with Prim editing, Prim
  30. * characteristics and Kinetic motion.
  31. * ODEDynamics.cs contains methods dealing with Prim Physical motion
  32. * (dynamics) and the associated settings. Old Linear and angular
  33. * motors for dynamic motion have been replace with MoveLinear()
  34. * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
  35. * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
  36. * switch between 'VEHICLE' parameter use and general dynamics
  37. * settings use.
  38. */
  39. using System;
  40. using System.Collections.Generic;
  41. using System.Reflection;
  42. using System.Runtime.InteropServices;
  43. using log4net;
  44. using OpenMetaverse;
  45. using OpenSim.Framework;
  46. using OpenSim.Region.PhysicsModules.SharedBase;
  47. namespace OpenSim.Region.PhysicsModule.ODE
  48. {
  49. public class ODEDynamics
  50. {
  51. public Vehicle Type
  52. {
  53. get { return m_type; }
  54. }
  55. public IntPtr Body
  56. {
  57. get { return m_body; }
  58. }
  59. private int frcount = 0; // Used to limit dynamics debug output to
  60. // every 100th frame
  61. // private OdeScene m_parentScene = null;
  62. private IntPtr m_body = IntPtr.Zero;
  63. // private IntPtr m_jointGroup = IntPtr.Zero;
  64. // private IntPtr m_aMotor = IntPtr.Zero;
  65. // Vehicle properties
  66. private Vehicle m_type = Vehicle.TYPE_NONE; // If a 'VEHICLE', and what kind
  67. // private Quaternion m_referenceFrame = Quaternion.Identity; // Axis modifier
  68. private VehicleFlag m_flags = (VehicleFlag) 0; // Boolean settings:
  69. // HOVER_TERRAIN_ONLY
  70. // HOVER_GLOBAL_HEIGHT
  71. // NO_DEFLECTION_UP
  72. // HOVER_WATER_ONLY
  73. // HOVER_UP_ONLY
  74. // LIMIT_MOTOR_UP
  75. // LIMIT_ROLL_ONLY
  76. private VehicleFlag m_Hoverflags = (VehicleFlag)0;
  77. private Vector3 m_BlockingEndPoint = Vector3.Zero;
  78. private Quaternion m_RollreferenceFrame = Quaternion.Identity;
  79. // Linear properties
  80. private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
  81. private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
  82. private Vector3 m_dir = Vector3.Zero; // velocity applied to body
  83. private Vector3 m_linearFrictionTimescale = Vector3.Zero;
  84. private float m_linearMotorDecayTimescale = 0;
  85. private float m_linearMotorTimescale = 0;
  86. private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
  87. private d.Vector3 m_lastPositionVector = new d.Vector3();
  88. // private bool m_LinearMotorSetLastFrame = false;
  89. // private Vector3 m_linearMotorOffset = Vector3.Zero;
  90. //Angular properties
  91. private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
  92. private int m_angularMotorApply = 0; // application frame counter
  93. private Vector3 m_angularMotorVelocity = Vector3.Zero; // current angular motor velocity
  94. private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
  95. private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
  96. private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
  97. private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
  98. // private Vector3 m_lastVertAttractor = Vector3.Zero; // what VA was last applied to body
  99. //Deflection properties
  100. // private float m_angularDeflectionEfficiency = 0;
  101. // private float m_angularDeflectionTimescale = 0;
  102. // private float m_linearDeflectionEfficiency = 0;
  103. // private float m_linearDeflectionTimescale = 0;
  104. //Banking properties
  105. // private float m_bankingEfficiency = 0;
  106. // private float m_bankingMix = 0;
  107. // private float m_bankingTimescale = 0;
  108. //Hover and Buoyancy properties
  109. private float m_VhoverHeight = 0f;
  110. // private float m_VhoverEfficiency = 0f;
  111. private float m_VhoverTimescale = 0f;
  112. private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
  113. private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
  114. // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
  115. // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
  116. // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
  117. //Attractor properties
  118. private float m_verticalAttractionEfficiency = 1.0f; // damped
  119. private float m_verticalAttractionTimescale = 500f; // Timescale > 300 means no vert attractor.
  120. internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
  121. {
  122. switch (pParam)
  123. {
  124. case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
  125. if (pValue < 0.01f) pValue = 0.01f;
  126. // m_angularDeflectionEfficiency = pValue;
  127. break;
  128. case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
  129. if (pValue < 0.01f) pValue = 0.01f;
  130. // m_angularDeflectionTimescale = pValue;
  131. break;
  132. case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
  133. if (pValue < 0.01f) pValue = 0.01f;
  134. m_angularMotorDecayTimescale = pValue;
  135. break;
  136. case Vehicle.ANGULAR_MOTOR_TIMESCALE:
  137. if (pValue < 0.01f) pValue = 0.01f;
  138. m_angularMotorTimescale = pValue;
  139. break;
  140. case Vehicle.BANKING_EFFICIENCY:
  141. if (pValue < 0.01f) pValue = 0.01f;
  142. // m_bankingEfficiency = pValue;
  143. break;
  144. case Vehicle.BANKING_MIX:
  145. if (pValue < 0.01f) pValue = 0.01f;
  146. // m_bankingMix = pValue;
  147. break;
  148. case Vehicle.BANKING_TIMESCALE:
  149. if (pValue < 0.01f) pValue = 0.01f;
  150. // m_bankingTimescale = pValue;
  151. break;
  152. case Vehicle.BUOYANCY:
  153. if (pValue < -1f) pValue = -1f;
  154. if (pValue > 1f) pValue = 1f;
  155. m_VehicleBuoyancy = pValue;
  156. break;
  157. // case Vehicle.HOVER_EFFICIENCY:
  158. // if (pValue < 0f) pValue = 0f;
  159. // if (pValue > 1f) pValue = 1f;
  160. // m_VhoverEfficiency = pValue;
  161. // break;
  162. case Vehicle.HOVER_HEIGHT:
  163. m_VhoverHeight = pValue;
  164. break;
  165. case Vehicle.HOVER_TIMESCALE:
  166. if (pValue < 0.01f) pValue = 0.01f;
  167. m_VhoverTimescale = pValue;
  168. break;
  169. case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
  170. if (pValue < 0.01f) pValue = 0.01f;
  171. // m_linearDeflectionEfficiency = pValue;
  172. break;
  173. case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
  174. if (pValue < 0.01f) pValue = 0.01f;
  175. // m_linearDeflectionTimescale = pValue;
  176. break;
  177. case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
  178. if (pValue < 0.01f) pValue = 0.01f;
  179. m_linearMotorDecayTimescale = pValue;
  180. break;
  181. case Vehicle.LINEAR_MOTOR_TIMESCALE:
  182. if (pValue < 0.01f) pValue = 0.01f;
  183. m_linearMotorTimescale = pValue;
  184. break;
  185. case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
  186. if (pValue < 0.1f) pValue = 0.1f; // Less goes unstable
  187. if (pValue > 1.0f) pValue = 1.0f;
  188. m_verticalAttractionEfficiency = pValue;
  189. break;
  190. case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
  191. if (pValue < 0.01f) pValue = 0.01f;
  192. m_verticalAttractionTimescale = pValue;
  193. break;
  194. // These are vector properties but the engine lets you use a single float value to
  195. // set all of the components to the same value
  196. case Vehicle.ANGULAR_FRICTION_TIMESCALE:
  197. m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
  198. break;
  199. case Vehicle.ANGULAR_MOTOR_DIRECTION:
  200. m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
  201. m_angularMotorApply = 10;
  202. break;
  203. case Vehicle.LINEAR_FRICTION_TIMESCALE:
  204. m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
  205. break;
  206. case Vehicle.LINEAR_MOTOR_DIRECTION:
  207. m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
  208. m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
  209. break;
  210. case Vehicle.LINEAR_MOTOR_OFFSET:
  211. // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
  212. break;
  213. }
  214. }//end ProcessFloatVehicleParam
  215. internal void ProcessVectorVehicleParam(Vehicle pParam, Vector3 pValue)
  216. {
  217. switch (pParam)
  218. {
  219. case Vehicle.ANGULAR_FRICTION_TIMESCALE:
  220. m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
  221. break;
  222. case Vehicle.ANGULAR_MOTOR_DIRECTION:
  223. m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
  224. // Limit requested angular speed to 2 rps= 4 pi rads/sec
  225. if (m_angularMotorDirection.X > 12.56f) m_angularMotorDirection.X = 12.56f;
  226. if (m_angularMotorDirection.X < - 12.56f) m_angularMotorDirection.X = - 12.56f;
  227. if (m_angularMotorDirection.Y > 12.56f) m_angularMotorDirection.Y = 12.56f;
  228. if (m_angularMotorDirection.Y < - 12.56f) m_angularMotorDirection.Y = - 12.56f;
  229. if (m_angularMotorDirection.Z > 12.56f) m_angularMotorDirection.Z = 12.56f;
  230. if (m_angularMotorDirection.Z < - 12.56f) m_angularMotorDirection.Z = - 12.56f;
  231. m_angularMotorApply = 10;
  232. break;
  233. case Vehicle.LINEAR_FRICTION_TIMESCALE:
  234. m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
  235. break;
  236. case Vehicle.LINEAR_MOTOR_DIRECTION:
  237. m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
  238. m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
  239. break;
  240. case Vehicle.LINEAR_MOTOR_OFFSET:
  241. // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
  242. break;
  243. case Vehicle.BLOCK_EXIT:
  244. m_BlockingEndPoint = new Vector3(pValue.X, pValue.Y, pValue.Z);
  245. break;
  246. }
  247. }//end ProcessVectorVehicleParam
  248. internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
  249. {
  250. switch (pParam)
  251. {
  252. case Vehicle.REFERENCE_FRAME:
  253. // m_referenceFrame = pValue;
  254. break;
  255. case Vehicle.ROLL_FRAME:
  256. m_RollreferenceFrame = pValue;
  257. break;
  258. }
  259. }//end ProcessRotationVehicleParam
  260. internal void ProcessVehicleFlags(int pParam, bool remove)
  261. {
  262. if (remove)
  263. {
  264. if (pParam == -1)
  265. {
  266. m_flags = (VehicleFlag)0;
  267. m_Hoverflags = (VehicleFlag)0;
  268. return;
  269. }
  270. if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT)
  271. {
  272. if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != (VehicleFlag)0)
  273. m_Hoverflags &= ~(VehicleFlag.HOVER_GLOBAL_HEIGHT);
  274. }
  275. if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY)
  276. {
  277. if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != (VehicleFlag)0)
  278. m_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY);
  279. }
  280. if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY)
  281. {
  282. if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != (VehicleFlag)0)
  283. m_Hoverflags &= ~(VehicleFlag.HOVER_UP_ONLY);
  284. }
  285. if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY)
  286. {
  287. if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != (VehicleFlag)0)
  288. m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY);
  289. }
  290. if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP)
  291. {
  292. if ((m_flags & VehicleFlag.LIMIT_MOTOR_UP) != (VehicleFlag)0)
  293. m_flags &= ~(VehicleFlag.LIMIT_MOTOR_UP);
  294. }
  295. if ((pParam & (int)VehicleFlag.LIMIT_ROLL_ONLY) == (int)VehicleFlag.LIMIT_ROLL_ONLY)
  296. {
  297. if ((m_flags & VehicleFlag.LIMIT_ROLL_ONLY) != (VehicleFlag)0)
  298. m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY);
  299. }
  300. if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK)
  301. {
  302. if ((m_flags & VehicleFlag.MOUSELOOK_BANK) != (VehicleFlag)0)
  303. m_flags &= ~(VehicleFlag.MOUSELOOK_BANK);
  304. }
  305. if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER)
  306. {
  307. if ((m_flags & VehicleFlag.MOUSELOOK_STEER) != (VehicleFlag)0)
  308. m_flags &= ~(VehicleFlag.MOUSELOOK_STEER);
  309. }
  310. if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP)
  311. {
  312. if ((m_flags & VehicleFlag.NO_DEFLECTION_UP) != (VehicleFlag)0)
  313. m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP);
  314. }
  315. if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED)
  316. {
  317. if ((m_flags & VehicleFlag.CAMERA_DECOUPLED) != (VehicleFlag)0)
  318. m_flags &= ~(VehicleFlag.CAMERA_DECOUPLED);
  319. }
  320. if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X)
  321. {
  322. if ((m_flags & VehicleFlag.NO_X) != (VehicleFlag)0)
  323. m_flags &= ~(VehicleFlag.NO_X);
  324. }
  325. if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y)
  326. {
  327. if ((m_flags & VehicleFlag.NO_Y) != (VehicleFlag)0)
  328. m_flags &= ~(VehicleFlag.NO_Y);
  329. }
  330. if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z)
  331. {
  332. if ((m_flags & VehicleFlag.NO_Z) != (VehicleFlag)0)
  333. m_flags &= ~(VehicleFlag.NO_Z);
  334. }
  335. if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT)
  336. {
  337. if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != (VehicleFlag)0)
  338. m_Hoverflags &= ~(VehicleFlag.LOCK_HOVER_HEIGHT);
  339. }
  340. if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION)
  341. {
  342. if ((m_flags & VehicleFlag.NO_DEFLECTION) != (VehicleFlag)0)
  343. m_flags &= ~(VehicleFlag.NO_DEFLECTION);
  344. }
  345. if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION)
  346. {
  347. if ((m_flags & VehicleFlag.LOCK_ROTATION) != (VehicleFlag)0)
  348. m_flags &= ~(VehicleFlag.LOCK_ROTATION);
  349. }
  350. }
  351. else
  352. {
  353. if ((pParam & (int)VehicleFlag.HOVER_GLOBAL_HEIGHT) == (int)VehicleFlag.HOVER_GLOBAL_HEIGHT)
  354. {
  355. m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT | m_flags);
  356. }
  357. if ((pParam & (int)VehicleFlag.HOVER_TERRAIN_ONLY) == (int)VehicleFlag.HOVER_TERRAIN_ONLY)
  358. {
  359. m_Hoverflags |= (VehicleFlag.HOVER_TERRAIN_ONLY | m_flags);
  360. }
  361. if ((pParam & (int)VehicleFlag.HOVER_UP_ONLY) == (int)VehicleFlag.HOVER_UP_ONLY)
  362. {
  363. m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY | m_flags);
  364. }
  365. if ((pParam & (int)VehicleFlag.HOVER_WATER_ONLY) == (int)VehicleFlag.HOVER_WATER_ONLY)
  366. {
  367. m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY | m_flags);
  368. }
  369. if ((pParam & (int)VehicleFlag.LIMIT_MOTOR_UP) == (int)VehicleFlag.LIMIT_MOTOR_UP)
  370. {
  371. m_flags |= (VehicleFlag.LIMIT_MOTOR_UP | m_flags);
  372. }
  373. if ((pParam & (int)VehicleFlag.MOUSELOOK_BANK) == (int)VehicleFlag.MOUSELOOK_BANK)
  374. {
  375. m_flags |= (VehicleFlag.MOUSELOOK_BANK | m_flags);
  376. }
  377. if ((pParam & (int)VehicleFlag.MOUSELOOK_STEER) == (int)VehicleFlag.MOUSELOOK_STEER)
  378. {
  379. m_flags |= (VehicleFlag.MOUSELOOK_STEER | m_flags);
  380. }
  381. if ((pParam & (int)VehicleFlag.NO_DEFLECTION_UP) == (int)VehicleFlag.NO_DEFLECTION_UP)
  382. {
  383. m_flags |= (VehicleFlag.NO_DEFLECTION_UP | m_flags);
  384. }
  385. if ((pParam & (int)VehicleFlag.CAMERA_DECOUPLED) == (int)VehicleFlag.CAMERA_DECOUPLED)
  386. {
  387. m_flags |= (VehicleFlag.CAMERA_DECOUPLED | m_flags);
  388. }
  389. if ((pParam & (int)VehicleFlag.NO_X) == (int)VehicleFlag.NO_X)
  390. {
  391. m_flags |= (VehicleFlag.NO_X);
  392. }
  393. if ((pParam & (int)VehicleFlag.NO_Y) == (int)VehicleFlag.NO_Y)
  394. {
  395. m_flags |= (VehicleFlag.NO_Y);
  396. }
  397. if ((pParam & (int)VehicleFlag.NO_Z) == (int)VehicleFlag.NO_Z)
  398. {
  399. m_flags |= (VehicleFlag.NO_Z);
  400. }
  401. if ((pParam & (int)VehicleFlag.LOCK_HOVER_HEIGHT) == (int)VehicleFlag.LOCK_HOVER_HEIGHT)
  402. {
  403. m_Hoverflags |= (VehicleFlag.LOCK_HOVER_HEIGHT);
  404. }
  405. if ((pParam & (int)VehicleFlag.NO_DEFLECTION) == (int)VehicleFlag.NO_DEFLECTION)
  406. {
  407. m_flags |= (VehicleFlag.NO_DEFLECTION);
  408. }
  409. if ((pParam & (int)VehicleFlag.LOCK_ROTATION) == (int)VehicleFlag.LOCK_ROTATION)
  410. {
  411. m_flags |= (VehicleFlag.LOCK_ROTATION);
  412. }
  413. }
  414. }//end ProcessVehicleFlags
  415. internal void ProcessTypeChange(Vehicle pType)
  416. {
  417. // Set Defaults For Type
  418. m_type = pType;
  419. switch (pType)
  420. {
  421. case Vehicle.TYPE_NONE:
  422. m_linearFrictionTimescale = new Vector3(0, 0, 0);
  423. m_angularFrictionTimescale = new Vector3(0, 0, 0);
  424. m_linearMotorDirection = Vector3.Zero;
  425. m_linearMotorTimescale = 0;
  426. m_linearMotorDecayTimescale = 0;
  427. m_angularMotorDirection = Vector3.Zero;
  428. m_angularMotorTimescale = 0;
  429. m_angularMotorDecayTimescale = 0;
  430. m_VhoverHeight = 0;
  431. m_VhoverTimescale = 0;
  432. m_VehicleBuoyancy = 0;
  433. m_flags = (VehicleFlag)0;
  434. break;
  435. case Vehicle.TYPE_SLED:
  436. m_linearFrictionTimescale = new Vector3(30, 1, 1000);
  437. m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
  438. m_linearMotorDirection = Vector3.Zero;
  439. m_linearMotorTimescale = 1000;
  440. m_linearMotorDecayTimescale = 120;
  441. m_angularMotorDirection = Vector3.Zero;
  442. m_angularMotorTimescale = 1000;
  443. m_angularMotorDecayTimescale = 120;
  444. m_VhoverHeight = 0;
  445. // m_VhoverEfficiency = 1;
  446. m_VhoverTimescale = 10;
  447. m_VehicleBuoyancy = 0;
  448. // m_linearDeflectionEfficiency = 1;
  449. // m_linearDeflectionTimescale = 1;
  450. // m_angularDeflectionEfficiency = 1;
  451. // m_angularDeflectionTimescale = 1000;
  452. // m_bankingEfficiency = 0;
  453. // m_bankingMix = 1;
  454. // m_bankingTimescale = 10;
  455. // m_referenceFrame = Quaternion.Identity;
  456. m_Hoverflags &=
  457. ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
  458. VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
  459. m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
  460. break;
  461. case Vehicle.TYPE_CAR:
  462. m_linearFrictionTimescale = new Vector3(100, 2, 1000);
  463. m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
  464. m_linearMotorDirection = Vector3.Zero;
  465. m_linearMotorTimescale = 1;
  466. m_linearMotorDecayTimescale = 60;
  467. m_angularMotorDirection = Vector3.Zero;
  468. m_angularMotorTimescale = 1;
  469. m_angularMotorDecayTimescale = 0.8f;
  470. m_VhoverHeight = 0;
  471. // m_VhoverEfficiency = 0;
  472. m_VhoverTimescale = 1000;
  473. m_VehicleBuoyancy = 0;
  474. // // m_linearDeflectionEfficiency = 1;
  475. // // m_linearDeflectionTimescale = 2;
  476. // // m_angularDeflectionEfficiency = 0;
  477. // m_angularDeflectionTimescale = 10;
  478. m_verticalAttractionEfficiency = 1f;
  479. m_verticalAttractionTimescale = 10f;
  480. // m_bankingEfficiency = -0.2f;
  481. // m_bankingMix = 1;
  482. // m_bankingTimescale = 1;
  483. // m_referenceFrame = Quaternion.Identity;
  484. m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
  485. m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY |
  486. VehicleFlag.LIMIT_MOTOR_UP);
  487. m_Hoverflags |= (VehicleFlag.HOVER_UP_ONLY);
  488. break;
  489. case Vehicle.TYPE_BOAT:
  490. m_linearFrictionTimescale = new Vector3(10, 3, 2);
  491. m_angularFrictionTimescale = new Vector3(10,10,10);
  492. m_linearMotorDirection = Vector3.Zero;
  493. m_linearMotorTimescale = 5;
  494. m_linearMotorDecayTimescale = 60;
  495. m_angularMotorDirection = Vector3.Zero;
  496. m_angularMotorTimescale = 4;
  497. m_angularMotorDecayTimescale = 4;
  498. m_VhoverHeight = 0;
  499. // m_VhoverEfficiency = 0.5f;
  500. m_VhoverTimescale = 2;
  501. m_VehicleBuoyancy = 1;
  502. // m_linearDeflectionEfficiency = 0.5f;
  503. // m_linearDeflectionTimescale = 3;
  504. // m_angularDeflectionEfficiency = 0.5f;
  505. // m_angularDeflectionTimescale = 5;
  506. m_verticalAttractionEfficiency = 0.5f;
  507. m_verticalAttractionTimescale = 5f;
  508. // m_bankingEfficiency = -0.3f;
  509. // m_bankingMix = 0.8f;
  510. // m_bankingTimescale = 1;
  511. // m_referenceFrame = Quaternion.Identity;
  512. m_Hoverflags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY |
  513. VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
  514. m_flags &= ~(VehicleFlag.LIMIT_ROLL_ONLY);
  515. m_flags |= (VehicleFlag.NO_DEFLECTION_UP |
  516. VehicleFlag.LIMIT_MOTOR_UP);
  517. m_Hoverflags |= (VehicleFlag.HOVER_WATER_ONLY);
  518. break;
  519. case Vehicle.TYPE_AIRPLANE:
  520. m_linearFrictionTimescale = new Vector3(200, 10, 5);
  521. m_angularFrictionTimescale = new Vector3(20, 20, 20);
  522. m_linearMotorDirection = Vector3.Zero;
  523. m_linearMotorTimescale = 2;
  524. m_linearMotorDecayTimescale = 60;
  525. m_angularMotorDirection = Vector3.Zero;
  526. m_angularMotorTimescale = 4;
  527. m_angularMotorDecayTimescale = 4;
  528. m_VhoverHeight = 0;
  529. // m_VhoverEfficiency = 0.5f;
  530. m_VhoverTimescale = 1000;
  531. m_VehicleBuoyancy = 0;
  532. // m_linearDeflectionEfficiency = 0.5f;
  533. // m_linearDeflectionTimescale = 3;
  534. // m_angularDeflectionEfficiency = 1;
  535. // m_angularDeflectionTimescale = 2;
  536. m_verticalAttractionEfficiency = 0.9f;
  537. m_verticalAttractionTimescale = 2f;
  538. // m_bankingEfficiency = 1;
  539. // m_bankingMix = 0.7f;
  540. // m_bankingTimescale = 2;
  541. // m_referenceFrame = Quaternion.Identity;
  542. m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
  543. VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
  544. m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP);
  545. m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
  546. break;
  547. case Vehicle.TYPE_BALLOON:
  548. m_linearFrictionTimescale = new Vector3(5, 5, 5);
  549. m_angularFrictionTimescale = new Vector3(10, 10, 10);
  550. m_linearMotorDirection = Vector3.Zero;
  551. m_linearMotorTimescale = 5;
  552. m_linearMotorDecayTimescale = 60;
  553. m_angularMotorDirection = Vector3.Zero;
  554. m_angularMotorTimescale = 6;
  555. m_angularMotorDecayTimescale = 10;
  556. m_VhoverHeight = 5;
  557. // m_VhoverEfficiency = 0.8f;
  558. m_VhoverTimescale = 10;
  559. m_VehicleBuoyancy = 1;
  560. // m_linearDeflectionEfficiency = 0;
  561. // m_linearDeflectionTimescale = 5;
  562. // m_angularDeflectionEfficiency = 0;
  563. // m_angularDeflectionTimescale = 5;
  564. m_verticalAttractionEfficiency = 1f;
  565. m_verticalAttractionTimescale = 100f;
  566. // m_bankingEfficiency = 0;
  567. // m_bankingMix = 0.7f;
  568. // m_bankingTimescale = 5;
  569. // m_referenceFrame = Quaternion.Identity;
  570. m_Hoverflags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
  571. VehicleFlag.HOVER_UP_ONLY);
  572. m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_MOTOR_UP);
  573. m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
  574. m_Hoverflags |= (VehicleFlag.HOVER_GLOBAL_HEIGHT);
  575. break;
  576. }
  577. }//end SetDefaultsForType
  578. internal void Enable(IntPtr pBody, OdeScene pParentScene)
  579. {
  580. if (m_type == Vehicle.TYPE_NONE)
  581. return;
  582. m_body = pBody;
  583. }
  584. internal void Stop()
  585. {
  586. m_lastLinearVelocityVector = Vector3.Zero;
  587. m_lastAngularVelocity = Vector3.Zero;
  588. m_lastPositionVector = d.BodyGetPosition(Body);
  589. }
  590. internal void Step(float pTimestep, OdeScene pParentScene)
  591. {
  592. if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
  593. return;
  594. frcount++; // used to limit debug comment output
  595. if (frcount > 100)
  596. frcount = 0;
  597. MoveLinear(pTimestep, pParentScene);
  598. MoveAngular(pTimestep);
  599. LimitRotation(pTimestep);
  600. }// end Step
  601. private void MoveLinear(float pTimestep, OdeScene _pParentScene)
  602. {
  603. if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
  604. {
  605. if (!d.BodyIsEnabled(Body))
  606. d.BodyEnable(Body);
  607. // add drive to body
  608. Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
  609. m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector?
  610. // This will work temporarily, but we really need to compare speed on an axis
  611. // KF: Limit body velocity to applied velocity?
  612. if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
  613. m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
  614. if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
  615. m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
  616. if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
  617. m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
  618. // decay applied velocity
  619. Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
  620. //Console.WriteLine("decay: " + decayfraction);
  621. m_linearMotorDirection -= m_linearMotorDirection * decayfraction * 0.5f;
  622. //Console.WriteLine("actual: " + m_linearMotorDirection);
  623. }
  624. else
  625. { // requested is not significant
  626. // if what remains of applied is small, zero it.
  627. if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
  628. m_lastLinearVelocityVector = Vector3.Zero;
  629. }
  630. // convert requested object velocity to world-referenced vector
  631. m_dir = m_lastLinearVelocityVector;
  632. d.Quaternion rot = d.BodyGetQuaternion(Body);
  633. Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
  634. m_dir *= rotq; // apply obj rotation to velocity vector
  635. // add Gravity andBuoyancy
  636. // KF: So far I have found no good method to combine a script-requested
  637. // .Z velocity and gravity. Therefore only 0g will used script-requested
  638. // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
  639. Vector3 grav = Vector3.Zero;
  640. // There is some gravity, make a gravity force vector
  641. // that is applied after object velocity.
  642. d.Mass objMass;
  643. d.BodyGetMass(Body, out objMass);
  644. // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
  645. grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
  646. // Preserve the current Z velocity
  647. d.Vector3 vel_now = d.BodyGetLinearVel(Body);
  648. m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
  649. d.Vector3 pos = d.BodyGetPosition(Body);
  650. // Vector3 accel = new Vector3(-(m_dir.X - m_lastLinearVelocityVector.X / 0.1f), -(m_dir.Y - m_lastLinearVelocityVector.Y / 0.1f), m_dir.Z - m_lastLinearVelocityVector.Z / 0.1f);
  651. Vector3 posChange = new Vector3();
  652. posChange.X = pos.X - m_lastPositionVector.X;
  653. posChange.Y = pos.Y - m_lastPositionVector.Y;
  654. posChange.Z = pos.Z - m_lastPositionVector.Z;
  655. double Zchange = Math.Abs(posChange.Z);
  656. if (m_BlockingEndPoint != Vector3.Zero)
  657. {
  658. if (pos.X >= (m_BlockingEndPoint.X - (float)1))
  659. {
  660. pos.X -= posChange.X + 1;
  661. d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
  662. }
  663. if (pos.Y >= (m_BlockingEndPoint.Y - (float)1))
  664. {
  665. pos.Y -= posChange.Y + 1;
  666. d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
  667. }
  668. if (pos.Z >= (m_BlockingEndPoint.Z - (float)1))
  669. {
  670. pos.Z -= posChange.Z + 1;
  671. d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
  672. }
  673. if (pos.X <= 0)
  674. {
  675. pos.X += posChange.X + 1;
  676. d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
  677. }
  678. if (pos.Y <= 0)
  679. {
  680. pos.Y += posChange.Y + 1;
  681. d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
  682. }
  683. }
  684. if (pos.Z < _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y))
  685. {
  686. pos.Z = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + 2;
  687. d.BodySetPosition(Body, pos.X, pos.Y, pos.Z);
  688. }
  689. // Check if hovering
  690. if ((m_Hoverflags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
  691. {
  692. // We should hover, get the target height
  693. if ((m_Hoverflags & VehicleFlag.HOVER_WATER_ONLY) != 0)
  694. {
  695. m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
  696. }
  697. if ((m_Hoverflags & VehicleFlag.HOVER_TERRAIN_ONLY) != 0)
  698. {
  699. m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
  700. }
  701. if ((m_Hoverflags & VehicleFlag.HOVER_GLOBAL_HEIGHT) != 0)
  702. {
  703. m_VhoverTargetHeight = m_VhoverHeight;
  704. }
  705. if ((m_Hoverflags & VehicleFlag.HOVER_UP_ONLY) != 0)
  706. {
  707. // If body is aready heigher, use its height as target height
  708. if (pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
  709. }
  710. if ((m_Hoverflags & VehicleFlag.LOCK_HOVER_HEIGHT) != 0)
  711. {
  712. if ((pos.Z - m_VhoverTargetHeight) > .2 || (pos.Z - m_VhoverTargetHeight) < -.2)
  713. {
  714. d.BodySetPosition(Body, pos.X, pos.Y, m_VhoverTargetHeight);
  715. }
  716. }
  717. else
  718. {
  719. float herr0 = pos.Z - m_VhoverTargetHeight;
  720. // Replace Vertical speed with correction figure if significant
  721. if (Math.Abs(herr0) > 0.01f)
  722. {
  723. m_dir.Z = -((herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
  724. //KF: m_VhoverEfficiency is not yet implemented
  725. }
  726. else
  727. {
  728. m_dir.Z = 0f;
  729. }
  730. }
  731. // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
  732. // m_VhoverTimescale = 0f; // time to acheive height
  733. // pTimestep is time since last frame,in secs
  734. }
  735. if ((m_flags & (VehicleFlag.LIMIT_MOTOR_UP)) != 0)
  736. {
  737. //Start Experimental Values
  738. if (Zchange > .3)
  739. {
  740. grav.Z = (float)(grav.Z * 3);
  741. }
  742. if (Zchange > .15)
  743. {
  744. grav.Z = (float)(grav.Z * 2);
  745. }
  746. if (Zchange > .75)
  747. {
  748. grav.Z = (float)(grav.Z * 1.5);
  749. }
  750. if (Zchange > .05)
  751. {
  752. grav.Z = (float)(grav.Z * 1.25);
  753. }
  754. if (Zchange > .025)
  755. {
  756. grav.Z = (float)(grav.Z * 1.125);
  757. }
  758. float terraintemp = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y);
  759. float postemp = (pos.Z - terraintemp);
  760. if (postemp > 2.5f)
  761. {
  762. grav.Z = (float)(grav.Z * 1.037125);
  763. }
  764. //End Experimental Values
  765. }
  766. if ((m_flags & (VehicleFlag.NO_X)) != 0)
  767. {
  768. m_dir.X = 0;
  769. }
  770. if ((m_flags & (VehicleFlag.NO_Y)) != 0)
  771. {
  772. m_dir.Y = 0;
  773. }
  774. if ((m_flags & (VehicleFlag.NO_Z)) != 0)
  775. {
  776. m_dir.Z = 0;
  777. }
  778. m_lastPositionVector = d.BodyGetPosition(Body);
  779. // Apply velocity
  780. d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
  781. // apply gravity force
  782. d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
  783. // apply friction
  784. Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
  785. m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
  786. } // end MoveLinear()
  787. private void MoveAngular(float pTimestep)
  788. {
  789. /*
  790. private Vector3 m_angularMotorDirection = Vector3.Zero; // angular velocity requested by LSL motor
  791. private int m_angularMotorApply = 0; // application frame counter
  792. private float m_angularMotorVelocity = 0; // current angular motor velocity (ramps up and down)
  793. private float m_angularMotorTimescale = 0; // motor angular velocity ramp up rate
  794. private float m_angularMotorDecayTimescale = 0; // motor angular velocity decay rate
  795. private Vector3 m_angularFrictionTimescale = Vector3.Zero; // body angular velocity decay rate
  796. private Vector3 m_lastAngularVelocity = Vector3.Zero; // what was last applied to body
  797. */
  798. // Get what the body is doing, this includes 'external' influences
  799. d.Vector3 angularVelocity = d.BodyGetAngularVel(Body);
  800. // Vector3 angularVelocity = Vector3.Zero;
  801. if (m_angularMotorApply > 0)
  802. {
  803. // ramp up to new value
  804. // current velocity += error / (time to get there / step interval)
  805. // requested speed - last motor speed
  806. m_angularMotorVelocity.X += (m_angularMotorDirection.X - m_angularMotorVelocity.X) / (m_angularMotorTimescale / pTimestep);
  807. m_angularMotorVelocity.Y += (m_angularMotorDirection.Y - m_angularMotorVelocity.Y) / (m_angularMotorTimescale / pTimestep);
  808. m_angularMotorVelocity.Z += (m_angularMotorDirection.Z - m_angularMotorVelocity.Z) / (m_angularMotorTimescale / pTimestep);
  809. m_angularMotorApply--; // This is done so that if script request rate is less than phys frame rate the expected
  810. // velocity may still be acheived.
  811. }
  812. else
  813. {
  814. // no motor recently applied, keep the body velocity
  815. /* m_angularMotorVelocity.X = angularVelocity.X;
  816. m_angularMotorVelocity.Y = angularVelocity.Y;
  817. m_angularMotorVelocity.Z = angularVelocity.Z; */
  818. // and decay the velocity
  819. m_angularMotorVelocity -= m_angularMotorVelocity / (m_angularMotorDecayTimescale / pTimestep);
  820. } // end motor section
  821. // Vertical attractor section
  822. Vector3 vertattr = Vector3.Zero;
  823. if (m_verticalAttractionTimescale < 300)
  824. {
  825. float VAservo = 0.2f / (m_verticalAttractionTimescale * pTimestep);
  826. // get present body rotation
  827. d.Quaternion rot = d.BodyGetQuaternion(Body);
  828. Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
  829. // make a vector pointing up
  830. Vector3 verterr = Vector3.Zero;
  831. verterr.Z = 1.0f;
  832. // rotate it to Body Angle
  833. verterr = verterr * rotq;
  834. // verterr.X and .Y are the World error ammounts. They are 0 when there is no error (Vehicle Body is 'vertical'), and .Z will be 1.
  835. // As the body leans to its side |.X| will increase to 1 and .Z fall to 0. As body inverts |.X| will fall and .Z will go
  836. // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
  837. if (verterr.Z < 0.0f)
  838. {
  839. verterr.X = 2.0f - verterr.X;
  840. verterr.Y = 2.0f - verterr.Y;
  841. }
  842. // Error is 0 (no error) to +/- 2 (max error)
  843. // scale it by VAservo
  844. verterr = verterr * VAservo;
  845. //if (frcount == 0) Console.WriteLine("VAerr=" + verterr);
  846. // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
  847. // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
  848. vertattr.X = verterr.Y;
  849. vertattr.Y = - verterr.X;
  850. vertattr.Z = 0f;
  851. // scaling appears better usingsquare-law
  852. float bounce = 1.0f - (m_verticalAttractionEfficiency * m_verticalAttractionEfficiency);
  853. vertattr.X += bounce * angularVelocity.X;
  854. vertattr.Y += bounce * angularVelocity.Y;
  855. } // else vertical attractor is off
  856. // m_lastVertAttractor = vertattr;
  857. // Bank section tba
  858. // Deflection section tba
  859. // Sum velocities
  860. m_lastAngularVelocity = m_angularMotorVelocity + vertattr; // + bank + deflection
  861. if ((m_flags & (VehicleFlag.NO_DEFLECTION_UP)) != 0)
  862. {
  863. m_lastAngularVelocity.X = 0;
  864. m_lastAngularVelocity.Y = 0;
  865. }
  866. if (!m_lastAngularVelocity.ApproxEquals(Vector3.Zero, 0.01f))
  867. {
  868. if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
  869. }
  870. else
  871. {
  872. m_lastAngularVelocity = Vector3.Zero; // Reduce small value to zero.
  873. }
  874. // apply friction
  875. Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
  876. m_lastAngularVelocity -= m_lastAngularVelocity * decayamount;
  877. // Apply to the body
  878. d.BodySetAngularVel (Body, m_lastAngularVelocity.X, m_lastAngularVelocity.Y, m_lastAngularVelocity.Z);
  879. } //end MoveAngular
  880. internal void LimitRotation(float timestep)
  881. {
  882. d.Quaternion rot = d.BodyGetQuaternion(Body);
  883. Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
  884. d.Quaternion m_rot = new d.Quaternion();
  885. bool changed = false;
  886. m_rot.X = rotq.X;
  887. m_rot.Y = rotq.Y;
  888. m_rot.Z = rotq.Z;
  889. m_rot.W = rotq.W;
  890. if (m_RollreferenceFrame != Quaternion.Identity)
  891. {
  892. if (rotq.X >= m_RollreferenceFrame.X)
  893. {
  894. m_rot.X = rotq.X - (m_RollreferenceFrame.X / 2);
  895. }
  896. if (rotq.Y >= m_RollreferenceFrame.Y)
  897. {
  898. m_rot.Y = rotq.Y - (m_RollreferenceFrame.Y / 2);
  899. }
  900. if (rotq.X <= -m_RollreferenceFrame.X)
  901. {
  902. m_rot.X = rotq.X + (m_RollreferenceFrame.X / 2);
  903. }
  904. if (rotq.Y <= -m_RollreferenceFrame.Y)
  905. {
  906. m_rot.Y = rotq.Y + (m_RollreferenceFrame.Y / 2);
  907. }
  908. changed = true;
  909. }
  910. if ((m_flags & VehicleFlag.LOCK_ROTATION) != 0)
  911. {
  912. m_rot.X = 0;
  913. m_rot.Y = 0;
  914. changed = true;
  915. }
  916. if (changed)
  917. d.BodySetQuaternion(Body, ref m_rot);
  918. }
  919. }
  920. }