1
0

ODEDynamics.c_comments 30 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630
  1. /*
  2. * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
  3. * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
  4. * ODEPrim.cs contains methods dealing with Prim editing, Prim
  5. * characteristics and Kinetic motion.
  6. * ODEDynamics.cs contains methods dealing with Prim Physical motion
  7. * (dynamics) and the associated settings. Old Linear and angular
  8. * motors for dynamic motion have been replace with MoveLinear()
  9. * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
  10. * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
  11. * switch between 'VEHICLE' parameter use and general dynamics
  12. * settings use.
  13. *
  14. * Copyright (c) Contributors, http://opensimulator.org/
  15. * See CONTRIBUTORS.TXT for a full list of copyright holders.
  16. *
  17. * Redistribution and use in source and binary forms, with or without
  18. * modification, are permitted provided that the following conditions are met:
  19. * * Redistributions of source code must retain the above copyright
  20. * notice, this list of conditions and the following disclaimer.
  21. * * Redistributions in binary form must reproduce the above copyright
  22. * notice, this list of conditions and the following disclaimer in the
  23. * documentation and/or other materials provided with the distribution.
  24. * * Neither the name of the OpenSimulator Project nor the
  25. * names of its contributors may be used to endorse or promote products
  26. * derived from this software without specific prior written permission.
  27. *
  28. * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
  29. * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  30. * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  31. * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
  32. * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  33. * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  34. * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  35. * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  36. * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  37. * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  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. // Linear properties
  78. private Vector3 m_linearMotorDirection = Vector3.Zero; // velocity requested by LSL, decayed by time
  79. private Vector3 m_linearMotorDirectionLASTSET = Vector3.Zero; // velocity requested by LSL
  80. private Vector3 m_dir = Vector3.Zero; // velocity applied to body
  81. private Vector3 m_linearFrictionTimescale = Vector3.Zero;
  82. private float m_linearMotorDecayTimescale = 0;
  83. private float m_linearMotorTimescale = 0;
  84. private Vector3 m_lastLinearVelocityVector = Vector3.Zero;
  85. // private bool m_LinearMotorSetLastFrame = false;
  86. // private Vector3 m_linearMotorOffset = Vector3.Zero;
  87. //Angular properties
  88. private Vector3 m_angularMotorDirection = Vector3.Zero;
  89. private Vector3 m_angularMotorDirectionLASTSET = Vector3.Zero;
  90. private Vector3 m_angularFrictionTimescale = Vector3.Zero;
  91. private float m_angularMotorDecayTimescale = 0;
  92. private float m_angularMotorTimescale = 0;
  93. private Vector3 m_lastAngularVelocityVector = Vector3.Zero;
  94. //Deflection properties
  95. // private float m_angularDeflectionEfficiency = 0;
  96. // private float m_angularDeflectionTimescale = 0;
  97. // private float m_linearDeflectionEfficiency = 0;
  98. // private float m_linearDeflectionTimescale = 0;
  99. //Banking properties
  100. // private float m_bankingEfficiency = 0;
  101. // private float m_bankingMix = 0;
  102. // private float m_bankingTimescale = 0;
  103. //Hover and Buoyancy properties
  104. private float m_VhoverHeight = 0f;
  105. private float m_VhoverEfficiency = 0f;
  106. private float m_VhoverTimescale = 0f;
  107. private float m_VhoverTargetHeight = -1.0f; // if <0 then no hover, else its the current target height
  108. private float m_VehicleBuoyancy = 0f; //KF: m_VehicleBuoyancy is set by VEHICLE_BUOYANCY for a vehicle.
  109. // Modifies gravity. Slider between -1 (double-gravity) and 1 (full anti-gravity)
  110. // KF: So far I have found no good method to combine a script-requested .Z velocity and gravity.
  111. // Therefore only m_VehicleBuoyancy=1 (0g) will use the script-requested .Z velocity.
  112. //Attractor properties
  113. private float m_verticalAttractionEfficiency = 0;
  114. private float m_verticalAttractionTimescale = 0;
  115. internal void ProcessFloatVehicleParam(Vehicle pParam, float pValue)
  116. {
  117. switch (pParam)
  118. {
  119. case Vehicle.ANGULAR_DEFLECTION_EFFICIENCY:
  120. if (pValue < 0.01f) pValue = 0.01f;
  121. // m_angularDeflectionEfficiency = pValue;
  122. break;
  123. case Vehicle.ANGULAR_DEFLECTION_TIMESCALE:
  124. if (pValue < 0.01f) pValue = 0.01f;
  125. // m_angularDeflectionTimescale = pValue;
  126. break;
  127. case Vehicle.ANGULAR_MOTOR_DECAY_TIMESCALE:
  128. if (pValue < 0.01f) pValue = 0.01f;
  129. m_angularMotorDecayTimescale = pValue;
  130. break;
  131. case Vehicle.ANGULAR_MOTOR_TIMESCALE:
  132. if (pValue < 0.01f) pValue = 0.01f;
  133. m_angularMotorTimescale = pValue;
  134. break;
  135. case Vehicle.BANKING_EFFICIENCY:
  136. if (pValue < 0.01f) pValue = 0.01f;
  137. // m_bankingEfficiency = pValue;
  138. break;
  139. case Vehicle.BANKING_MIX:
  140. if (pValue < 0.01f) pValue = 0.01f;
  141. // m_bankingMix = pValue;
  142. break;
  143. case Vehicle.BANKING_TIMESCALE:
  144. if (pValue < 0.01f) pValue = 0.01f;
  145. // m_bankingTimescale = pValue;
  146. break;
  147. case Vehicle.BUOYANCY:
  148. if (pValue < -1f) pValue = -1f;
  149. if (pValue > 1f) pValue = 1f;
  150. m_VehicleBuoyancy = pValue;
  151. break;
  152. case Vehicle.HOVER_EFFICIENCY:
  153. if (pValue < 0f) pValue = 0f;
  154. if (pValue > 1f) pValue = 1f;
  155. m_VhoverEfficiency = pValue;
  156. break;
  157. case Vehicle.HOVER_HEIGHT:
  158. m_VhoverHeight = pValue;
  159. break;
  160. case Vehicle.HOVER_TIMESCALE:
  161. if (pValue < 0.01f) pValue = 0.01f;
  162. m_VhoverTimescale = pValue;
  163. break;
  164. case Vehicle.LINEAR_DEFLECTION_EFFICIENCY:
  165. if (pValue < 0.01f) pValue = 0.01f;
  166. // m_linearDeflectionEfficiency = pValue;
  167. break;
  168. case Vehicle.LINEAR_DEFLECTION_TIMESCALE:
  169. if (pValue < 0.01f) pValue = 0.01f;
  170. // m_linearDeflectionTimescale = pValue;
  171. break;
  172. case Vehicle.LINEAR_MOTOR_DECAY_TIMESCALE:
  173. if (pValue < 0.01f) pValue = 0.01f;
  174. m_linearMotorDecayTimescale = pValue;
  175. break;
  176. case Vehicle.LINEAR_MOTOR_TIMESCALE:
  177. if (pValue < 0.01f) pValue = 0.01f;
  178. m_linearMotorTimescale = pValue;
  179. break;
  180. case Vehicle.VERTICAL_ATTRACTION_EFFICIENCY:
  181. if (pValue < 0.0f) pValue = 0.0f;
  182. if (pValue > 1.0f) pValue = 1.0f;
  183. m_verticalAttractionEfficiency = pValue;
  184. break;
  185. case Vehicle.VERTICAL_ATTRACTION_TIMESCALE:
  186. if (pValue < 0.01f) pValue = 0.01f;
  187. m_verticalAttractionTimescale = pValue;
  188. break;
  189. // These are vector properties but the engine lets you use a single float value to
  190. // set all of the components to the same value
  191. case Vehicle.ANGULAR_FRICTION_TIMESCALE:
  192. m_angularFrictionTimescale = new Vector3(pValue, pValue, pValue);
  193. break;
  194. case Vehicle.ANGULAR_MOTOR_DIRECTION:
  195. m_angularMotorDirection = new Vector3(pValue, pValue, pValue);
  196. m_angularMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
  197. break;
  198. case Vehicle.LINEAR_FRICTION_TIMESCALE:
  199. m_linearFrictionTimescale = new Vector3(pValue, pValue, pValue);
  200. break;
  201. case Vehicle.LINEAR_MOTOR_DIRECTION:
  202. m_linearMotorDirection = new Vector3(pValue, pValue, pValue);
  203. m_linearMotorDirectionLASTSET = new Vector3(pValue, pValue, pValue);
  204. break;
  205. case Vehicle.LINEAR_MOTOR_OFFSET:
  206. // m_linearMotorOffset = new Vector3(pValue, pValue, pValue);
  207. break;
  208. }
  209. }//end ProcessFloatVehicleParam
  210. internal void ProcessVectorVehicleParam(Vehicle pParam, PhysicsVector pValue)
  211. {
  212. switch (pParam)
  213. {
  214. case Vehicle.ANGULAR_FRICTION_TIMESCALE:
  215. m_angularFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
  216. break;
  217. case Vehicle.ANGULAR_MOTOR_DIRECTION:
  218. m_angularMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
  219. m_angularMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
  220. break;
  221. case Vehicle.LINEAR_FRICTION_TIMESCALE:
  222. m_linearFrictionTimescale = new Vector3(pValue.X, pValue.Y, pValue.Z);
  223. break;
  224. case Vehicle.LINEAR_MOTOR_DIRECTION:
  225. m_linearMotorDirection = new Vector3(pValue.X, pValue.Y, pValue.Z);
  226. m_linearMotorDirectionLASTSET = new Vector3(pValue.X, pValue.Y, pValue.Z);
  227. break;
  228. case Vehicle.LINEAR_MOTOR_OFFSET:
  229. // m_linearMotorOffset = new Vector3(pValue.X, pValue.Y, pValue.Z);
  230. break;
  231. }
  232. }//end ProcessVectorVehicleParam
  233. internal void ProcessRotationVehicleParam(Vehicle pParam, Quaternion pValue)
  234. {
  235. switch (pParam)
  236. {
  237. case Vehicle.REFERENCE_FRAME:
  238. // m_referenceFrame = pValue;
  239. break;
  240. }
  241. }//end ProcessRotationVehicleParam
  242. internal void ProcessTypeChange(Vehicle pType)
  243. {
  244. Console.WriteLine("ProcessTypeChange to " + pType);
  245. // Set Defaults For Type
  246. m_type = pType;
  247. switch (pType)
  248. {
  249. case Vehicle.TYPE_SLED:
  250. m_linearFrictionTimescale = new Vector3(30, 1, 1000);
  251. m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
  252. m_linearMotorDirection = Vector3.Zero;
  253. m_linearMotorTimescale = 1000;
  254. m_linearMotorDecayTimescale = 120;
  255. m_angularMotorDirection = Vector3.Zero;
  256. m_angularMotorTimescale = 1000;
  257. m_angularMotorDecayTimescale = 120;
  258. m_VhoverHeight = 0;
  259. m_VhoverEfficiency = 1;
  260. m_VhoverTimescale = 10;
  261. m_VehicleBuoyancy = 0;
  262. // m_linearDeflectionEfficiency = 1;
  263. // m_linearDeflectionTimescale = 1;
  264. // m_angularDeflectionEfficiency = 1;
  265. // m_angularDeflectionTimescale = 1000;
  266. // m_bankingEfficiency = 0;
  267. // m_bankingMix = 1;
  268. // m_bankingTimescale = 10;
  269. // m_referenceFrame = Quaternion.Identity;
  270. m_flags &=
  271. ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
  272. VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
  273. m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
  274. break;
  275. case Vehicle.TYPE_CAR:
  276. m_linearFrictionTimescale = new Vector3(100, 2, 1000);
  277. m_angularFrictionTimescale = new Vector3(1000, 1000, 1000);
  278. m_linearMotorDirection = Vector3.Zero;
  279. m_linearMotorTimescale = 1;
  280. m_linearMotorDecayTimescale = 60;
  281. m_angularMotorDirection = Vector3.Zero;
  282. m_angularMotorTimescale = 1;
  283. m_angularMotorDecayTimescale = 0.8f;
  284. m_VhoverHeight = 0;
  285. m_VhoverEfficiency = 0;
  286. m_VhoverTimescale = 1000;
  287. m_VehicleBuoyancy = 0;
  288. // // m_linearDeflectionEfficiency = 1;
  289. // // m_linearDeflectionTimescale = 2;
  290. // // m_angularDeflectionEfficiency = 0;
  291. // m_angularDeflectionTimescale = 10;
  292. m_verticalAttractionEfficiency = 1;
  293. m_verticalAttractionTimescale = 10;
  294. // m_bankingEfficiency = -0.2f;
  295. // m_bankingMix = 1;
  296. // m_bankingTimescale = 1;
  297. // m_referenceFrame = Quaternion.Identity;
  298. m_flags &= ~(VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
  299. m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_UP_ONLY |
  300. VehicleFlag.LIMIT_MOTOR_UP);
  301. break;
  302. case Vehicle.TYPE_BOAT:
  303. m_linearFrictionTimescale = new Vector3(10, 3, 2);
  304. m_angularFrictionTimescale = new Vector3(10,10,10);
  305. m_linearMotorDirection = Vector3.Zero;
  306. m_linearMotorTimescale = 5;
  307. m_linearMotorDecayTimescale = 60;
  308. m_angularMotorDirection = Vector3.Zero;
  309. m_angularMotorTimescale = 4;
  310. m_angularMotorDecayTimescale = 4;
  311. m_VhoverHeight = 0;
  312. m_VhoverEfficiency = 0.5f;
  313. m_VhoverTimescale = 2;
  314. m_VehicleBuoyancy = 1;
  315. // m_linearDeflectionEfficiency = 0.5f;
  316. // m_linearDeflectionTimescale = 3;
  317. // m_angularDeflectionEfficiency = 0.5f;
  318. // m_angularDeflectionTimescale = 5;
  319. m_verticalAttractionEfficiency = 0.5f;
  320. m_verticalAttractionTimescale = 5;
  321. // m_bankingEfficiency = -0.3f;
  322. // m_bankingMix = 0.8f;
  323. // m_bankingTimescale = 1;
  324. // m_referenceFrame = Quaternion.Identity;
  325. m_flags &= ~(VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.LIMIT_ROLL_ONLY |
  326. VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY);
  327. m_flags |= (VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY |
  328. VehicleFlag.LIMIT_MOTOR_UP);
  329. break;
  330. case Vehicle.TYPE_AIRPLANE:
  331. m_linearFrictionTimescale = new Vector3(200, 10, 5);
  332. m_angularFrictionTimescale = new Vector3(20, 20, 20);
  333. m_linearMotorDirection = Vector3.Zero;
  334. m_linearMotorTimescale = 2;
  335. m_linearMotorDecayTimescale = 60;
  336. m_angularMotorDirection = Vector3.Zero;
  337. m_angularMotorTimescale = 4;
  338. m_angularMotorDecayTimescale = 4;
  339. m_VhoverHeight = 0;
  340. m_VhoverEfficiency = 0.5f;
  341. m_VhoverTimescale = 1000;
  342. m_VehicleBuoyancy = 0;
  343. // m_linearDeflectionEfficiency = 0.5f;
  344. // m_linearDeflectionTimescale = 3;
  345. // m_angularDeflectionEfficiency = 1;
  346. // m_angularDeflectionTimescale = 2;
  347. m_verticalAttractionEfficiency = 0.9f;
  348. m_verticalAttractionTimescale = 2;
  349. // m_bankingEfficiency = 1;
  350. // m_bankingMix = 0.7f;
  351. // m_bankingTimescale = 2;
  352. // m_referenceFrame = Quaternion.Identity;
  353. m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
  354. VehicleFlag.HOVER_GLOBAL_HEIGHT | VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
  355. m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY);
  356. break;
  357. case Vehicle.TYPE_BALLOON:
  358. m_linearFrictionTimescale = new Vector3(5, 5, 5);
  359. m_angularFrictionTimescale = new Vector3(10, 10, 10);
  360. m_linearMotorDirection = Vector3.Zero;
  361. m_linearMotorTimescale = 5;
  362. m_linearMotorDecayTimescale = 60;
  363. m_angularMotorDirection = Vector3.Zero;
  364. m_angularMotorTimescale = 6;
  365. m_angularMotorDecayTimescale = 10;
  366. m_VhoverHeight = 5;
  367. m_VhoverEfficiency = 0.8f;
  368. m_VhoverTimescale = 10;
  369. m_VehicleBuoyancy = 1;
  370. // m_linearDeflectionEfficiency = 0;
  371. // m_linearDeflectionTimescale = 5;
  372. // m_angularDeflectionEfficiency = 0;
  373. // m_angularDeflectionTimescale = 5;
  374. m_verticalAttractionEfficiency = 1;
  375. m_verticalAttractionTimescale = 1000;
  376. // m_bankingEfficiency = 0;
  377. // m_bankingMix = 0.7f;
  378. // m_bankingTimescale = 5;
  379. // m_referenceFrame = Quaternion.Identity;
  380. m_flags &= ~(VehicleFlag.NO_DEFLECTION_UP | VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY |
  381. VehicleFlag.HOVER_UP_ONLY | VehicleFlag.LIMIT_MOTOR_UP);
  382. m_flags |= (VehicleFlag.LIMIT_ROLL_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT);
  383. break;
  384. }
  385. }//end SetDefaultsForType
  386. internal void Enable(IntPtr pBody, OdeScene pParentScene)
  387. {
  388. //Console.WriteLine("Enable m_type=" + m_type + " m_VehicleBuoyancy=" + m_VehicleBuoyancy);
  389. if (m_type == Vehicle.TYPE_NONE)
  390. return;
  391. m_body = pBody;
  392. //KF: This used to set up the linear and angular joints
  393. }
  394. internal void Step(float pTimestep, OdeScene pParentScene)
  395. {
  396. if (m_body == IntPtr.Zero || m_type == Vehicle.TYPE_NONE)
  397. return;
  398. frcount++; // used to limit debug comment output
  399. if (frcount > 100)
  400. frcount = 0;
  401. MoveLinear(pTimestep, pParentScene);
  402. MoveAngular(pTimestep);
  403. }// end Step
  404. private void MoveLinear(float pTimestep, OdeScene _pParentScene)
  405. {
  406. if (!m_linearMotorDirection.ApproxEquals(Vector3.Zero, 0.01f)) // requested m_linearMotorDirection is significant
  407. {
  408. if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
  409. // add drive to body
  410. Vector3 addAmount = m_linearMotorDirection/(m_linearMotorTimescale/pTimestep);
  411. m_lastLinearVelocityVector += (addAmount*10); // lastLinearVelocityVector is the current body velocity vector?
  412. // This will work temporarily, but we really need to compare speed on an axis
  413. // KF: Limit body velocity to applied velocity?
  414. if (Math.Abs(m_lastLinearVelocityVector.X) > Math.Abs(m_linearMotorDirectionLASTSET.X))
  415. m_lastLinearVelocityVector.X = m_linearMotorDirectionLASTSET.X;
  416. if (Math.Abs(m_lastLinearVelocityVector.Y) > Math.Abs(m_linearMotorDirectionLASTSET.Y))
  417. m_lastLinearVelocityVector.Y = m_linearMotorDirectionLASTSET.Y;
  418. if (Math.Abs(m_lastLinearVelocityVector.Z) > Math.Abs(m_linearMotorDirectionLASTSET.Z))
  419. m_lastLinearVelocityVector.Z = m_linearMotorDirectionLASTSET.Z;
  420. // decay applied velocity
  421. Vector3 decayfraction = ((Vector3.One/(m_linearMotorDecayTimescale/pTimestep)));
  422. //Console.WriteLine("decay: " + decayfraction);
  423. m_linearMotorDirection -= m_linearMotorDirection * decayfraction;
  424. //Console.WriteLine("actual: " + m_linearMotorDirection);
  425. }
  426. else
  427. { // requested is not significant
  428. // if what remains of applied is small, zero it.
  429. if (m_lastLinearVelocityVector.ApproxEquals(Vector3.Zero, 0.01f))
  430. m_lastLinearVelocityVector = Vector3.Zero;
  431. }
  432. // convert requested object velocity to world-referenced vector
  433. m_dir = m_lastLinearVelocityVector;
  434. d.Quaternion rot = d.BodyGetQuaternion(Body);
  435. Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W); // rotq = rotation of object
  436. m_dir *= rotq; // apply obj rotation to velocity vector
  437. // add Gravity andBuoyancy
  438. // KF: So far I have found no good method to combine a script-requested
  439. // .Z velocity and gravity. Therefore only 0g will used script-requested
  440. // .Z velocity. >0g (m_VehicleBuoyancy < 1) will used modified gravity only.
  441. Vector3 grav = Vector3.Zero;
  442. if(m_VehicleBuoyancy < 1.0f)
  443. {
  444. // There is some gravity, make a gravity force vector
  445. // that is applied after object velocity.
  446. d.Mass objMass;
  447. d.BodyGetMass(Body, out objMass);
  448. // m_VehicleBuoyancy: -1=2g; 0=1g; 1=0g;
  449. grav.Z = _pParentScene.gravityz * objMass.mass * (1f - m_VehicleBuoyancy);
  450. // Preserve the current Z velocity
  451. d.Vector3 vel_now = d.BodyGetLinearVel(Body);
  452. m_dir.Z = vel_now.Z; // Preserve the accumulated falling velocity
  453. } // else its 1.0, no gravity.
  454. // Check if hovering
  455. if( (m_flags & (VehicleFlag.HOVER_WATER_ONLY | VehicleFlag.HOVER_TERRAIN_ONLY | VehicleFlag.HOVER_GLOBAL_HEIGHT)) != 0)
  456. {
  457. // We should hover, get the target height
  458. d.Vector3 pos = d.BodyGetPosition(Body);
  459. if((m_flags & VehicleFlag.HOVER_WATER_ONLY) == VehicleFlag.HOVER_WATER_ONLY)
  460. {
  461. m_VhoverTargetHeight = _pParentScene.GetWaterLevel() + m_VhoverHeight;
  462. }
  463. else if((m_flags & VehicleFlag.HOVER_TERRAIN_ONLY) == VehicleFlag.HOVER_TERRAIN_ONLY)
  464. {
  465. m_VhoverTargetHeight = _pParentScene.GetTerrainHeightAtXY(pos.X, pos.Y) + m_VhoverHeight;
  466. }
  467. else if((m_flags & VehicleFlag.HOVER_GLOBAL_HEIGHT) == VehicleFlag.HOVER_GLOBAL_HEIGHT)
  468. {
  469. m_VhoverTargetHeight = m_VhoverHeight;
  470. }
  471. if((m_flags & VehicleFlag.HOVER_UP_ONLY) == VehicleFlag.HOVER_UP_ONLY)
  472. {
  473. // If body is aready heigher, use its height as target height
  474. if(pos.Z > m_VhoverTargetHeight) m_VhoverTargetHeight = pos.Z;
  475. }
  476. // m_VhoverEfficiency = 0f; // 0=boucy, 1=Crit.damped
  477. // m_VhoverTimescale = 0f; // time to acheive height
  478. // pTimestep is time since last frame,in secs
  479. float herr0 = pos.Z - m_VhoverTargetHeight;
  480. //if(frcount == 0) Console.WriteLine("herr0=" + herr0);
  481. // Replace Vertical speed with correction figure if significant
  482. if(Math.Abs(herr0) > 0.01f )
  483. {
  484. d.Mass objMass;
  485. d.BodyGetMass(Body, out objMass);
  486. m_dir.Z = - ( (herr0 * pTimestep * 50.0f) / m_VhoverTimescale);
  487. // m_VhoverEfficiency is not yet implemented
  488. }
  489. else
  490. {
  491. m_dir.Z = 0f;
  492. }
  493. }
  494. // Apply velocity
  495. d.BodySetLinearVel(Body, m_dir.X, m_dir.Y, m_dir.Z);
  496. //if(frcount == 0) Console.WriteLine("Move " + Body + ":"+ m_dir.X + " " + m_dir.Y + " " + m_dir.Z);
  497. // apply gravity force
  498. d.BodyAddForce(Body, grav.X, grav.Y, grav.Z);
  499. //if(frcount == 0) Console.WriteLine("Force " + Body + ":" + grav.X + " " + grav.Y + " " + grav.Z);
  500. // apply friction
  501. Vector3 decayamount = Vector3.One / (m_linearFrictionTimescale / pTimestep);
  502. m_lastLinearVelocityVector -= m_lastLinearVelocityVector * decayamount;
  503. } // end MoveLinear()
  504. private void MoveAngular(float pTimestep)
  505. {
  506. // m_angularMotorDirection is the latest value from the script, and is decayed here
  507. // m_angularMotorDirectionLASTSET is the latest value from the script
  508. // m_lastAngularVelocityVector is what is being applied to the Body, varied up and down here
  509. if (!m_angularMotorDirection.ApproxEquals(Vector3.Zero, 0.01f))
  510. {
  511. if(!d.BodyIsEnabled (Body)) d.BodyEnable (Body);
  512. // ramp up to new value
  513. Vector3 addAmount = m_angularMotorDirection / (m_angularMotorTimescale / pTimestep);
  514. m_lastAngularVelocityVector += (addAmount * 10f);
  515. //if(frcount == 0) Console.WriteLine("add: " + addAmount);
  516. // limit applied value to what was set by script
  517. // This will work temporarily, but we really need to compare speed on an axis
  518. if (Math.Abs(m_lastAngularVelocityVector.X) > Math.Abs(m_angularMotorDirectionLASTSET.X))
  519. m_lastAngularVelocityVector.X = m_angularMotorDirectionLASTSET.X;
  520. if (Math.Abs(m_lastAngularVelocityVector.Y) > Math.Abs(m_angularMotorDirectionLASTSET.Y))
  521. m_lastAngularVelocityVector.Y = m_angularMotorDirectionLASTSET.Y;
  522. if (Math.Abs(m_lastAngularVelocityVector.Z) > Math.Abs(m_angularMotorDirectionLASTSET.Z))
  523. m_lastAngularVelocityVector.Z = m_angularMotorDirectionLASTSET.Z;
  524. // decay the requested value
  525. Vector3 decayfraction = ((Vector3.One / (m_angularMotorDecayTimescale / pTimestep)));
  526. //Console.WriteLine("decay: " + decayfraction);
  527. m_angularMotorDirection -= m_angularMotorDirection * decayfraction;
  528. //Console.WriteLine("actual: " + m_linearMotorDirection);
  529. }
  530. // KF: m_lastAngularVelocityVector is rotational speed in rad/sec ?
  531. // Vertical attractor section
  532. // d.Mass objMass;
  533. // d.BodyGetMass(Body, out objMass);
  534. // float servo = 100f * objMass.mass * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep);
  535. float servo = 0.1f * m_verticalAttractionEfficiency / (m_verticalAttractionTimescale * pTimestep);
  536. // get present body rotation
  537. d.Quaternion rot = d.BodyGetQuaternion(Body);
  538. Quaternion rotq = new Quaternion(rot.X, rot.Y, rot.Z, rot.W);
  539. // make a vector pointing up
  540. Vector3 verterr = Vector3.Zero;
  541. verterr.Z = 1.0f;
  542. // rotate it to Body Angle
  543. verterr = verterr * rotq;
  544. // 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.
  545. // 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
  546. // negative. Similar for tilt and |.Y|. .X and .Y must be modulated to prevent a stable inverted body.
  547. if (verterr.Z < 0.0f)
  548. {
  549. verterr.X = 2.0f - verterr.X;
  550. verterr.Y = 2.0f - verterr.Y;
  551. }
  552. // Error is 0 (no error) to +/- 2 (max error)
  553. // scale it by servo
  554. verterr = verterr * servo;
  555. // rotate to object frame
  556. // verterr = verterr * rotq;
  557. // As the body rotates around the X axis, then verterr.Y increases; Rotated around Y then .X increases, so
  558. // Change Body angular velocity X based on Y, and Y based on X. Z is not changed.
  559. m_lastAngularVelocityVector.X += verterr.Y;
  560. m_lastAngularVelocityVector.Y -= verterr.X;
  561. /*
  562. if(frcount == 0)
  563. {
  564. // Console.WriteLine("AngleMotor " + m_lastAngularVelocityVector);
  565. Console.WriteLine(String.Format("VA Body:{0} servo:{1} err:<{2},{3},{4}> VAE:{5}",
  566. Body, servo, verterr.X, verterr.Y, verterr.Z, m_verticalAttractionEfficiency));
  567. }
  568. */
  569. d.BodySetAngularVel (Body, m_lastAngularVelocityVector.X, m_lastAngularVelocityVector.Y, m_lastAngularVelocityVector.Z);
  570. // apply friction
  571. Vector3 decayamount = Vector3.One / (m_angularFrictionTimescale / pTimestep);
  572. m_lastAngularVelocityVector -= m_lastAngularVelocityVector * decayamount;
  573. } //end MoveAngular
  574. }
  575. }