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.cs800
1 files changed, 800 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..7fe3109
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/ODERayCastRequestManager.cs
@@ -0,0 +1,800 @@
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 IntPtr Sphere;
60 IntPtr Box;
61 IntPtr Plane;
62
63 private int CollisionContactGeomsPerTest = 25;
64 private const int DefaultMaxCount = 25;
65 private const int MaxTimePerCallMS = 30;
66
67 /// <summary>
68 /// ODE near callback delegate
69 /// </summary>
70 private d.NearCallback nearCallback;
71 private d.NearCallback nearProbeCallback;
72 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
73 private List<ContactResult> m_contactResults = new List<ContactResult>();
74 private RayFilterFlags CurrentRayFilter;
75 private int CurrentMaxCount;
76
77 public ODERayCastRequestManager(OdeScene pScene)
78 {
79 m_scene = pScene;
80 nearCallback = near;
81 nearProbeCallback = nearProbe;
82 ray = d.CreateRay(IntPtr.Zero, 1.0f);
83 d.GeomSetCategoryBits(ray, 0);
84 Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f);
85 d.GeomSetCategoryBits(Box, 0);
86 Sphere = d.CreateSphere(IntPtr.Zero,1.0f);
87 d.GeomSetCategoryBits(Sphere, 0);
88 Plane = d.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f);
89 d.GeomSetCategoryBits(Sphere, 0);
90 }
91
92 public void QueueRequest(ODERayRequest req)
93 {
94 if (req.Count == 0)
95 req.Count = DefaultMaxCount;
96
97 m_PendingRequests.Enqueue(req);
98 }
99
100 /// <summary>
101 /// Process all queued raycast requests
102 /// </summary>
103 /// <returns>Time in MS the raycasts took to process.</returns>
104 public int ProcessQueuedRequests()
105 {
106
107 if (m_PendingRequests.Count <= 0)
108 return 0;
109
110 if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero)
111 // oops something got wrong or scene isn't ready still
112 {
113 m_PendingRequests.Clear();
114 return 0;
115 }
116
117 int time = Util.EnvironmentTickCount();
118
119 ODERayRequest req;
120 int closestHit;
121 int backfacecull;
122 CollisionCategories catflags;
123
124 while (m_PendingRequests.Dequeue(out req))
125 {
126 if (req.callbackMethod != null)
127 {
128 CurrentRayFilter = req.filter;
129 CurrentMaxCount = req.Count;
130
131 CollisionContactGeomsPerTest = req.Count & 0xffff;
132
133 closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1);
134 backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1);
135
136 if (req.callbackMethod is ProbeBoxCallback)
137 {
138 if (CollisionContactGeomsPerTest > 80)
139 CollisionContactGeomsPerTest = 80;
140 d.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z);
141 d.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z);
142 d.Quaternion qtmp;
143 qtmp.X = req.orientation.X;
144 qtmp.Y = req.orientation.Y;
145 qtmp.Z = req.orientation.Z;
146 qtmp.W = req.orientation.W;
147 d.GeomSetQuaternion(Box, ref qtmp);
148 }
149 else if (req.callbackMethod is ProbeSphereCallback)
150 {
151 if (CollisionContactGeomsPerTest > 80)
152 CollisionContactGeomsPerTest = 80;
153
154 d.GeomSphereSetRadius(Sphere, req.length);
155 d.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z);
156 }
157 else if (req.callbackMethod is ProbePlaneCallback)
158 {
159 if (CollisionContactGeomsPerTest > 80)
160 CollisionContactGeomsPerTest = 80;
161
162 d.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length);
163 }
164
165 else
166 {
167 if (CollisionContactGeomsPerTest > 25)
168 CollisionContactGeomsPerTest = 25;
169
170 d.GeomRaySetLength(ray, req.length);
171 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
172 d.GeomRaySetParams(ray, 0, backfacecull);
173 d.GeomRaySetClosestHit(ray, closestHit);
174
175 if (req.callbackMethod is RaycastCallback)
176 {
177 // if we only want one get only one per Collision pair saving memory
178 CurrentRayFilter |= RayFilterFlags.ClosestHit;
179 d.GeomRaySetClosestHit(ray, 1);
180 }
181 else
182 d.GeomRaySetClosestHit(ray, closestHit);
183 }
184
185 if ((CurrentRayFilter & RayFilterFlags.ContactsUnImportant) != 0)
186 unchecked
187 {
188 CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT;
189 }
190
191 if (req.geom == IntPtr.Zero)
192 {
193 // translate ray filter to Collision flags
194 catflags = 0;
195 if ((CurrentRayFilter & RayFilterFlags.volumedtc) != 0)
196 catflags |= CollisionCategories.VolumeDtc;
197 if ((CurrentRayFilter & RayFilterFlags.phantom) != 0)
198 catflags |= CollisionCategories.Phantom;
199 if ((CurrentRayFilter & RayFilterFlags.agent) != 0)
200 catflags |= CollisionCategories.Character;
201 if ((CurrentRayFilter & RayFilterFlags.PrimsNonPhantom) != 0)
202 catflags |= CollisionCategories.Geom;
203 if ((CurrentRayFilter & RayFilterFlags.land) != 0)
204 catflags |= CollisionCategories.Land;
205 if ((CurrentRayFilter & RayFilterFlags.water) != 0)
206 catflags |= CollisionCategories.Water;
207
208 if (catflags != 0)
209 {
210 if (req.callbackMethod is ProbeBoxCallback)
211 {
212 catflags |= CollisionCategories.Space;
213 d.GeomSetCollideBits(Box, (uint)catflags);
214 d.GeomSetCategoryBits(Box, (uint)catflags);
215 doProbe(req, Box);
216 }
217 else if (req.callbackMethod is ProbeSphereCallback)
218 {
219 catflags |= CollisionCategories.Space;
220 d.GeomSetCollideBits(Sphere, (uint)catflags);
221 d.GeomSetCategoryBits(Sphere, (uint)catflags);
222 doProbe(req, Sphere);
223 }
224 else if (req.callbackMethod is ProbePlaneCallback)
225 {
226 catflags |= CollisionCategories.Space;
227 d.GeomSetCollideBits(Plane, (uint)catflags);
228 d.GeomSetCategoryBits(Plane, (uint)catflags);
229 doPlane(req);
230 }
231 else
232 {
233 d.GeomSetCollideBits(ray, (uint)catflags);
234 doSpaceRay(req);
235 }
236 }
237 }
238 else
239 {
240 // if we select a geom don't use filters
241
242 if (req.callbackMethod is ProbePlaneCallback)
243 {
244 d.GeomSetCollideBits(Plane, (uint)CollisionCategories.All);
245 doPlane(req);
246 }
247 else
248 {
249 d.GeomSetCollideBits(ray, (uint)CollisionCategories.All);
250 doGeomRay(req);
251 }
252 }
253 }
254
255 if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS)
256 break;
257 }
258
259 lock (m_contactResults)
260 m_contactResults.Clear();
261
262 return Util.EnvironmentTickCountSubtract(time);
263 }
264 /// <summary>
265 /// Method that actually initiates the raycast with spaces
266 /// </summary>
267 /// <param name="req"></param>
268 ///
269
270 private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhanton;
271// private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.land | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
272 private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhanton;
273
274 private void doSpaceRay(ODERayRequest req)
275 {
276 // Collide tests
277 if ((CurrentRayFilter & FilterActiveSpace) != 0)
278 {
279 d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
280 d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
281 }
282 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
283 d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
284 if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
285 {
286 // current ode land to ray collisions is very bad
287 // so for now limit its range badly
288
289 if (req.length > 30.0f)
290 d.GeomRaySetLength(ray, 30.0f);
291
292 d.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
293 }
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
304 // Find closest contact and object.
305 lock (m_contactResults)
306 {
307 foreach (ContactResult cResult in m_contactResults)
308 {
309 if(cResult.Depth < distance)
310 {
311 closestcontact = cResult.Pos;
312 hitConsumerID = cResult.ConsumerID;
313 distance = cResult.Depth;
314 snormal = cResult.Normal;
315 }
316 }
317 m_contactResults.Clear();
318 }
319
320 if (distance > 0 && distance < float.MaxValue)
321 hitYN = true;
322 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
323 }
324 else
325 {
326 List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
327 lock (m_PendingRequests)
328 {
329 cresult.AddRange(m_contactResults);
330 m_contactResults.Clear();
331 }
332 ((RayCallback)req.callbackMethod)(cresult);
333 }
334 }
335
336 private void doProbe(ODERayRequest req, IntPtr probe)
337 {
338 // Collide tests
339 if ((CurrentRayFilter & FilterActiveSpace) != 0)
340 {
341 d.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
342 d.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
343 }
344 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
345 d.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
346 if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
347 d.SpaceCollide2(probe, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
348
349 List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
350 lock (m_PendingRequests)
351 {
352 cresult.AddRange(m_contactResults);
353 m_contactResults.Clear();
354 }
355 if (req.callbackMethod is ProbeBoxCallback)
356 ((ProbeBoxCallback)req.callbackMethod)(cresult);
357 else if (req.callbackMethod is ProbeSphereCallback)
358 ((ProbeSphereCallback)req.callbackMethod)(cresult);
359 }
360
361 private void doPlane(ODERayRequest req)
362 {
363 // Collide tests
364 if (req.geom == IntPtr.Zero)
365 {
366 if ((CurrentRayFilter & FilterActiveSpace) != 0)
367 {
368 d.SpaceCollide2(Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
369 d.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
370 }
371 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
372 d.SpaceCollide2(Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
373 if ((CurrentRayFilter & RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
374 d.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
375 }
376 else
377 {
378 d.SpaceCollide2(Plane, req.geom, IntPtr.Zero, nearCallback);
379 }
380
381 List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
382 lock (m_PendingRequests)
383 {
384 cresult.AddRange(m_contactResults);
385 m_contactResults.Clear();
386 }
387
388 ((ProbePlaneCallback)req.callbackMethod)(cresult);
389 }
390
391 /// <summary>
392 /// Method that actually initiates the raycast with a geom
393 /// </summary>
394 /// <param name="req"></param>
395 private void doGeomRay(ODERayRequest req)
396 {
397 // Collide test
398 d.SpaceCollide2(ray, req.geom, IntPtr.Zero, nearCallback); // still do this to have full AABB pre test
399
400 if (req.callbackMethod is RaycastCallback)
401 {
402 // Define default results
403 bool hitYN = false;
404 uint hitConsumerID = 0;
405 float distance = float.MaxValue;
406 Vector3 closestcontact = Vector3.Zero;
407 Vector3 snormal = Vector3.Zero;
408
409 // Find closest contact and object.
410 lock (m_contactResults)
411 {
412 foreach (ContactResult cResult in m_contactResults)
413 {
414 if(cResult.Depth < distance )
415 {
416 closestcontact = cResult.Pos;
417 hitConsumerID = cResult.ConsumerID;
418 distance = cResult.Depth;
419 snormal = cResult.Normal;
420 }
421 }
422 m_contactResults.Clear();
423 }
424
425 if (distance > 0 && distance < float.MaxValue)
426 hitYN = true;
427
428 ((RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
429 }
430 else
431 {
432 List<ContactResult> cresult = new List<ContactResult>(m_contactResults.Count);
433 lock (m_PendingRequests)
434 {
435 cresult.AddRange(m_contactResults);
436 m_contactResults.Clear();
437 }
438 ((RayCallback)req.callbackMethod)(cresult);
439 }
440 }
441
442 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
443 {
444 IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
445 if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest)
446 return false;
447
448 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
449 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
450 return true;
451 }
452
453 // This is the standard Near. g1 is the ray
454 private void near(IntPtr space, IntPtr g1, IntPtr g2)
455 {
456 if (g2 == IntPtr.Zero || g1 == g2)
457 return;
458
459 if (m_contactResults.Count >= CurrentMaxCount)
460 return;
461
462 if (d.GeomIsSpace(g2))
463 {
464 try
465 {
466 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
467 }
468 catch (Exception e)
469 {
470 m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
471 }
472 return;
473 }
474
475 int count = 0;
476 try
477 {
478 count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
479 }
480 catch (Exception e)
481 {
482 m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
483 return;
484 }
485
486 if (count == 0)
487 return;
488/*
489 uint cat1 = d.GeomGetCategoryBits(g1);
490 uint cat2 = d.GeomGetCategoryBits(g2);
491 uint col1 = d.GeomGetCollideBits(g1);
492 uint col2 = d.GeomGetCollideBits(g2);
493*/
494
495 uint ID = 0;
496 PhysicsActor p2 = null;
497
498 m_scene.actor_name_map.TryGetValue(g2, out p2);
499
500 if (p2 == null)
501 return;
502
503 switch (p2.PhysicsActorType)
504 {
505 case (int)ActorTypes.Prim:
506
507 RayFilterFlags thisFlags;
508
509 if (p2.IsPhysical)
510 thisFlags = RayFilterFlags.physical;
511 else
512 thisFlags = RayFilterFlags.nonphysical;
513
514 if (p2.Phantom)
515 thisFlags |= RayFilterFlags.phantom;
516
517 if (p2.IsVolumeDtc)
518 thisFlags |= RayFilterFlags.volumedtc;
519
520 if ((thisFlags & CurrentRayFilter) == 0)
521 return;
522
523 ID = ((OdePrim)p2).LocalID;
524 break;
525
526 case (int)ActorTypes.Agent:
527
528 if ((CurrentRayFilter & RayFilterFlags.agent) == 0)
529 return;
530 else
531 ID = ((OdeCharacter)p2).LocalID;
532 break;
533
534 case (int)ActorTypes.Ground:
535
536 if ((CurrentRayFilter & RayFilterFlags.land) == 0)
537 return;
538 break;
539
540 case (int)ActorTypes.Water:
541
542 if ((CurrentRayFilter & RayFilterFlags.water) == 0)
543 return;
544 break;
545
546 default:
547 break;
548 }
549
550 d.ContactGeom curcontact = new d.ContactGeom();
551
552 // closestHit for now only works for meshs, so must do it for others
553 if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0)
554 {
555 // Loop all contacts, build results.
556 for (int i = 0; i < count; i++)
557 {
558 if (!GetCurContactGeom(i, ref curcontact))
559 break;
560
561 ContactResult collisionresult = new ContactResult();
562 collisionresult.ConsumerID = ID;
563 collisionresult.Pos.X = curcontact.pos.X;
564 collisionresult.Pos.Y = curcontact.pos.Y;
565 collisionresult.Pos.Z = curcontact.pos.Z;
566 collisionresult.Depth = curcontact.depth;
567 collisionresult.Normal.X = curcontact.normal.X;
568 collisionresult.Normal.Y = curcontact.normal.Y;
569 collisionresult.Normal.Z = curcontact.normal.Z;
570 lock (m_contactResults)
571 {
572 m_contactResults.Add(collisionresult);
573 if (m_contactResults.Count >= CurrentMaxCount)
574 return;
575 }
576 }
577 }
578 else
579 {
580 // keep only closest contact
581 ContactResult collisionresult = new ContactResult();
582 collisionresult.ConsumerID = ID;
583 collisionresult.Depth = float.MaxValue;
584
585 for (int i = 0; i < count; i++)
586 {
587 if (!GetCurContactGeom(i, ref curcontact))
588 break;
589
590 if (curcontact.depth < collisionresult.Depth)
591 {
592 collisionresult.Pos.X = curcontact.pos.X;
593 collisionresult.Pos.Y = curcontact.pos.Y;
594 collisionresult.Pos.Z = curcontact.pos.Z;
595 collisionresult.Depth = curcontact.depth;
596 collisionresult.Normal.X = curcontact.normal.X;
597 collisionresult.Normal.Y = curcontact.normal.Y;
598 collisionresult.Normal.Z = curcontact.normal.Z;
599 }
600 }
601
602 if (collisionresult.Depth != float.MaxValue)
603 {
604 lock (m_contactResults)
605 m_contactResults.Add(collisionresult);
606 }
607 }
608 }
609
610 private void nearProbe(IntPtr space, IntPtr g1, IntPtr g2)
611 {
612 if (g1 == IntPtr.Zero || g1 == g2)
613 return;
614
615 if (m_contactResults.Count >= CurrentMaxCount)
616 return;
617
618 if (d.GeomIsSpace(g1))
619 {
620 try
621 {
622 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearProbeCallback);
623 }
624 catch (Exception e)
625 {
626 m_log.WarnFormat("[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
627 }
628 return;
629 }
630
631 int count = 0;
632 try
633 {
634 count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
635 }
636 catch (Exception e)
637 {
638 m_log.WarnFormat("[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
639 return;
640 }
641
642 if (count == 0)
643 return;
644
645 uint ID = 0;
646 PhysicsActor p1 = null;
647
648 m_scene.actor_name_map.TryGetValue(g1, out p1);
649
650 if (p1 == null)
651 return;
652
653 switch (p1.PhysicsActorType)
654 {
655 case (int)ActorTypes.Prim:
656
657 RayFilterFlags thisFlags;
658
659 if (p1.IsPhysical)
660 thisFlags = RayFilterFlags.physical;
661 else
662 thisFlags = RayFilterFlags.nonphysical;
663
664 if (p1.Phantom)
665 thisFlags |= RayFilterFlags.phantom;
666
667 if (p1.IsVolumeDtc)
668 thisFlags |= RayFilterFlags.volumedtc;
669
670 if ((thisFlags & CurrentRayFilter) == 0)
671 return;
672
673 ID = ((OdePrim)p1).LocalID;
674 break;
675
676 case (int)ActorTypes.Agent:
677
678 if ((CurrentRayFilter & RayFilterFlags.agent) == 0)
679 return;
680 else
681 ID = ((OdeCharacter)p1).LocalID;
682 break;
683
684 case (int)ActorTypes.Ground:
685
686 if ((CurrentRayFilter & RayFilterFlags.land) == 0)
687 return;
688 break;
689
690 case (int)ActorTypes.Water:
691
692 if ((CurrentRayFilter & RayFilterFlags.water) == 0)
693 return;
694 break;
695
696 default:
697 break;
698 }
699
700 d.ContactGeom curcontact = new d.ContactGeom();
701
702 // closestHit for now only works for meshs, so must do it for others
703 if ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0)
704 {
705 // Loop all contacts, build results.
706 for (int i = 0; i < count; i++)
707 {
708 if (!GetCurContactGeom(i, ref curcontact))
709 break;
710
711 ContactResult collisionresult = new ContactResult();
712 collisionresult.ConsumerID = ID;
713 collisionresult.Pos.X = curcontact.pos.X;
714 collisionresult.Pos.Y = curcontact.pos.Y;
715 collisionresult.Pos.Z = curcontact.pos.Z;
716 collisionresult.Depth = curcontact.depth;
717 collisionresult.Normal.X = curcontact.normal.X;
718 collisionresult.Normal.Y = curcontact.normal.Y;
719 collisionresult.Normal.Z = curcontact.normal.Z;
720 lock (m_contactResults)
721 {
722 m_contactResults.Add(collisionresult);
723 if (m_contactResults.Count >= CurrentMaxCount)
724 return;
725 }
726 }
727 }
728 else
729 {
730 // keep only closest contact
731 ContactResult collisionresult = new ContactResult();
732 collisionresult.ConsumerID = ID;
733 collisionresult.Depth = float.MaxValue;
734
735 for (int i = 0; i < count; i++)
736 {
737 if (!GetCurContactGeom(i, ref curcontact))
738 break;
739
740 if (curcontact.depth < collisionresult.Depth)
741 {
742 collisionresult.Pos.X = curcontact.pos.X;
743 collisionresult.Pos.Y = curcontact.pos.Y;
744 collisionresult.Pos.Z = curcontact.pos.Z;
745 collisionresult.Depth = curcontact.depth;
746 collisionresult.Normal.X = curcontact.normal.X;
747 collisionresult.Normal.Y = curcontact.normal.Y;
748 collisionresult.Normal.Z = curcontact.normal.Z;
749 }
750 }
751
752 if (collisionresult.Depth != float.MaxValue)
753 {
754 lock (m_contactResults)
755 m_contactResults.Add(collisionresult);
756 }
757 }
758 }
759
760 /// <summary>
761 /// Dereference the creator scene so that it can be garbage collected if needed.
762 /// </summary>
763 internal void Dispose()
764 {
765 m_scene = null;
766 if (ray != IntPtr.Zero)
767 {
768 d.GeomDestroy(ray);
769 ray = IntPtr.Zero;
770 }
771 if (Box != IntPtr.Zero)
772 {
773 d.GeomDestroy(Box);
774 Box = IntPtr.Zero;
775 }
776 if (Sphere != IntPtr.Zero)
777 {
778 d.GeomDestroy(Sphere);
779 Sphere = IntPtr.Zero;
780 }
781 if (Plane != IntPtr.Zero)
782 {
783 d.GeomDestroy(Plane);
784 Plane = IntPtr.Zero;
785 }
786 }
787 }
788
789 public struct ODERayRequest
790 {
791 public IntPtr geom;
792 public Vector3 Origin;
793 public Vector3 Normal;
794 public int Count;
795 public float length;
796 public object callbackMethod;
797 public RayFilterFlags filter;
798 public Quaternion orientation;
799 }
800} \ No newline at end of file
generated by cgit v1.1 at 2024-12-19 02:39:12 +0000