BulletXPlugin.cs 59 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762
  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 OpenSim 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. #region References
  28. using System;
  29. using System.Collections.Generic;
  30. using MonoXnaCompactMaths;
  31. using OpenSim.Framework;
  32. using OpenSim.Region.Physics.Manager;
  33. using XnaDevRu.BulletX;
  34. using XnaDevRu.BulletX.Dynamics;
  35. using Nini.Config;
  36. using AxiomQuaternion = Axiom.Math.Quaternion;
  37. #endregion
  38. namespace OpenSim.Region.Physics.BulletXPlugin
  39. {
  40. /// <summary>
  41. /// BulletXConversions are called now BulletXMaths
  42. /// This Class converts objects and types for BulletX and give some operations
  43. /// </summary>
  44. public class BulletXMaths
  45. {
  46. //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
  47. //Vector3
  48. public static Vector3 PhysicsVectorToXnaVector3(PhysicsVector physicsVector)
  49. {
  50. return new Vector3(physicsVector.X, physicsVector.Y, physicsVector.Z);
  51. }
  52. public static PhysicsVector XnaVector3ToPhysicsVector(Vector3 xnaVector3)
  53. {
  54. return new PhysicsVector(xnaVector3.X, xnaVector3.Y, xnaVector3.Z);
  55. }
  56. //Quaternion
  57. public static Quaternion AxiomQuaternionToXnaQuaternion(AxiomQuaternion axiomQuaternion)
  58. {
  59. return new Quaternion(axiomQuaternion.x, axiomQuaternion.y, axiomQuaternion.z, axiomQuaternion.w);
  60. }
  61. public static AxiomQuaternion XnaQuaternionToAxiomQuaternion(Quaternion xnaQuaternion)
  62. {
  63. return new AxiomQuaternion(xnaQuaternion.W, xnaQuaternion.X, xnaQuaternion.Y, xnaQuaternion.Z);
  64. }
  65. //Next methods are extracted from XnaDevRu.BulletX(See 3rd party license):
  66. //- SetRotation (class MatrixOperations)
  67. //- GetRotation (class MatrixOperations)
  68. //- GetElement (class MathHelper)
  69. //- SetElement (class MathHelper)
  70. internal static void SetRotation(ref Matrix m, Quaternion q)
  71. {
  72. float d = q.LengthSquared();
  73. float s = 2f/d;
  74. float xs = q.X*s, ys = q.Y*s, zs = q.Z*s;
  75. float wx = q.W*xs, wy = q.W*ys, wz = q.W*zs;
  76. float xx = q.X*xs, xy = q.X*ys, xz = q.X*zs;
  77. float yy = q.Y*ys, yz = q.Y*zs, zz = q.Z*zs;
  78. m = new Matrix(1 - (yy + zz), xy - wz, xz + wy, 0,
  79. xy + wz, 1 - (xx + zz), yz - wx, 0,
  80. xz - wy, yz + wx, 1 - (xx + yy), 0,
  81. m.M41, m.M42, m.M43, 1);
  82. }
  83. internal static Quaternion GetRotation(Matrix m)
  84. {
  85. Quaternion q = new Quaternion();
  86. float trace = m.M11 + m.M22 + m.M33;
  87. if (trace > 0)
  88. {
  89. float s = (float) Math.Sqrt(trace + 1);
  90. q.W = s*0.5f;
  91. s = 0.5f/s;
  92. q.X = (m.M32 - m.M23)*s;
  93. q.Y = (m.M13 - m.M31)*s;
  94. q.Z = (m.M21 - m.M12)*s;
  95. }
  96. else
  97. {
  98. int i = m.M11 < m.M22
  99. ?
  100. (m.M22 < m.M33 ? 2 : 1)
  101. :
  102. (m.M11 < m.M33 ? 2 : 0);
  103. int j = (i + 1)%3;
  104. int k = (i + 2)%3;
  105. float s = (float) Math.Sqrt(GetElement(m, i, i) - GetElement(m, j, j) - GetElement(m, k, k) + 1);
  106. SetElement(ref q, i, s*0.5f);
  107. s = 0.5f/s;
  108. q.W = (GetElement(m, k, j) - GetElement(m, j, k))*s;
  109. SetElement(ref q, j, (GetElement(m, j, i) + GetElement(m, i, j))*s);
  110. SetElement(ref q, k, (GetElement(m, k, i) + GetElement(m, i, k))*s);
  111. }
  112. return q;
  113. }
  114. internal static float SetElement(ref Quaternion q, int index, float value)
  115. {
  116. switch (index)
  117. {
  118. case 0:
  119. q.X = value;
  120. break;
  121. case 1:
  122. q.Y = value;
  123. break;
  124. case 2:
  125. q.Z = value;
  126. break;
  127. case 3:
  128. q.W = value;
  129. break;
  130. }
  131. return 0;
  132. }
  133. internal static float GetElement(Matrix mat, int row, int col)
  134. {
  135. switch (row)
  136. {
  137. case 0:
  138. switch (col)
  139. {
  140. case 0:
  141. return mat.M11;
  142. case 1:
  143. return mat.M12;
  144. case 2:
  145. return mat.M13;
  146. }
  147. break;
  148. case 1:
  149. switch (col)
  150. {
  151. case 0:
  152. return mat.M21;
  153. case 1:
  154. return mat.M22;
  155. case 2:
  156. return mat.M23;
  157. }
  158. break;
  159. case 2:
  160. switch (col)
  161. {
  162. case 0:
  163. return mat.M31;
  164. case 1:
  165. return mat.M32;
  166. case 2:
  167. return mat.M33;
  168. }
  169. break;
  170. }
  171. return 0;
  172. }
  173. }
  174. /// <summary>
  175. /// PhysicsPlugin Class for BulletX
  176. /// </summary>
  177. public class BulletXPlugin : IPhysicsPlugin
  178. {
  179. private BulletXScene _mScene;
  180. public BulletXPlugin()
  181. {
  182. }
  183. public bool Init()
  184. {
  185. return true;
  186. }
  187. public PhysicsScene GetScene()
  188. {
  189. if (_mScene == null)
  190. {
  191. _mScene = new BulletXScene();
  192. }
  193. return (_mScene);
  194. }
  195. public string GetName()
  196. {
  197. return ("modified_BulletX"); //Changed!! "BulletXEngine" To "modified_BulletX"
  198. }
  199. public void Dispose()
  200. {
  201. }
  202. }
  203. // Class to detect and debug collisions
  204. // Mainly used for debugging purposes
  205. internal class CollisionDispatcherLocal : CollisionDispatcher
  206. {
  207. private BulletXScene relatedScene;
  208. public CollisionDispatcherLocal(BulletXScene s)
  209. : base()
  210. {
  211. relatedScene = s;
  212. }
  213. public override bool NeedsCollision(CollisionObject bodyA, CollisionObject bodyB)
  214. {
  215. RigidBody rb;
  216. BulletXCharacter bxcA = null;
  217. BulletXPrim bxpA = null;
  218. Type t = bodyA.GetType();
  219. if (t == typeof (RigidBody))
  220. {
  221. rb = (RigidBody) bodyA;
  222. relatedScene._characters.TryGetValue(rb, out bxcA);
  223. relatedScene._prims.TryGetValue(rb, out bxpA);
  224. }
  225. // String nameA;
  226. // if (bxcA != null)
  227. // nameA = bxcA._name;
  228. // else if (bxpA != null)
  229. // nameA = bxpA._name;
  230. // else
  231. // nameA = "null";
  232. BulletXCharacter bxcB = null;
  233. BulletXPrim bxpB = null;
  234. t = bodyB.GetType();
  235. if (t == typeof (RigidBody))
  236. {
  237. rb = (RigidBody) bodyB;
  238. relatedScene._characters.TryGetValue(rb, out bxcB);
  239. relatedScene._prims.TryGetValue(rb, out bxpB);
  240. }
  241. // String nameB;
  242. // if (bxcB != null)
  243. // nameB = bxcB._name;
  244. // else if (bxpB != null)
  245. // nameB = bxpB._name;
  246. // else
  247. // nameB = "null";
  248. bool needsCollision;// = base.NeedsCollision(bodyA, bodyB);
  249. int c1 = 3;
  250. int c2 = 3;
  251. ////////////////////////////////////////////////////////
  252. //BulletX Mesh Collisions
  253. //added by Jed zhu
  254. //data: May 07,2005
  255. ////////////////////////////////////////////////////////
  256. #region BulletXMeshCollisions Fields
  257. if (bxcA != null && bxpB != null)
  258. c1 = Collision(bxcA, bxpB);
  259. if (bxpA != null && bxcB != null)
  260. c2 = Collision(bxcB, bxpA);
  261. if (c1 < 2)
  262. needsCollision = (c1 > 0) ? true : false;
  263. else if (c2 < 2)
  264. needsCollision = (c2 > 0) ? true : false;
  265. else
  266. needsCollision = base.NeedsCollision(bodyA, bodyB);
  267. #endregion
  268. //m_log.DebugFormat("[BulletX]: A collision was detected between {0} and {1} --> {2}", nameA, nameB,
  269. //needsCollision);
  270. return needsCollision;
  271. }
  272. //added by jed zhu
  273. //calculas the collision between the Prim and Actor
  274. //
  275. private int Collision(BulletXCharacter actorA, BulletXPrim primB)
  276. {
  277. int[] indexBase;
  278. Vector3[] vertexBase;
  279. Vector3 vNormal;
  280. // Vector3 vP1;
  281. // Vector3 vP2;
  282. // Vector3 vP3;
  283. IMesh mesh = primB.GetMesh();
  284. float fdistance;
  285. if (primB == null)
  286. return 3;
  287. if (mesh == null)
  288. return 2;
  289. if (actorA == null)
  290. return 3;
  291. int iVertexCount = mesh.getVertexList().Count;
  292. int iIndexCount = mesh.getIndexListAsInt().Length;
  293. if (iVertexCount == 0)
  294. return 3;
  295. if (iIndexCount == 0)
  296. return 3;
  297. lock (BulletXScene.BulletXLock)
  298. {
  299. indexBase = mesh.getIndexListAsInt();
  300. vertexBase = new Vector3[iVertexCount];
  301. for (int i = 0; i < iVertexCount; i++)
  302. {
  303. PhysicsVector v = mesh.getVertexList()[i];
  304. if (v != null) // Note, null has special meaning. See meshing code for details
  305. vertexBase[i] = BulletXMaths.PhysicsVectorToXnaVector3(v);
  306. else
  307. vertexBase[i] = Vector3.Zero;
  308. }
  309. for (int ix = 0; ix < iIndexCount; ix += 3)
  310. {
  311. int ia = indexBase[ix + 0];
  312. int ib = indexBase[ix + 1];
  313. int ic = indexBase[ix + 2];
  314. //
  315. Vector3 v1 = vertexBase[ib] - vertexBase[ia];
  316. Vector3 v2 = vertexBase[ic] - vertexBase[ia];
  317. Vector3.Cross(ref v1, ref v2, out vNormal);
  318. Vector3.Normalize(ref vNormal, out vNormal);
  319. fdistance = Vector3.Dot(vNormal, vertexBase[ia]) + 0.50f;
  320. if (preCheckCollision(actorA, vNormal, fdistance) == 1)
  321. {
  322. if (CheckCollision(actorA, ia, ib, ic, vNormal, vertexBase) == 1)
  323. {
  324. //PhysicsVector v = actorA.Position;
  325. //Vector3 v3 = BulletXMaths.PhysicsVectorToXnaVector3(v);
  326. //Vector3 vp = vNormal * (fdistance - Vector3.Dot(vNormal, v3) + 0.2f);
  327. //actorA.Position += BulletXMaths.XnaVector3ToPhysicsVector(vp);
  328. return 1;
  329. }
  330. }
  331. }
  332. }
  333. return 0;
  334. }
  335. //added by jed zhu
  336. //return value 1: need second check
  337. //return value 0: no need check
  338. private int preCheckCollision(BulletXActor actA, Vector3 vNormal, float fDist)
  339. {
  340. float fstartSide;
  341. PhysicsVector v = actA.Position;
  342. Vector3 v3 = BulletXMaths.PhysicsVectorToXnaVector3(v);
  343. fstartSide = Vector3.Dot(vNormal, v3) - fDist;
  344. if (fstartSide > 0) return 0;
  345. else return 1;
  346. }
  347. //added by jed zhu
  348. private int CheckCollision(BulletXActor actA, int ia, int ib, int ic, Vector3 vNormal, Vector3[] vertBase)
  349. {
  350. Vector3 perPlaneNormal;
  351. float fPerPlaneDist;
  352. PhysicsVector v = actA.Position;
  353. Vector3 v3 = BulletXMaths.PhysicsVectorToXnaVector3(v);
  354. //check AB
  355. Vector3 v1;
  356. v1 = vertBase[ib] - vertBase[ia];
  357. Vector3.Cross(ref vNormal, ref v1, out perPlaneNormal);
  358. Vector3.Normalize(ref perPlaneNormal, out perPlaneNormal);
  359. if (Vector3.Dot((vertBase[ic] - vertBase[ia]), perPlaneNormal) < 0)
  360. perPlaneNormal = -perPlaneNormal;
  361. fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ia]) - 0.50f;
  362. if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) < 0)
  363. return 0;
  364. fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ic]) + 0.50f;
  365. if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) > 0)
  366. return 0;
  367. //check BC
  368. v1 = vertBase[ic] - vertBase[ib];
  369. Vector3.Cross(ref vNormal, ref v1, out perPlaneNormal);
  370. Vector3.Normalize(ref perPlaneNormal, out perPlaneNormal);
  371. if (Vector3.Dot((vertBase[ia] - vertBase[ib]), perPlaneNormal) < 0)
  372. perPlaneNormal = -perPlaneNormal;
  373. fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ib]) - 0.50f;
  374. if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) < 0)
  375. return 0;
  376. fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ia]) + 0.50f;
  377. if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) > 0)
  378. return 0;
  379. //check CA
  380. v1 = vertBase[ia] - vertBase[ic];
  381. Vector3.Cross(ref vNormal, ref v1, out perPlaneNormal);
  382. Vector3.Normalize(ref perPlaneNormal, out perPlaneNormal);
  383. if (Vector3.Dot((vertBase[ib] - vertBase[ic]), perPlaneNormal) < 0)
  384. perPlaneNormal = -perPlaneNormal;
  385. fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ic]) - 0.50f;
  386. if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) < 0)
  387. return 0;
  388. fPerPlaneDist = Vector3.Dot(perPlaneNormal, vertBase[ib]) + 0.50f;
  389. if ((Vector3.Dot(perPlaneNormal, v3) - fPerPlaneDist) > 0)
  390. return 0;
  391. return 1;
  392. }
  393. }
  394. /// <summary>
  395. /// PhysicsScene Class for BulletX
  396. /// </summary>
  397. public class BulletXScene : PhysicsScene
  398. {
  399. #region BulletXScene Fields
  400. public DiscreteDynamicsWorld ddWorld;
  401. private CollisionDispatcher cDispatcher;
  402. private OverlappingPairCache opCache;
  403. private SequentialImpulseConstraintSolver sicSolver;
  404. public static Object BulletXLock = new Object();
  405. private const int minXY = 0;
  406. private const int minZ = 0;
  407. private const int maxXY = (int)Constants.RegionSize;
  408. private const int maxZ = 4096;
  409. private const int maxHandles = 32766; //Why? I don't know
  410. private const float gravity = 9.8f;
  411. private const float heightLevel0 = 77.0f;
  412. private const float heightLevel1 = 200.0f;
  413. private const float lowGravityFactor = 0.2f;
  414. //OpenSim calls Simulate 10 times per seconds. So FPS = "Simulate Calls" * simulationSubSteps = 100 FPS
  415. private const int simulationSubSteps = 10;
  416. //private float[] _heightmap;
  417. private BulletXPlanet _simFlatPlanet;
  418. internal Dictionary<RigidBody, BulletXCharacter> _characters = new Dictionary<RigidBody, BulletXCharacter>();
  419. internal Dictionary<RigidBody, BulletXPrim> _prims = new Dictionary<RigidBody, BulletXPrim>();
  420. public IMesher mesher;
  421. // private IConfigSource m_config;
  422. public static float Gravity
  423. {
  424. get { return gravity; }
  425. }
  426. public static float HeightLevel0
  427. {
  428. get { return heightLevel0; }
  429. }
  430. public static float HeightLevel1
  431. {
  432. get { return heightLevel1; }
  433. }
  434. public static float LowGravityFactor
  435. {
  436. get { return lowGravityFactor; }
  437. }
  438. public static int MaxXY
  439. {
  440. get { return maxXY; }
  441. }
  442. public static int MaxZ
  443. {
  444. get { return maxZ; }
  445. }
  446. private List<RigidBody> _forgottenRigidBodies = new List<RigidBody>();
  447. internal string is_ex_message = "Can't remove rigidBody!: ";
  448. #endregion
  449. public BulletXScene()
  450. {
  451. cDispatcher = new CollisionDispatcherLocal(this);
  452. Vector3 worldMinDim = new Vector3((float) minXY, (float) minXY, (float) minZ);
  453. Vector3 worldMaxDim = new Vector3((float) maxXY, (float) maxXY, (float) maxZ);
  454. opCache = new AxisSweep3(worldMinDim, worldMaxDim, maxHandles);
  455. sicSolver = new SequentialImpulseConstraintSolver();
  456. lock (BulletXLock)
  457. {
  458. ddWorld = new DiscreteDynamicsWorld(cDispatcher, opCache, sicSolver);
  459. ddWorld.Gravity = new Vector3(0, 0, -gravity);
  460. }
  461. //this._heightmap = new float[65536];
  462. }
  463. public override void Initialise(IMesher meshmerizer, IConfigSource config)
  464. {
  465. mesher = meshmerizer;
  466. // m_config = config;
  467. }
  468. public override void Dispose()
  469. {
  470. }
  471. public override Dictionary<uint, float> GetTopColliders()
  472. {
  473. Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
  474. return returncolliders;
  475. }
  476. public override void SetWaterLevel(float baseheight)
  477. {
  478. }
  479. public override PhysicsActor AddAvatar(string avName, PhysicsVector position, PhysicsVector size)
  480. {
  481. PhysicsVector pos = new PhysicsVector();
  482. pos.X = position.X;
  483. pos.Y = position.Y;
  484. pos.Z = position.Z + 20;
  485. BulletXCharacter newAv = null;
  486. lock (BulletXLock)
  487. {
  488. newAv = new BulletXCharacter(avName, this, pos);
  489. _characters.Add(newAv.RigidBody, newAv);
  490. }
  491. return newAv;
  492. }
  493. public override void RemoveAvatar(PhysicsActor actor)
  494. {
  495. if (actor is BulletXCharacter)
  496. {
  497. lock (BulletXLock)
  498. {
  499. try
  500. {
  501. ddWorld.RemoveRigidBody(((BulletXCharacter) actor).RigidBody);
  502. }
  503. catch (Exception ex)
  504. {
  505. BulletXMessage(is_ex_message + ex.Message, true);
  506. ((BulletXCharacter) actor).RigidBody.ActivationState = ActivationState.DisableSimulation;
  507. AddForgottenRigidBody(((BulletXCharacter) actor).RigidBody);
  508. }
  509. _characters.Remove(((BulletXCharacter) actor).RigidBody);
  510. }
  511. GC.Collect();
  512. }
  513. }
  514. public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position,
  515. PhysicsVector size, AxiomQuaternion rotation)
  516. {
  517. return AddPrimShape(primName, pbs, position, size, rotation, false);
  518. }
  519. public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position,
  520. PhysicsVector size, AxiomQuaternion rotation, bool isPhysical)
  521. {
  522. PhysicsActor result;
  523. switch (pbs.ProfileShape)
  524. {
  525. case ProfileShape.Square:
  526. /// support simple box & hollow box now; later, more shapes
  527. if (pbs.ProfileHollow == 0)
  528. {
  529. result = AddPrim(primName, position, size, rotation, null, null, isPhysical);
  530. }
  531. else
  532. {
  533. IMesh mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical);
  534. result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical);
  535. }
  536. break;
  537. default:
  538. result = AddPrim(primName, position, size, rotation, null, null, isPhysical);
  539. break;
  540. }
  541. return result;
  542. }
  543. public PhysicsActor AddPrim(String name, PhysicsVector position, PhysicsVector size, AxiomQuaternion rotation,
  544. IMesh mesh, PrimitiveBaseShape pbs, bool isPhysical)
  545. {
  546. BulletXPrim newPrim = null;
  547. lock (BulletXLock)
  548. {
  549. newPrim = new BulletXPrim(name, this, position, size, rotation, mesh, pbs, isPhysical);
  550. _prims.Add(newPrim.RigidBody, newPrim);
  551. }
  552. return newPrim;
  553. }
  554. public override void RemovePrim(PhysicsActor prim)
  555. {
  556. if (prim is BulletXPrim)
  557. {
  558. lock (BulletXLock)
  559. {
  560. try
  561. {
  562. ddWorld.RemoveRigidBody(((BulletXPrim) prim).RigidBody);
  563. }
  564. catch (Exception ex)
  565. {
  566. BulletXMessage(is_ex_message + ex.Message, true);
  567. ((BulletXPrim) prim).RigidBody.ActivationState = ActivationState.DisableSimulation;
  568. AddForgottenRigidBody(((BulletXPrim) prim).RigidBody);
  569. }
  570. _prims.Remove(((BulletXPrim) prim).RigidBody);
  571. }
  572. GC.Collect();
  573. }
  574. }
  575. public override void AddPhysicsActorTaint(PhysicsActor prim)
  576. {
  577. }
  578. public override float Simulate(float timeStep)
  579. {
  580. float fps = 0;
  581. lock (BulletXLock)
  582. {
  583. //Try to remove garbage
  584. RemoveForgottenRigidBodies();
  585. //End of remove
  586. MoveAllObjects(timeStep);
  587. fps = (timeStep*simulationSubSteps);
  588. ddWorld.StepSimulation(timeStep, simulationSubSteps, timeStep);
  589. //Extra Heightmap Validation: BulletX's HeightFieldTerrain somestimes doesn't work so fine.
  590. ValidateHeightForAll();
  591. //End heightmap validation.
  592. UpdateKineticsForAll();
  593. }
  594. return fps;
  595. }
  596. private void MoveAllObjects(float timeStep)
  597. {
  598. foreach (BulletXCharacter actor in _characters.Values)
  599. {
  600. actor.Move(timeStep);
  601. }
  602. }
  603. private void ValidateHeightForAll()
  604. {
  605. float _height;
  606. foreach (BulletXCharacter actor in _characters.Values)
  607. {
  608. //_height = HeightValue(actor.RigidBodyPosition);
  609. _height = _simFlatPlanet.HeightValue(actor.RigidBodyPosition);
  610. actor.ValidateHeight(_height);
  611. //if (_simFlatPlanet.heightIsNotValid(actor.RigidBodyPosition, out _height)) actor.ValidateHeight(_height);
  612. }
  613. foreach (BulletXPrim prim in _prims.Values)
  614. {
  615. //_height = HeightValue(prim.RigidBodyPosition);
  616. _height = _simFlatPlanet.HeightValue(prim.RigidBodyPosition);
  617. prim.ValidateHeight(_height);
  618. //if (_simFlatPlanet.heightIsNotValid(prim.RigidBodyPosition, out _height)) prim.ValidateHeight(_height);
  619. }
  620. //foreach (BulletXCharacter actor in _characters)
  621. //{
  622. // actor.ValidateHeight(0);
  623. //}
  624. //foreach (BulletXPrim prim in _prims)
  625. //{
  626. // prim.ValidateHeight(0);
  627. //}
  628. }
  629. private void UpdateKineticsForAll()
  630. {
  631. //UpdatePosition > UpdateKinetics.
  632. //Not only position will be updated, also velocity cause acceleration.
  633. foreach (BulletXCharacter actor in _characters.Values)
  634. {
  635. actor.UpdateKinetics();
  636. }
  637. foreach (BulletXPrim prim in _prims.Values)
  638. {
  639. prim.UpdateKinetics();
  640. }
  641. //if (this._simFlatPlanet!=null) this._simFlatPlanet.Restore();
  642. }
  643. public override void GetResults()
  644. {
  645. }
  646. public override bool IsThreaded
  647. {
  648. get
  649. {
  650. return (false); // for now we won't be multithreaded
  651. }
  652. }
  653. public override void SetTerrain(float[] heightMap)
  654. {
  655. ////As the same as ODE, heightmap (x,y) must be swapped for BulletX
  656. //for (int i = 0; i < 65536; i++)
  657. //{
  658. // // this._heightmap[i] = (double)heightMap[i];
  659. // // dbm (danx0r) -- heightmap x,y must be swapped for Ode (should fix ODE, but for now...)
  660. // int x = i & 0xff;
  661. // int y = i >> 8;
  662. // this._heightmap[i] = heightMap[x * 256 + y];
  663. //}
  664. //float[] swappedHeightMap = new float[65536];
  665. ////As the same as ODE, heightmap (x,y) must be swapped for BulletX
  666. //for (int i = 0; i < 65536; i++)
  667. //{
  668. // // this._heightmap[i] = (double)heightMap[i];
  669. // // dbm (danx0r) -- heightmap x,y must be swapped for Ode (should fix ODE, but for now...)
  670. // int x = i & 0xff;
  671. // int y = i >> 8;
  672. // swappedHeightMap[i] = heightMap[x * 256 + y];
  673. //}
  674. DeleteTerrain();
  675. //There is a BulletXLock inside the constructor of BulletXPlanet
  676. //this._simFlatPlanet = new BulletXPlanet(this, swappedHeightMap);
  677. _simFlatPlanet = new BulletXPlanet(this, heightMap);
  678. //this._heightmap = heightMap;
  679. }
  680. public override void DeleteTerrain()
  681. {
  682. if (_simFlatPlanet != null)
  683. {
  684. lock (BulletXLock)
  685. {
  686. try
  687. {
  688. ddWorld.RemoveRigidBody(_simFlatPlanet.RigidBody);
  689. }
  690. catch (Exception ex)
  691. {
  692. BulletXMessage(is_ex_message + ex.Message, true);
  693. _simFlatPlanet.RigidBody.ActivationState = ActivationState.DisableSimulation;
  694. AddForgottenRigidBody(_simFlatPlanet.RigidBody);
  695. }
  696. }
  697. _simFlatPlanet = null;
  698. GC.Collect();
  699. BulletXMessage("Terrain erased!", false);
  700. }
  701. //this._heightmap = null;
  702. }
  703. internal void AddForgottenRigidBody(RigidBody forgottenRigidBody)
  704. {
  705. _forgottenRigidBodies.Add(forgottenRigidBody);
  706. }
  707. private void RemoveForgottenRigidBodies()
  708. {
  709. RigidBody forgottenRigidBody;
  710. int nRigidBodies = _forgottenRigidBodies.Count;
  711. for (int i = nRigidBodies - 1; i >= 0; i--)
  712. {
  713. forgottenRigidBody = _forgottenRigidBodies[i];
  714. try
  715. {
  716. ddWorld.RemoveRigidBody(forgottenRigidBody);
  717. _forgottenRigidBodies.Remove(forgottenRigidBody);
  718. BulletXMessage("Forgotten Rigid Body Removed", false);
  719. }
  720. catch (Exception ex)
  721. {
  722. BulletXMessage("Can't remove forgottenRigidBody!: " + ex.Message, false);
  723. }
  724. }
  725. GC.Collect();
  726. }
  727. internal static void BulletXMessage(string message, bool isWarning)
  728. {
  729. PhysicsPluginManager.PhysicsPluginMessage("[Modified BulletX]:\t" + message, isWarning);
  730. }
  731. //temp
  732. //private float HeightValue(MonoXnaCompactMaths.Vector3 position)
  733. //{
  734. // int li_x, li_y;
  735. // float height;
  736. // li_x = (int)Math.Round(position.X); if (li_x < 0) li_x = 0;
  737. // li_y = (int)Math.Round(position.Y); if (li_y < 0) li_y = 0;
  738. // height = this._heightmap[li_y * 256 + li_x];
  739. // if (height < 0) height = 0;
  740. // else if (height > maxZ) height = maxZ;
  741. // return height;
  742. //}
  743. }
  744. /// <summary>
  745. /// Generic Physics Actor for BulletX inherit from PhysicActor
  746. /// </summary>
  747. public class BulletXActor : PhysicsActor
  748. {
  749. protected bool flying = false;
  750. protected bool _physical = false;
  751. protected PhysicsVector _position;
  752. protected PhysicsVector _velocity;
  753. protected PhysicsVector _size;
  754. protected PhysicsVector _acceleration;
  755. protected AxiomQuaternion _orientation;
  756. protected PhysicsVector m_rotationalVelocity = PhysicsVector.Zero;
  757. protected RigidBody rigidBody;
  758. protected int m_PhysicsActorType;
  759. private Boolean iscolliding = false;
  760. internal string _name;
  761. public BulletXActor(String name)
  762. {
  763. _name = name;
  764. }
  765. public override bool Stopped
  766. {
  767. get { return false; }
  768. }
  769. public override PhysicsVector Position
  770. {
  771. get { return _position; }
  772. set
  773. {
  774. lock (BulletXScene.BulletXLock)
  775. {
  776. _position = value;
  777. Translate();
  778. }
  779. }
  780. }
  781. public override PhysicsVector RotationalVelocity
  782. {
  783. get { return m_rotationalVelocity; }
  784. set { m_rotationalVelocity = value; }
  785. }
  786. public override PhysicsVector Velocity
  787. {
  788. get { return _velocity; }
  789. set
  790. {
  791. lock (BulletXScene.BulletXLock)
  792. {
  793. //Static objects don' have linear velocity
  794. if (_physical)
  795. {
  796. _velocity = value;
  797. Speed();
  798. }
  799. else
  800. {
  801. _velocity = new PhysicsVector();
  802. }
  803. }
  804. }
  805. }
  806. public override float CollisionScore
  807. {
  808. get { return 0f; }
  809. set { }
  810. }
  811. public override PhysicsVector Size
  812. {
  813. get { return _size; }
  814. set
  815. {
  816. lock (BulletXScene.BulletXLock)
  817. {
  818. _size = value;
  819. }
  820. }
  821. }
  822. public override PhysicsVector Force
  823. {
  824. get { return PhysicsVector.Zero; }
  825. }
  826. public override PhysicsVector CenterOfMass
  827. {
  828. get { return PhysicsVector.Zero; }
  829. }
  830. public override PhysicsVector GeometricCenter
  831. {
  832. get { return PhysicsVector.Zero; }
  833. }
  834. public override PrimitiveBaseShape Shape
  835. {
  836. set { return; }
  837. }
  838. public override bool SetAlwaysRun
  839. {
  840. get { return false; }
  841. set { return; }
  842. }
  843. public override PhysicsVector Acceleration
  844. {
  845. get { return _acceleration; }
  846. }
  847. public override AxiomQuaternion Orientation
  848. {
  849. get { return _orientation; }
  850. set
  851. {
  852. lock (BulletXScene.BulletXLock)
  853. {
  854. _orientation = value;
  855. ReOrient();
  856. }
  857. }
  858. }
  859. public override void link(PhysicsActor obj)
  860. {
  861. }
  862. public override void delink()
  863. {
  864. }
  865. public override void LockAngularMotion(PhysicsVector axis)
  866. {
  867. }
  868. public override float Mass
  869. {
  870. get { return ActorMass; }
  871. }
  872. public virtual float ActorMass
  873. {
  874. get { return 0; }
  875. }
  876. public override int PhysicsActorType
  877. {
  878. get { return (int) m_PhysicsActorType; }
  879. set { m_PhysicsActorType = value; }
  880. }
  881. public RigidBody RigidBody
  882. {
  883. get { return rigidBody; }
  884. }
  885. public Vector3 RigidBodyPosition
  886. {
  887. get { return rigidBody.CenterOfMassPosition; }
  888. }
  889. public override bool IsPhysical
  890. {
  891. get { return _physical; }
  892. set { _physical = value; }
  893. }
  894. public override bool Flying
  895. {
  896. get { return flying; }
  897. set { flying = value; }
  898. }
  899. public override bool ThrottleUpdates
  900. {
  901. get { return false; }
  902. set { return; }
  903. }
  904. public override bool IsColliding
  905. {
  906. get { return iscolliding; }
  907. set { iscolliding = value; }
  908. }
  909. public override bool CollidingGround
  910. {
  911. get { return false; }
  912. set { return; }
  913. }
  914. public override bool CollidingObj
  915. {
  916. get { return false; }
  917. set { return; }
  918. }
  919. public override uint LocalID
  920. {
  921. set { return; }
  922. }
  923. public override bool Grabbed
  924. {
  925. set { return; }
  926. }
  927. public override bool Selected
  928. {
  929. set { return; }
  930. }
  931. public override float Buoyancy
  932. {
  933. get { return 0f; }
  934. set { return; }
  935. }
  936. public override bool FloatOnWater
  937. {
  938. set { return; }
  939. }
  940. public virtual void SetAcceleration(PhysicsVector accel)
  941. {
  942. lock (BulletXScene.BulletXLock)
  943. {
  944. _acceleration = accel;
  945. }
  946. }
  947. public override bool Kinematic
  948. {
  949. get { return false; }
  950. set { }
  951. }
  952. public override void AddForce(PhysicsVector force, bool pushforce)
  953. {
  954. }
  955. public override void SetMomentum(PhysicsVector momentum)
  956. {
  957. }
  958. internal virtual void ValidateHeight(float heighmapPositionValue)
  959. {
  960. }
  961. internal virtual void UpdateKinetics()
  962. {
  963. }
  964. #region Methods for updating values of RigidBody
  965. protected internal void Translate()
  966. {
  967. Translate(_position);
  968. }
  969. protected internal void Translate(PhysicsVector _newPos)
  970. {
  971. Vector3 _translation;
  972. _translation = BulletXMaths.PhysicsVectorToXnaVector3(_newPos) - rigidBody.CenterOfMassPosition;
  973. rigidBody.Translate(_translation);
  974. }
  975. protected internal void Speed()
  976. {
  977. Speed(_velocity);
  978. }
  979. protected internal void Speed(PhysicsVector _newSpeed)
  980. {
  981. Vector3 _speed;
  982. _speed = BulletXMaths.PhysicsVectorToXnaVector3(_newSpeed);
  983. rigidBody.LinearVelocity = _speed;
  984. }
  985. protected internal void ReOrient()
  986. {
  987. ReOrient(_orientation);
  988. }
  989. protected internal void ReOrient(AxiomQuaternion _newOrient)
  990. {
  991. Quaternion _newOrientation;
  992. _newOrientation = BulletXMaths.AxiomQuaternionToXnaQuaternion(_newOrient);
  993. Matrix _comTransform = rigidBody.CenterOfMassTransform;
  994. BulletXMaths.SetRotation(ref _comTransform, _newOrientation);
  995. rigidBody.CenterOfMassTransform = _comTransform;
  996. }
  997. protected internal void ReSize()
  998. {
  999. ReSize(_size);
  1000. }
  1001. protected internal virtual void ReSize(PhysicsVector _newSize)
  1002. {
  1003. }
  1004. public virtual void ScheduleTerseUpdate()
  1005. {
  1006. base.RequestPhysicsterseUpdate();
  1007. }
  1008. #endregion
  1009. public override void CrossingFailure()
  1010. {
  1011. }
  1012. public override PhysicsVector PIDTarget { set { return; } }
  1013. public override bool PIDActive { set { return; } }
  1014. public override float PIDTau { set { return; } }
  1015. public override void SubscribeEvents(int ms)
  1016. {
  1017. }
  1018. public override void UnSubscribeEvents()
  1019. {
  1020. }
  1021. public override bool SubscribedEvents()
  1022. {
  1023. return false;
  1024. }
  1025. }
  1026. /// <summary>
  1027. /// PhysicsActor Character Class for BulletX
  1028. /// </summary>
  1029. public class BulletXCharacter : BulletXActor
  1030. {
  1031. public BulletXCharacter(BulletXScene parent_scene, PhysicsVector pos)
  1032. : this(String.Empty, parent_scene, pos)
  1033. {
  1034. }
  1035. public BulletXCharacter(String avName, BulletXScene parent_scene, PhysicsVector pos)
  1036. : this(avName, parent_scene, pos, new PhysicsVector(), new PhysicsVector(), new PhysicsVector(),
  1037. AxiomQuaternion.Identity)
  1038. {
  1039. }
  1040. public BulletXCharacter(String avName, BulletXScene parent_scene, PhysicsVector pos, PhysicsVector velocity,
  1041. PhysicsVector size, PhysicsVector acceleration, AxiomQuaternion orientation)
  1042. : base(avName)
  1043. {
  1044. //This fields will be removed. They're temporal
  1045. float _sizeX = 0.5f;
  1046. float _sizeY = 0.5f;
  1047. float _sizeZ = 1.6f;
  1048. //.
  1049. _position = pos;
  1050. _velocity = velocity;
  1051. _size = size;
  1052. //---
  1053. _size.X = _sizeX;
  1054. _size.Y = _sizeY;
  1055. _size.Z = _sizeZ;
  1056. //.
  1057. _acceleration = acceleration;
  1058. _orientation = orientation;
  1059. _physical = true;
  1060. float _mass = 50.0f; //This depends of avatar's dimensions
  1061. //For RigidBody Constructor. The next values might change
  1062. float _linearDamping = 0.0f;
  1063. float _angularDamping = 0.0f;
  1064. float _friction = 0.5f;
  1065. float _restitution = 0.0f;
  1066. Matrix _startTransform = Matrix.Identity;
  1067. Matrix _centerOfMassOffset = Matrix.Identity;
  1068. lock (BulletXScene.BulletXLock)
  1069. {
  1070. _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos);
  1071. //CollisionShape _collisionShape = new BoxShape(new MonoXnaCompactMaths.Vector3(1.0f, 1.0f, 1.60f));
  1072. //For now, like ODE, collisionShape = sphere of radious = 1.0
  1073. CollisionShape _collisionShape = new SphereShape(1.0f);
  1074. DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
  1075. Vector3 _localInertia = new Vector3();
  1076. _collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0
  1077. rigidBody =
  1078. new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping,
  1079. _friction, _restitution);
  1080. //rigidBody.ActivationState = ActivationState.DisableDeactivation;
  1081. //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
  1082. Vector3 _vDebugTranslation;
  1083. _vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
  1084. rigidBody.Translate(_vDebugTranslation);
  1085. parent_scene.ddWorld.AddRigidBody(rigidBody);
  1086. }
  1087. }
  1088. public override int PhysicsActorType
  1089. {
  1090. get { return (int) ActorTypes.Agent; }
  1091. set { return; }
  1092. }
  1093. public override PhysicsVector Position
  1094. {
  1095. get { return base.Position; }
  1096. set { base.Position = value; }
  1097. }
  1098. public override PhysicsVector Velocity
  1099. {
  1100. get { return base.Velocity; }
  1101. set { base.Velocity = value; }
  1102. }
  1103. public override PhysicsVector Size
  1104. {
  1105. get { return base.Size; }
  1106. set { base.Size = value; }
  1107. }
  1108. public override PhysicsVector Acceleration
  1109. {
  1110. get { return base.Acceleration; }
  1111. }
  1112. public override AxiomQuaternion Orientation
  1113. {
  1114. get { return base.Orientation; }
  1115. set { base.Orientation = value; }
  1116. }
  1117. public override bool Flying
  1118. {
  1119. get { return base.Flying; }
  1120. set { base.Flying = value; }
  1121. }
  1122. public override bool IsColliding
  1123. {
  1124. get { return base.IsColliding; }
  1125. set { base.IsColliding = value; }
  1126. }
  1127. public override bool Kinematic
  1128. {
  1129. get { return base.Kinematic; }
  1130. set { base.Kinematic = value; }
  1131. }
  1132. public override void SetAcceleration(PhysicsVector accel)
  1133. {
  1134. base.SetAcceleration(accel);
  1135. }
  1136. public override void AddForce(PhysicsVector force, bool pushforce)
  1137. {
  1138. base.AddForce(force, pushforce);
  1139. }
  1140. public override void SetMomentum(PhysicsVector momentum)
  1141. {
  1142. base.SetMomentum(momentum);
  1143. }
  1144. internal void Move(float timeStep)
  1145. {
  1146. Vector3 vec = new Vector3();
  1147. //At this point it's supossed that:
  1148. //_velocity == rigidBody.LinearVelocity
  1149. vec.X = _velocity.X;
  1150. vec.Y = _velocity.Y;
  1151. vec.Z = _velocity.Z;
  1152. if ((vec.X != 0.0f) || (vec.Y != 0.0f) || (vec.Z != 0.0f)) rigidBody.Activate();
  1153. if (flying)
  1154. {
  1155. //Antigravity with movement
  1156. if (_position.Z <= BulletXScene.HeightLevel0)
  1157. {
  1158. vec.Z += BulletXScene.Gravity*timeStep;
  1159. }
  1160. //Lowgravity with movement
  1161. else if ((_position.Z > BulletXScene.HeightLevel0)
  1162. && (_position.Z <= BulletXScene.HeightLevel1))
  1163. {
  1164. vec.Z += BulletXScene.Gravity*timeStep*(1.0f - BulletXScene.LowGravityFactor);
  1165. }
  1166. //Lowgravity with...
  1167. else if (_position.Z > BulletXScene.HeightLevel1)
  1168. {
  1169. if (vec.Z > 0) //no movement
  1170. vec.Z = BulletXScene.Gravity*timeStep*(1.0f - BulletXScene.LowGravityFactor);
  1171. else
  1172. vec.Z += BulletXScene.Gravity*timeStep*(1.0f - BulletXScene.LowGravityFactor);
  1173. }
  1174. }
  1175. rigidBody.LinearVelocity = vec;
  1176. }
  1177. //This validation is very basic
  1178. internal override void ValidateHeight(float heighmapPositionValue)
  1179. {
  1180. if (rigidBody.CenterOfMassPosition.Z < heighmapPositionValue + _size.Z/2.0f)
  1181. {
  1182. Matrix m = rigidBody.WorldTransform;
  1183. Vector3 v3 = m.Translation;
  1184. v3.Z = heighmapPositionValue + _size.Z/2.0f;
  1185. m.Translation = v3;
  1186. rigidBody.WorldTransform = m;
  1187. //When an Avie touch the ground it's vertical velocity it's reduced to ZERO
  1188. Speed(new PhysicsVector(rigidBody.LinearVelocity.X, rigidBody.LinearVelocity.Y, 0.0f));
  1189. }
  1190. }
  1191. internal override void UpdateKinetics()
  1192. {
  1193. _position = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.CenterOfMassPosition);
  1194. _velocity = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.LinearVelocity);
  1195. //Orientation it seems that it will be the default.
  1196. ReOrient();
  1197. }
  1198. }
  1199. /// <summary>
  1200. /// PhysicsActor Prim Class for BulletX
  1201. /// </summary>
  1202. public class BulletXPrim : BulletXActor
  1203. {
  1204. //Density it will depends of material.
  1205. //For now all prims have the same density, all prims are made of water. Be water my friend! :D
  1206. private const float _density = 1000.0f;
  1207. private BulletXScene _parent_scene;
  1208. private PhysicsVector m_prev_position = new PhysicsVector(0, 0, 0);
  1209. private bool m_lastUpdateSent = false;
  1210. //added by jed zhu
  1211. private IMesh _mesh;
  1212. public IMesh GetMesh() { return _mesh; }
  1213. public BulletXPrim(String primName, BulletXScene parent_scene, PhysicsVector pos, PhysicsVector size,
  1214. AxiomQuaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool isPhysical)
  1215. : this(
  1216. primName, parent_scene, pos, new PhysicsVector(), size, new PhysicsVector(), rotation, mesh, pbs,
  1217. isPhysical)
  1218. {
  1219. }
  1220. public BulletXPrim(String primName, BulletXScene parent_scene, PhysicsVector pos, PhysicsVector velocity,
  1221. PhysicsVector size,
  1222. PhysicsVector acceleration, AxiomQuaternion rotation, IMesh mesh, PrimitiveBaseShape pbs,
  1223. bool isPhysical)
  1224. : base(primName)
  1225. {
  1226. if ((size.X == 0) || (size.Y == 0) || (size.Z == 0)) throw new Exception("Size 0");
  1227. if (rotation.Norm == 0f) rotation = AxiomQuaternion.Identity;
  1228. _position = pos;
  1229. _physical = isPhysical;
  1230. _velocity = _physical ? velocity : new PhysicsVector();
  1231. _size = size;
  1232. _acceleration = acceleration;
  1233. _orientation = rotation;
  1234. _parent_scene = parent_scene;
  1235. CreateRigidBody(parent_scene, mesh, pos, size);
  1236. }
  1237. public override int PhysicsActorType
  1238. {
  1239. get { return (int) ActorTypes.Prim; }
  1240. set { return; }
  1241. }
  1242. public override PhysicsVector Position
  1243. {
  1244. get { return base.Position; }
  1245. set { base.Position = value; }
  1246. }
  1247. public override PhysicsVector Velocity
  1248. {
  1249. get { return base.Velocity; }
  1250. set { base.Velocity = value; }
  1251. }
  1252. public override PhysicsVector Size
  1253. {
  1254. get { return _size; }
  1255. set
  1256. {
  1257. lock (BulletXScene.BulletXLock)
  1258. {
  1259. _size = value;
  1260. ReSize();
  1261. }
  1262. }
  1263. }
  1264. public override PhysicsVector Acceleration
  1265. {
  1266. get { return base.Acceleration; }
  1267. }
  1268. public override AxiomQuaternion Orientation
  1269. {
  1270. get { return base.Orientation; }
  1271. set { base.Orientation = value; }
  1272. }
  1273. public override float ActorMass
  1274. {
  1275. get
  1276. {
  1277. //For now all prims are boxes
  1278. return (_physical ? 1 : 0)*_density*_size.X*_size.Y*_size.Z;
  1279. }
  1280. }
  1281. public override bool IsPhysical
  1282. {
  1283. get { return base.IsPhysical; }
  1284. set
  1285. {
  1286. base.IsPhysical = value;
  1287. if (value)
  1288. {
  1289. //---
  1290. PhysicsPluginManager.PhysicsPluginMessage("Physical - Recreate", true);
  1291. //---
  1292. ReCreateRigidBody(_size);
  1293. }
  1294. else
  1295. {
  1296. //---
  1297. PhysicsPluginManager.PhysicsPluginMessage("Physical - SetMassProps", true);
  1298. //---
  1299. rigidBody.SetMassProps(Mass, new Vector3());
  1300. }
  1301. }
  1302. }
  1303. public override bool Flying
  1304. {
  1305. get { return base.Flying; }
  1306. set { base.Flying = value; }
  1307. }
  1308. public override bool IsColliding
  1309. {
  1310. get { return base.IsColliding; }
  1311. set { base.IsColliding = value; }
  1312. }
  1313. public override bool Kinematic
  1314. {
  1315. get { return base.Kinematic; }
  1316. set { base.Kinematic = value; }
  1317. }
  1318. public override void SetAcceleration(PhysicsVector accel)
  1319. {
  1320. lock (BulletXScene.BulletXLock)
  1321. {
  1322. _acceleration = accel;
  1323. }
  1324. }
  1325. public override void AddForce(PhysicsVector force, bool pushforce)
  1326. {
  1327. base.AddForce(force,pushforce);
  1328. }
  1329. public override void SetMomentum(PhysicsVector momentum)
  1330. {
  1331. base.SetMomentum(momentum);
  1332. }
  1333. internal override void ValidateHeight(float heighmapPositionValue)
  1334. {
  1335. if (rigidBody.CenterOfMassPosition.Z < heighmapPositionValue + _size.Z/2.0f)
  1336. {
  1337. Matrix m = rigidBody.WorldTransform;
  1338. Vector3 v3 = m.Translation;
  1339. v3.Z = heighmapPositionValue + _size.Z/2.0f;
  1340. m.Translation = v3;
  1341. rigidBody.WorldTransform = m;
  1342. //When a Prim touch the ground it's vertical velocity it's reduced to ZERO
  1343. //Static objects don't have linear velocity
  1344. if (_physical)
  1345. Speed(new PhysicsVector(rigidBody.LinearVelocity.X, rigidBody.LinearVelocity.Y, 0.0f));
  1346. }
  1347. }
  1348. internal override void UpdateKinetics()
  1349. {
  1350. if (_physical) //Updates properties. Prim updates its properties physically
  1351. {
  1352. _position = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.CenterOfMassPosition);
  1353. _velocity = BulletXMaths.XnaVector3ToPhysicsVector(rigidBody.LinearVelocity);
  1354. _orientation = BulletXMaths.XnaQuaternionToAxiomQuaternion(rigidBody.Orientation);
  1355. if ((Math.Abs(m_prev_position.X - _position.X) < 0.03)
  1356. && (Math.Abs(m_prev_position.Y - _position.Y) < 0.03)
  1357. && (Math.Abs(m_prev_position.Z - _position.Z) < 0.03))
  1358. {
  1359. if (!m_lastUpdateSent)
  1360. {
  1361. _velocity = new PhysicsVector(0, 0, 0);
  1362. base.ScheduleTerseUpdate();
  1363. m_lastUpdateSent = true;
  1364. }
  1365. }
  1366. else
  1367. {
  1368. m_lastUpdateSent = false;
  1369. base.ScheduleTerseUpdate();
  1370. }
  1371. m_prev_position = _position;
  1372. }
  1373. else //Doesn't updates properties. That's a cancel
  1374. {
  1375. Translate();
  1376. //Speed(); //<- Static objects don't have linear velocity
  1377. ReOrient();
  1378. }
  1379. }
  1380. #region Methods for updating values of RigidBody
  1381. protected internal void CreateRigidBody(BulletXScene parent_scene, IMesh mesh, PhysicsVector pos,
  1382. PhysicsVector size)
  1383. {
  1384. //For RigidBody Constructor. The next values might change
  1385. float _linearDamping = 0.0f;
  1386. float _angularDamping = 0.0f;
  1387. float _friction = 1.0f;
  1388. float _restitution = 0.0f;
  1389. Matrix _startTransform = Matrix.Identity;
  1390. Matrix _centerOfMassOffset = Matrix.Identity;
  1391. //added by jed zhu
  1392. _mesh = mesh;
  1393. lock (BulletXScene.BulletXLock)
  1394. {
  1395. _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(pos);
  1396. //For now all prims are boxes
  1397. CollisionShape _collisionShape;
  1398. if (mesh == null)
  1399. {
  1400. _collisionShape = new BoxShape(BulletXMaths.PhysicsVectorToXnaVector3(size)/2.0f);
  1401. }
  1402. else
  1403. {
  1404. int iVertexCount = mesh.getVertexList().Count;
  1405. int[] indices = mesh.getIndexListAsInt();
  1406. Vector3[] v3Vertices = new Vector3[iVertexCount];
  1407. for (int i = 0; i < iVertexCount; i++)
  1408. {
  1409. PhysicsVector v = mesh.getVertexList()[i];
  1410. if (v != null) // Note, null has special meaning. See meshing code for details
  1411. v3Vertices[i] = BulletXMaths.PhysicsVectorToXnaVector3(v);
  1412. else
  1413. v3Vertices[i] = Vector3.Zero;
  1414. }
  1415. TriangleIndexVertexArray triMesh = new TriangleIndexVertexArray(indices, v3Vertices);
  1416. _collisionShape = new TriangleMeshShape(triMesh);
  1417. }
  1418. DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
  1419. Vector3 _localInertia = new Vector3();
  1420. if (_physical) _collisionShape.CalculateLocalInertia(Mass, out _localInertia); //Always when mass > 0
  1421. rigidBody =
  1422. new RigidBody(Mass, _motionState, _collisionShape, _localInertia, _linearDamping, _angularDamping,
  1423. _friction, _restitution);
  1424. //rigidBody.ActivationState = ActivationState.DisableDeactivation;
  1425. //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
  1426. Vector3 _vDebugTranslation;
  1427. _vDebugTranslation = _startTransform.Translation - rigidBody.CenterOfMassPosition;
  1428. rigidBody.Translate(_vDebugTranslation);
  1429. //---
  1430. parent_scene.ddWorld.AddRigidBody(rigidBody);
  1431. }
  1432. }
  1433. protected internal void ReCreateRigidBody(PhysicsVector size)
  1434. {
  1435. //There is a bug when trying to remove a rigidBody that is colliding with something..
  1436. try
  1437. {
  1438. _parent_scene.ddWorld.RemoveRigidBody(rigidBody);
  1439. }
  1440. catch (Exception ex)
  1441. {
  1442. BulletXScene.BulletXMessage(_parent_scene.is_ex_message + ex.Message, true);
  1443. rigidBody.ActivationState = ActivationState.DisableSimulation;
  1444. _parent_scene.AddForgottenRigidBody(rigidBody);
  1445. }
  1446. CreateRigidBody(_parent_scene, null, _position, size);
  1447. // Note, null for the meshing definitely is wrong. It's here for the moment to apease the compiler
  1448. if (_physical) Speed(); //Static objects don't have linear velocity
  1449. ReOrient();
  1450. GC.Collect();
  1451. }
  1452. protected internal override void ReSize(PhysicsVector _newSize)
  1453. {
  1454. //I wonder to know how to resize with a simple instruction in BulletX. It seems that for now there isn't
  1455. //so i have to do it manually. That's recreating rigidbody
  1456. ReCreateRigidBody(_newSize);
  1457. }
  1458. #endregion
  1459. }
  1460. /// <summary>
  1461. /// This Class manage a HeighField as a RigidBody. This is for to be added in the BulletXScene
  1462. /// </summary>
  1463. internal class BulletXPlanet
  1464. {
  1465. private PhysicsVector _staticPosition;
  1466. // private PhysicsVector _staticVelocity;
  1467. // private AxiomQuaternion _staticOrientation;
  1468. private float _mass;
  1469. // private BulletXScene _parentscene;
  1470. internal float[] _heightField;
  1471. private RigidBody _flatPlanet;
  1472. internal RigidBody RigidBody
  1473. {
  1474. get { return _flatPlanet; }
  1475. }
  1476. internal BulletXPlanet(BulletXScene parent_scene, float[] heightField)
  1477. {
  1478. _staticPosition = new PhysicsVector(BulletXScene.MaxXY/2, BulletXScene.MaxXY/2, 0);
  1479. // _staticVelocity = new PhysicsVector();
  1480. // _staticOrientation = AxiomQuaternion.Identity;
  1481. _mass = 0; //No active
  1482. // _parentscene = parent_scene;
  1483. _heightField = heightField;
  1484. float _linearDamping = 0.0f;
  1485. float _angularDamping = 0.0f;
  1486. float _friction = 0.5f;
  1487. float _restitution = 0.0f;
  1488. Matrix _startTransform = Matrix.Identity;
  1489. Matrix _centerOfMassOffset = Matrix.Identity;
  1490. lock (BulletXScene.BulletXLock)
  1491. {
  1492. try
  1493. {
  1494. _startTransform.Translation = BulletXMaths.PhysicsVectorToXnaVector3(_staticPosition);
  1495. CollisionShape _collisionShape =
  1496. new HeightfieldTerrainShape(BulletXScene.MaxXY, BulletXScene.MaxXY, _heightField,
  1497. (float) BulletXScene.MaxZ, 2, true, false);
  1498. DefaultMotionState _motionState = new DefaultMotionState(_startTransform, _centerOfMassOffset);
  1499. Vector3 _localInertia = new Vector3();
  1500. //_collisionShape.CalculateLocalInertia(_mass, out _localInertia); //Always when mass > 0
  1501. _flatPlanet =
  1502. new RigidBody(_mass, _motionState, _collisionShape, _localInertia, _linearDamping,
  1503. _angularDamping, _friction, _restitution);
  1504. //It's seems that there are a bug with rigidBody constructor and its CenterOfMassPosition
  1505. Vector3 _vDebugTranslation;
  1506. _vDebugTranslation = _startTransform.Translation - _flatPlanet.CenterOfMassPosition;
  1507. _flatPlanet.Translate(_vDebugTranslation);
  1508. parent_scene.ddWorld.AddRigidBody(_flatPlanet);
  1509. }
  1510. catch (Exception ex)
  1511. {
  1512. BulletXScene.BulletXMessage(ex.Message, true);
  1513. }
  1514. }
  1515. BulletXScene.BulletXMessage("BulletXPlanet created.", false);
  1516. }
  1517. internal float HeightValue(Vector3 position)
  1518. {
  1519. int li_x, li_y;
  1520. float height;
  1521. li_x = (int) Math.Round(position.X);
  1522. if (li_x < 0) li_x = 0;
  1523. if (li_x >= BulletXScene.MaxXY) li_x = BulletXScene.MaxXY - 1;
  1524. li_y = (int) Math.Round(position.Y);
  1525. if (li_y < 0) li_y = 0;
  1526. if (li_y >= BulletXScene.MaxXY) li_y = BulletXScene.MaxXY - 1;
  1527. height = ((HeightfieldTerrainShape) _flatPlanet.CollisionShape).getHeightFieldValue(li_x, li_y);
  1528. if (height < 0) height = 0;
  1529. else if (height > BulletXScene.MaxZ) height = BulletXScene.MaxZ;
  1530. return height;
  1531. }
  1532. }
  1533. }