BulletXPlugin.cs 62 KB

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