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