BSAPIXNA.cs 113 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387238823892390239123922393239423952396239723982399240024012402240324042405240624072408240924102411241224132414241524162417241824192420242124222423242424252426242724282429243024312432243324342435243624372438243924402441244224432444244524462447244824492450245124522453245424552456245724582459246024612462246324642465246624672468246924702471247224732474247524762477247824792480248124822483248424852486248724882489249024912492249324942495249624972498249925002501250225032504250525062507250825092510251125122513251425152516251725182519252025212522252325242525252625272528252925302531253225332534253525362537253825392540254125422543254425452546254725482549255025512552255325542555255625572558255925602561256225632564256525662567256825692570257125722573257425752576257725782579258025812582258325842585258625872588258925902591259225932594259525962597
  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 copyrightD
  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. using System;
  28. using System.Collections.Generic;
  29. using System.IO;
  30. using System.Runtime.InteropServices;
  31. using System.Text;
  32. using OpenSim.Framework;
  33. using OpenMetaverse;
  34. using BulletXNA;
  35. using BulletXNA.LinearMath;
  36. using BulletXNA.BulletCollision;
  37. using BulletXNA.BulletDynamics;
  38. using BulletXNA.BulletCollision.CollisionDispatch;
  39. namespace OpenSim.Region.PhysicsModule.BulletS
  40. {
  41. public sealed class BSAPIXNA : BSAPITemplate
  42. {
  43. private sealed class BulletWorldXNA : BulletWorld
  44. {
  45. public DiscreteDynamicsWorld world;
  46. public BulletWorldXNA(uint id, BSScene physScene, DiscreteDynamicsWorld xx)
  47. : base(id, physScene)
  48. {
  49. world = xx;
  50. }
  51. }
  52. private sealed class BulletBodyXNA : BulletBody
  53. {
  54. public CollisionObject body;
  55. public RigidBody rigidBody { get { return RigidBody.Upcast(body); } }
  56. public BulletBodyXNA(uint id, CollisionObject xx)
  57. : base(id)
  58. {
  59. body = xx;
  60. }
  61. public override bool HasPhysicalBody
  62. {
  63. get { return body != null; }
  64. }
  65. public override void Clear()
  66. {
  67. body = null;
  68. }
  69. public override string AddrString
  70. {
  71. get { return "XNARigidBody"; }
  72. }
  73. }
  74. private sealed class BulletShapeXNA : BulletShape
  75. {
  76. public CollisionShape shape;
  77. public BulletShapeXNA(CollisionShape xx, BSPhysicsShapeType typ)
  78. : base()
  79. {
  80. shape = xx;
  81. shapeType = typ;
  82. }
  83. public override bool HasPhysicalShape
  84. {
  85. get { return shape != null; }
  86. }
  87. public override void Clear()
  88. {
  89. shape = null;
  90. }
  91. public override BulletShape Clone()
  92. {
  93. return new BulletShapeXNA(shape, shapeType);
  94. }
  95. public override bool ReferenceSame(BulletShape other)
  96. {
  97. BulletShapeXNA otheru = other as BulletShapeXNA;
  98. return (otheru != null) && (this.shape == otheru.shape);
  99. }
  100. public override string AddrString
  101. {
  102. get { return "XNACollisionShape"; }
  103. }
  104. }
  105. private sealed class BulletConstraintXNA : BulletConstraint
  106. {
  107. public TypedConstraint constrain;
  108. public BulletConstraintXNA(TypedConstraint xx) : base()
  109. {
  110. constrain = xx;
  111. }
  112. public override void Clear()
  113. {
  114. constrain = null;
  115. }
  116. public override bool HasPhysicalConstraint { get { return constrain != null; } }
  117. // Used for log messages for a unique display of the memory/object allocated to this instance
  118. public override string AddrString
  119. {
  120. get { return "XNAConstraint"; }
  121. }
  122. }
  123. internal int m_maxCollisions;
  124. internal CollisionDesc[] UpdatedCollisions;
  125. internal int LastCollisionDesc = 0;
  126. internal int m_maxUpdatesPerFrame;
  127. internal int LastEntityProperty = 0;
  128. internal EntityProperties[] UpdatedObjects;
  129. internal Dictionary<uint, GhostObject> specialCollisionObjects;
  130. private static int m_collisionsThisFrame;
  131. private BSScene PhysicsScene { get; set; }
  132. public override string BulletEngineName { get { return "BulletXNA"; } }
  133. public override string BulletEngineVersion { get; protected set; }
  134. public BSAPIXNA(string paramName, BSScene physScene)
  135. {
  136. PhysicsScene = physScene;
  137. }
  138. /// <summary>
  139. ///
  140. /// </summary>
  141. /// <param name="p"></param>
  142. /// <param name="p_2"></param>
  143. public override bool RemoveObjectFromWorld(BulletWorld pWorld, BulletBody pBody)
  144. {
  145. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  146. RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
  147. CollisionObject collisionObject = ((BulletBodyXNA)pBody).body;
  148. if (body != null)
  149. world.RemoveRigidBody(body);
  150. else if (collisionObject != null)
  151. world.RemoveCollisionObject(collisionObject);
  152. else
  153. return false;
  154. return true;
  155. }
  156. public override bool ClearCollisionProxyCache(BulletWorld pWorld, BulletBody pBody)
  157. {
  158. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  159. RigidBody body = ((BulletBodyXNA)pBody).rigidBody;
  160. CollisionObject collisionObject = ((BulletBodyXNA)pBody).body;
  161. if (body != null && collisionObject != null && collisionObject.GetBroadphaseHandle() != null)
  162. {
  163. world.RemoveCollisionObject(collisionObject);
  164. world.AddCollisionObject(collisionObject);
  165. }
  166. return true;
  167. }
  168. public override bool AddConstraintToWorld(BulletWorld pWorld, BulletConstraint pConstraint, bool pDisableCollisionsBetweenLinkedObjects)
  169. {
  170. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  171. TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
  172. world.AddConstraint(constraint, pDisableCollisionsBetweenLinkedObjects);
  173. return true;
  174. }
  175. public override bool RemoveConstraintFromWorld(BulletWorld pWorld, BulletConstraint pConstraint)
  176. {
  177. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  178. TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
  179. world.RemoveConstraint(constraint);
  180. return true;
  181. }
  182. public override void SetRestitution(BulletBody pCollisionObject, float pRestitution)
  183. {
  184. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  185. collisionObject.SetRestitution(pRestitution);
  186. }
  187. public override int GetShapeType(BulletShape pShape)
  188. {
  189. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  190. return (int)shape.GetShapeType();
  191. }
  192. public override void SetMargin(BulletShape pShape, float pMargin)
  193. {
  194. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  195. shape.SetMargin(pMargin);
  196. }
  197. public override float GetMargin(BulletShape pShape)
  198. {
  199. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  200. return shape.GetMargin();
  201. }
  202. public override void SetLocalScaling(BulletShape pShape, Vector3 pScale)
  203. {
  204. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  205. IndexedVector3 vec = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
  206. shape.SetLocalScaling(ref vec);
  207. }
  208. public override void SetContactProcessingThreshold(BulletBody pCollisionObject, float contactprocessingthreshold)
  209. {
  210. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  211. collisionObject.SetContactProcessingThreshold(contactprocessingthreshold);
  212. }
  213. public override void SetCcdMotionThreshold(BulletBody pCollisionObject, float pccdMotionThreashold)
  214. {
  215. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  216. collisionObject.SetCcdMotionThreshold(pccdMotionThreashold);
  217. }
  218. public override void SetCcdSweptSphereRadius(BulletBody pCollisionObject, float pCcdSweptSphereRadius)
  219. {
  220. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  221. collisionObject.SetCcdSweptSphereRadius(pCcdSweptSphereRadius);
  222. }
  223. public override void SetAngularFactorV(BulletBody pBody, Vector3 pAngularFactor)
  224. {
  225. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  226. body.SetAngularFactor(new IndexedVector3(pAngularFactor.X, pAngularFactor.Y, pAngularFactor.Z));
  227. }
  228. public override CollisionFlags AddToCollisionFlags(BulletBody pCollisionObject, CollisionFlags pcollisionFlags)
  229. {
  230. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  231. CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)collisionObject.GetCollisionFlags();
  232. existingcollisionFlags |= pcollisionFlags;
  233. collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
  234. return (CollisionFlags) (uint) existingcollisionFlags;
  235. }
  236. public override bool AddObjectToWorld(BulletWorld pWorld, BulletBody pBody)
  237. {
  238. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  239. CollisionObject cbody = (pBody as BulletBodyXNA).body;
  240. RigidBody rbody = cbody as RigidBody;
  241. // Bullet resets several variables when an object is added to the world. In particular,
  242. // BulletXNA resets position and rotation. Gravity is also reset depending on the static/dynamic
  243. // type. Of course, the collision flags in the broadphase proxy are initialized to default.
  244. IndexedMatrix origPos = cbody.GetWorldTransform();
  245. if (rbody != null)
  246. {
  247. IndexedVector3 origGrav = rbody.GetGravity();
  248. world.AddRigidBody(rbody);
  249. rbody.SetGravity(origGrav);
  250. }
  251. else
  252. {
  253. world.AddCollisionObject(cbody);
  254. }
  255. cbody.SetWorldTransform(origPos);
  256. pBody.ApplyCollisionMask(pWorld.physicsScene);
  257. //if (body.GetBroadphaseHandle() != null)
  258. // world.UpdateSingleAabb(body);
  259. return true;
  260. }
  261. public override void ForceActivationState(BulletBody pCollisionObject, ActivationState pActivationState)
  262. {
  263. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  264. collisionObject.ForceActivationState((BulletXNA.BulletCollision.ActivationState)(uint)pActivationState);
  265. }
  266. public override void UpdateSingleAabb(BulletWorld pWorld, BulletBody pCollisionObject)
  267. {
  268. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  269. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  270. world.UpdateSingleAabb(collisionObject);
  271. }
  272. public override void UpdateAabbs(BulletWorld pWorld) {
  273. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  274. world.UpdateAabbs();
  275. }
  276. public override bool GetForceUpdateAllAabbs(BulletWorld pWorld) {
  277. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  278. return world.GetForceUpdateAllAabbs();
  279. }
  280. public override void SetForceUpdateAllAabbs(BulletWorld pWorld, bool pForce)
  281. {
  282. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  283. world.SetForceUpdateAllAabbs(pForce);
  284. }
  285. public override bool SetCollisionGroupMask(BulletBody pCollisionObject, uint pGroup, uint pMask)
  286. {
  287. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  288. collisionObject.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
  289. collisionObject.GetBroadphaseHandle().m_collisionFilterGroup = (BulletXNA.BulletCollision.CollisionFilterGroups) pGroup;
  290. if ((uint) collisionObject.GetBroadphaseHandle().m_collisionFilterGroup == 0)
  291. return false;
  292. return true;
  293. }
  294. public override void ClearAllForces(BulletBody pCollisionObject)
  295. {
  296. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  297. IndexedVector3 zeroVector = new IndexedVector3(0, 0, 0);
  298. collisionObject.SetInterpolationLinearVelocity(ref zeroVector);
  299. collisionObject.SetInterpolationAngularVelocity(ref zeroVector);
  300. IndexedMatrix bodytransform = collisionObject.GetWorldTransform();
  301. collisionObject.SetInterpolationWorldTransform(ref bodytransform);
  302. if (collisionObject is RigidBody)
  303. {
  304. RigidBody rigidbody = collisionObject as RigidBody;
  305. rigidbody.SetLinearVelocity(zeroVector);
  306. rigidbody.SetAngularVelocity(zeroVector);
  307. rigidbody.ClearForces();
  308. }
  309. }
  310. public override void SetInterpolationAngularVelocity(BulletBody pCollisionObject, Vector3 pVector3)
  311. {
  312. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  313. IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
  314. collisionObject.SetInterpolationAngularVelocity(ref vec);
  315. }
  316. public override void SetAngularVelocity(BulletBody pBody, Vector3 pVector3)
  317. {
  318. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  319. IndexedVector3 vec = new IndexedVector3(pVector3.X, pVector3.Y, pVector3.Z);
  320. body.SetAngularVelocity(ref vec);
  321. }
  322. public override Vector3 GetTotalForce(BulletBody pBody)
  323. {
  324. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  325. IndexedVector3 iv3 = body.GetTotalForce();
  326. return new Vector3(iv3.X, iv3.Y, iv3.Z);
  327. }
  328. public override Vector3 GetTotalTorque(BulletBody pBody)
  329. {
  330. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  331. IndexedVector3 iv3 = body.GetTotalTorque();
  332. return new Vector3(iv3.X, iv3.Y, iv3.Z);
  333. }
  334. public override Vector3 GetInvInertiaDiagLocal(BulletBody pBody)
  335. {
  336. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  337. IndexedVector3 iv3 = body.GetInvInertiaDiagLocal();
  338. return new Vector3(iv3.X, iv3.Y, iv3.Z);
  339. }
  340. public override void SetInvInertiaDiagLocal(BulletBody pBody, Vector3 inert)
  341. {
  342. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  343. IndexedVector3 iv3 = new IndexedVector3(inert.X, inert.Y, inert.Z);
  344. body.SetInvInertiaDiagLocal(ref iv3);
  345. }
  346. public override void ApplyForce(BulletBody pBody, Vector3 force, Vector3 pos)
  347. {
  348. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  349. IndexedVector3 forceiv3 = new IndexedVector3(force.X, force.Y, force.Z);
  350. IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
  351. body.ApplyForce(ref forceiv3, ref posiv3);
  352. }
  353. public override void ApplyImpulse(BulletBody pBody, Vector3 imp, Vector3 pos)
  354. {
  355. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  356. IndexedVector3 impiv3 = new IndexedVector3(imp.X, imp.Y, imp.Z);
  357. IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
  358. body.ApplyImpulse(ref impiv3, ref posiv3);
  359. }
  360. public override void ClearForces(BulletBody pBody)
  361. {
  362. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  363. body.ClearForces();
  364. }
  365. public override void SetTranslation(BulletBody pCollisionObject, Vector3 _position, Quaternion _orientation)
  366. {
  367. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  368. IndexedVector3 vposition = new IndexedVector3(_position.X, _position.Y, _position.Z);
  369. IndexedQuaternion vquaternion = new IndexedQuaternion(_orientation.X, _orientation.Y, _orientation.Z,
  370. _orientation.W);
  371. IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(vquaternion);
  372. mat._origin = vposition;
  373. collisionObject.SetWorldTransform(mat);
  374. }
  375. public override Vector3 GetPosition(BulletBody pCollisionObject)
  376. {
  377. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  378. IndexedVector3 pos = collisionObject.GetInterpolationWorldTransform()._origin;
  379. return new Vector3(pos.X, pos.Y, pos.Z);
  380. }
  381. public override Vector3 CalculateLocalInertia(BulletShape pShape, float pphysMass)
  382. {
  383. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  384. IndexedVector3 inertia = IndexedVector3.Zero;
  385. shape.CalculateLocalInertia(pphysMass, out inertia);
  386. return new Vector3(inertia.X, inertia.Y, inertia.Z);
  387. }
  388. public override void SetMassProps(BulletBody pBody, float pphysMass, Vector3 plocalInertia)
  389. {
  390. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  391. if (body != null) // Can't set mass props on collision object.
  392. {
  393. IndexedVector3 inertia = new IndexedVector3(plocalInertia.X, plocalInertia.Y, plocalInertia.Z);
  394. body.SetMassProps(pphysMass, inertia);
  395. }
  396. }
  397. public override void SetObjectForce(BulletBody pBody, Vector3 _force)
  398. {
  399. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  400. IndexedVector3 force = new IndexedVector3(_force.X, _force.Y, _force.Z);
  401. body.SetTotalForce(ref force);
  402. }
  403. public override void SetFriction(BulletBody pCollisionObject, float _currentFriction)
  404. {
  405. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  406. collisionObject.SetFriction(_currentFriction);
  407. }
  408. public override void SetLinearVelocity(BulletBody pBody, Vector3 _velocity)
  409. {
  410. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  411. IndexedVector3 velocity = new IndexedVector3(_velocity.X, _velocity.Y, _velocity.Z);
  412. body.SetLinearVelocity(velocity);
  413. }
  414. public override void Activate(BulletBody pCollisionObject, bool pforceactivation)
  415. {
  416. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  417. collisionObject.Activate(pforceactivation);
  418. }
  419. public override Quaternion GetOrientation(BulletBody pCollisionObject)
  420. {
  421. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  422. IndexedQuaternion mat = collisionObject.GetInterpolationWorldTransform().GetRotation();
  423. return new Quaternion(mat.X, mat.Y, mat.Z, mat.W);
  424. }
  425. public override CollisionFlags RemoveFromCollisionFlags(BulletBody pCollisionObject, CollisionFlags pcollisionFlags)
  426. {
  427. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  428. CollisionFlags existingcollisionFlags = (CollisionFlags)(uint)collisionObject.GetCollisionFlags();
  429. existingcollisionFlags &= ~pcollisionFlags;
  430. collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags)(uint)existingcollisionFlags);
  431. return (CollisionFlags)(uint)existingcollisionFlags;
  432. }
  433. public override float GetCcdMotionThreshold(BulletBody pCollisionObject)
  434. {
  435. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  436. return collisionObject.GetCcdSquareMotionThreshold();
  437. }
  438. public override float GetCcdSweptSphereRadius(BulletBody pCollisionObject)
  439. {
  440. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  441. return collisionObject.GetCcdSweptSphereRadius();
  442. }
  443. public override IntPtr GetUserPointer(BulletBody pCollisionObject)
  444. {
  445. CollisionObject shape = (pCollisionObject as BulletBodyXNA).body;
  446. return (IntPtr)shape.GetUserPointer();
  447. }
  448. public override void SetUserPointer(BulletBody pCollisionObject, IntPtr val)
  449. {
  450. CollisionObject shape = (pCollisionObject as BulletBodyXNA).body;
  451. shape.SetUserPointer(val);
  452. }
  453. public override void SetGravity(BulletBody pBody, Vector3 pGravity)
  454. {
  455. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  456. if (body != null) // Can't set collisionobject.set gravity
  457. {
  458. IndexedVector3 gravity = new IndexedVector3(pGravity.X, pGravity.Y, pGravity.Z);
  459. body.SetGravity(gravity);
  460. }
  461. }
  462. public override bool DestroyConstraint(BulletWorld pWorld, BulletConstraint pConstraint)
  463. {
  464. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  465. TypedConstraint constraint = (pConstraint as BulletConstraintXNA).constrain;
  466. world.RemoveConstraint(constraint);
  467. return true;
  468. }
  469. public override bool SetLinearLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high)
  470. {
  471. Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
  472. IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
  473. IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
  474. constraint.SetLinearLowerLimit(lowlimit);
  475. constraint.SetLinearUpperLimit(highlimit);
  476. return true;
  477. }
  478. public override bool SetAngularLimits(BulletConstraint pConstraint, Vector3 low, Vector3 high)
  479. {
  480. Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
  481. IndexedVector3 lowlimit = new IndexedVector3(low.X, low.Y, low.Z);
  482. IndexedVector3 highlimit = new IndexedVector3(high.X, high.Y, high.Z);
  483. constraint.SetAngularLowerLimit(lowlimit);
  484. constraint.SetAngularUpperLimit(highlimit);
  485. return true;
  486. }
  487. public override void SetConstraintNumSolverIterations(BulletConstraint pConstraint, float cnt)
  488. {
  489. Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
  490. constraint.SetOverrideNumSolverIterations((int)cnt);
  491. }
  492. public override bool CalculateTransforms(BulletConstraint pConstraint)
  493. {
  494. Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
  495. constraint.CalculateTransforms();
  496. return true;
  497. }
  498. public override void SetConstraintEnable(BulletConstraint pConstraint, float p_2)
  499. {
  500. Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
  501. constraint.SetEnabled((p_2 == 0) ? false : true);
  502. }
  503. public override BulletConstraint Create6DofConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
  504. Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot,
  505. bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
  506. {
  507. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  508. RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
  509. RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
  510. IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
  511. IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
  512. IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
  513. frame1._origin = frame1v;
  514. IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
  515. IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
  516. IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
  517. frame2._origin = frame1v;
  518. Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2,
  519. puseLinearReferenceFrameA);
  520. consttr.CalculateTransforms();
  521. world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies);
  522. return new BulletConstraintXNA(consttr);
  523. }
  524. public override BulletConstraint Create6DofConstraintFixed(BulletWorld pWorld, BulletBody pBody1,
  525. Vector3 pframe1, Quaternion pframe1rot,
  526. bool pUseLinearReferenceFrameB, bool pdisableCollisionsBetweenLinkedBodies)
  527. {
  528. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  529. RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
  530. IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
  531. IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
  532. IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
  533. frame1._origin = frame1v;
  534. Generic6DofConstraint consttr = new Generic6DofConstraint(body1, ref frame1, pUseLinearReferenceFrameB);
  535. consttr.CalculateTransforms();
  536. world.AddConstraint(consttr,pdisableCollisionsBetweenLinkedBodies);
  537. return new BulletConstraintXNA(consttr);
  538. }
  539. /// <summary>
  540. ///
  541. /// </summary>
  542. /// <param name="pWorld"></param>
  543. /// <param name="pBody1"></param>
  544. /// <param name="pBody2"></param>
  545. /// <param name="pjoinPoint"></param>
  546. /// <param name="puseLinearReferenceFrameA"></param>
  547. /// <param name="pdisableCollisionsBetweenLinkedBodies"></param>
  548. /// <returns></returns>
  549. public override BulletConstraint Create6DofConstraintToPoint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2, Vector3 pjoinPoint, bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
  550. {
  551. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  552. RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
  553. RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
  554. IndexedMatrix frame1 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
  555. IndexedMatrix frame2 = new IndexedMatrix(IndexedBasisMatrix.Identity, new IndexedVector3(0, 0, 0));
  556. IndexedVector3 joinPoint = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
  557. IndexedMatrix mat = IndexedMatrix.Identity;
  558. mat._origin = new IndexedVector3(pjoinPoint.X, pjoinPoint.Y, pjoinPoint.Z);
  559. frame1._origin = body1.GetWorldTransform().Inverse()*joinPoint;
  560. frame2._origin = body2.GetWorldTransform().Inverse()*joinPoint;
  561. Generic6DofConstraint consttr = new Generic6DofConstraint(body1, body2, ref frame1, ref frame2, puseLinearReferenceFrameA);
  562. consttr.CalculateTransforms();
  563. world.AddConstraint(consttr, pdisableCollisionsBetweenLinkedBodies);
  564. return new BulletConstraintXNA(consttr);
  565. }
  566. //SetFrames(m_constraint.ptr, frameA, frameArot, frameB, frameBrot);
  567. public override bool SetFrames(BulletConstraint pConstraint, Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot)
  568. {
  569. Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
  570. IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
  571. IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
  572. IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
  573. frame1._origin = frame1v;
  574. IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
  575. IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
  576. IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
  577. frame2._origin = frame2v;
  578. constraint.SetFrames(ref frame1, ref frame2);
  579. return true;
  580. }
  581. public override Vector3 GetLinearVelocity(BulletBody pBody)
  582. {
  583. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  584. IndexedVector3 iv3 = body.GetLinearVelocity();
  585. return new Vector3(iv3.X, iv3.Y, iv3.Z);
  586. }
  587. public override Vector3 GetAngularVelocity(BulletBody pBody)
  588. {
  589. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  590. IndexedVector3 iv3 = body.GetAngularVelocity();
  591. return new Vector3(iv3.X, iv3.Y, iv3.Z);
  592. }
  593. public override Vector3 GetVelocityInLocalPoint(BulletBody pBody, Vector3 pos)
  594. {
  595. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  596. IndexedVector3 posiv3 = new IndexedVector3(pos.X, pos.Y, pos.Z);
  597. IndexedVector3 iv3 = body.GetVelocityInLocalPoint(ref posiv3);
  598. return new Vector3(iv3.X, iv3.Y, iv3.Z);
  599. }
  600. public override void Translate(BulletBody pCollisionObject, Vector3 trans)
  601. {
  602. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  603. collisionObject.Translate(new IndexedVector3(trans.X,trans.Y,trans.Z));
  604. }
  605. public override void UpdateDeactivation(BulletBody pBody, float timeStep)
  606. {
  607. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  608. body.UpdateDeactivation(timeStep);
  609. }
  610. public override bool WantsSleeping(BulletBody pBody)
  611. {
  612. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  613. return body.WantsSleeping();
  614. }
  615. public override void SetAngularFactor(BulletBody pBody, float factor)
  616. {
  617. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  618. body.SetAngularFactor(factor);
  619. }
  620. public override Vector3 GetAngularFactor(BulletBody pBody)
  621. {
  622. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  623. IndexedVector3 iv3 = body.GetAngularFactor();
  624. return new Vector3(iv3.X, iv3.Y, iv3.Z);
  625. }
  626. public override bool IsInWorld(BulletWorld pWorld, BulletBody pCollisionObject)
  627. {
  628. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  629. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  630. return world.IsInWorld(collisionObject);
  631. }
  632. public override void AddConstraintRef(BulletBody pBody, BulletConstraint pConstraint)
  633. {
  634. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  635. TypedConstraint constrain = (pConstraint as BulletConstraintXNA).constrain;
  636. body.AddConstraintRef(constrain);
  637. }
  638. public override void RemoveConstraintRef(BulletBody pBody, BulletConstraint pConstraint)
  639. {
  640. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  641. TypedConstraint constrain = (pConstraint as BulletConstraintXNA).constrain;
  642. body.RemoveConstraintRef(constrain);
  643. }
  644. public override BulletConstraint GetConstraintRef(BulletBody pBody, int index)
  645. {
  646. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  647. return new BulletConstraintXNA(body.GetConstraintRef(index));
  648. }
  649. public override int GetNumConstraintRefs(BulletBody pBody)
  650. {
  651. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  652. return body.GetNumConstraintRefs();
  653. }
  654. public override void SetInterpolationLinearVelocity(BulletBody pCollisionObject, Vector3 VehicleVelocity)
  655. {
  656. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  657. IndexedVector3 velocity = new IndexedVector3(VehicleVelocity.X, VehicleVelocity.Y, VehicleVelocity.Z);
  658. collisionObject.SetInterpolationLinearVelocity(ref velocity);
  659. }
  660. public override bool UseFrameOffset(BulletConstraint pConstraint, float onOff)
  661. {
  662. Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
  663. constraint.SetUseFrameOffset((onOff == 0) ? false : true);
  664. return true;
  665. }
  666. //SetBreakingImpulseThreshold(m_constraint.ptr, threshold);
  667. public override bool SetBreakingImpulseThreshold(BulletConstraint pConstraint, float threshold)
  668. {
  669. Generic6DofConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
  670. constraint.SetBreakingImpulseThreshold(threshold);
  671. return true;
  672. }
  673. public override bool HingeSetLimits(BulletConstraint pConstraint, float low, float high, float softness, float bias, float relaxation)
  674. {
  675. HingeConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as HingeConstraint;
  676. if (softness == HINGE_NOT_SPECIFIED)
  677. constraint.SetLimit(low, high);
  678. else
  679. constraint.SetLimit(low, high, softness, bias, relaxation);
  680. return true;
  681. }
  682. public override bool SpringEnable(BulletConstraint pConstraint, int index, float numericTrueFalse)
  683. {
  684. Generic6DofSpringConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofSpringConstraint;
  685. constraint.EnableSpring(index, (numericTrueFalse == 0f ? false : true));
  686. return true;
  687. }
  688. public override bool SpringSetEquilibriumPoint(BulletConstraint pConstraint, int index, float equilibriumPoint)
  689. {
  690. Generic6DofSpringConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofSpringConstraint;
  691. if (index == SPRING_NOT_SPECIFIED)
  692. {
  693. constraint.SetEquilibriumPoint();
  694. }
  695. else
  696. {
  697. if (equilibriumPoint == SPRING_NOT_SPECIFIED)
  698. constraint.SetEquilibriumPoint(index);
  699. else
  700. constraint.SetEquilibriumPoint(index, equilibriumPoint);
  701. }
  702. return true;
  703. }
  704. public override bool SpringSetStiffness(BulletConstraint pConstraint, int index, float stiffness)
  705. {
  706. Generic6DofSpringConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofSpringConstraint;
  707. constraint.SetStiffness(index, stiffness);
  708. return true;
  709. }
  710. public override bool SpringSetDamping(BulletConstraint pConstraint, int index, float damping)
  711. {
  712. Generic6DofSpringConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as Generic6DofSpringConstraint;
  713. constraint.SetDamping(index, damping);
  714. return true;
  715. }
  716. public override bool SliderSetLimits(BulletConstraint pConstraint, int lowerUpper, int linAng, float val)
  717. {
  718. SliderConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as SliderConstraint;
  719. switch (lowerUpper)
  720. {
  721. case SLIDER_LOWER_LIMIT:
  722. switch (linAng)
  723. {
  724. case SLIDER_LINEAR:
  725. constraint.SetLowerLinLimit(val);
  726. break;
  727. case SLIDER_ANGULAR:
  728. constraint.SetLowerAngLimit(val);
  729. break;
  730. }
  731. break;
  732. case SLIDER_UPPER_LIMIT:
  733. switch (linAng)
  734. {
  735. case SLIDER_LINEAR:
  736. constraint.SetUpperLinLimit(val);
  737. break;
  738. case SLIDER_ANGULAR:
  739. constraint.SetUpperAngLimit(val);
  740. break;
  741. }
  742. break;
  743. }
  744. return true;
  745. }
  746. public override bool SliderSet(BulletConstraint pConstraint, int softRestDamp, int dirLimOrtho, int linAng, float val)
  747. {
  748. SliderConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as SliderConstraint;
  749. switch (softRestDamp)
  750. {
  751. case SLIDER_SET_SOFTNESS:
  752. switch (dirLimOrtho)
  753. {
  754. case SLIDER_SET_DIRECTION:
  755. switch (linAng)
  756. {
  757. case SLIDER_LINEAR: constraint.SetSoftnessDirLin(val); break;
  758. case SLIDER_ANGULAR: constraint.SetSoftnessDirAng(val); break;
  759. }
  760. break;
  761. case SLIDER_SET_LIMIT:
  762. switch (linAng)
  763. {
  764. case SLIDER_LINEAR: constraint.SetSoftnessLimLin(val); break;
  765. case SLIDER_ANGULAR: constraint.SetSoftnessLimAng(val); break;
  766. }
  767. break;
  768. case SLIDER_SET_ORTHO:
  769. switch (linAng)
  770. {
  771. case SLIDER_LINEAR: constraint.SetSoftnessOrthoLin(val); break;
  772. case SLIDER_ANGULAR: constraint.SetSoftnessOrthoAng(val); break;
  773. }
  774. break;
  775. }
  776. break;
  777. case SLIDER_SET_RESTITUTION:
  778. switch (dirLimOrtho)
  779. {
  780. case SLIDER_SET_DIRECTION:
  781. switch (linAng)
  782. {
  783. case SLIDER_LINEAR: constraint.SetRestitutionDirLin(val); break;
  784. case SLIDER_ANGULAR: constraint.SetRestitutionDirAng(val); break;
  785. }
  786. break;
  787. case SLIDER_SET_LIMIT:
  788. switch (linAng)
  789. {
  790. case SLIDER_LINEAR: constraint.SetRestitutionLimLin(val); break;
  791. case SLIDER_ANGULAR: constraint.SetRestitutionLimAng(val); break;
  792. }
  793. break;
  794. case SLIDER_SET_ORTHO:
  795. switch (linAng)
  796. {
  797. case SLIDER_LINEAR: constraint.SetRestitutionOrthoLin(val); break;
  798. case SLIDER_ANGULAR: constraint.SetRestitutionOrthoAng(val); break;
  799. }
  800. break;
  801. }
  802. break;
  803. case SLIDER_SET_DAMPING:
  804. switch (dirLimOrtho)
  805. {
  806. case SLIDER_SET_DIRECTION:
  807. switch (linAng)
  808. {
  809. case SLIDER_LINEAR: constraint.SetDampingDirLin(val); break;
  810. case SLIDER_ANGULAR: constraint.SetDampingDirAng(val); break;
  811. }
  812. break;
  813. case SLIDER_SET_LIMIT:
  814. switch (linAng)
  815. {
  816. case SLIDER_LINEAR: constraint.SetDampingLimLin(val); break;
  817. case SLIDER_ANGULAR: constraint.SetDampingLimAng(val); break;
  818. }
  819. break;
  820. case SLIDER_SET_ORTHO:
  821. switch (linAng)
  822. {
  823. case SLIDER_LINEAR: constraint.SetDampingOrthoLin(val); break;
  824. case SLIDER_ANGULAR: constraint.SetDampingOrthoAng(val); break;
  825. }
  826. break;
  827. }
  828. break;
  829. }
  830. return true;
  831. }
  832. public override bool SliderMotorEnable(BulletConstraint pConstraint, int linAng, float numericTrueFalse)
  833. {
  834. SliderConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as SliderConstraint;
  835. switch (linAng)
  836. {
  837. case SLIDER_LINEAR:
  838. constraint.SetPoweredLinMotor(numericTrueFalse == 0.0 ? false : true);
  839. break;
  840. case SLIDER_ANGULAR:
  841. constraint.SetPoweredAngMotor(numericTrueFalse == 0.0 ? false : true);
  842. break;
  843. }
  844. return true;
  845. }
  846. public override bool SliderMotor(BulletConstraint pConstraint, int forceVel, int linAng, float val)
  847. {
  848. SliderConstraint constraint = (pConstraint as BulletConstraintXNA).constrain as SliderConstraint;
  849. switch (forceVel)
  850. {
  851. case SLIDER_MOTOR_VELOCITY:
  852. switch (linAng)
  853. {
  854. case SLIDER_LINEAR:
  855. constraint.SetTargetLinMotorVelocity(val);
  856. break;
  857. case SLIDER_ANGULAR:
  858. constraint.SetTargetAngMotorVelocity(val);
  859. break;
  860. }
  861. break;
  862. case SLIDER_MAX_MOTOR_FORCE:
  863. switch (linAng)
  864. {
  865. case SLIDER_LINEAR:
  866. constraint.SetMaxLinMotorForce(val);
  867. break;
  868. case SLIDER_ANGULAR:
  869. constraint.SetMaxAngMotorForce(val);
  870. break;
  871. }
  872. break;
  873. }
  874. return true;
  875. }
  876. //BulletSimAPI.SetAngularDamping(Prim.PhysBody.ptr, angularDamping);
  877. public override void SetAngularDamping(BulletBody pBody, float angularDamping)
  878. {
  879. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  880. float lineardamping = body.GetLinearDamping();
  881. body.SetDamping(lineardamping, angularDamping);
  882. }
  883. public override void UpdateInertiaTensor(BulletBody pBody)
  884. {
  885. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  886. if (body != null) // can't update inertia tensor on CollisionObject
  887. body.UpdateInertiaTensor();
  888. }
  889. public override void RecalculateCompoundShapeLocalAabb(BulletShape pCompoundShape)
  890. {
  891. CompoundShape shape = (pCompoundShape as BulletShapeXNA).shape as CompoundShape;
  892. shape.RecalculateLocalAabb();
  893. }
  894. //BulletSimAPI.GetCollisionFlags(PhysBody.ptr)
  895. public override CollisionFlags GetCollisionFlags(BulletBody pCollisionObject)
  896. {
  897. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  898. uint flags = (uint)collisionObject.GetCollisionFlags();
  899. return (CollisionFlags) flags;
  900. }
  901. public override void SetDamping(BulletBody pBody, float pLinear, float pAngular)
  902. {
  903. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  904. body.SetDamping(pLinear, pAngular);
  905. }
  906. //PhysBody.ptr, PhysicsScene.Params.deactivationTime);
  907. public override void SetDeactivationTime(BulletBody pCollisionObject, float pDeactivationTime)
  908. {
  909. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  910. collisionObject.SetDeactivationTime(pDeactivationTime);
  911. }
  912. //SetSleepingThresholds(PhysBody.ptr, PhysicsScene.Params.linearSleepingThreshold, PhysicsScene.Params.angularSleepingThreshold);
  913. public override void SetSleepingThresholds(BulletBody pBody, float plinearSleepingThreshold, float pangularSleepingThreshold)
  914. {
  915. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  916. body.SetSleepingThresholds(plinearSleepingThreshold, pangularSleepingThreshold);
  917. }
  918. public override CollisionObjectTypes GetBodyType(BulletBody pCollisionObject)
  919. {
  920. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  921. return (CollisionObjectTypes)(int) collisionObject.GetInternalType();
  922. }
  923. public override void ApplyGravity(BulletBody pBody)
  924. {
  925. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  926. body.ApplyGravity();
  927. }
  928. public override Vector3 GetGravity(BulletBody pBody)
  929. {
  930. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  931. IndexedVector3 gravity = body.GetGravity();
  932. return new Vector3(gravity.X, gravity.Y, gravity.Z);
  933. }
  934. public override void SetLinearDamping(BulletBody pBody, float lin_damping)
  935. {
  936. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  937. float angularDamping = body.GetAngularDamping();
  938. body.SetDamping(lin_damping, angularDamping);
  939. }
  940. public override float GetLinearDamping(BulletBody pBody)
  941. {
  942. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  943. return body.GetLinearDamping();
  944. }
  945. public override float GetAngularDamping(BulletBody pBody)
  946. {
  947. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  948. return body.GetAngularDamping();
  949. }
  950. public override float GetLinearSleepingThreshold(BulletBody pBody)
  951. {
  952. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  953. return body.GetLinearSleepingThreshold();
  954. }
  955. public override void ApplyDamping(BulletBody pBody, float timeStep)
  956. {
  957. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  958. body.ApplyDamping(timeStep);
  959. }
  960. public override Vector3 GetLinearFactor(BulletBody pBody)
  961. {
  962. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  963. IndexedVector3 linearFactor = body.GetLinearFactor();
  964. return new Vector3(linearFactor.X, linearFactor.Y, linearFactor.Z);
  965. }
  966. public override void SetLinearFactor(BulletBody pBody, Vector3 factor)
  967. {
  968. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  969. body.SetLinearFactor(new IndexedVector3(factor.X, factor.Y, factor.Z));
  970. }
  971. public override void SetCenterOfMassByPosRot(BulletBody pBody, Vector3 pos, Quaternion rot)
  972. {
  973. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  974. IndexedQuaternion quat = new IndexedQuaternion(rot.X, rot.Y, rot.Z,rot.W);
  975. IndexedMatrix mat = IndexedMatrix.CreateFromQuaternion(quat);
  976. mat._origin = new IndexedVector3(pos.X, pos.Y, pos.Z);
  977. body.SetCenterOfMassTransform( ref mat);
  978. /* TODO: double check this */
  979. }
  980. //BulletSimAPI.ApplyCentralForce(PhysBody.ptr, fSum);
  981. public override void ApplyCentralForce(BulletBody pBody, Vector3 pfSum)
  982. {
  983. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  984. IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
  985. body.ApplyCentralForce(ref fSum);
  986. }
  987. public override void ApplyCentralImpulse(BulletBody pBody, Vector3 pfSum)
  988. {
  989. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  990. IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
  991. body.ApplyCentralImpulse(ref fSum);
  992. }
  993. public override void ApplyTorque(BulletBody pBody, Vector3 pfSum)
  994. {
  995. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  996. IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
  997. body.ApplyTorque(ref fSum);
  998. }
  999. public override void ApplyTorqueImpulse(BulletBody pBody, Vector3 pfSum)
  1000. {
  1001. RigidBody body = (pBody as BulletBodyXNA).rigidBody;
  1002. IndexedVector3 fSum = new IndexedVector3(pfSum.X, pfSum.Y, pfSum.Z);
  1003. body.ApplyTorqueImpulse(ref fSum);
  1004. }
  1005. public override void DestroyObject(BulletWorld pWorld, BulletBody pBody)
  1006. {
  1007. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1008. CollisionObject co = (pBody as BulletBodyXNA).rigidBody;
  1009. RigidBody bo = co as RigidBody;
  1010. if (bo == null)
  1011. {
  1012. if (world.IsInWorld(co))
  1013. {
  1014. world.RemoveCollisionObject(co);
  1015. }
  1016. }
  1017. else
  1018. {
  1019. if (world.IsInWorld(bo))
  1020. {
  1021. world.RemoveRigidBody(bo);
  1022. }
  1023. }
  1024. if (co != null)
  1025. {
  1026. if (co.GetUserPointer() != null)
  1027. {
  1028. uint localId = (uint) co.GetUserPointer();
  1029. if (specialCollisionObjects.ContainsKey(localId))
  1030. {
  1031. specialCollisionObjects.Remove(localId);
  1032. }
  1033. }
  1034. }
  1035. }
  1036. public override void Shutdown(BulletWorld pWorld)
  1037. {
  1038. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1039. world.Cleanup();
  1040. }
  1041. public override BulletShape DuplicateCollisionShape(BulletWorld pWorld, BulletShape pShape, uint id)
  1042. {
  1043. CollisionShape shape1 = (pShape as BulletShapeXNA).shape;
  1044. // TODO: Turn this from a reference copy to a Value Copy.
  1045. BulletShapeXNA shape2 = new BulletShapeXNA(shape1, BSShapeTypeFromBroadPhaseNativeType(shape1.GetShapeType()));
  1046. return shape2;
  1047. }
  1048. public override bool DeleteCollisionShape(BulletWorld pWorld, BulletShape pShape)
  1049. {
  1050. //TODO:
  1051. return false;
  1052. }
  1053. //(sim.ptr, shape.ptr, prim.LocalID, prim.RawPosition, prim.RawOrientation);
  1054. public override BulletBody CreateBodyFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
  1055. {
  1056. CollisionWorld world = (pWorld as BulletWorldXNA).world;
  1057. IndexedMatrix mat =
  1058. IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
  1059. pRawOrientation.Z, pRawOrientation.W));
  1060. mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
  1061. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1062. //UpdateSingleAabb(world, shape);
  1063. // TODO: Feed Update array into null
  1064. SimMotionState motionState = new SimMotionState(this, pLocalID, mat, null);
  1065. RigidBody body = new RigidBody(0,motionState,shape,IndexedVector3.Zero);
  1066. RigidBodyConstructionInfo constructionInfo = new RigidBodyConstructionInfo(0, motionState, shape, IndexedVector3.Zero)
  1067. {
  1068. m_mass = 0
  1069. };
  1070. /*
  1071. m_mass = mass;
  1072. m_motionState =motionState;
  1073. m_collisionShape = collisionShape;
  1074. m_localInertia = localInertia;
  1075. m_linearDamping = 0f;
  1076. m_angularDamping = 0f;
  1077. m_friction = 0.5f;
  1078. m_restitution = 0f;
  1079. m_linearSleepingThreshold = 0.8f;
  1080. m_angularSleepingThreshold = 1f;
  1081. m_additionalDamping = false;
  1082. m_additionalDampingFactor = 0.005f;
  1083. m_additionalLinearDampingThresholdSqr = 0.01f;
  1084. m_additionalAngularDampingThresholdSqr = 0.01f;
  1085. m_additionalAngularDampingFactor = 0.01f;
  1086. m_startWorldTransform = IndexedMatrix.Identity;
  1087. */
  1088. body.SetUserPointer(pLocalID);
  1089. return new BulletBodyXNA(pLocalID, body);
  1090. }
  1091. public override BulletBody CreateBodyWithDefaultMotionState( BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
  1092. {
  1093. IndexedMatrix mat =
  1094. IndexedMatrix.CreateFromQuaternion(new IndexedQuaternion(pRawOrientation.X, pRawOrientation.Y,
  1095. pRawOrientation.Z, pRawOrientation.W));
  1096. mat._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
  1097. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1098. // TODO: Feed Update array into null
  1099. RigidBody body = new RigidBody(0, new DefaultMotionState( mat, IndexedMatrix.Identity), shape, IndexedVector3.Zero);
  1100. body.SetWorldTransform(mat);
  1101. body.SetUserPointer(pLocalID);
  1102. return new BulletBodyXNA(pLocalID, body);
  1103. }
  1104. //(m_mapInfo.terrainBody.ptr, CollisionFlags.CF_STATIC_OBJECT);
  1105. public override CollisionFlags SetCollisionFlags(BulletBody pCollisionObject, CollisionFlags collisionFlags)
  1106. {
  1107. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  1108. collisionObject.SetCollisionFlags((BulletXNA.BulletCollision.CollisionFlags) (uint) collisionFlags);
  1109. return (CollisionFlags)collisionObject.GetCollisionFlags();
  1110. }
  1111. public override Vector3 GetAnisotripicFriction(BulletConstraint pconstrain)
  1112. {
  1113. /* TODO */
  1114. return Vector3.Zero;
  1115. }
  1116. public override Vector3 SetAnisotripicFriction(BulletConstraint pconstrain, Vector3 frict) { /* TODO */ return Vector3.Zero; }
  1117. public override bool HasAnisotripicFriction(BulletConstraint pconstrain) { /* TODO */ return false; }
  1118. public override float GetContactProcessingThreshold(BulletBody pBody) { /* TODO */ return 0f; }
  1119. public override bool IsStaticObject(BulletBody pCollisionObject)
  1120. {
  1121. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  1122. return collisionObject.IsStaticObject();
  1123. }
  1124. public override bool IsKinematicObject(BulletBody pCollisionObject)
  1125. {
  1126. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  1127. return collisionObject.IsKinematicObject();
  1128. }
  1129. public override bool IsStaticOrKinematicObject(BulletBody pCollisionObject)
  1130. {
  1131. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  1132. return collisionObject.IsStaticOrKinematicObject();
  1133. }
  1134. public override bool HasContactResponse(BulletBody pCollisionObject)
  1135. {
  1136. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  1137. return collisionObject.HasContactResponse();
  1138. }
  1139. public override int GetActivationState(BulletBody pBody) { /* TODO */ return 0; }
  1140. public override void SetActivationState(BulletBody pBody, int state) { /* TODO */ }
  1141. public override float GetDeactivationTime(BulletBody pBody) { /* TODO */ return 0f; }
  1142. public override bool IsActive(BulletBody pBody) { /* TODO */ return false; }
  1143. public override float GetRestitution(BulletBody pBody) { /* TODO */ return 0f; }
  1144. public override float GetFriction(BulletBody pBody) { /* TODO */ return 0f; }
  1145. public override void SetInterpolationVelocity(BulletBody pBody, Vector3 linearVel, Vector3 angularVel) { /* TODO */ }
  1146. public override float GetHitFraction(BulletBody pBody) { /* TODO */ return 0f; }
  1147. //(m_mapInfo.terrainBody.ptr, PhysicsScene.Params.terrainHitFraction);
  1148. public override void SetHitFraction(BulletBody pCollisionObject, float pHitFraction)
  1149. {
  1150. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  1151. collisionObject.SetHitFraction(pHitFraction);
  1152. }
  1153. //BuildCapsuleShape(physicsScene.World.ptr, 1f, 1f, prim.Scale);
  1154. public override BulletShape BuildCapsuleShape(BulletWorld pWorld, float pRadius, float pHeight, Vector3 pScale)
  1155. {
  1156. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1157. IndexedVector3 scale = new IndexedVector3(pScale.X, pScale.Y, pScale.Z);
  1158. CapsuleShapeZ capsuleShapeZ = new CapsuleShapeZ(pRadius, pHeight);
  1159. capsuleShapeZ.SetMargin(world.WorldSettings.Params.collisionMargin);
  1160. capsuleShapeZ.SetLocalScaling(ref scale);
  1161. return new BulletShapeXNA(capsuleShapeZ, BSPhysicsShapeType.SHAPE_CAPSULE); ;
  1162. }
  1163. public override BulletWorld Initialize(Vector3 maxPosition, ConfigurationParameters parms,
  1164. int maxCollisions, ref CollisionDesc[] collisionArray,
  1165. int maxUpdates, ref EntityProperties[] updateArray
  1166. )
  1167. {
  1168. UpdatedObjects = updateArray;
  1169. UpdatedCollisions = collisionArray;
  1170. /* TODO */
  1171. ConfigurationParameters[] configparms = new ConfigurationParameters[1];
  1172. configparms[0] = parms;
  1173. Vector3 worldExtent = maxPosition;
  1174. m_maxCollisions = maxCollisions;
  1175. m_maxUpdatesPerFrame = maxUpdates;
  1176. specialCollisionObjects = new Dictionary<uint, GhostObject>();
  1177. return new BulletWorldXNA(1, PhysicsScene, BSAPIXNA.Initialize2(worldExtent, configparms, maxCollisions, ref collisionArray, maxUpdates, ref updateArray, null));
  1178. }
  1179. private static DiscreteDynamicsWorld Initialize2(Vector3 worldExtent,
  1180. ConfigurationParameters[] o,
  1181. int mMaxCollisionsPerFrame, ref CollisionDesc[] collisionArray,
  1182. int mMaxUpdatesPerFrame, ref EntityProperties[] updateArray,
  1183. object mDebugLogCallbackHandle)
  1184. {
  1185. CollisionWorld.WorldData.ParamData p = new CollisionWorld.WorldData.ParamData();
  1186. p.angularDamping = BSParam.AngularDamping;
  1187. p.defaultFriction = o[0].defaultFriction;
  1188. p.defaultFriction = o[0].defaultFriction;
  1189. p.defaultDensity = o[0].defaultDensity;
  1190. p.defaultRestitution = o[0].defaultRestitution;
  1191. p.collisionMargin = o[0].collisionMargin;
  1192. p.gravity = o[0].gravity;
  1193. p.linearDamping = BSParam.LinearDamping;
  1194. p.angularDamping = BSParam.AngularDamping;
  1195. p.deactivationTime = BSParam.DeactivationTime;
  1196. p.linearSleepingThreshold = BSParam.LinearSleepingThreshold;
  1197. p.angularSleepingThreshold = BSParam.AngularSleepingThreshold;
  1198. p.ccdMotionThreshold = BSParam.CcdMotionThreshold;
  1199. p.ccdSweptSphereRadius = BSParam.CcdSweptSphereRadius;
  1200. p.contactProcessingThreshold = BSParam.ContactProcessingThreshold;
  1201. p.terrainImplementation = BSParam.TerrainImplementation;
  1202. p.terrainFriction = BSParam.TerrainFriction;
  1203. p.terrainHitFraction = BSParam.TerrainHitFraction;
  1204. p.terrainRestitution = BSParam.TerrainRestitution;
  1205. p.terrainCollisionMargin = BSParam.TerrainCollisionMargin;
  1206. p.avatarFriction = BSParam.AvatarFriction;
  1207. p.avatarStandingFriction = BSParam.AvatarStandingFriction;
  1208. p.avatarDensity = BSParam.AvatarDensity;
  1209. p.avatarRestitution = BSParam.AvatarRestitution;
  1210. p.avatarCapsuleWidth = BSParam.AvatarCapsuleWidth;
  1211. p.avatarCapsuleDepth = BSParam.AvatarCapsuleDepth;
  1212. p.avatarCapsuleHeight = BSParam.AvatarCapsuleHeight;
  1213. p.avatarContactProcessingThreshold = BSParam.AvatarContactProcessingThreshold;
  1214. p.vehicleAngularDamping = BSParam.VehicleAngularDamping;
  1215. p.maxPersistantManifoldPoolSize = o[0].maxPersistantManifoldPoolSize;
  1216. p.maxCollisionAlgorithmPoolSize = o[0].maxCollisionAlgorithmPoolSize;
  1217. p.shouldDisableContactPoolDynamicAllocation = o[0].shouldDisableContactPoolDynamicAllocation;
  1218. p.shouldForceUpdateAllAabbs = o[0].shouldForceUpdateAllAabbs;
  1219. p.shouldRandomizeSolverOrder = o[0].shouldRandomizeSolverOrder;
  1220. p.shouldSplitSimulationIslands = o[0].shouldSplitSimulationIslands;
  1221. p.shouldEnableFrictionCaching = o[0].shouldEnableFrictionCaching;
  1222. p.numberOfSolverIterations = o[0].numberOfSolverIterations;
  1223. p.linksetImplementation = BSParam.LinksetImplementation;
  1224. p.linkConstraintUseFrameOffset = BSParam.NumericBool(BSParam.LinkConstraintUseFrameOffset);
  1225. p.linkConstraintEnableTransMotor = BSParam.NumericBool(BSParam.LinkConstraintEnableTransMotor);
  1226. p.linkConstraintTransMotorMaxVel = BSParam.LinkConstraintTransMotorMaxVel;
  1227. p.linkConstraintTransMotorMaxForce = BSParam.LinkConstraintTransMotorMaxForce;
  1228. p.linkConstraintERP = BSParam.LinkConstraintERP;
  1229. p.linkConstraintCFM = BSParam.LinkConstraintCFM;
  1230. p.linkConstraintSolverIterations = BSParam.LinkConstraintSolverIterations;
  1231. p.physicsLoggingFrames = o[0].physicsLoggingFrames;
  1232. DefaultCollisionConstructionInfo ccci = new DefaultCollisionConstructionInfo();
  1233. DefaultCollisionConfiguration cci = new DefaultCollisionConfiguration();
  1234. CollisionDispatcher m_dispatcher = new CollisionDispatcher(cci);
  1235. if (p.maxPersistantManifoldPoolSize > 0)
  1236. cci.m_persistentManifoldPoolSize = (int)p.maxPersistantManifoldPoolSize;
  1237. if (p.shouldDisableContactPoolDynamicAllocation !=0)
  1238. m_dispatcher.SetDispatcherFlags(DispatcherFlags.CD_DISABLE_CONTACTPOOL_DYNAMIC_ALLOCATION);
  1239. //if (p.maxCollisionAlgorithmPoolSize >0 )
  1240. DbvtBroadphase m_broadphase = new DbvtBroadphase();
  1241. //IndexedVector3 aabbMin = new IndexedVector3(0, 0, 0);
  1242. //IndexedVector3 aabbMax = new IndexedVector3(256, 256, 256);
  1243. //AxisSweep3Internal m_broadphase2 = new AxisSweep3Internal(ref aabbMin, ref aabbMax, Convert.ToInt32(0xfffe), 0xffff, ushort.MaxValue/2, null, true);
  1244. m_broadphase.GetOverlappingPairCache().SetInternalGhostPairCallback(new GhostPairCallback());
  1245. SequentialImpulseConstraintSolver m_solver = new SequentialImpulseConstraintSolver();
  1246. DiscreteDynamicsWorld world = new DiscreteDynamicsWorld(m_dispatcher, m_broadphase, m_solver, cci);
  1247. world.LastCollisionDesc = 0;
  1248. world.LastEntityProperty = 0;
  1249. world.WorldSettings.Params = p;
  1250. world.SetForceUpdateAllAabbs(p.shouldForceUpdateAllAabbs != 0);
  1251. world.GetSolverInfo().m_solverMode = SolverMode.SOLVER_USE_WARMSTARTING | SolverMode.SOLVER_SIMD;
  1252. if (p.shouldRandomizeSolverOrder != 0)
  1253. world.GetSolverInfo().m_solverMode |= SolverMode.SOLVER_RANDMIZE_ORDER;
  1254. world.GetSimulationIslandManager().SetSplitIslands(p.shouldSplitSimulationIslands != 0);
  1255. //world.GetDispatchInfo().m_enableSatConvex Not implemented in C# port
  1256. if (p.shouldEnableFrictionCaching != 0)
  1257. world.GetSolverInfo().m_solverMode |= SolverMode.SOLVER_ENABLE_FRICTION_DIRECTION_CACHING;
  1258. if (p.numberOfSolverIterations > 0)
  1259. world.GetSolverInfo().m_numIterations = (int) p.numberOfSolverIterations;
  1260. world.GetSolverInfo().m_damping = world.WorldSettings.Params.linearDamping;
  1261. world.GetSolverInfo().m_restitution = world.WorldSettings.Params.defaultRestitution;
  1262. world.GetSolverInfo().m_globalCfm = 0.0f;
  1263. world.GetSolverInfo().m_tau = 0.6f;
  1264. world.GetSolverInfo().m_friction = 0.3f;
  1265. world.GetSolverInfo().m_maxErrorReduction = 20f;
  1266. world.GetSolverInfo().m_numIterations = 10;
  1267. world.GetSolverInfo().m_erp = 0.2f;
  1268. world.GetSolverInfo().m_erp2 = 0.1f;
  1269. world.GetSolverInfo().m_sor = 1.0f;
  1270. world.GetSolverInfo().m_splitImpulse = false;
  1271. world.GetSolverInfo().m_splitImpulsePenetrationThreshold = -0.02f;
  1272. world.GetSolverInfo().m_linearSlop = 0.0f;
  1273. world.GetSolverInfo().m_warmstartingFactor = 0.85f;
  1274. world.GetSolverInfo().m_restingContactRestitutionThreshold = 2;
  1275. world.SetForceUpdateAllAabbs(true);
  1276. //BSParam.TerrainImplementation = 0;
  1277. world.SetGravity(new IndexedVector3(0,0,p.gravity));
  1278. // Turn off Pooling since globals and pooling are bad for threading.
  1279. BulletGlobals.VoronoiSimplexSolverPool.SetPoolingEnabled(false);
  1280. BulletGlobals.SubSimplexConvexCastPool.SetPoolingEnabled(false);
  1281. BulletGlobals.ManifoldPointPool.SetPoolingEnabled(false);
  1282. BulletGlobals.CastResultPool.SetPoolingEnabled(false);
  1283. BulletGlobals.SphereShapePool.SetPoolingEnabled(false);
  1284. BulletGlobals.DbvtNodePool.SetPoolingEnabled(false);
  1285. BulletGlobals.SingleRayCallbackPool.SetPoolingEnabled(false);
  1286. BulletGlobals.SubSimplexClosestResultPool.SetPoolingEnabled(false);
  1287. BulletGlobals.GjkPairDetectorPool.SetPoolingEnabled(false);
  1288. BulletGlobals.DbvtTreeColliderPool.SetPoolingEnabled(false);
  1289. BulletGlobals.SingleSweepCallbackPool.SetPoolingEnabled(false);
  1290. BulletGlobals.BroadphaseRayTesterPool.SetPoolingEnabled(false);
  1291. BulletGlobals.ClosestNotMeConvexResultCallbackPool.SetPoolingEnabled(false);
  1292. BulletGlobals.GjkEpaPenetrationDepthSolverPool.SetPoolingEnabled(false);
  1293. BulletGlobals.ContinuousConvexCollisionPool.SetPoolingEnabled(false);
  1294. BulletGlobals.DbvtStackDataBlockPool.SetPoolingEnabled(false);
  1295. BulletGlobals.BoxBoxCollisionAlgorithmPool.SetPoolingEnabled(false);
  1296. BulletGlobals.CompoundCollisionAlgorithmPool.SetPoolingEnabled(false);
  1297. BulletGlobals.ConvexConcaveCollisionAlgorithmPool.SetPoolingEnabled(false);
  1298. BulletGlobals.ConvexConvexAlgorithmPool.SetPoolingEnabled(false);
  1299. BulletGlobals.ConvexPlaneAlgorithmPool.SetPoolingEnabled(false);
  1300. BulletGlobals.SphereBoxCollisionAlgorithmPool.SetPoolingEnabled(false);
  1301. BulletGlobals.SphereSphereCollisionAlgorithmPool.SetPoolingEnabled(false);
  1302. BulletGlobals.SphereTriangleCollisionAlgorithmPool.SetPoolingEnabled(false);
  1303. BulletGlobals.GImpactCollisionAlgorithmPool.SetPoolingEnabled(false);
  1304. BulletGlobals.GjkEpaSolver2MinkowskiDiffPool.SetPoolingEnabled(false);
  1305. BulletGlobals.PersistentManifoldPool.SetPoolingEnabled(false);
  1306. BulletGlobals.ManifoldResultPool.SetPoolingEnabled(false);
  1307. BulletGlobals.GJKPool.SetPoolingEnabled(false);
  1308. BulletGlobals.GIM_ShapeRetrieverPool.SetPoolingEnabled(false);
  1309. BulletGlobals.TriangleShapePool.SetPoolingEnabled(false);
  1310. BulletGlobals.SphereTriangleDetectorPool.SetPoolingEnabled(false);
  1311. BulletGlobals.CompoundLeafCallbackPool.SetPoolingEnabled(false);
  1312. BulletGlobals.GjkConvexCastPool.SetPoolingEnabled(false);
  1313. BulletGlobals.LocalTriangleSphereCastCallbackPool.SetPoolingEnabled(false);
  1314. BulletGlobals.BridgeTriangleRaycastCallbackPool.SetPoolingEnabled(false);
  1315. BulletGlobals.BridgeTriangleConcaveRaycastCallbackPool.SetPoolingEnabled(false);
  1316. BulletGlobals.BridgeTriangleConvexcastCallbackPool.SetPoolingEnabled(false);
  1317. BulletGlobals.MyNodeOverlapCallbackPool.SetPoolingEnabled(false);
  1318. BulletGlobals.ClosestRayResultCallbackPool.SetPoolingEnabled(false);
  1319. BulletGlobals.DebugDrawcallbackPool.SetPoolingEnabled(false);
  1320. return world;
  1321. }
  1322. //m_constraint.ptr, ConstraintParams.BT_CONSTRAINT_STOP_CFM, cfm, ConstraintParamAxis.AXIS_ALL
  1323. public override bool SetConstraintParam(BulletConstraint pConstraint, ConstraintParams paramIndex, float paramvalue, ConstraintParamAxis axis)
  1324. {
  1325. Generic6DofConstraint constrain = (pConstraint as BulletConstraintXNA).constrain as Generic6DofConstraint;
  1326. if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL || axis == ConstraintParamAxis.AXIS_ALL)
  1327. {
  1328. constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 0);
  1329. constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 1);
  1330. constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams) (int) paramIndex, paramvalue, 2);
  1331. }
  1332. if (axis == ConstraintParamAxis.AXIS_ANGULAR_ALL || axis == ConstraintParamAxis.AXIS_ALL)
  1333. {
  1334. constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 3);
  1335. constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 4);
  1336. constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, 5);
  1337. }
  1338. if (axis == ConstraintParamAxis.AXIS_LINEAR_ALL)
  1339. {
  1340. constrain.SetParam((BulletXNA.BulletDynamics.ConstraintParams)(int)paramIndex, paramvalue, (int)axis);
  1341. }
  1342. return true;
  1343. }
  1344. public override bool PushUpdate(BulletBody pCollisionObject)
  1345. {
  1346. bool ret = false;
  1347. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  1348. RigidBody rb = collisionObject as RigidBody;
  1349. if (rb != null)
  1350. {
  1351. SimMotionState sms = rb.GetMotionState() as SimMotionState;
  1352. if (sms != null)
  1353. {
  1354. IndexedMatrix wt = IndexedMatrix.Identity;
  1355. sms.GetWorldTransform(out wt);
  1356. sms.SetWorldTransform(ref wt, true);
  1357. ret = true;
  1358. }
  1359. }
  1360. return ret;
  1361. }
  1362. public override float GetAngularMotionDisc(BulletShape pShape)
  1363. {
  1364. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1365. return shape.GetAngularMotionDisc();
  1366. }
  1367. public override float GetContactBreakingThreshold(BulletShape pShape, float defaultFactor)
  1368. {
  1369. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1370. return shape.GetContactBreakingThreshold(defaultFactor);
  1371. }
  1372. public override bool IsCompound(BulletShape pShape)
  1373. {
  1374. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1375. return shape.IsCompound();
  1376. }
  1377. public override bool IsSoftBody(BulletShape pShape)
  1378. {
  1379. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1380. return shape.IsSoftBody();
  1381. }
  1382. public override bool IsPolyhedral(BulletShape pShape)
  1383. {
  1384. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1385. return shape.IsPolyhedral();
  1386. }
  1387. public override bool IsConvex2d(BulletShape pShape)
  1388. {
  1389. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1390. return shape.IsConvex2d();
  1391. }
  1392. public override bool IsConvex(BulletShape pShape)
  1393. {
  1394. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1395. return shape.IsConvex();
  1396. }
  1397. public override bool IsNonMoving(BulletShape pShape)
  1398. {
  1399. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1400. return shape.IsNonMoving();
  1401. }
  1402. public override bool IsConcave(BulletShape pShape)
  1403. {
  1404. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1405. return shape.IsConcave();
  1406. }
  1407. public override bool IsInfinite(BulletShape pShape)
  1408. {
  1409. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1410. return shape.IsInfinite();
  1411. }
  1412. public override bool IsNativeShape(BulletShape pShape)
  1413. {
  1414. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1415. bool ret;
  1416. switch (shape.GetShapeType())
  1417. {
  1418. case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
  1419. case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
  1420. case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
  1421. case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
  1422. ret = true;
  1423. break;
  1424. default:
  1425. ret = false;
  1426. break;
  1427. }
  1428. return ret;
  1429. }
  1430. public override void SetShapeCollisionMargin(BulletShape pShape, float pMargin)
  1431. {
  1432. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1433. shape.SetMargin(pMargin);
  1434. }
  1435. //sim.ptr, shape.ptr,prim.LocalID, prim.RawPosition, prim.RawOrientation
  1436. public override BulletBody CreateGhostFromShape(BulletWorld pWorld, BulletShape pShape, uint pLocalID, Vector3 pRawPosition, Quaternion pRawOrientation)
  1437. {
  1438. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1439. IndexedMatrix bodyTransform = new IndexedMatrix();
  1440. bodyTransform._origin = new IndexedVector3(pRawPosition.X, pRawPosition.Y, pRawPosition.Z);
  1441. bodyTransform.SetRotation(new IndexedQuaternion(pRawOrientation.X,pRawOrientation.Y,pRawOrientation.Z,pRawOrientation.W));
  1442. GhostObject gObj = new PairCachingGhostObject();
  1443. gObj.SetWorldTransform(bodyTransform);
  1444. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1445. gObj.SetCollisionShape(shape);
  1446. gObj.SetUserPointer(pLocalID);
  1447. if (specialCollisionObjects.ContainsKey(pLocalID))
  1448. specialCollisionObjects[pLocalID] = gObj;
  1449. else
  1450. specialCollisionObjects.Add(pLocalID, gObj);
  1451. // TODO: Add to Special CollisionObjects!
  1452. return new BulletBodyXNA(pLocalID, gObj);
  1453. }
  1454. public override void SetCollisionShape(BulletWorld pWorld, BulletBody pCollisionObject, BulletShape pShape)
  1455. {
  1456. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1457. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).body;
  1458. if (pShape == null)
  1459. {
  1460. collisionObject.SetCollisionShape(new EmptyShape());
  1461. }
  1462. else
  1463. {
  1464. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  1465. collisionObject.SetCollisionShape(shape);
  1466. }
  1467. }
  1468. public override BulletShape GetCollisionShape(BulletBody pCollisionObject)
  1469. {
  1470. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  1471. CollisionShape shape = collisionObject.GetCollisionShape();
  1472. return new BulletShapeXNA(shape, BSShapeTypeFromBroadPhaseNativeType(shape.GetShapeType()));
  1473. }
  1474. //(PhysicsScene.World.ptr, nativeShapeData)
  1475. public override BulletShape BuildNativeShape(BulletWorld pWorld, ShapeData pShapeData)
  1476. {
  1477. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1478. CollisionShape shape = null;
  1479. switch (pShapeData.Type)
  1480. {
  1481. case BSPhysicsShapeType.SHAPE_BOX:
  1482. shape = new BoxShape(new IndexedVector3(0.5f,0.5f,0.5f));
  1483. break;
  1484. case BSPhysicsShapeType.SHAPE_CONE:
  1485. shape = new ConeShapeZ(0.5f, 1.0f);
  1486. break;
  1487. case BSPhysicsShapeType.SHAPE_CYLINDER:
  1488. shape = new CylinderShapeZ(new IndexedVector3(0.5f, 0.5f, 0.5f));
  1489. break;
  1490. case BSPhysicsShapeType.SHAPE_SPHERE:
  1491. shape = new SphereShape(0.5f);
  1492. break;
  1493. }
  1494. if (shape != null)
  1495. {
  1496. IndexedVector3 scaling = new IndexedVector3(pShapeData.Scale.X, pShapeData.Scale.Y, pShapeData.Scale.Z);
  1497. shape.SetMargin(world.WorldSettings.Params.collisionMargin);
  1498. shape.SetLocalScaling(ref scaling);
  1499. }
  1500. return new BulletShapeXNA(shape, pShapeData.Type);
  1501. }
  1502. //PhysicsScene.World.ptr, false
  1503. public override BulletShape CreateCompoundShape(BulletWorld pWorld, bool enableDynamicAabbTree)
  1504. {
  1505. return new BulletShapeXNA(new CompoundShape(enableDynamicAabbTree), BSPhysicsShapeType.SHAPE_COMPOUND);
  1506. }
  1507. public override int GetNumberOfCompoundChildren(BulletShape pCompoundShape)
  1508. {
  1509. CompoundShape compoundshape = (pCompoundShape as BulletShapeXNA).shape as CompoundShape;
  1510. return compoundshape.GetNumChildShapes();
  1511. }
  1512. //LinksetRoot.PhysShape.ptr, newShape.ptr, displacementPos, displacementRot
  1513. public override void AddChildShapeToCompoundShape(BulletShape pCShape, BulletShape paddShape, Vector3 displacementPos, Quaternion displacementRot)
  1514. {
  1515. IndexedMatrix relativeTransform = new IndexedMatrix();
  1516. CompoundShape compoundshape = (pCShape as BulletShapeXNA).shape as CompoundShape;
  1517. CollisionShape addshape = (paddShape as BulletShapeXNA).shape;
  1518. relativeTransform._origin = new IndexedVector3(displacementPos.X, displacementPos.Y, displacementPos.Z);
  1519. relativeTransform.SetRotation(new IndexedQuaternion(displacementRot.X,displacementRot.Y,displacementRot.Z,displacementRot.W));
  1520. compoundshape.AddChildShape(ref relativeTransform, addshape);
  1521. }
  1522. public override BulletShape RemoveChildShapeFromCompoundShapeIndex(BulletShape pCShape, int pii)
  1523. {
  1524. CompoundShape compoundshape = (pCShape as BulletShapeXNA).shape as CompoundShape;
  1525. CollisionShape ret = null;
  1526. ret = compoundshape.GetChildShape(pii);
  1527. compoundshape.RemoveChildShapeByIndex(pii);
  1528. return new BulletShapeXNA(ret, BSShapeTypeFromBroadPhaseNativeType(ret.GetShapeType()));
  1529. }
  1530. public override BulletShape GetChildShapeFromCompoundShapeIndex(BulletShape cShape, int indx) {
  1531. if (cShape == null)
  1532. return null;
  1533. CompoundShape compoundShape = (cShape as BulletShapeXNA).shape as CompoundShape;
  1534. CollisionShape shape = compoundShape.GetChildShape(indx);
  1535. BulletShape retShape = new BulletShapeXNA(shape, BSShapeTypeFromBroadPhaseNativeType(shape.GetShapeType()));
  1536. return retShape;
  1537. }
  1538. public BSPhysicsShapeType BSShapeTypeFromBroadPhaseNativeType(BroadphaseNativeTypes pin)
  1539. {
  1540. BSPhysicsShapeType ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1541. switch (pin)
  1542. {
  1543. case BroadphaseNativeTypes.BOX_SHAPE_PROXYTYPE:
  1544. ret = BSPhysicsShapeType.SHAPE_BOX;
  1545. break;
  1546. case BroadphaseNativeTypes.TRIANGLE_SHAPE_PROXYTYPE:
  1547. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1548. break;
  1549. case BroadphaseNativeTypes.TETRAHEDRAL_SHAPE_PROXYTYPE:
  1550. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1551. break;
  1552. case BroadphaseNativeTypes.CONVEX_TRIANGLEMESH_SHAPE_PROXYTYPE:
  1553. ret = BSPhysicsShapeType.SHAPE_CONVEXHULL;
  1554. break;
  1555. case BroadphaseNativeTypes.CONVEX_HULL_SHAPE_PROXYTYPE:
  1556. ret = BSPhysicsShapeType.SHAPE_HULL;
  1557. break;
  1558. case BroadphaseNativeTypes.CONVEX_POINT_CLOUD_SHAPE_PROXYTYPE:
  1559. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1560. break;
  1561. case BroadphaseNativeTypes.CUSTOM_POLYHEDRAL_SHAPE_TYPE:
  1562. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1563. break;
  1564. //implicit convex shapes
  1565. case BroadphaseNativeTypes.IMPLICIT_CONVEX_SHAPES_START_HERE:
  1566. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1567. break;
  1568. case BroadphaseNativeTypes.SPHERE_SHAPE_PROXYTYPE:
  1569. ret = BSPhysicsShapeType.SHAPE_SPHERE;
  1570. break;
  1571. case BroadphaseNativeTypes.MULTI_SPHERE_SHAPE_PROXYTYPE:
  1572. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1573. break;
  1574. case BroadphaseNativeTypes.CAPSULE_SHAPE_PROXYTYPE:
  1575. ret = BSPhysicsShapeType.SHAPE_CAPSULE;
  1576. break;
  1577. case BroadphaseNativeTypes.CONE_SHAPE_PROXYTYPE:
  1578. ret = BSPhysicsShapeType.SHAPE_CONE;
  1579. break;
  1580. case BroadphaseNativeTypes.CONVEX_SHAPE_PROXYTYPE:
  1581. ret = BSPhysicsShapeType.SHAPE_CONVEXHULL;
  1582. break;
  1583. case BroadphaseNativeTypes.CYLINDER_SHAPE_PROXYTYPE:
  1584. ret = BSPhysicsShapeType.SHAPE_CYLINDER;
  1585. break;
  1586. case BroadphaseNativeTypes.UNIFORM_SCALING_SHAPE_PROXYTYPE:
  1587. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1588. break;
  1589. case BroadphaseNativeTypes.MINKOWSKI_SUM_SHAPE_PROXYTYPE:
  1590. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1591. break;
  1592. case BroadphaseNativeTypes.MINKOWSKI_DIFFERENCE_SHAPE_PROXYTYPE:
  1593. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1594. break;
  1595. case BroadphaseNativeTypes.BOX_2D_SHAPE_PROXYTYPE:
  1596. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1597. break;
  1598. case BroadphaseNativeTypes.CONVEX_2D_SHAPE_PROXYTYPE:
  1599. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1600. break;
  1601. case BroadphaseNativeTypes.CUSTOM_CONVEX_SHAPE_TYPE:
  1602. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1603. break;
  1604. //concave shape
  1605. case BroadphaseNativeTypes.CONCAVE_SHAPES_START_HERE:
  1606. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1607. break;
  1608. //keep all the convex shapetype below here, for the check IsConvexShape in broadphase proxy!
  1609. case BroadphaseNativeTypes.TRIANGLE_MESH_SHAPE_PROXYTYPE:
  1610. ret = BSPhysicsShapeType.SHAPE_MESH;
  1611. break;
  1612. case BroadphaseNativeTypes.SCALED_TRIANGLE_MESH_SHAPE_PROXYTYPE:
  1613. ret = BSPhysicsShapeType.SHAPE_MESH;
  1614. break;
  1615. ///used for demo integration FAST/Swift collision library and Bullet
  1616. case BroadphaseNativeTypes.FAST_CONCAVE_MESH_PROXYTYPE:
  1617. ret = BSPhysicsShapeType.SHAPE_MESH;
  1618. break;
  1619. //terrain
  1620. case BroadphaseNativeTypes.TERRAIN_SHAPE_PROXYTYPE:
  1621. ret = BSPhysicsShapeType.SHAPE_HEIGHTMAP;
  1622. break;
  1623. ///Used for GIMPACT Trimesh integration
  1624. case BroadphaseNativeTypes.GIMPACT_SHAPE_PROXYTYPE:
  1625. ret = BSPhysicsShapeType.SHAPE_GIMPACT;
  1626. break;
  1627. ///Multimaterial mesh
  1628. case BroadphaseNativeTypes.MULTIMATERIAL_TRIANGLE_MESH_PROXYTYPE:
  1629. ret = BSPhysicsShapeType.SHAPE_MESH;
  1630. break;
  1631. case BroadphaseNativeTypes.EMPTY_SHAPE_PROXYTYPE:
  1632. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1633. break;
  1634. case BroadphaseNativeTypes.STATIC_PLANE_PROXYTYPE:
  1635. ret = BSPhysicsShapeType.SHAPE_GROUNDPLANE;
  1636. break;
  1637. case BroadphaseNativeTypes.CUSTOM_CONCAVE_SHAPE_TYPE:
  1638. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1639. break;
  1640. case BroadphaseNativeTypes.CONCAVE_SHAPES_END_HERE:
  1641. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1642. break;
  1643. case BroadphaseNativeTypes.COMPOUND_SHAPE_PROXYTYPE:
  1644. ret = BSPhysicsShapeType.SHAPE_COMPOUND;
  1645. break;
  1646. case BroadphaseNativeTypes.SOFTBODY_SHAPE_PROXYTYPE:
  1647. ret = BSPhysicsShapeType.SHAPE_MESH;
  1648. break;
  1649. case BroadphaseNativeTypes.HFFLUID_SHAPE_PROXYTYPE:
  1650. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1651. break;
  1652. case BroadphaseNativeTypes.HFFLUID_BUOYANT_CONVEX_SHAPE_PROXYTYPE:
  1653. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1654. break;
  1655. case BroadphaseNativeTypes.INVALID_SHAPE_PROXYTYPE:
  1656. ret = BSPhysicsShapeType.SHAPE_UNKNOWN;
  1657. break;
  1658. }
  1659. return ret;
  1660. }
  1661. public override void RemoveChildShapeFromCompoundShape(BulletShape cShape, BulletShape removeShape) { /* TODO */ }
  1662. public override void UpdateChildTransform(BulletShape pShape, int childIndex, Vector3 pos, Quaternion rot, bool shouldRecalculateLocalAabb) { /* TODO */ }
  1663. public override BulletShape CreateGroundPlaneShape(uint pLocalId, float pheight, float pcollisionMargin)
  1664. {
  1665. StaticPlaneShape m_planeshape = new StaticPlaneShape(new IndexedVector3(0,0,1),(int)pheight );
  1666. m_planeshape.SetMargin(pcollisionMargin);
  1667. m_planeshape.SetUserPointer(pLocalId);
  1668. return new BulletShapeXNA(m_planeshape, BSPhysicsShapeType.SHAPE_GROUNDPLANE);
  1669. }
  1670. public override BulletConstraint Create6DofSpringConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
  1671. Vector3 pframe1, Quaternion pframe1rot, Vector3 pframe2, Quaternion pframe2rot,
  1672. bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
  1673. {
  1674. Generic6DofSpringConstraint constrain = null;
  1675. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1676. RigidBody body1 = (pBody1 as BulletBodyXNA).rigidBody;
  1677. RigidBody body2 = (pBody2 as BulletBodyXNA).rigidBody;
  1678. if (body1 != null && body2 != null)
  1679. {
  1680. IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
  1681. IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
  1682. IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
  1683. frame1._origin = frame1v;
  1684. IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
  1685. IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
  1686. IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
  1687. frame2._origin = frame1v;
  1688. constrain = new Generic6DofSpringConstraint(body1, body2, ref frame1, ref frame2, puseLinearReferenceFrameA);
  1689. world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
  1690. constrain.CalculateTransforms();
  1691. }
  1692. return new BulletConstraintXNA(constrain);
  1693. }
  1694. public override BulletConstraint CreateHingeConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
  1695. Vector3 ppivotInA, Vector3 ppivotInB, Vector3 paxisInA, Vector3 paxisInB,
  1696. bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
  1697. {
  1698. HingeConstraint constrain = null;
  1699. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1700. RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
  1701. RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
  1702. if (rb1 != null && rb2 != null)
  1703. {
  1704. IndexedVector3 pivotInA = new IndexedVector3(ppivotInA.X, ppivotInA.Y, ppivotInA.Z);
  1705. IndexedVector3 pivotInB = new IndexedVector3(ppivotInB.X, ppivotInB.Y, ppivotInB.Z);
  1706. IndexedVector3 axisInA = new IndexedVector3(paxisInA.X, paxisInA.Y, paxisInA.Z);
  1707. IndexedVector3 axisInB = new IndexedVector3(paxisInB.X, paxisInB.Y, paxisInB.Z);
  1708. constrain = new HingeConstraint(rb1, rb2, ref pivotInA, ref pivotInB, ref axisInA, ref axisInB, puseLinearReferenceFrameA);
  1709. world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
  1710. }
  1711. return new BulletConstraintXNA(constrain);
  1712. }
  1713. public override BulletConstraint CreateSliderConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
  1714. Vector3 pframe1, Quaternion pframe1rot,
  1715. Vector3 pframe2, Quaternion pframe2rot,
  1716. bool puseLinearReferenceFrameA, bool pdisableCollisionsBetweenLinkedBodies)
  1717. {
  1718. SliderConstraint constrain = null;
  1719. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1720. RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
  1721. RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
  1722. if (rb1 != null && rb2 != null)
  1723. {
  1724. IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
  1725. IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
  1726. IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
  1727. frame1._origin = frame1v;
  1728. IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
  1729. IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
  1730. IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
  1731. frame2._origin = frame1v;
  1732. constrain = new SliderConstraint(rb1, rb2, ref frame1, ref frame2, puseLinearReferenceFrameA);
  1733. world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
  1734. }
  1735. return new BulletConstraintXNA(constrain);
  1736. }
  1737. public override BulletConstraint CreateConeTwistConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
  1738. Vector3 pframe1, Quaternion pframe1rot,
  1739. Vector3 pframe2, Quaternion pframe2rot,
  1740. bool pdisableCollisionsBetweenLinkedBodies)
  1741. {
  1742. ConeTwistConstraint constrain = null;
  1743. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1744. RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
  1745. RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
  1746. if (rb1 != null && rb2 != null)
  1747. {
  1748. IndexedVector3 frame1v = new IndexedVector3(pframe1.X, pframe1.Y, pframe1.Z);
  1749. IndexedQuaternion frame1rot = new IndexedQuaternion(pframe1rot.X, pframe1rot.Y, pframe1rot.Z, pframe1rot.W);
  1750. IndexedMatrix frame1 = IndexedMatrix.CreateFromQuaternion(frame1rot);
  1751. frame1._origin = frame1v;
  1752. IndexedVector3 frame2v = new IndexedVector3(pframe2.X, pframe2.Y, pframe2.Z);
  1753. IndexedQuaternion frame2rot = new IndexedQuaternion(pframe2rot.X, pframe2rot.Y, pframe2rot.Z, pframe2rot.W);
  1754. IndexedMatrix frame2 = IndexedMatrix.CreateFromQuaternion(frame2rot);
  1755. frame2._origin = frame1v;
  1756. constrain = new ConeTwistConstraint(rb1, rb2, ref frame1, ref frame2);
  1757. world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
  1758. }
  1759. return new BulletConstraintXNA(constrain);
  1760. }
  1761. public override BulletConstraint CreateGearConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
  1762. Vector3 paxisInA, Vector3 paxisInB,
  1763. float pratio, bool pdisableCollisionsBetweenLinkedBodies)
  1764. {
  1765. Generic6DofConstraint constrain = null;
  1766. /* BulletXNA does not have a gear constraint
  1767. GearConstraint constrain = null;
  1768. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1769. RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
  1770. RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
  1771. if (rb1 != null && rb2 != null)
  1772. {
  1773. IndexedVector3 axis1 = new IndexedVector3(paxisInA.X, paxisInA.Y, paxisInA.Z);
  1774. IndexedVector3 axis2 = new IndexedVector3(paxisInB.X, paxisInB.Y, paxisInB.Z);
  1775. constrain = new GearConstraint(rb1, rb2, ref axis1, ref axis2, pratio);
  1776. world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
  1777. }
  1778. */
  1779. return new BulletConstraintXNA(constrain);
  1780. }
  1781. public override BulletConstraint CreatePoint2PointConstraint(BulletWorld pWorld, BulletBody pBody1, BulletBody pBody2,
  1782. Vector3 ppivotInA, Vector3 ppivotInB,
  1783. bool pdisableCollisionsBetweenLinkedBodies)
  1784. {
  1785. Point2PointConstraint constrain = null;
  1786. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1787. RigidBody rb1 = (pBody1 as BulletBodyXNA).rigidBody;
  1788. RigidBody rb2 = (pBody2 as BulletBodyXNA).rigidBody;
  1789. if (rb1 != null && rb2 != null)
  1790. {
  1791. IndexedVector3 pivotInA = new IndexedVector3(ppivotInA.X, ppivotInA.Y, ppivotInA.Z);
  1792. IndexedVector3 pivotInB = new IndexedVector3(ppivotInB.X, ppivotInB.Y, ppivotInB.Z);
  1793. constrain = new Point2PointConstraint(rb1, rb2, ref pivotInA, ref pivotInB);
  1794. world.AddConstraint(constrain, pdisableCollisionsBetweenLinkedBodies);
  1795. }
  1796. return new BulletConstraintXNA(constrain);
  1797. }
  1798. public override BulletShape CreateHullShape(BulletWorld pWorld, int pHullCount, float[] pConvHulls)
  1799. {
  1800. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1801. CompoundShape compoundshape = new CompoundShape(false);
  1802. compoundshape.SetMargin(world.WorldSettings.Params.collisionMargin);
  1803. int ii = 1;
  1804. for (int i = 0; i < pHullCount; i++)
  1805. {
  1806. int vertexCount = (int) pConvHulls[ii];
  1807. IndexedVector3 centroid = new IndexedVector3(pConvHulls[ii + 1], pConvHulls[ii + 2], pConvHulls[ii + 3]);
  1808. IndexedMatrix childTrans = IndexedMatrix.Identity;
  1809. childTrans._origin = centroid;
  1810. List<IndexedVector3> virts = new List<IndexedVector3>();
  1811. int ender = ((ii + 4) + (vertexCount*3));
  1812. for (int iii = ii + 4; iii < ender; iii+=3)
  1813. {
  1814. virts.Add(new IndexedVector3(pConvHulls[iii], pConvHulls[iii + 1], pConvHulls[iii +2]));
  1815. }
  1816. ConvexHullShape convexShape = new ConvexHullShape(virts, vertexCount);
  1817. convexShape.SetMargin(world.WorldSettings.Params.collisionMargin);
  1818. compoundshape.AddChildShape(ref childTrans, convexShape);
  1819. ii += (vertexCount*3 + 4);
  1820. }
  1821. return new BulletShapeXNA(compoundshape, BSPhysicsShapeType.SHAPE_HULL);
  1822. }
  1823. public override BulletShape BuildHullShapeFromMesh(BulletWorld world, BulletShape meshShape, HACDParams parms)
  1824. {
  1825. /* TODO */ return null;
  1826. }
  1827. public override BulletShape BuildConvexHullShapeFromMesh(BulletWorld world, BulletShape meshShape)
  1828. {
  1829. /* TODO */ return null;
  1830. }
  1831. public override BulletShape CreateConvexHullShape(BulletWorld pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats)
  1832. {
  1833. /* TODO */ return null;
  1834. }
  1835. public override BulletShape CreateMeshShape(BulletWorld pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats)
  1836. {
  1837. //DumpRaw(indices,verticesAsFloats,pIndicesCount,pVerticesCount);
  1838. for (int iter = 0; iter < pVerticesCount; iter++)
  1839. {
  1840. if (verticesAsFloats[iter] > 0 && verticesAsFloats[iter] < 0.0001) verticesAsFloats[iter] = 0;
  1841. if (verticesAsFloats[iter] < 0 && verticesAsFloats[iter] > -0.0001) verticesAsFloats[iter] = 0;
  1842. }
  1843. ObjectArray<int> indicesarr = new ObjectArray<int>(indices);
  1844. ObjectArray<float> vertices = new ObjectArray<float>(verticesAsFloats);
  1845. DumpRaw(indicesarr,vertices,pIndicesCount,pVerticesCount);
  1846. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1847. IndexedMesh mesh = new IndexedMesh();
  1848. mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
  1849. mesh.m_numTriangles = pIndicesCount/3;
  1850. mesh.m_numVertices = pVerticesCount;
  1851. mesh.m_triangleIndexBase = indicesarr;
  1852. mesh.m_vertexBase = vertices;
  1853. mesh.m_vertexStride = 3;
  1854. mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
  1855. mesh.m_triangleIndexStride = 3;
  1856. TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
  1857. tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
  1858. BvhTriangleMeshShape meshShape = new BvhTriangleMeshShape(tribuilder, true,true);
  1859. meshShape.SetMargin(world.WorldSettings.Params.collisionMargin);
  1860. // world.UpdateSingleAabb(meshShape);
  1861. return new BulletShapeXNA(meshShape, BSPhysicsShapeType.SHAPE_MESH);
  1862. }
  1863. public override BulletShape CreateGImpactShape(BulletWorld pWorld, int pIndicesCount, int[] indices, int pVerticesCount, float[] verticesAsFloats)
  1864. {
  1865. // TODO:
  1866. return null;
  1867. }
  1868. public static void DumpRaw(ObjectArray<int>indices, ObjectArray<float> vertices, int pIndicesCount,int pVerticesCount )
  1869. {
  1870. String fileName = "objTest3.raw";
  1871. String completePath = System.IO.Path.Combine(Util.configDir(), fileName);
  1872. StreamWriter sw = new StreamWriter(completePath);
  1873. IndexedMesh mesh = new IndexedMesh();
  1874. mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
  1875. mesh.m_numTriangles = pIndicesCount / 3;
  1876. mesh.m_numVertices = pVerticesCount;
  1877. mesh.m_triangleIndexBase = indices;
  1878. mesh.m_vertexBase = vertices;
  1879. mesh.m_vertexStride = 3;
  1880. mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
  1881. mesh.m_triangleIndexStride = 3;
  1882. TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
  1883. tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
  1884. for (int i = 0; i < pVerticesCount; i++)
  1885. {
  1886. string s = vertices[indices[i * 3]].ToString("0.0000");
  1887. s += " " + vertices[indices[i * 3 + 1]].ToString("0.0000");
  1888. s += " " + vertices[indices[i * 3 + 2]].ToString("0.0000");
  1889. sw.Write(s + "\n");
  1890. }
  1891. sw.Close();
  1892. }
  1893. public static void DumpRaw(int[] indices, float[] vertices, int pIndicesCount, int pVerticesCount)
  1894. {
  1895. String fileName = "objTest6.raw";
  1896. String completePath = System.IO.Path.Combine(Util.configDir(), fileName);
  1897. StreamWriter sw = new StreamWriter(completePath);
  1898. IndexedMesh mesh = new IndexedMesh();
  1899. mesh.m_indexType = PHY_ScalarType.PHY_INTEGER;
  1900. mesh.m_numTriangles = pIndicesCount / 3;
  1901. mesh.m_numVertices = pVerticesCount;
  1902. mesh.m_triangleIndexBase = indices;
  1903. mesh.m_vertexBase = vertices;
  1904. mesh.m_vertexStride = 3;
  1905. mesh.m_vertexType = PHY_ScalarType.PHY_FLOAT;
  1906. mesh.m_triangleIndexStride = 3;
  1907. TriangleIndexVertexArray tribuilder = new TriangleIndexVertexArray();
  1908. tribuilder.AddIndexedMesh(mesh, PHY_ScalarType.PHY_INTEGER);
  1909. sw.WriteLine("Indices");
  1910. sw.WriteLine(string.Format("int[] indices = new int[{0}];",pIndicesCount));
  1911. for (int iter = 0; iter < indices.Length; iter++)
  1912. {
  1913. sw.WriteLine(string.Format("indices[{0}]={1};",iter,indices[iter]));
  1914. }
  1915. sw.WriteLine("VerticesFloats");
  1916. sw.WriteLine(string.Format("float[] vertices = new float[{0}];", pVerticesCount));
  1917. for (int iter = 0; iter < vertices.Length; iter++)
  1918. {
  1919. sw.WriteLine(string.Format("Vertices[{0}]={1};", iter, vertices[iter].ToString("0.0000")));
  1920. }
  1921. // for (int i = 0; i < pVerticesCount; i++)
  1922. // {
  1923. //
  1924. // string s = vertices[indices[i * 3]].ToString("0.0000");
  1925. // s += " " + vertices[indices[i * 3 + 1]].ToString("0.0000");
  1926. // s += " " + vertices[indices[i * 3 + 2]].ToString("0.0000");
  1927. //
  1928. // sw.Write(s + "\n");
  1929. //}
  1930. sw.Close();
  1931. }
  1932. public override BulletShape CreateTerrainShape(uint id, Vector3 size, float minHeight, float maxHeight, float[] heightMap,
  1933. float scaleFactor, float collisionMargin)
  1934. {
  1935. const int upAxis = 2;
  1936. HeightfieldTerrainShape terrainShape = new HeightfieldTerrainShape((int)size.X, (int)size.Y,
  1937. heightMap, scaleFactor,
  1938. minHeight, maxHeight, upAxis,
  1939. false);
  1940. terrainShape.SetMargin(collisionMargin);
  1941. terrainShape.SetUseDiamondSubdivision(true);
  1942. terrainShape.SetUserPointer(id);
  1943. return new BulletShapeXNA(terrainShape, BSPhysicsShapeType.SHAPE_TERRAIN);
  1944. }
  1945. public override bool TranslationalLimitMotor(BulletConstraint pConstraint, float ponOff, float targetVelocity, float maxMotorForce)
  1946. {
  1947. TypedConstraint tconstrain = (pConstraint as BulletConstraintXNA).constrain;
  1948. bool onOff = ponOff != 0;
  1949. bool ret = false;
  1950. switch (tconstrain.GetConstraintType())
  1951. {
  1952. case TypedConstraintType.D6_CONSTRAINT_TYPE:
  1953. Generic6DofConstraint constrain = tconstrain as Generic6DofConstraint;
  1954. constrain.GetTranslationalLimitMotor().m_enableMotor[0] = onOff;
  1955. constrain.GetTranslationalLimitMotor().m_targetVelocity[0] = targetVelocity;
  1956. constrain.GetTranslationalLimitMotor().m_maxMotorForce[0] = maxMotorForce;
  1957. ret = true;
  1958. break;
  1959. }
  1960. return ret;
  1961. }
  1962. public override int PhysicsStep(BulletWorld world, float timeStep, int maxSubSteps, float fixedTimeStep,
  1963. out int updatedEntityCount, out int collidersCount)
  1964. {
  1965. /* TODO */
  1966. updatedEntityCount = 0;
  1967. collidersCount = 0;
  1968. int ret = PhysicsStep2(world,timeStep,maxSubSteps,fixedTimeStep,out updatedEntityCount,out world.physicsScene.m_updateArray, out collidersCount, out world.physicsScene.m_collisionArray);
  1969. return ret;
  1970. }
  1971. private int PhysicsStep2(BulletWorld pWorld, float timeStep, int m_maxSubSteps, float m_fixedTimeStep,
  1972. out int updatedEntityCount, out EntityProperties[] updatedEntities,
  1973. out int collidersCount, out CollisionDesc[] colliders)
  1974. {
  1975. int epic = PhysicsStepint(pWorld, timeStep, m_maxSubSteps, m_fixedTimeStep, out updatedEntityCount, out updatedEntities,
  1976. out collidersCount, out colliders, m_maxCollisions, m_maxUpdatesPerFrame);
  1977. return epic;
  1978. }
  1979. private int PhysicsStepint(BulletWorld pWorld,float timeStep, int m_maxSubSteps, float m_fixedTimeStep, out int updatedEntityCount,
  1980. out EntityProperties[] updatedEntities, out int collidersCount, out CollisionDesc[] colliders, int maxCollisions, int maxUpdates)
  1981. {
  1982. int numSimSteps = 0;
  1983. Array.Clear(UpdatedObjects, 0, UpdatedObjects.Length);
  1984. Array.Clear(UpdatedCollisions, 0, UpdatedCollisions.Length);
  1985. LastEntityProperty=0;
  1986. LastCollisionDesc=0;
  1987. updatedEntityCount = 0;
  1988. collidersCount = 0;
  1989. if (pWorld is BulletWorldXNA)
  1990. {
  1991. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  1992. world.LastCollisionDesc = 0;
  1993. world.LastEntityProperty = 0;
  1994. numSimSteps = world.StepSimulation(timeStep, m_maxSubSteps, m_fixedTimeStep);
  1995. PersistentManifold contactManifold;
  1996. CollisionObject objA;
  1997. CollisionObject objB;
  1998. ManifoldPoint manifoldPoint;
  1999. PairCachingGhostObject pairCachingGhostObject;
  2000. m_collisionsThisFrame = 0;
  2001. int numManifolds = world.GetDispatcher().GetNumManifolds();
  2002. for (int j = 0; j < numManifolds; j++)
  2003. {
  2004. contactManifold = world.GetDispatcher().GetManifoldByIndexInternal(j);
  2005. int numContacts = contactManifold.GetNumContacts();
  2006. if (numContacts == 0)
  2007. continue;
  2008. objA = contactManifold.GetBody0() as CollisionObject;
  2009. objB = contactManifold.GetBody1() as CollisionObject;
  2010. manifoldPoint = contactManifold.GetContactPoint(0);
  2011. //IndexedVector3 contactPoint = manifoldPoint.GetPositionWorldOnB();
  2012. // IndexedVector3 contactNormal = -manifoldPoint.m_normalWorldOnB; // make relative to A
  2013. RecordCollision(this, objA, objB, manifoldPoint.GetPositionWorldOnB(), -manifoldPoint.m_normalWorldOnB, manifoldPoint.GetDistance());
  2014. m_collisionsThisFrame ++;
  2015. if (m_collisionsThisFrame >= 9999999)
  2016. break;
  2017. }
  2018. foreach (GhostObject ghostObject in specialCollisionObjects.Values)
  2019. {
  2020. pairCachingGhostObject = ghostObject as PairCachingGhostObject;
  2021. if (pairCachingGhostObject != null)
  2022. {
  2023. RecordGhostCollisions(pairCachingGhostObject);
  2024. }
  2025. }
  2026. updatedEntityCount = LastEntityProperty;
  2027. updatedEntities = UpdatedObjects;
  2028. collidersCount = LastCollisionDesc;
  2029. colliders = UpdatedCollisions;
  2030. }
  2031. else
  2032. {
  2033. //if (updatedEntities is null)
  2034. //updatedEntities = new List<BulletXNA.EntityProperties>();
  2035. //updatedEntityCount = 0;
  2036. //collidersCount = 0;
  2037. updatedEntities = new EntityProperties[0];
  2038. colliders = new CollisionDesc[0];
  2039. }
  2040. return numSimSteps;
  2041. }
  2042. public void RecordGhostCollisions(PairCachingGhostObject obj)
  2043. {
  2044. IOverlappingPairCache cache = obj.GetOverlappingPairCache();
  2045. ObjectArray<BroadphasePair> pairs = cache.GetOverlappingPairArray();
  2046. DiscreteDynamicsWorld world = (PhysicsScene.World as BulletWorldXNA).world;
  2047. PersistentManifoldArray manifoldArray = new PersistentManifoldArray();
  2048. BroadphasePair collisionPair;
  2049. PersistentManifold contactManifold;
  2050. CollisionObject objA;
  2051. CollisionObject objB;
  2052. ManifoldPoint pt;
  2053. int numPairs = pairs.Count;
  2054. for (int i = 0; i < numPairs; i++)
  2055. {
  2056. manifoldArray.Clear();
  2057. if (LastCollisionDesc < UpdatedCollisions.Length)
  2058. break;
  2059. collisionPair = world.GetPairCache().FindPair(pairs[i].m_pProxy0, pairs[i].m_pProxy1);
  2060. if (collisionPair == null)
  2061. continue;
  2062. collisionPair.m_algorithm.GetAllContactManifolds(manifoldArray);
  2063. for (int j = 0; j < manifoldArray.Count; j++)
  2064. {
  2065. contactManifold = manifoldArray[j];
  2066. int numContacts = contactManifold.GetNumContacts();
  2067. objA = contactManifold.GetBody0() as CollisionObject;
  2068. objB = contactManifold.GetBody1() as CollisionObject;
  2069. for (int p = 0; p < numContacts; p++)
  2070. {
  2071. pt = contactManifold.GetContactPoint(p);
  2072. if (pt.GetDistance() < 0.0f)
  2073. {
  2074. RecordCollision(this, objA, objB, pt.GetPositionWorldOnA(), -pt.m_normalWorldOnB,pt.GetDistance());
  2075. break;
  2076. }
  2077. }
  2078. }
  2079. }
  2080. }
  2081. private static void RecordCollision(BSAPIXNA world, CollisionObject objA, CollisionObject objB, IndexedVector3 contact, IndexedVector3 norm, float penetration)
  2082. {
  2083. IndexedVector3 contactNormal = norm;
  2084. if ((objA.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0 &&
  2085. (objB.GetCollisionFlags() & BulletXNA.BulletCollision.CollisionFlags.BS_WANTS_COLLISIONS) == 0)
  2086. {
  2087. return;
  2088. }
  2089. uint idA = (uint)objA.GetUserPointer();
  2090. uint idB = (uint)objB.GetUserPointer();
  2091. if (idA > idB)
  2092. {
  2093. uint temp = idA;
  2094. idA = idB;
  2095. idB = temp;
  2096. contactNormal = -contactNormal;
  2097. }
  2098. //ulong collisionID = ((ulong) idA << 32) | idB;
  2099. CollisionDesc cDesc = new CollisionDesc()
  2100. {
  2101. aID = idA,
  2102. bID = idB,
  2103. point = new Vector3(contact.X,contact.Y,contact.Z),
  2104. normal = new Vector3(contactNormal.X,contactNormal.Y,contactNormal.Z),
  2105. penetration = penetration
  2106. };
  2107. if (world.LastCollisionDesc < world.UpdatedCollisions.Length)
  2108. world.UpdatedCollisions[world.LastCollisionDesc++] = (cDesc);
  2109. m_collisionsThisFrame++;
  2110. }
  2111. private static EntityProperties GetDebugProperties(BulletWorld pWorld, BulletBody pCollisionObject)
  2112. {
  2113. EntityProperties ent = new EntityProperties();
  2114. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  2115. CollisionObject collisionObject = (pCollisionObject as BulletBodyXNA).rigidBody;
  2116. IndexedMatrix transform = collisionObject.GetWorldTransform();
  2117. IndexedVector3 LinearVelocity = collisionObject.GetInterpolationLinearVelocity();
  2118. IndexedVector3 AngularVelocity = collisionObject.GetInterpolationAngularVelocity();
  2119. IndexedQuaternion rotation = transform.GetRotation();
  2120. ent.Acceleration = Vector3.Zero;
  2121. ent.ID = (uint)collisionObject.GetUserPointer();
  2122. ent.Position = new Vector3(transform._origin.X,transform._origin.Y,transform._origin.Z);
  2123. ent.Rotation = new Quaternion(rotation.X,rotation.Y,rotation.Z,rotation.W);
  2124. ent.Velocity = new Vector3(LinearVelocity.X, LinearVelocity.Y, LinearVelocity.Z);
  2125. ent.RotationalVelocity = new Vector3(AngularVelocity.X, AngularVelocity.Y, AngularVelocity.Z);
  2126. return ent;
  2127. }
  2128. public override bool UpdateParameter(BulletWorld world, uint localID, String parm, float value) { /* TODO */
  2129. return false; }
  2130. public override Vector3 GetLocalScaling(BulletShape pShape)
  2131. {
  2132. CollisionShape shape = (pShape as BulletShapeXNA).shape;
  2133. IndexedVector3 scale = shape.GetLocalScaling();
  2134. return new Vector3(scale.X,scale.Y,scale.Z);
  2135. }
  2136. public bool RayCastGround(BulletWorld pWorld, Vector3 _RayOrigin, float pRayHeight, BulletBody NotMe)
  2137. {
  2138. DiscreteDynamicsWorld world = (pWorld as BulletWorldXNA).world;
  2139. if (world != null)
  2140. {
  2141. if (NotMe is BulletBodyXNA && NotMe.HasPhysicalBody)
  2142. {
  2143. CollisionObject AvoidBody = (NotMe as BulletBodyXNA).body;
  2144. IndexedVector3 rOrigin = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z);
  2145. IndexedVector3 rEnd = new IndexedVector3(_RayOrigin.X, _RayOrigin.Y, _RayOrigin.Z - pRayHeight);
  2146. using (
  2147. ClosestNotMeRayResultCallback rayCallback =
  2148. new ClosestNotMeRayResultCallback(rOrigin, rEnd, AvoidBody)
  2149. )
  2150. {
  2151. world.RayTest(ref rOrigin, ref rEnd, rayCallback);
  2152. if (rayCallback.HasHit())
  2153. {
  2154. IndexedVector3 hitLocation = rayCallback.m_hitPointWorld;
  2155. }
  2156. return rayCallback.HasHit();
  2157. }
  2158. }
  2159. }
  2160. return false;
  2161. }
  2162. public override SweepHit ConvexSweepTest2(BulletWorld world, BulletBody obj, Vector3 from, Vector3 to, float margin) {
  2163. return new SweepHit();
  2164. }
  2165. public override RaycastHit RayTest2(BulletWorld world, Vector3 from, Vector3 to, uint filterGroup, uint filterMask) {
  2166. return new RaycastHit();
  2167. }
  2168. }
  2169. public class SimMotionState : DefaultMotionState
  2170. {
  2171. public RigidBody Rigidbody;
  2172. public Vector3 ZeroVect;
  2173. private IndexedMatrix m_xform;
  2174. private EntityProperties m_properties;
  2175. private EntityProperties m_lastProperties;
  2176. private BSAPIXNA m_world;
  2177. const float POSITION_TOLERANCE = 0.05f;
  2178. const float VELOCITY_TOLERANCE = 0.001f;
  2179. const float ROTATION_TOLERANCE = 0.01f;
  2180. const float ANGULARVELOCITY_TOLERANCE = 0.01f;
  2181. public SimMotionState(BSAPIXNA pWorld, uint id, IndexedMatrix starTransform, object frameUpdates)
  2182. {
  2183. IndexedQuaternion OrientationQuaterion = starTransform.GetRotation();
  2184. m_properties = new EntityProperties()
  2185. {
  2186. ID = id,
  2187. Position = new Vector3(starTransform._origin.X, starTransform._origin.Y,starTransform._origin.Z),
  2188. Rotation = new Quaternion(OrientationQuaterion.X,OrientationQuaterion.Y,OrientationQuaterion.Z,OrientationQuaterion.W)
  2189. };
  2190. m_lastProperties = new EntityProperties()
  2191. {
  2192. ID = id,
  2193. Position = new Vector3(starTransform._origin.X, starTransform._origin.Y, starTransform._origin.Z),
  2194. Rotation = new Quaternion(OrientationQuaterion.X, OrientationQuaterion.Y, OrientationQuaterion.Z, OrientationQuaterion.W)
  2195. };
  2196. m_world = pWorld;
  2197. m_xform = starTransform;
  2198. }
  2199. public override void GetWorldTransform(out IndexedMatrix worldTrans)
  2200. {
  2201. worldTrans = m_xform;
  2202. }
  2203. public override void SetWorldTransform(IndexedMatrix worldTrans)
  2204. {
  2205. SetWorldTransform(ref worldTrans);
  2206. }
  2207. public override void SetWorldTransform(ref IndexedMatrix worldTrans)
  2208. {
  2209. SetWorldTransform(ref worldTrans, false);
  2210. }
  2211. public void SetWorldTransform(ref IndexedMatrix worldTrans, bool force)
  2212. {
  2213. m_xform = worldTrans;
  2214. // Put the new transform into m_properties
  2215. IndexedQuaternion OrientationQuaternion = m_xform.GetRotation();
  2216. IndexedVector3 LinearVelocityVector = Rigidbody.GetLinearVelocity();
  2217. IndexedVector3 AngularVelocityVector = Rigidbody.GetAngularVelocity();
  2218. m_properties.Position = new Vector3(m_xform._origin.X, m_xform._origin.Y, m_xform._origin.Z);
  2219. m_properties.Rotation = new Quaternion(OrientationQuaternion.X, OrientationQuaternion.Y,
  2220. OrientationQuaternion.Z, OrientationQuaternion.W);
  2221. // A problem with stock Bullet is that we don't get an event when an object is deactivated.
  2222. // This means that the last non-zero values for linear and angular velocity
  2223. // are left in the viewer who does dead reconning and the objects look like
  2224. // they float off.
  2225. // BulletSim ships with a patch to Bullet which creates such an event.
  2226. m_properties.Velocity = new Vector3(LinearVelocityVector.X, LinearVelocityVector.Y, LinearVelocityVector.Z);
  2227. m_properties.RotationalVelocity = new Vector3(AngularVelocityVector.X, AngularVelocityVector.Y, AngularVelocityVector.Z);
  2228. if (force
  2229. || !AlmostEqual(ref m_lastProperties.Position, ref m_properties.Position, POSITION_TOLERANCE)
  2230. || !AlmostEqual(ref m_properties.Rotation, ref m_lastProperties.Rotation, ROTATION_TOLERANCE)
  2231. // If the Velocity and AngularVelocity are zero, most likely the object has
  2232. // been deactivated. If they both are zero and they have become zero recently,
  2233. // make sure a property update is sent so the zeros make it to the viewer.
  2234. || ((m_properties.Velocity == ZeroVect && m_properties.RotationalVelocity == ZeroVect)
  2235. &&
  2236. (m_properties.Velocity != m_lastProperties.Velocity ||
  2237. m_properties.RotationalVelocity != m_lastProperties.RotationalVelocity))
  2238. // If Velocity and AngularVelocity are non-zero but have changed, send an update.
  2239. || !AlmostEqual(ref m_properties.Velocity, ref m_lastProperties.Velocity, VELOCITY_TOLERANCE)
  2240. ||
  2241. !AlmostEqual(ref m_properties.RotationalVelocity, ref m_lastProperties.RotationalVelocity,
  2242. ANGULARVELOCITY_TOLERANCE)
  2243. )
  2244. {
  2245. // Add this update to the list of updates for this frame.
  2246. m_lastProperties = m_properties;
  2247. if (m_world.LastEntityProperty < m_world.UpdatedObjects.Length)
  2248. m_world.UpdatedObjects[m_world.LastEntityProperty++]=(m_properties);
  2249. //(*m_updatesThisFrame)[m_properties.ID] = &m_properties;
  2250. }
  2251. }
  2252. public override void SetRigidBody(RigidBody body)
  2253. {
  2254. Rigidbody = body;
  2255. }
  2256. internal static bool AlmostEqual(ref Vector3 v1, ref Vector3 v2, float nEpsilon)
  2257. {
  2258. return
  2259. (((v1.X - nEpsilon) < v2.X) && (v2.X < (v1.X + nEpsilon))) &&
  2260. (((v1.Y - nEpsilon) < v2.Y) && (v2.Y < (v1.Y + nEpsilon))) &&
  2261. (((v1.Z - nEpsilon) < v2.Z) && (v2.Z < (v1.Z + nEpsilon)));
  2262. }
  2263. internal static bool AlmostEqual(ref Quaternion v1, ref Quaternion v2, float nEpsilon)
  2264. {
  2265. return
  2266. (((v1.X - nEpsilon) < v2.X) && (v2.X < (v1.X + nEpsilon))) &&
  2267. (((v1.Y - nEpsilon) < v2.Y) && (v2.Y < (v1.Y + nEpsilon))) &&
  2268. (((v1.Z - nEpsilon) < v2.Z) && (v2.Z < (v1.Z + nEpsilon))) &&
  2269. (((v1.W - nEpsilon) < v2.W) && (v2.W < (v1.W + nEpsilon)));
  2270. }
  2271. }
  2272. }