ODERayCastRequestManager.cs 28 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687
  1. /*
  2. * Copyright (c) Contributors, http://opensimulator.org/
  3. * See CONTRIBUTORS.TXT for a full list of copyright holders.
  4. *
  5. * Redistribution and use in source and binary forms, with or without
  6. * modification, are permitted provided that the following conditions are met:
  7. * * Redistributions of source code must retain the above copyright
  8. * notice, this list of conditions and the following disclaimer.
  9. * * Redistributions in binary form must reproduce the above copyright
  10. * notice, this list of conditions and the following disclaimer in the
  11. * documentation and/or other materials provided with the distribution.
  12. * * Neither the name of the OpenSimulator Project nor the
  13. * names of its contributors may be used to endorse or promote products
  14. * derived from this software without specific prior written permission.
  15. *
  16. * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
  17. * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  18. * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  19. * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
  20. * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  21. * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  22. * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  23. * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  24. * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  25. * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26. */
  27. using System;
  28. using System.Collections.Generic;
  29. using System.Reflection;
  30. using System.Runtime.InteropServices;
  31. using OpenSim.Framework;
  32. using OpenSim.Region.PhysicsModules.SharedBase;
  33. using log4net;
  34. using OpenMetaverse;
  35. namespace OpenSim.Region.PhysicsModule.ubOde
  36. {
  37. /// <summary>
  38. /// Processes raycast requests as ODE is in a state to be able to do them.
  39. /// This ensures that it's thread safe and there will be no conflicts.
  40. /// Requests get returned by a different thread then they were requested by.
  41. /// </summary>
  42. public class ODERayCastRequestManager
  43. {
  44. /// <summary>
  45. /// Pending ray requests
  46. /// </summary>
  47. protected OpenSim.Framework.LocklessQueue<ODERayRequest> m_PendingRequests = new OpenSim.Framework.LocklessQueue<ODERayRequest>();
  48. /// <summary>
  49. /// Scene that created this object.
  50. /// </summary>
  51. private ODEScene m_scene;
  52. IntPtr ray; // the ray. we only need one for our lifetime
  53. IntPtr Sphere;
  54. IntPtr Box;
  55. IntPtr Plane;
  56. private int CollisionContactGeomsPerTest = 25;
  57. private const int DefaultMaxCount = 25;
  58. private const int MaxTimePerCallMS = 30;
  59. /// <summary>
  60. /// ODE near callback delegate
  61. /// </summary>
  62. private SafeNativeMethods.NearCallback nearCallback;
  63. private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
  64. private List<ContactResult> m_contactResults = new List<ContactResult>();
  65. private RayFilterFlags CurrentRayFilter;
  66. private int CurrentMaxCount;
  67. public ODERayCastRequestManager(ODEScene pScene)
  68. {
  69. m_scene = pScene;
  70. nearCallback = near;
  71. ray = SafeNativeMethods.CreateRay(IntPtr.Zero, 1.0f);
  72. SafeNativeMethods.GeomSetCategoryBits(ray, 0);
  73. Box = SafeNativeMethods.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f);
  74. SafeNativeMethods.GeomSetCategoryBits(Box, 0);
  75. Sphere = SafeNativeMethods.CreateSphere(IntPtr.Zero,1.0f);
  76. SafeNativeMethods.GeomSetCategoryBits(Sphere, 0);
  77. Plane = SafeNativeMethods.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f);
  78. SafeNativeMethods.GeomSetCategoryBits(Sphere, 0);
  79. }
  80. public void QueueRequest(ODERayRequest req)
  81. {
  82. if (req.Count == 0)
  83. req.Count = DefaultMaxCount;
  84. m_PendingRequests.Enqueue(req);
  85. }
  86. /// <summary>
  87. /// Process all queued raycast requests
  88. /// </summary>
  89. /// <returns>Time in MS the raycasts took to process.</returns>
  90. public int ProcessQueuedRequests()
  91. {
  92. if (m_PendingRequests.Count <= 0)
  93. return 0;
  94. if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero)
  95. // oops something got wrong or scene isn't ready still
  96. {
  97. m_PendingRequests.Clear();
  98. return 0;
  99. }
  100. int time = Util.EnvironmentTickCount();
  101. ODERayRequest req;
  102. int closestHit;
  103. int backfacecull;
  104. CollisionCategories catflags;
  105. while (m_PendingRequests.Dequeue(out req))
  106. {
  107. if (req.callbackMethod != null)
  108. {
  109. IntPtr geom = IntPtr.Zero;
  110. if (req.actor != null)
  111. {
  112. if (m_scene.haveActor(req.actor))
  113. {
  114. if (req.actor is OdePrim)
  115. geom = ((OdePrim)req.actor).prim_geom;
  116. else if (req.actor is OdeCharacter)
  117. geom = ((OdePrim)req.actor).prim_geom;
  118. }
  119. if (geom == IntPtr.Zero)
  120. {
  121. NoContacts(req);
  122. continue;
  123. }
  124. }
  125. CurrentRayFilter = req.filter;
  126. CurrentMaxCount = req.Count;
  127. CollisionContactGeomsPerTest = req.Count & 0xffff;
  128. closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1);
  129. backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1);
  130. if (req.callbackMethod is ProbeBoxCallback)
  131. {
  132. if (CollisionContactGeomsPerTest > 80)
  133. CollisionContactGeomsPerTest = 80;
  134. SafeNativeMethods.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z);
  135. SafeNativeMethods.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z);
  136. SafeNativeMethods.Quaternion qtmp;
  137. qtmp.X = req.orientation.X;
  138. qtmp.Y = req.orientation.Y;
  139. qtmp.Z = req.orientation.Z;
  140. qtmp.W = req.orientation.W;
  141. SafeNativeMethods.GeomSetQuaternion(Box, ref qtmp);
  142. }
  143. else if (req.callbackMethod is ProbeSphereCallback)
  144. {
  145. if (CollisionContactGeomsPerTest > 80)
  146. CollisionContactGeomsPerTest = 80;
  147. SafeNativeMethods.GeomSphereSetRadius(Sphere, req.length);
  148. SafeNativeMethods.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z);
  149. }
  150. else if (req.callbackMethod is ProbePlaneCallback)
  151. {
  152. if (CollisionContactGeomsPerTest > 80)
  153. CollisionContactGeomsPerTest = 80;
  154. SafeNativeMethods.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length);
  155. }
  156. else
  157. {
  158. if (CollisionContactGeomsPerTest > 25)
  159. CollisionContactGeomsPerTest = 25;
  160. SafeNativeMethods.GeomRaySetLength(ray, req.length);
  161. SafeNativeMethods.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
  162. SafeNativeMethods.GeomRaySetParams(ray, 0, backfacecull);
  163. if (req.callbackMethod is RaycastCallback)
  164. {
  165. // if we only want one get only one per Collision pair saving memory
  166. CurrentRayFilter |= RayFilterFlags.ClosestHit;
  167. SafeNativeMethods.GeomRaySetClosestHit(ray, 1);
  168. }
  169. else
  170. SafeNativeMethods.GeomRaySetClosestHit(ray, closestHit);
  171. }
  172. if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0)
  173. unchecked
  174. {
  175. CollisionContactGeomsPerTest |= (int)SafeNativeMethods.CONTACTS_UNIMPORTANT;
  176. }
  177. if (geom == IntPtr.Zero)
  178. {
  179. // translate ray filter to Collision flags
  180. catflags = 0;
  181. if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0)
  182. catflags |= CollisionCategories.VolumeDtc;
  183. if ((CurrentRayFilter & RayFilterFlags.phantom) != 0)
  184. catflags |= CollisionCategories.Phantom;
  185. if ((CurrentRayFilter & RayFilterFlags.agent) != 0)
  186. catflags |= CollisionCategories.Character;
  187. if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0)
  188. catflags |= CollisionCategories.Geom;
  189. if ((CurrentRayFilter & RayFilterFlags.land) != 0)
  190. catflags |= CollisionCategories.Land;
  191. if ((CurrentRayFilter & RayFilterFlags.water) != 0)
  192. catflags |= CollisionCategories.Water;
  193. if (catflags != 0)
  194. {
  195. if (req.callbackMethod is ProbeBoxCallback)
  196. {
  197. catflags |= CollisionCategories.Space;
  198. SafeNativeMethods.GeomSetCollideBits(Box, (uint)catflags);
  199. SafeNativeMethods.GeomSetCategoryBits(Box, (uint)catflags);
  200. doProbe(req, Box);
  201. }
  202. else if (req.callbackMethod is ProbeSphereCallback)
  203. {
  204. catflags |= CollisionCategories.Space;
  205. SafeNativeMethods.GeomSetCollideBits(Sphere, (uint)catflags);
  206. SafeNativeMethods.GeomSetCategoryBits(Sphere, (uint)catflags);
  207. doProbe(req, Sphere);
  208. }
  209. else if (req.callbackMethod is ProbePlaneCallback)
  210. {
  211. catflags |= CollisionCategories.Space;
  212. SafeNativeMethods.GeomSetCollideBits(Plane, (uint)catflags);
  213. SafeNativeMethods.GeomSetCategoryBits(Plane, (uint)catflags);
  214. doPlane(req,IntPtr.Zero);
  215. }
  216. else
  217. {
  218. SafeNativeMethods.GeomSetCollideBits(ray, (uint)catflags);
  219. doSpaceRay(req);
  220. }
  221. }
  222. }
  223. else
  224. {
  225. // if we select a geom don't use filters
  226. if (req.callbackMethod is ProbePlaneCallback)
  227. {
  228. SafeNativeMethods.GeomSetCollideBits(Plane, (uint)CollisionCategories.All);
  229. doPlane(req,geom);
  230. }
  231. else
  232. {
  233. SafeNativeMethods.GeomSetCollideBits(ray, (uint)CollisionCategories.All);
  234. doGeomRay(req,geom);
  235. }
  236. }
  237. }
  238. if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS)
  239. break;
  240. }
  241. lock (m_contactResults)
  242. m_contactResults.Clear();
  243. return Util.EnvironmentTickCountSubtract(time);
  244. }
  245. /// <summary>
  246. /// Method that actually initiates the raycast with spaces
  247. /// </summary>
  248. /// <param name="req"></param>
  249. ///
  250. private void NoContacts(ODERayRequest req)
  251. {
  252. if (req.callbackMethod is RaycastCallback)
  253. {
  254. ((RaycastCallback)req.callbackMethod)(false, Vector3.Zero, 0, 0, Vector3.Zero);
  255. return;
  256. }
  257. List<ContactResult> cresult = new List<ContactResult>();
  258. if (req.callbackMethod is RayCallback)
  259. ((RayCallback)req.callbackMethod)(cresult);
  260. else if (req.callbackMethod is ProbeBoxCallback)
  261. ((ProbeBoxCallback)req.callbackMethod)(cresult);
  262. else if (req.callbackMethod is ProbeSphereCallback)
  263. ((ProbeSphereCallback)req.callbackMethod)(cresult);
  264. }
  265. private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhantom;
  266. // private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
  267. private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhantom;
  268. private void doSpaceRay(ODERayRequest req)
  269. {
  270. // Collide tests
  271. if ((CurrentRayFilter & FilterActiveSpace) != 0)
  272. {
  273. SafeNativeMethods.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
  274. SafeNativeMethods.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
  275. }
  276. if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
  277. SafeNativeMethods.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
  278. if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
  279. {
  280. // current ode land to ray collisions is very bad
  281. // so for now limit its range badly
  282. if (req.length > 60.0f)
  283. {
  284. Vector3 t = req.Normal * req.length;
  285. float tmp = t.X * t.X + t.Y * t.Y;
  286. if(tmp > 2500)
  287. {
  288. float tmp2 = req.length * req.length - tmp + 2500;
  289. tmp2 = (float)Math.Sqrt(tmp2);
  290. SafeNativeMethods.GeomRaySetLength(ray, tmp2);
  291. }
  292. }
  293. SafeNativeMethods.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
  294. }
  295. if (req.callbackMethod is RaycastCallback)
  296. {
  297. // Define default results
  298. bool hitYN = false;
  299. uint hitConsumerID = 0;
  300. float distance = float.MaxValue;
  301. Vector3 closestcontact = Vector3.Zero;
  302. Vector3 snormal = Vector3.Zero;
  303. // Find closest contact and object.
  304. lock (m_contactResults)
  305. {
  306. foreach (ContactResult cResult in m_contactResults)
  307. {
  308. if(cResult.Depth < distance)
  309. {
  310. closestcontact = cResult.Pos;
  311. hitConsumerID = cResult.ConsumerID;
  312. distance = cResult.Depth;
  313. snormal = cResult.Normal;
  314. }
  315. }
  316. m_contactResults.Clear();
  317. }
  318. if (distance > 0 && distance < float.MaxValue)
  319. hitYN = true;
  320. ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
  321. }
  322. else
  323. {
  324. List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
  325. lock (m_PendingRequests)
  326. {
  327. cresult.AddRange(m_contactResults);
  328. m_contactResults.Clear();
  329. }
  330. ((RayCallback)req.callbackMethod)(cresult);
  331. }
  332. }
  333. private void doProbe(ODERayRequest req, IntPtr probe)
  334. {
  335. // Collide tests
  336. if ((CurrentRayFilter & FilterActiveSpace) != 0)
  337. {
  338. SafeNativeMethods.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
  339. SafeNativeMethods.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
  340. }
  341. if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
  342. SafeNativeMethods.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
  343. if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
  344. SafeNativeMethods.SpaceCollide2(probe, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
  345. List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
  346. lock (m_PendingRequests)
  347. {
  348. cresult.AddRange(m_contactResults);
  349. m_contactResults.Clear();
  350. }
  351. if (req.callbackMethod is ProbeBoxCallback)
  352. ((ProbeBoxCallback)req.callbackMethod)(cresult);
  353. else if (req.callbackMethod is ProbeSphereCallback)
  354. ((ProbeSphereCallback)req.callbackMethod)(cresult);
  355. }
  356. private void doPlane(ODERayRequest req,IntPtr geom)
  357. {
  358. // Collide tests
  359. if (geom == IntPtr.Zero)
  360. {
  361. if ((CurrentRayFilter & FilterActiveSpace) != 0)
  362. {
  363. SafeNativeMethods.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
  364. SafeNativeMethods.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
  365. }
  366. if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
  367. SafeNativeMethods.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
  368. if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
  369. SafeNativeMethods.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
  370. }
  371. else
  372. {
  373. SafeNativeMethods.SpaceCollide2(Plane, geom, IntPtr.Zero, nearCallback);
  374. }
  375. List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
  376. lock (m_PendingRequests)
  377. {
  378. cresult.AddRange(m_contactResults);
  379. m_contactResults.Clear();
  380. }
  381. ((ProbePlaneCallback)req.callbackMethod)(cresult);
  382. }
  383. /// <summary>
  384. /// Method that actually initiates the raycast with a geom
  385. /// </summary>
  386. /// <param name="req"></param>
  387. private void doGeomRay(ODERayRequest req, IntPtr geom)
  388. {
  389. // Collide test
  390. SafeNativeMethods.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
  391. if (req.callbackMethod is RaycastCallback)
  392. {
  393. // Define default results
  394. bool hitYN = false;
  395. uint hitConsumerID = 0;
  396. float distance = float.MaxValue;
  397. Vector3 closestcontact = Vector3.Zero;
  398. Vector3 snormal = Vector3.Zero;
  399. // Find closest contact and object.
  400. lock (m_contactResults)
  401. {
  402. foreach (ContactResult cResult in m_contactResults)
  403. {
  404. if(cResult.Depth < distance )
  405. {
  406. closestcontact = cResult.Pos;
  407. hitConsumerID = cResult.ConsumerID;
  408. distance = cResult.Depth;
  409. snormal = cResult.Normal;
  410. }
  411. }
  412. m_contactResults.Clear();
  413. }
  414. if (distance > 0 && distance < float.MaxValue)
  415. hitYN = true;
  416. ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
  417. }
  418. else
  419. {
  420. List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
  421. lock (m_PendingRequests)
  422. {
  423. cresult.AddRange(m_contactResults);
  424. m_contactResults.Clear();
  425. }
  426. ((RayCallback)req.callbackMethod)(cresult);
  427. }
  428. }
  429. private bool GetCurContactGeom(int index, ref SafeNativeMethods.ContactGeom newcontactgeom)
  430. {
  431. IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
  432. if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest)
  433. return false;
  434. IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * SafeNativeMethods.ContactGeom.unmanagedSizeOf));
  435. newcontactgeom = (SafeNativeMethods.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(SafeNativeMethods.ContactGeom));
  436. return true;
  437. }
  438. // This is the standard Near. g1 is the ray
  439. private void near(IntPtr space, IntPtr g1, IntPtr g2)
  440. {
  441. if (g2 == IntPtr.Zero || g1 == g2)
  442. return;
  443. if (m_contactResults.Count >= CurrentMaxCount)
  444. return;
  445. if (SafeNativeMethods.GeomIsSpace(g2))
  446. {
  447. try
  448. {
  449. SafeNativeMethods.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
  450. }
  451. catch (Exception e)
  452. {
  453. m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
  454. }
  455. return;
  456. }
  457. int count = 0;
  458. try
  459. {
  460. count = SafeNativeMethods.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf);
  461. }
  462. catch (Exception e)
  463. {
  464. m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
  465. return;
  466. }
  467. if (count == 0)
  468. return;
  469. /*
  470. uint cat1 = d.GeomGetCategoryBits(g1);
  471. uint cat2 = d.GeomGetCategoryBits(g2);
  472. uint col1 = d.GeomGetCollideBits(g1);
  473. uint col2 = d.GeomGetCollideBits(g2);
  474. */
  475. uint ID = 0;
  476. PhysicsActor p2 = null;
  477. m_scene.actor_name_map.TryGetValue(g2, out p2);
  478. if (p2 == null)
  479. return;
  480. switch (p2.PhysicsActorType)
  481. {
  482. case (int)ActorTypes.Prim:
  483. RayFilterFlags thisFlags;
  484. if (p2.IsPhysical)
  485. thisFlags = RayFilterFlags.physical;
  486. else
  487. thisFlags = RayFilterFlags.nonphysical;
  488. if (p2.Phantom)
  489. thisFlags |= RayFilterFlags.phantom;
  490. if (p2.IsVolumeDtc)
  491. thisFlags |= RayFilterFlags.volumedtc;
  492. if ((thisFlags & CurrentRayFilter) == 0)
  493. return;
  494. ID = ((OdePrim)p2).LocalID;
  495. break;
  496. case (int)ActorTypes.Agent:
  497. if ((CurrentRayFilter & RayFilterFlags.agent) == 0)
  498. return;
  499. else
  500. ID = ((OdeCharacter)p2).LocalID;
  501. break;
  502. case (int)ActorTypes.Ground:
  503. if ((CurrentRayFilter & RayFilterFlags.land) == 0)
  504. return;
  505. break;
  506. case (int)ActorTypes.Water:
  507. if ((CurrentRayFilter & RayFilterFlags.water) == 0)
  508. return;
  509. break;
  510. default:
  511. break;
  512. }
  513. SafeNativeMethods.ContactGeom curcontact = new SafeNativeMethods.ContactGeom();
  514. // closestHit for now only works for meshs, so must do it for others
  515. if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0)
  516. {
  517. // Loop all contacts, build results.
  518. for (int i = 0; i < count; i++)
  519. {
  520. if (!GetCurContactGeom(i, ref curcontact))
  521. break;
  522. ContactResult collisionresult = new ContactResult();
  523. collisionresult.ConsumerID = ID;
  524. collisionresult.Pos.X = curcontact.pos.X;
  525. collisionresult.Pos.Y = curcontact.pos.Y;
  526. collisionresult.Pos.Z = curcontact.pos.Z;
  527. collisionresult.Depth = curcontact.depth;
  528. collisionresult.Normal.X = curcontact.normal.X;
  529. collisionresult.Normal.Y = curcontact.normal.Y;
  530. collisionresult.Normal.Z = curcontact.normal.Z;
  531. lock (m_contactResults)
  532. {
  533. m_contactResults.Add(collisionresult);
  534. if (m_contactResults.Count >= CurrentMaxCount)
  535. return;
  536. }
  537. }
  538. }
  539. else
  540. {
  541. // keep only closest contact
  542. ContactResult collisionresult = new ContactResult();
  543. collisionresult.ConsumerID = ID;
  544. collisionresult.Depth = float.MaxValue;
  545. for (int i = 0; i < count; i++)
  546. {
  547. if (!GetCurContactGeom(i, ref curcontact))
  548. break;
  549. if (curcontact.depth < collisionresult.Depth)
  550. {
  551. collisionresult.Pos.X = curcontact.pos.X;
  552. collisionresult.Pos.Y = curcontact.pos.Y;
  553. collisionresult.Pos.Z = curcontact.pos.Z;
  554. collisionresult.Depth = curcontact.depth;
  555. collisionresult.Normal.X = curcontact.normal.X;
  556. collisionresult.Normal.Y = curcontact.normal.Y;
  557. collisionresult.Normal.Z = curcontact.normal.Z;
  558. }
  559. }
  560. if (collisionresult.Depth != float.MaxValue)
  561. {
  562. lock (m_contactResults)
  563. m_contactResults.Add(collisionresult);
  564. }
  565. }
  566. }
  567. /// <summary>
  568. /// Dereference the creator scene so that it can be garbage collected if needed.
  569. /// </summary>
  570. internal void Dispose()
  571. {
  572. m_scene = null;
  573. if (ray != IntPtr.Zero)
  574. {
  575. SafeNativeMethods.GeomDestroy(ray);
  576. ray = IntPtr.Zero;
  577. }
  578. if (Box != IntPtr.Zero)
  579. {
  580. SafeNativeMethods.GeomDestroy(Box);
  581. Box = IntPtr.Zero;
  582. }
  583. if (Sphere != IntPtr.Zero)
  584. {
  585. SafeNativeMethods.GeomDestroy(Sphere);
  586. Sphere = IntPtr.Zero;
  587. }
  588. if (Plane != IntPtr.Zero)
  589. {
  590. SafeNativeMethods.GeomDestroy(Plane);
  591. Plane = IntPtr.Zero;
  592. }
  593. }
  594. }
  595. public struct ODERayRequest
  596. {
  597. public PhysicsActor actor;
  598. public Vector3 Origin;
  599. public Vector3 Normal;
  600. public int Count;
  601. public float length;
  602. public object callbackMethod;
  603. public RayFilterFlags filter;
  604. public Quaternion orientation;
  605. }
  606. }