aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs640
1 files changed, 640 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
new file mode 100644
index 0000000..561ab1c
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,640 @@
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
28using System;
29using System.Collections.Generic;
30using System.Reflection;
31using System.Runtime.InteropServices;
32using System.Text;
33using OpenSim.Framework;
34using OpenSim.Region.Physics.Manager;
35using OdeAPI;
36using log4net;
37using OpenMetaverse;
38
39namespace OpenSim.Region.Physics.OdePlugin
40{
41 /// <summary>
42 /// Processes raycast requests as ODE is in a state to be able to do them.
43 /// This ensures that it's thread safe and there will be no conflicts.
44 /// Requests get returned by a different thread then they were requested by.
45 /// </summary>
46 public class ODERayCastRequestManager
47 {
48 /// <summary>
49 /// Pending ray requests
50 /// </summary>
51 protected OpenSim.Framework.LocklessQueue<ODERayRequest> m_PendingRequests = new OpenSim.Framework.LocklessQueue<ODERayRequest>();
52
53 /// <summary>
54 /// Scene that created this object.
55 /// </summary>
56 private OdeScene m_scene;
57
58 IntPtr ray; // the ray. we only need one for our lifetime
59
60 private const int ColisionContactGeomsPerTest = 5;
61 private const int DefaultMaxCount = 25;
62 private const int MaxTimePerCallMS = 30;
63
64 /// <summary>
65 /// ODE near callback delegate
66 /// </summary>
67 private d.NearCallback nearCallback;
68 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
69 private List<ContactResult> m_contactResults = new List<ContactResult>();
70 private RayFilterFlags CurrentRayFilter;
71 private int CurrentMaxCount;
72
73 public ODERayCastRequestManager(OdeScene pScene)
74 {
75 m_scene = pScene;
76 nearCallback = near;
77 ray = d.CreateRay(IntPtr.Zero, 1.0f);
78 d.GeomSetCategoryBits(ray,0);
79 }
80
81 /// <summary>
82 /// Queues request for a raycast to all world
83 /// </summary>
84 /// <param name="position">Origin of Ray</param>
85 /// <param name="direction">Ray direction</param>
86 /// <param name="length">Ray length</param>
87 /// <param name="retMethod">Return method to send the results</param>
88 public void QueueRequest(Vector3 position, Vector3 direction, float length, RayCallback retMethod)
89 {
90 ODERayRequest req = new ODERayRequest();
91 req.geom = IntPtr.Zero;
92 req.callbackMethod = retMethod;
93 req.Count = DefaultMaxCount;
94 req.length = length;
95 req.Normal = direction;
96 req.Origin = position;
97 req.filter = RayFilterFlags.AllPrims;
98
99 m_PendingRequests.Enqueue(req);
100 }
101
102 /// <summary>
103 /// Queues request for a raycast to particular part
104 /// </summary>
105 /// <param name="position">Origin of Ray</param>
106 /// <param name="direction">Ray direction</param>
107 /// <param name="length">Ray length</param>
108 /// <param name="retMethod">Return method to send the results</param>
109 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RayCallback retMethod)
110 {
111 ODERayRequest req = new ODERayRequest();
112 req.geom = geom;
113 req.callbackMethod = retMethod;
114 req.length = length;
115 req.Normal = direction;
116 req.Origin = position;
117 req.Count = DefaultMaxCount;
118 req.filter = RayFilterFlags.AllPrims;
119
120 m_PendingRequests.Enqueue(req);
121 }
122
123 public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
124 {
125 ODERayRequest req = new ODERayRequest();
126 req.geom = IntPtr.Zero;
127 req.callbackMethod = retMethod;
128 req.Count = DefaultMaxCount;
129 req.length = length;
130 req.Normal = direction;
131 req.Origin = position;
132 req.filter = RayFilterFlags.AllPrims | RayFilterFlags.land;
133
134 m_PendingRequests.Enqueue(req);
135 }
136
137 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
138 {
139 ODERayRequest req = new ODERayRequest();
140 req.geom = geom;
141 req.callbackMethod = retMethod;
142 req.length = length;
143 req.Normal = direction;
144 req.Origin = position;
145 req.Count = DefaultMaxCount;
146 req.filter = RayFilterFlags.AllPrims;
147
148 m_PendingRequests.Enqueue(req);
149 }
150
151 /// <summary>
152 /// Queues a raycast
153 /// </summary>
154 /// <param name="position">Origin of Ray</param>
155 /// <param name="direction">Ray normal</param>
156 /// <param name="length">Ray length</param>
157 /// <param name="count"></param>
158 /// <param name="retMethod">Return method to send the results</param>
159 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
160 {
161 ODERayRequest req = new ODERayRequest();
162 req.geom = IntPtr.Zero;
163 req.callbackMethod = retMethod;
164 req.length = length;
165 req.Normal = direction;
166 req.Origin = position;
167 req.Count = count;
168 req.filter = RayFilterFlags.AllPrims;
169
170 m_PendingRequests.Enqueue(req);
171 }
172
173
174 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count,RayFilterFlags filter , RayCallback retMethod)
175 {
176 ODERayRequest req = new ODERayRequest();
177 req.geom = IntPtr.Zero;
178 req.callbackMethod = retMethod;
179 req.length = length;
180 req.Normal = direction;
181 req.Origin = position;
182 req.Count = count;
183 req.filter = filter;
184
185 m_PendingRequests.Enqueue(req);
186 }
187
188 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod)
189 {
190 ODERayRequest req = new ODERayRequest();
191 req.geom = geom;
192 req.callbackMethod = retMethod;
193 req.length = length;
194 req.Normal = direction;
195 req.Origin = position;
196 req.Count = count;
197 req.filter = RayFilterFlags.AllPrims;
198
199 m_PendingRequests.Enqueue(req);
200 }
201
202 public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
203 {
204 ODERayRequest req = new ODERayRequest();
205 req.geom = IntPtr.Zero;
206 req.callbackMethod = retMethod;
207 req.length = length;
208 req.Normal = direction;
209 req.Origin = position;
210 req.Count = count;
211 req.filter = RayFilterFlags.AllPrims;
212
213 m_PendingRequests.Enqueue(req);
214 }
215
216 public void QueueRequest(IntPtr geom, Vector3 position, Vector3 direction, float length, int count, RaycastCallback retMethod)
217 {
218 ODERayRequest req = new ODERayRequest();
219 req.geom = geom;
220 req.callbackMethod = retMethod;
221 req.length = length;
222 req.Normal = direction;
223 req.Origin = position;
224 req.Count = count;
225 req.filter = RayFilterFlags.AllPrims;
226
227 m_PendingRequests.Enqueue(req);
228 }
229
230 /// <summary>
231 /// Process all queued raycast requests
232 /// </summary>
233 /// <returns>Time in MS the raycasts took to process.</returns>
234 public int ProcessQueuedRequests()
235 {
236
237 if (m_PendingRequests.Count <= 0)
238 return 0;
239
240 if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero)
241 // oops something got wrong or scene isn't ready still
242 {
243 m_PendingRequests.Clear();
244 return 0;
245 }
246
247 int time = Util.EnvironmentTickCount();
248
249 ODERayRequest req;
250 int closestHit;
251 int backfacecull;
252 CollisionCategories catflags;
253
254 while (m_PendingRequests.Dequeue(out req))
255 {
256 if (req.callbackMethod != null)
257 {
258 CurrentRayFilter = req.filter;
259 CurrentMaxCount = req.Count;
260
261 closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1);
262 backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1);
263
264 d.GeomRaySetLength(ray, req.length);
265 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
266 d.GeomRaySetParams(ray, 0, backfacecull);
267 d.GeomRaySetClosestHit(ray, closestHit);
268
269 if (req.callbackMethod is RaycastCallback)
270 // if we only want one get only one per colision pair saving memory
271 CurrentRayFilter |= RayFilterFlags.ClosestHit;
272
273 if (req.geom == IntPtr.Zero)
274 {
275 // translate ray filter to colision flags
276 catflags = 0;
277 if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0)
278 catflags |= CollisionCategories.VolumeDtc;
279 if ((CurrentRayFilter & RayFilterFlags.phantom) != 0)
280 catflags |= CollisionCategories.Phantom;
281 if ((CurrentRayFilter & RayFilterFlags.agent) != 0)
282 catflags |= CollisionCategories.Character;
283 if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0)
284 catflags |= CollisionCategories.Geom;
285 if ((CurrentRayFilter & RayFilterFlags.land) != 0)
286 catflags |= CollisionCategories.Land;
287 if ((CurrentRayFilter & RayFilterFlags.water) != 0)
288 catflags |= CollisionCategories.Water;
289
290 if (catflags != 0)
291 {
292 d.GeomSetCollideBits(ray, (uint)catflags);
293 doSpaceRay(req);
294 }
295 }
296 else
297 {
298 // if we select a geom don't use filters
299 d.GeomSetCollideBits(ray, (uint)CollisionCategories.All);
300 doGeomRay(req);
301 }
302 }
303
304 if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS)
305 break;
306 }
307
308 lock (m_contactResults)
309 m_contactResults.Clear();
310
311 return Util.EnvironmentTickCountSubtract(time);
312 }
313 /// <summary>
314 /// Method that actually initiates the raycast with spaces
315 /// </summary>
316 /// <param name="req"></param>
317 ///
318
319 private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhanton;
320// private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
321 private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
322
323 private void doSpaceRay(ODERayRequest req)
324 {
325 // Collide tests
326 if ((CurrentRayFilter & FilterActiveSpace) != 0)
327 {
328 d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
329 d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
330 }
331 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
332 d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
333 if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
334 {
335 // current ode land to ray collisions is very bad
336 // so for now limit its range badly
337
338 if (req.length > 30.0f)
339 d.GeomRaySetLength(ray, 30.0f);
340
341 d.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
342 }
343
344 if (req.callbackMethod is RaycastCallback)
345 {
346 // Define default results
347 bool hitYN = false;
348 uint hitConsumerID = 0;
349 float distance = float.MaxValue;
350 Vector3 closestcontact = Vector3.Zero;
351 Vector3 snormal = Vector3.Zero;
352
353 // Find closest contact and object.
354 lock (m_contactResults)
355 {
356 foreach (ContactResult cResult in m_contactResults)
357 {
358 if(cResult.Depth < distance)
359 {
360 closestcontact = cResult.Pos;
361 hitConsumerID = cResult.ConsumerID;
362 distance = cResult.Depth;
363 snormal = cResult.Normal;
364 }
365 }
366 m_contactResults.Clear();
367 }
368
369 if (distance > 0 && distance < float.MaxValue)
370 hitYN = true;
371 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
372 }
373 else
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 ((RayCallback)req.callbackMethod)(cresult);
382 }
383 }
384
385 /// <summary>
386 /// Method that actually initiates the raycast with a geom
387 /// </summary>
388 /// <param name="req"></param>
389 private void doGeomRay(ODERayRequest req)
390 {
391 // Collide test
392 d.SpaceCollide2(ray, req.geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
393
394 if (req.callbackMethod is RaycastCallback)
395 {
396 // Define default results
397 bool hitYN = false;
398 uint hitConsumerID = 0;
399 float distance = float.MaxValue;
400 Vector3 closestcontact = Vector3.Zero;
401 Vector3 snormal = Vector3.Zero;
402
403 // Find closest contact and object.
404 lock (m_contactResults)
405 {
406 foreach (ContactResult cResult in m_contactResults)
407 {
408 if(cResult.Depth < distance )
409 {
410 closestcontact = cResult.Pos;
411 hitConsumerID = cResult.ConsumerID;
412 distance = cResult.Depth;
413 snormal = cResult.Normal;
414 }
415 }
416 m_contactResults.Clear();
417 }
418
419 if (distance > 0 && distance < float.MaxValue)
420 hitYN = true;
421
422 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
423 }
424 else
425 {
426 List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
427 lock (m_PendingRequests)
428 {
429 cresult.AddRange(m_contactResults);
430 m_contactResults.Clear();
431 }
432 ((RayCallback)req.callbackMethod)(cresult);
433 }
434 }
435
436 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
437 {
438 IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
439 if (ContactgeomsArray == IntPtr.Zero || index >= ColisionContactGeomsPerTest)
440 return false;
441
442 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
443 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
444 return true;
445 }
446
447 // This is the standard Near. g1 is the ray
448 private void near(IntPtr space, IntPtr g1, IntPtr g2)
449 {
450 if (g2 == IntPtr.Zero || g1 == g2)
451 return;
452
453 if (m_contactResults.Count >= CurrentMaxCount)
454 return;
455
456 if (d.GeomIsSpace(g2))
457 {
458 try
459 {
460 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
461 }
462 catch (Exception e)
463 {
464 m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
465 }
466 return;
467 }
468
469 int count = 0;
470 try
471 {
472 count = d.CollidePtr(g1, g2, ColisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
473 }
474 catch (Exception e)
475 {
476 m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
477 return;
478 }
479
480 if (count == 0)
481 return;
482
483 uint ID = 0;
484 PhysicsActor p2 = null;
485
486 m_scene.actor_name_map.TryGetValue(g2, out p2);
487
488 if (p2 == null)
489 {
490 /*
491 string name;
492
493 if (!m_scene.geom_name_map.TryGetValue(g2, out name))
494 return;
495
496 if (name == "Terrain")
497 {
498 // land colision
499 if ((CurrentRayFilter & RayFilterFlags.land) == 0)
500 return;
501 }
502 else if (name == "Water")
503 {
504 if ((CurrentRayFilter & RayFilterFlags.water) == 0)
505 return;
506 }
507 else
508 return;
509 */
510 return;
511 }
512 else
513 {
514 switch (p2.PhysicsActorType)
515 {
516 case (int)ActorTypes.Prim:
517
518 RayFilterFlags thisFlags;
519
520 if (p2.IsPhysical)
521 thisFlags = RayFilterFlags.physical;
522 else
523 thisFlags = RayFilterFlags.nonphysical;
524
525 if (p2.Phantom)
526 thisFlags |= RayFilterFlags.phantom;
527
528 if (p2.IsVolumeDtc)
529 thisFlags |= RayFilterFlags.volumedtc;
530
531 if ((thisFlags & CurrentRayFilter) == 0)
532 return;
533
534 ID = ((OdePrim)p2).LocalID;
535 break;
536
537 case (int)ActorTypes.Agent:
538
539 if ((CurrentRayFilter & RayFilterFlags.agent) == 0)
540 return;
541 else
542 ID = ((OdeCharacter)p2).LocalID;
543 break;
544
545 case (int)ActorTypes.Ground:
546
547 if ((CurrentRayFilter & RayFilterFlags.land) == 0)
548 return;
549 break;
550
551 case (int)ActorTypes.Water:
552
553 if ((CurrentRayFilter & RayFilterFlags.water) == 0)
554 return;
555 break;
556
557 default:
558 break;
559 }
560 }
561
562 d.ContactGeom curcontact = new d.ContactGeom();
563
564 // closestHit for now only works for meshs, so must do it for others
565 if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0)
566 {
567 // Loop all contacts, build results.
568 for (int i = 0; i < count; i++)
569 {
570 if (!GetCurContactGeom(i, ref curcontact))
571 break;
572
573 ContactResult collisionresult = new ContactResult();
574 collisionresult.ConsumerID = ID;
575 collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
576 collisionresult.Depth = curcontact.depth;
577 collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
578 curcontact.normal.Z);
579 lock (m_contactResults)
580 {
581 m_contactResults.Add(collisionresult);
582 if (m_contactResults.Count >= CurrentMaxCount)
583 return;
584 }
585 }
586 }
587 else
588 {
589 // keep only closest contact
590 ContactResult collisionresult = new ContactResult();
591 collisionresult.ConsumerID = ID;
592 collisionresult.Depth = float.MaxValue;
593
594 for (int i = 0; i < count; i++)
595 {
596 if (!GetCurContactGeom(i, ref curcontact))
597 break;
598
599 if (curcontact.depth < collisionresult.Depth)
600 {
601 collisionresult.Pos = new Vector3(curcontact.pos.X, curcontact.pos.Y, curcontact.pos.Z);
602 collisionresult.Depth = curcontact.depth;
603 collisionresult.Normal = new Vector3(curcontact.normal.X, curcontact.normal.Y,
604 curcontact.normal.Z);
605 }
606 }
607
608 if (collisionresult.Depth != float.MaxValue)
609 {
610 lock (m_contactResults)
611 m_contactResults.Add(collisionresult);
612 }
613 }
614 }
615
616 /// <summary>
617 /// Dereference the creator scene so that it can be garbage collected if needed.
618 /// </summary>
619 internal void Dispose()
620 {
621 m_scene = null;
622 if (ray != IntPtr.Zero)
623 {
624 d.GeomDestroy(ray);
625 ray = IntPtr.Zero;
626 }
627 }
628 }
629
630 public struct ODERayRequest
631 {
632 public IntPtr geom;
633 public Vector3 Origin;
634 public Vector3 Normal;
635 public int Count;
636 public float length;
637 public object callbackMethod;
638 public RayFilterFlags filter;
639 }
640} \ No newline at end of file