1
0

ODEDynamics.cs 47 KB

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