diff options
author | Robert Adams | 2015-09-08 06:15:46 -0700 |
---|---|---|
committer | Robert Adams | 2015-09-08 06:15:46 -0700 |
commit | 4dd17c4117ea413fb0c4418511956cb3abfe258c (patch) | |
tree | 6f31a583610f8bf074d1f0b4e7ab6ccef37dec9a /OpenSim/Region/PhysicsModules/UbitOdePlugin/ODERayCastRequestManager.cs | |
parent | Merge of ubitworkvarnew with opensim/master as of 20150905. (diff) | |
download | opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.zip opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.gz opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.bz2 opensim-SC_OLD-4dd17c4117ea413fb0c4418511956cb3abfe258c.tar.xz |
More 'everything is a module' merging.
Have most of UbitOde converted.
There are compile errors in OpenSimBase as the new modules stuff is not all there.
Removed ChOdePlugin as it's connection to OdePlugin was tangled.
Diffstat (limited to 'OpenSim/Region/PhysicsModules/UbitOdePlugin/ODERayCastRequestManager.cs')
-rw-r--r-- | OpenSim/Region/PhysicsModules/UbitOdePlugin/ODERayCastRequestManager.cs | 683 |
1 files changed, 683 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/UbitOdePlugin/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/UbitOdePlugin/ODERayCastRequestManager.cs new file mode 100644 index 0000000..3ad9a47 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/UbitOdePlugin/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.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 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 | } | ||