29 using System.Collections.Generic;
30 using System.Reflection;
31 using System.Runtime.InteropServices;
33 using OpenSim.Framework;
34 using OpenSim.Region.PhysicsModules.SharedBase;
39 namespace OpenSim.
Region.PhysicsModule.ubOde
63 private int CollisionContactGeomsPerTest = 25;
64 private const int DefaultMaxCount = 25;
65 private const int MaxTimePerCallMS = 30;
70 private d.NearCallback nearCallback;
71 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
72 private List<ContactResult> m_contactResults =
new List<ContactResult>();
74 private int CurrentMaxCount;
80 ray = d.CreateRay(IntPtr.Zero, 1.0f);
81 d.GeomSetCategoryBits(ray, 0);
82 Box = d.CreateBox(IntPtr.Zero, 1.0f, 1.0f, 1.0f);
83 d.GeomSetCategoryBits(Box, 0);
84 Sphere = d.CreateSphere(IntPtr.Zero,1.0f);
85 d.GeomSetCategoryBits(Sphere, 0);
86 Plane = d.CreatePlane(IntPtr.Zero, 0f,0f,1f,1f);
87 d.GeomSetCategoryBits(Sphere, 0);
93 req.Count = DefaultMaxCount;
95 m_PendingRequests.Enqueue(req);
105 if (m_PendingRequests.Count <= 0)
108 if (m_scene.ContactgeomsArray == IntPtr.Zero || ray == IntPtr.Zero)
111 m_PendingRequests.Clear();
115 int time = Util.EnvironmentTickCount();
122 while (m_PendingRequests.Dequeue(out req))
124 if (req.callbackMethod != null)
126 IntPtr geom = IntPtr.Zero;
127 if (req.actor != null)
129 if (m_scene.haveActor(req.actor))
132 geom = ((OdePrim)req.actor).prim_geom;
134 geom = ((OdePrim)req.actor).prim_geom;
136 if (geom == IntPtr.Zero)
143 CurrentRayFilter = req.filter;
144 CurrentMaxCount = req.Count;
146 CollisionContactGeomsPerTest = req.Count & 0xffff;
148 closestHit = ((CurrentRayFilter & RayFilterFlags.ClosestHit) == 0 ? 0 : 1);
149 backfacecull = ((CurrentRayFilter & RayFilterFlags.BackFaceCull) == 0 ? 0 : 1);
153 if (CollisionContactGeomsPerTest > 80)
154 CollisionContactGeomsPerTest = 80;
155 d.GeomBoxSetLengths(Box, req.Normal.X, req.Normal.Y, req.Normal.Z);
156 d.GeomSetPosition(Box, req.Origin.X, req.Origin.Y, req.Origin.Z);
158 qtmp.X = req.orientation.X;
159 qtmp.Y = req.orientation.Y;
160 qtmp.Z = req.orientation.Z;
161 qtmp.W = req.orientation.W;
162 d.GeomSetQuaternion(Box, ref qtmp);
166 if (CollisionContactGeomsPerTest > 80)
167 CollisionContactGeomsPerTest = 80;
169 d.GeomSphereSetRadius(Sphere, req.length);
170 d.GeomSetPosition(Sphere, req.Origin.X, req.Origin.Y, req.Origin.Z);
174 if (CollisionContactGeomsPerTest > 80)
175 CollisionContactGeomsPerTest = 80;
177 d.GeomPlaneSetParams(Plane, req.Normal.X, req.Normal.Y, req.Normal.Z, req.length);
182 if (CollisionContactGeomsPerTest > 25)
183 CollisionContactGeomsPerTest = 25;
185 d.GeomRaySetLength(ray, req.length);
186 d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z);
187 d.GeomRaySetParams(ray, 0, backfacecull);
192 CurrentRayFilter |= RayFilterFlags.ClosestHit;
193 d.GeomRaySetClosestHit(ray, 1);
196 d.GeomRaySetClosestHit(ray, closestHit);
202 CollisionContactGeomsPerTest |= (int)d.CONTACTS_UNIMPORTANT;
205 if (geom == IntPtr.Zero)
224 if (req.callbackMethod is ProbeBoxCallback)
226 catflags |= CollisionCategories.Space;
227 d.GeomSetCollideBits(Box, (uint)catflags);
228 d.GeomSetCategoryBits(Box, (uint)catflags);
231 else if (req.callbackMethod is ProbeSphereCallback)
233 catflags |= CollisionCategories.Space;
234 d.GeomSetCollideBits(Sphere, (uint)catflags);
235 d.GeomSetCategoryBits(Sphere, (uint)catflags);
236 doProbe(req, Sphere);
238 else if (req.callbackMethod is ProbePlaneCallback)
240 catflags |= CollisionCategories.Space;
241 d.GeomSetCollideBits(Plane, (uint)catflags);
242 d.GeomSetCategoryBits(Plane, (uint)catflags);
243 doPlane(req,IntPtr.Zero);
247 d.GeomSetCollideBits(ray, (uint)catflags);
256 if (req.callbackMethod is ProbePlaneCallback)
269 if (Util.EnvironmentTickCountSubtract(time) > MaxTimePerCallMS)
273 lock (m_contactResults)
274 m_contactResults.Clear();
276 return Util.EnvironmentTickCountSubtract(time);
291 List<ContactResult> cresult =
new List<ContactResult>();
301 private const RayFilterFlags FilterActiveSpace = RayFilterFlags.agent | RayFilterFlags.physical | RayFilterFlags.LSLPhantom;
303 private const RayFilterFlags FilterStaticSpace = RayFilterFlags.water | RayFilterFlags.nonphysical | RayFilterFlags.LSLPhantom;
305 private void doSpaceRay(ODERayRequest req)
308 if ((CurrentRayFilter & FilterActiveSpace) != 0)
310 d.SpaceCollide2(ray, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
311 d.SpaceCollide2(ray, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
313 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
314 d.SpaceCollide2(ray, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
315 if ((CurrentRayFilter &
RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
319 if (req.length > 60.0f)
320 d.GeomRaySetLength(ray, 60.0f);
322 d.SpaceCollide2(ray, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
325 if (req.callbackMethod is RaycastCallback)
329 uint hitConsumerID = 0;
330 float distance = float.MaxValue;
331 Vector3 closestcontact = Vector3.Zero;
332 Vector3 snormal = Vector3.Zero;
335 lock (m_contactResults)
339 if(cResult.
Depth < distance)
341 closestcontact = cResult.Pos;
342 hitConsumerID = cResult.ConsumerID;
343 distance = cResult.Depth;
344 snormal = cResult.Normal;
347 m_contactResults.Clear();
350 if (distance > 0 && distance <
float.MaxValue)
352 ((
RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
356 List<ContactResult> cresult =
new List<ContactResult>(m_contactResults.Count);
357 lock (m_PendingRequests)
359 cresult.AddRange(m_contactResults);
360 m_contactResults.Clear();
366 private void doProbe(ODERayRequest req, IntPtr probe)
369 if ((CurrentRayFilter & FilterActiveSpace) != 0)
371 d.SpaceCollide2(probe, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
372 d.SpaceCollide2(probe, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
374 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
375 d.SpaceCollide2(probe, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
376 if ((CurrentRayFilter &
RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
377 d.SpaceCollide2(probe, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
379 List<ContactResult> cresult =
new List<ContactResult>(m_contactResults.Count);
380 lock (m_PendingRequests)
382 cresult.AddRange(m_contactResults);
383 m_contactResults.Clear();
385 if (req.callbackMethod is ProbeBoxCallback)
391 private void doPlane(ODERayRequest req,IntPtr geom)
394 if (geom == IntPtr.Zero)
396 if ((CurrentRayFilter & FilterActiveSpace) != 0)
398 d.SpaceCollide2(
Plane, m_scene.ActiveSpace, IntPtr.Zero, nearCallback);
399 d.SpaceCollide2(Plane, m_scene.CharsSpace, IntPtr.Zero, nearCallback);
401 if ((CurrentRayFilter & FilterStaticSpace) != 0 && (m_contactResults.Count < CurrentMaxCount))
402 d.SpaceCollide2(
Plane, m_scene.StaticSpace, IntPtr.Zero, nearCallback);
403 if ((CurrentRayFilter &
RayFilterFlags.land) != 0 && (m_contactResults.Count < CurrentMaxCount))
404 d.SpaceCollide2(Plane, m_scene.GroundSpace, IntPtr.Zero, nearCallback);
408 d.SpaceCollide2(Plane, geom, IntPtr.Zero, nearCallback);
411 List<ContactResult> cresult =
new List<ContactResult>(m_contactResults.Count);
412 lock (m_PendingRequests)
414 cresult.AddRange(m_contactResults);
415 m_contactResults.Clear();
425 private void doGeomRay(ODERayRequest req, IntPtr geom)
428 d.SpaceCollide2(ray, geom, IntPtr.Zero, nearCallback);
430 if (req.callbackMethod is RaycastCallback)
434 uint hitConsumerID = 0;
435 float distance = float.MaxValue;
436 Vector3 closestcontact = Vector3.Zero;
437 Vector3 snormal = Vector3.Zero;
440 lock (m_contactResults)
444 if(cResult.
Depth < distance )
446 closestcontact = cResult.Pos;
447 hitConsumerID = cResult.ConsumerID;
448 distance = cResult.Depth;
449 snormal = cResult.Normal;
452 m_contactResults.Clear();
455 if (distance > 0 && distance <
float.MaxValue)
458 ((
RaycastCallback)req.callbackMethod)(hitYN, closestcontact, hitConsumerID, distance, snormal);
462 List<ContactResult> cresult =
new List<ContactResult>(m_contactResults.Count);
463 lock (m_PendingRequests)
465 cresult.AddRange(m_contactResults);
466 m_contactResults.Clear();
472 private bool GetCurContactGeom(
int index, ref d.ContactGeom newcontactgeom)
474 IntPtr ContactgeomsArray = m_scene.ContactgeomsArray;
475 if (ContactgeomsArray == IntPtr.Zero || index >= CollisionContactGeomsPerTest)
478 IntPtr contactptr =
new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
479 newcontactgeom = (
d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
484 private void near(IntPtr space, IntPtr g1, IntPtr g2)
486 if (g2 == IntPtr.Zero || g1 == g2)
489 if (m_contactResults.Count >= CurrentMaxCount)
492 if (d.GeomIsSpace(g2))
496 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
500 m_log.WarnFormat(
"[PHYSICS Ray]: Unable to Space collide test an object: {0}", e.Message);
508 count = d.CollidePtr(g1, g2, CollisionContactGeomsPerTest, m_scene.ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
512 m_log.WarnFormat(
"[PHYSICS Ray]: Unable to collide test an object: {0}", e.Message);
528 m_scene.actor_name_map.TryGetValue(g2, out p2);
535 case (
int)ActorTypes.Prim:
540 thisFlags = RayFilterFlags.physical;
542 thisFlags = RayFilterFlags.nonphysical;
545 thisFlags |= RayFilterFlags.phantom;
548 thisFlags |= RayFilterFlags.volumedtc;
550 if ((thisFlags & CurrentRayFilter) == 0)
553 ID = ((OdePrim)p2).LocalID;
556 case (
int)ActorTypes.Agent:
561 ID = ((OdeCharacter)p2).LocalID;
564 case (
int)ActorTypes.Ground:
570 case (
int)ActorTypes.Water:
586 for (
int i = 0; i < count; i++)
588 if (!GetCurContactGeom(i, ref curcontact))
592 collisionresult.ConsumerID = ID;
593 collisionresult.Pos.X = curcontact.pos.X;
594 collisionresult.Pos.Y = curcontact.pos.Y;
595 collisionresult.Pos.Z = curcontact.pos.Z;
596 collisionresult.Depth = curcontact.depth;
597 collisionresult.Normal.X = curcontact.normal.X;
598 collisionresult.Normal.Y = curcontact.normal.Y;
599 collisionresult.Normal.Z = curcontact.normal.Z;
600 lock (m_contactResults)
602 m_contactResults.Add(collisionresult);
603 if (m_contactResults.Count >= CurrentMaxCount)
612 collisionresult.ConsumerID = ID;
613 collisionresult.Depth = float.MaxValue;
615 for (
int i = 0; i < count; i++)
617 if (!GetCurContactGeom(i, ref curcontact))
620 if (curcontact.depth < collisionresult.
Depth)
622 collisionresult.Pos.X = curcontact.pos.X;
623 collisionresult.Pos.Y = curcontact.pos.Y;
624 collisionresult.Pos.Z = curcontact.pos.Z;
625 collisionresult.Depth = curcontact.depth;
626 collisionresult.Normal.X = curcontact.normal.X;
627 collisionresult.Normal.Y = curcontact.normal.Y;
628 collisionresult.Normal.Z = curcontact.normal.Z;
632 if (collisionresult.
Depth !=
float.MaxValue)
634 lock (m_contactResults)
635 m_contactResults.Add(collisionresult);
643 internal
void Dispose()
646 if (ray != IntPtr.Zero)
651 if (Box != IntPtr.Zero)
656 if (Sphere != IntPtr.Zero)
661 if (Plane != IntPtr.Zero)
663 d.GeomDestroy(Plane);
abstract int PhysicsActorType
Processes raycast requests as ODE is in a state to be able to do them. This ensures that it's thread ...
delegate void ProbePlaneCallback(List< ContactResult > list)
delegate void RayCallback(List< ContactResult > list)
delegate void RaycastCallback(bool hitYN, Vector3 collisionPoint, uint localid, float distance, Vector3 normal)
delegate void ProbeBoxCallback(List< ContactResult > list)
delegate void ProbeSphereCallback(List< ContactResult > list)
int ProcessQueuedRequests()
Process all queued raycast requests
ODERayCastRequestManager(ODEScene pScene)
void QueueRequest(ODERayRequest req)