ODERayCastRequestManager.cs 14 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384
  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.PhysicsModules.SharedBase;
  34. using log4net;
  35. namespace OpenSim.Region.PhysicsModule.ODE
  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 raycast requests
  46. /// </summary>
  47. protected List<ODERayCastRequest> m_PendingRequests = new List<ODERayCastRequest>();
  48. /// <summary>
  49. /// Pending ray requests
  50. /// </summary>
  51. protected List<ODERayRequest> m_PendingRayRequests = new List<ODERayRequest>();
  52. /// <summary>
  53. /// Scene that created this object.
  54. /// </summary>
  55. private OdeScene m_scene;
  56. /// <summary>
  57. /// ODE contact array to be filled by the collision testing
  58. /// </summary>
  59. d.ContactGeom[] contacts = new d.ContactGeom[5];
  60. /// <summary>
  61. /// ODE near callback delegate
  62. /// </summary>
  63. private d.NearCallback nearCallback;
  64. private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
  65. private List<ContactResult> m_contactResults = new List<ContactResult>();
  66. public ODERayCastRequestManager(OdeScene pScene)
  67. {
  68. m_scene = pScene;
  69. nearCallback = near;
  70. }
  71. /// <summary>
  72. /// Queues a raycast
  73. /// </summary>
  74. /// <param name="position">Origin of Ray</param>
  75. /// <param name="direction">Ray normal</param>
  76. /// <param name="length">Ray length</param>
  77. /// <param name="retMethod">Return method to send the results</param>
  78. public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
  79. {
  80. lock (m_PendingRequests)
  81. {
  82. ODERayCastRequest req = new ODERayCastRequest();
  83. req.callbackMethod = retMethod;
  84. req.length = length;
  85. req.Normal = direction;
  86. req.Origin = position;
  87. m_PendingRequests.Add(req);
  88. }
  89. }
  90. /// <summary>
  91. /// Queues a raycast
  92. /// </summary>
  93. /// <param name="position">Origin of Ray</param>
  94. /// <param name="direction">Ray normal</param>
  95. /// <param name="length">Ray length</param>
  96. /// <param name="count"></param>
  97. /// <param name="retMethod">Return method to send the results</param>
  98. public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
  99. {
  100. lock (m_PendingRequests)
  101. {
  102. ODERayRequest req = new ODERayRequest();
  103. req.callbackMethod = retMethod;
  104. req.length = length;
  105. req.Normal = direction;
  106. req.Origin = position;
  107. req.Count = count;
  108. m_PendingRayRequests.Add(req);
  109. }
  110. }
  111. /// <summary>
  112. /// Process all queued raycast requests
  113. /// </summary>
  114. /// <returns>Time in MS the raycasts took to process.</returns>
  115. public int ProcessQueuedRequests()
  116. {
  117. int time = System.Environment.TickCount;
  118. lock (m_PendingRequests)
  119. {
  120. if (m_PendingRequests.Count > 0)
  121. {
  122. ODERayCastRequest[] reqs = m_PendingRequests.ToArray();
  123. for (int i = 0; i < reqs.Length; i++)
  124. {
  125. if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast
  126. RayCast(reqs[i]); // if there isn't anyone to send results
  127. }
  128. m_PendingRequests.Clear();
  129. }
  130. }
  131. lock (m_PendingRayRequests)
  132. {
  133. if (m_PendingRayRequests.Count > 0)
  134. {
  135. ODERayRequest[] reqs = m_PendingRayRequests.ToArray();
  136. for (int i = 0; i < reqs.Length; i++)
  137. {
  138. if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast
  139. RayCast(reqs[i]); // if there isn't anyone to send results
  140. }
  141. m_PendingRayRequests.Clear();
  142. }
  143. }
  144. lock (m_contactResults)
  145. m_contactResults.Clear();
  146. return System.Environment.TickCount - time;
  147. }
  148. /// <summary>
  149. /// Method that actually initiates the raycast
  150. /// </summary>
  151. /// <param name="req"></param>
  152. private void RayCast(ODERayCastRequest req)
  153. {
  154. // NOTE: limit ray length or collisions will take all avaiable stack space
  155. // this value may still be too large, depending on machine configuration
  156. // of maximum stack
  157. float len = req.length;
  158. if (len > 100f)
  159. len = 100f;
  160. // Create the ray
  161. IntPtr ray = d.CreateRay(m_scene.space, len);
  162. d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
  163. // Collide test
  164. d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback);
  165. // Remove Ray
  166. d.GeomDestroy(ray);
  167. // Define default results
  168. bool hitYN = false;
  169. uint hitConsumerID = 0;
  170. float distance = 999999999999f;
  171. Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f);
  172. Vector3 snormal = Vector3.Zero;
  173. // Find closest contact and object.
  174. lock (m_contactResults)
  175. {
  176. foreach (ContactResult cResult in m_contactResults)
  177. {
  178. if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact))
  179. {
  180. closestcontact = cResult.Pos;
  181. hitConsumerID = cResult.ConsumerID;
  182. distance = cResult.Depth;
  183. hitYN = true;
  184. snormal = cResult.Normal;
  185. }
  186. }
  187. m_contactResults.Clear();
  188. }
  189. // Return results
  190. if (req.callbackMethod != null)
  191. req.callbackMethod(hitYN, closestcontact, hitConsumerID, distance, snormal);
  192. }
  193. /// <summary>
  194. /// Method that actually initiates the raycast
  195. /// </summary>
  196. /// <param name="req"></param>
  197. private void RayCast(ODERayRequest req)
  198. {
  199. // limit ray length or collisions will take all avaiable stack space
  200. float len = req.length;
  201. if (len > 100f)
  202. len = 100f;
  203. // Create the ray
  204. IntPtr ray = d.CreateRay(m_scene.space, len);
  205. d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
  206. // Collide test
  207. d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback);
  208. // Remove Ray
  209. d.GeomDestroy(ray);
  210. // Find closest contact and object.
  211. lock (m_contactResults)
  212. {
  213. // Return results
  214. if (req.callbackMethod != null)
  215. req.callbackMethod(m_contactResults);
  216. }
  217. }
  218. // This is the standard Near. Uses space AABBs to speed up detection.
  219. private void near(IntPtr space, IntPtr g1, IntPtr g2)
  220. {
  221. if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
  222. return;
  223. // if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass)
  224. // return;
  225. // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms.
  226. if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
  227. {
  228. if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
  229. return;
  230. // Separating static prim geometry spaces.
  231. // We'll be calling near recursivly if one
  232. // of them is a space to find all of the
  233. // contact points in the space
  234. try
  235. {
  236. d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
  237. }
  238. catch (AccessViolationException)
  239. {
  240. m_log.Warn("[PHYSICS]: Unable to collide test a space");
  241. return;
  242. }
  243. //Colliding a space or a geom with a space or a geom. so drill down
  244. //Collide all geoms in each space..
  245. //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
  246. //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
  247. return;
  248. }
  249. if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
  250. return;
  251. int count = 0;
  252. try
  253. {
  254. if (g1 == g2)
  255. return; // Can't collide with yourself
  256. lock (contacts)
  257. {
  258. count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.unmanagedSizeOf);
  259. }
  260. }
  261. catch (SEHException)
  262. {
  263. 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.");
  264. }
  265. catch (Exception e)
  266. {
  267. m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
  268. return;
  269. }
  270. PhysicsActor p1 = null;
  271. PhysicsActor p2 = null;
  272. if (g1 != IntPtr.Zero)
  273. m_scene.actor_name_map.TryGetValue(g1, out p1);
  274. if (g2 != IntPtr.Zero)
  275. m_scene.actor_name_map.TryGetValue(g1, out p2);
  276. // Loop over contacts, build results.
  277. for (int i = 0; i < count; i++)
  278. {
  279. if (p1 != null)
  280. {
  281. if (p1 is OdePrim)
  282. {
  283. ContactResult collisionresult = new ContactResult();
  284. collisionresult.ConsumerID = p1.LocalID;
  285. collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
  286. collisionresult.Depth = contacts[i].depth;
  287. collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
  288. contacts[i].normal.Z);
  289. lock (m_contactResults)
  290. m_contactResults.Add(collisionresult);
  291. }
  292. }
  293. if (p2 != null)
  294. {
  295. if (p2 is OdePrim)
  296. {
  297. ContactResult collisionresult = new ContactResult();
  298. collisionresult.ConsumerID = p2.LocalID;
  299. collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z);
  300. collisionresult.Depth = contacts[i].depth;
  301. collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y,
  302. contacts[i].normal.Z);
  303. lock (m_contactResults)
  304. m_contactResults.Add(collisionresult);
  305. }
  306. }
  307. }
  308. }
  309. /// <summary>
  310. /// Dereference the creator scene so that it can be garbage collected if needed.
  311. /// </summary>
  312. internal void Dispose()
  313. {
  314. m_scene = null;
  315. }
  316. }
  317. public struct ODERayCastRequest
  318. {
  319. public Vector3 Origin;
  320. public Vector3 Normal;
  321. public float length;
  322. public RaycastCallback callbackMethod;
  323. }
  324. public struct ODERayRequest
  325. {
  326. public Vector3 Origin;
  327. public Vector3 Normal;
  328. public int Count;
  329. public float length;
  330. public RayCallback callbackMethod;
  331. }
  332. }