ODERayCastRequestManager.cs 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433
  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 System.Text;
  32. using OpenMetaverse;
  33. using OpenSim.Region.Physics.Manager;
  34. using Ode.NET;
  35. using log4net;
  36. namespace OpenSim.Region.Physics.OdePlugin
  37. {
  38. /// <summary>
  39. /// Processes raycast requests as ODE is in a state to be able to do them.
  40. /// This ensures that it's thread safe and there will be no conflicts.
  41. /// Requests get returned by a different thread then they were requested by.
  42. /// </summary>
  43. public class ODERayCastRequestManager
  44. {
  45. /// <summary>
  46. /// Pending raycast requests
  47. /// </summary>
  48. protected List<ODERayCastRequest> m_PendingRequests = new List<ODERayCastRequest>();
  49. /// <summary>
  50. /// Pending ray requests
  51. /// </summary>
  52. protected List<ODERayRequest> m_PendingRayRequests = new List<ODERayRequest>();
  53. /// <summary>
  54. /// Scene that created this object.
  55. /// </summary>
  56. private OdeScene m_scene;
  57. /// <summary>
  58. /// ODE contact array to be filled by the collision testing
  59. /// </summary>
  60. d.ContactGeom[] contacts = new d.ContactGeom[5];
  61. /// <summary>
  62. /// ODE near callback delegate
  63. /// </summary>
  64. private d.NearCallback nearCallback;
  65. private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
  66. private List<ContactResult> m_contactResults = new List<ContactResult>();
  67. public ODERayCastRequestManager(OdeScene pScene)
  68. {
  69. m_scene = pScene;
  70. nearCallback = near;
  71. }
  72. /// <summary>
  73. /// Queues a raycast
  74. /// </summary>
  75. /// <param name="position">Origin of Ray</param>
  76. /// <param name="direction">Ray normal</param>
  77. /// <param name="length">Ray length</param>
  78. /// <param name="retMethod">Return method to send the results</param>
  79. public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
  80. {
  81. lock (m_PendingRequests)
  82. {
  83. ODERayCastRequest req = new ODERayCastRequest();
  84. req.callbackMethod = retMethod;
  85. req.length = length;
  86. req.Normal = direction;
  87. req.Origin = position;
  88. m_PendingRequests.Add(req);
  89. }
  90. }
  91. /// <summary>
  92. /// Queues a raycast
  93. /// </summary>
  94. /// <param name="position">Origin of Ray</param>
  95. /// <param name="direction">Ray normal</param>
  96. /// <param name="length">Ray length</param>
  97. /// <param name="count"></param>
  98. /// <param name="retMethod">Return method to send the results</param>
  99. public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
  100. {
  101. lock (m_PendingRequests)
  102. {
  103. ODERayRequest req = new ODERayRequest();
  104. req.callbackMethod = retMethod;
  105. req.length = length;
  106. req.Normal = direction;
  107. req.Origin = position;
  108. req.Count = count;
  109. m_PendingRayRequests.Add(req);
  110. }
  111. }
  112. /// <summary>
  113. /// Process all queued raycast requests
  114. /// </summary>
  115. /// <returns>Time in MS the raycasts took to process.</returns>
  116. public int ProcessQueuedRequests()
  117. {
  118. int time = System.Environment.TickCount;
  119. lock (m_PendingRequests)
  120. {
  121. if (m_PendingRequests.Count > 0)
  122. {
  123. ODERayCastRequest[] reqs = m_PendingRequests.ToArray();
  124. for (int i = 0; i < reqs.Length; i++)
  125. {
  126. if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast
  127. RayCast(reqs[i]); // if there isn't anyone to send results
  128. }
  129. m_PendingRequests.Clear();
  130. }
  131. }
  132. lock (m_PendingRayRequests)
  133. {
  134. if (m_PendingRayRequests.Count > 0)
  135. {
  136. ODERayRequest[] reqs = m_PendingRayRequests.ToArray();
  137. for (int i = 0; i < reqs.Length; i++)
  138. {
  139. if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast
  140. RayCast(reqs[i]); // if there isn't anyone to send results
  141. }
  142. m_PendingRayRequests.Clear();
  143. }
  144. }
  145. lock (m_contactResults)
  146. m_contactResults.Clear();
  147. return System.Environment.TickCount - time;
  148. }
  149. /// <summary>
  150. /// Method that actually initiates the raycast
  151. /// </summary>
  152. /// <param name="req"></param>
  153. private void RayCast(ODERayCastRequest req)
  154. {
  155. // Create the ray
  156. IntPtr ray = d.CreateRay(m_scene.space, req.length);
  157. d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
  158. // Collide test
  159. d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback);
  160. // Remove Ray
  161. d.GeomDestroy(ray);
  162. // Define default results
  163. bool hitYN = false;
  164. uint hitConsumerID = 0;
  165. float distance = 999999999999f;
  166. Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
  167. Vector3 snormal = Vector3.Zero;
  168. // Find closest contact and object.
  169. lock (m_contactResults)
  170. {
  171. foreach (ContactResult cResult in m_contactResults)
  172. {
  173. if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
  174. {
  175. closestcontact = cResult.Pos;
  176. hitConsumerID = cResult.ConsumerID;
  177. distance = cResult.Depth;
  178. hitYN = true;
  179. snormal = cResult.Normal;
  180. }
  181. }
  182. m_contactResults.Clear();
  183. }
  184. // Return results
  185. if (req.callbackMethod != null)
  186. req.callbackMethod(hitYN, closestcontact, hitConsumerID, distance, snormal);
  187. }
  188. /// <summary>
  189. /// Method that actually initiates the raycast
  190. /// </summary>
  191. /// <param name="req"></param>
  192. private void RayCast(ODERayRequest req)
  193. {
  194. // Create the ray
  195. IntPtr ray = d.CreateRay(m_scene.space, req.length);
  196. d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
  197. // Collide test
  198. d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback);
  199. // Remove Ray
  200. d.GeomDestroy(ray);
  201. // Find closest contact and object.
  202. lock (m_contactResults)
  203. {
  204. // Return results
  205. if (req.callbackMethod != null)
  206. req.callbackMethod(m_contactResults);
  207. }
  208. }
  209. // This is the standard Near. Uses space AABBs to speed up detection.
  210. private void near(IntPtr space, IntPtr g1, IntPtr g2)
  211. {
  212. //Don't test against heightfield Geom, or you'll be sorry!
  213. /*
  214. terminate called after throwing an instance of 'std::bad_alloc'
  215. what(): std::bad_alloc
  216. Stacktrace:
  217. at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0x00004>
  218. at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0xffffffff>
  219. at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0x00280>
  220. at (wrapper native-to-managed) OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0xfff
  221. fffff>
  222. at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0x00004>
  223. at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0xffffffff>
  224. at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.RayCast (OpenSim.Region.Physics.OdePlugin.ODERayCastRequest) <
  225. 0x00114>
  226. at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.ProcessQueuedRequests () <0x000eb>
  227. at OpenSim.Region.Physics.OdePlugin.OdeScene.Simulate (single) <0x017e6>
  228. at OpenSim.Region.Framework.Scenes.SceneGraph.UpdatePhysics (double) <0x00042>
  229. at OpenSim.Region.Framework.Scenes.Scene.Update () <0x0039e>
  230. at OpenSim.Region.Framework.Scenes.Scene.Heartbeat (object) <0x00019>
  231. at (wrapper runtime-invoke) object.runtime_invoke_void__this___object (object,intptr,intptr,intptr) <0xffffffff>
  232. Native stacktrace:
  233. mono [0x80d2a42]
  234. [0xb7f5840c]
  235. /lib/i686/cmov/libc.so.6(abort+0x188) [0xb7d1a018]
  236. /usr/lib/libstdc++.so.6(_ZN9__gnu_cxx27__verbose_terminate_handlerEv+0x158) [0xb45fc988]
  237. /usr/lib/libstdc++.so.6 [0xb45fa865]
  238. /usr/lib/libstdc++.so.6 [0xb45fa8a2]
  239. /usr/lib/libstdc++.so.6 [0xb45fa9da]
  240. /usr/lib/libstdc++.so.6(_Znwj+0x83) [0xb45fb033]
  241. /usr/lib/libstdc++.so.6(_Znaj+0x1d) [0xb45fb11d]
  242. libode.so(_ZN13dxHeightfield23dCollideHeightfieldZoneEiiiiP6dxGeomiiP12dContactGeomi+0xd04) [0xb46678e4]
  243. libode.so(_Z19dCollideHeightfieldP6dxGeomS0_iP12dContactGeomi+0x54b) [0xb466832b]
  244. libode.so(dCollide+0x102) [0xb46571b2]
  245. [0x95cfdec9]
  246. [0x8ea07fe1]
  247. [0xab260146]
  248. libode.so [0xb465a5c4]
  249. libode.so(_ZN11dxHashSpace8collide2EPvP6dxGeomPFvS0_S2_S2_E+0x75) [0xb465bcf5]
  250. libode.so(dSpaceCollide2+0x177) [0xb465ac67]
  251. [0x95cf978e]
  252. [0x8ea07945]
  253. [0x95cf2bbc]
  254. [0xab2787e7]
  255. [0xab419fb3]
  256. [0xab416657]
  257. [0xab415bda]
  258. [0xb609b08e]
  259. mono(mono_runtime_delegate_invoke+0x34) [0x8192534]
  260. mono [0x81a2f0f]
  261. mono [0x81d28b6]
  262. mono [0x81ea2c6]
  263. /lib/i686/cmov/libpthread.so.0 [0xb7e744c0]
  264. /lib/i686/cmov/libc.so.6(clone+0x5e) [0xb7dcd6de]
  265. */
  266. // Exclude heightfield geom
  267. if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
  268. return;
  269. if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass)
  270. return;
  271. // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
  272. if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
  273. {
  274. if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
  275. return;
  276. // Separating static prim geometry spaces.
  277. // We'll be calling near recursivly if one
  278. // of them is a space to find all of the
  279. // contact points in the space
  280. try
  281. {
  282. d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
  283. }
  284. catch (AccessViolationException)
  285. {
  286. m_log.Warn("[PHYSICS]: Unable to collide test a space");
  287. return;
  288. }
  289. //Colliding a space or a geom with a space or a geom. so drill down
  290. //Collide all geoms in each space..
  291. //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
  292. //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
  293. return;
  294. }
  295. if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
  296. return;
  297. int count = 0;
  298. try
  299. {
  300. if (g1 == g2)
  301. return; // Can't collide with yourself
  302. lock (contacts)
  303. {
  304. count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf);
  305. }
  306. }
  307. catch (SEHException)
  308. {
  309. m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
  310. }
  311. catch (Exception e)
  312. {
  313. m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
  314. return;
  315. }
  316. PhysicsActor p1 = null;
  317. PhysicsActor p2 = null;
  318. if (g1 != IntPtr.Zero)
  319. m_scene.actor_name_map.TryGetValue(g1, out p1);
  320. if (g2 != IntPtr.Zero)
  321. m_scene.actor_name_map.TryGetValue(g1, out p2);
  322. // Loop over contacts, build results.
  323. for (int i = 0; i < count; i++)
  324. {
  325. if (p1 != null) {
  326. if (p1 is OdePrim)
  327. {
  328. ContactResult collisionresult = new ContactResult();
  329. collisionresult.ConsumerID = ((OdePrim)p1).m_localID;
  330. collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
  331. collisionresult.Depth = contacts[i].depth;
  332. collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
  333. contacts[i].normal.Z);
  334. lock (m_contactResults)
  335. m_contactResults.Add(collisionresult);
  336. }
  337. }
  338. if (p2 != null)
  339. {
  340. if (p2 is OdePrim)
  341. {
  342. ContactResult collisionresult = new ContactResult();
  343. collisionresult.ConsumerID = ((OdePrim)p2).m_localID;
  344. collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
  345. collisionresult.Depth = contacts[i].depth;
  346. collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
  347. contacts[i].normal.Z);
  348. lock (m_contactResults)
  349. m_contactResults.Add(collisionresult);
  350. }
  351. }
  352. }
  353. }
  354. /// <summary>
  355. /// Dereference the creator scene so that it can be garbage collected if needed.
  356. /// </summary>
  357. internal void Dispose()
  358. {
  359. m_scene = null;
  360. }
  361. }
  362. public struct ODERayCastRequest
  363. {
  364. public Vector3 Origin;
  365. public Vector3 Normal;
  366. public float length;
  367. public RaycastCallback callbackMethod;
  368. }
  369. public struct ODERayRequest
  370. {
  371. public Vector3 Origin;
  372. public Vector3 Normal;
  373. public int Count;
  374. public float length;
  375. public RayCallback callbackMethod;
  376. }
  377. }