diff options
Diffstat (limited to 'OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs')
-rw-r--r-- | OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs | 434 |
1 files changed, 434 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs b/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs new file mode 100644 index 0000000..8d7d3b3 --- /dev/null +++ b/OpenSim/Region/PhysicsModules/Ode/ODERayCastRequestManager.cs | |||
@@ -0,0 +1,434 @@ | |||
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 OpenMetaverse; | ||
34 | using OpenSim.Region.Physics.Manager; | ||
35 | using Ode.NET; | ||
36 | using log4net; | ||
37 | |||
38 | namespace OpenSim.Region.Physics.OdePlugin | ||
39 | { | ||
40 | /// <summary> | ||
41 | /// Processes raycast requests as ODE is in a state to be able to do them. | ||
42 | /// This ensures that it's thread safe and there will be no conflicts. | ||
43 | /// Requests get returned by a different thread then they were requested by. | ||
44 | /// </summary> | ||
45 | public class ODERayCastRequestManager | ||
46 | { | ||
47 | /// <summary> | ||
48 | /// Pending raycast requests | ||
49 | /// </summary> | ||
50 | protected List<ODERayCastRequest> m_PendingRequests = new List<ODERayCastRequest>(); | ||
51 | |||
52 | /// <summary> | ||
53 | /// Pending ray requests | ||
54 | /// </summary> | ||
55 | protected List<ODERayRequest> m_PendingRayRequests = new List<ODERayRequest>(); | ||
56 | |||
57 | /// <summary> | ||
58 | /// Scene that created this object. | ||
59 | /// </summary> | ||
60 | private OdeScene m_scene; | ||
61 | |||
62 | /// <summary> | ||
63 | /// ODE contact array to be filled by the collision testing | ||
64 | /// </summary> | ||
65 | d.ContactGeom[] contacts = new d.ContactGeom[5]; | ||
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 | |||
74 | |||
75 | public ODERayCastRequestManager(OdeScene pScene) | ||
76 | { | ||
77 | m_scene = pScene; | ||
78 | nearCallback = near; | ||
79 | |||
80 | } | ||
81 | |||
82 | /// <summary> | ||
83 | /// Queues a raycast | ||
84 | /// </summary> | ||
85 | /// <param name="position">Origin of Ray</param> | ||
86 | /// <param name="direction">Ray normal</param> | ||
87 | /// <param name="length">Ray length</param> | ||
88 | /// <param name="retMethod">Return method to send the results</param> | ||
89 | public void QueueRequest(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) | ||
90 | { | ||
91 | lock (m_PendingRequests) | ||
92 | { | ||
93 | ODERayCastRequest req = new ODERayCastRequest(); | ||
94 | req.callbackMethod = retMethod; | ||
95 | req.length = length; | ||
96 | req.Normal = direction; | ||
97 | req.Origin = position; | ||
98 | |||
99 | m_PendingRequests.Add(req); | ||
100 | } | ||
101 | } | ||
102 | |||
103 | /// <summary> | ||
104 | /// Queues a raycast | ||
105 | /// </summary> | ||
106 | /// <param name="position">Origin of Ray</param> | ||
107 | /// <param name="direction">Ray normal</param> | ||
108 | /// <param name="length">Ray length</param> | ||
109 | /// <param name="count"></param> | ||
110 | /// <param name="retMethod">Return method to send the results</param> | ||
111 | public void QueueRequest(Vector3 position, Vector3 direction, float length, int count, RayCallback retMethod) | ||
112 | { | ||
113 | lock (m_PendingRequests) | ||
114 | { | ||
115 | ODERayRequest req = new ODERayRequest(); | ||
116 | req.callbackMethod = retMethod; | ||
117 | req.length = length; | ||
118 | req.Normal = direction; | ||
119 | req.Origin = position; | ||
120 | req.Count = count; | ||
121 | |||
122 | m_PendingRayRequests.Add(req); | ||
123 | } | ||
124 | } | ||
125 | |||
126 | /// <summary> | ||
127 | /// Process all queued raycast requests | ||
128 | /// </summary> | ||
129 | /// <returns>Time in MS the raycasts took to process.</returns> | ||
130 | public int ProcessQueuedRequests() | ||
131 | { | ||
132 | int time = System.Environment.TickCount; | ||
133 | lock (m_PendingRequests) | ||
134 | { | ||
135 | if (m_PendingRequests.Count > 0) | ||
136 | { | ||
137 | ODERayCastRequest[] reqs = m_PendingRequests.ToArray(); | ||
138 | for (int i = 0; i < reqs.Length; i++) | ||
139 | { | ||
140 | if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast | ||
141 | RayCast(reqs[i]); // if there isn't anyone to send results | ||
142 | } | ||
143 | |||
144 | m_PendingRequests.Clear(); | ||
145 | } | ||
146 | } | ||
147 | |||
148 | lock (m_PendingRayRequests) | ||
149 | { | ||
150 | if (m_PendingRayRequests.Count > 0) | ||
151 | { | ||
152 | ODERayRequest[] reqs = m_PendingRayRequests.ToArray(); | ||
153 | for (int i = 0; i < reqs.Length; i++) | ||
154 | { | ||
155 | if (reqs[i].callbackMethod != null) // quick optimization here, don't raycast | ||
156 | RayCast(reqs[i]); // if there isn't anyone to send results | ||
157 | } | ||
158 | |||
159 | m_PendingRayRequests.Clear(); | ||
160 | } | ||
161 | } | ||
162 | |||
163 | lock (m_contactResults) | ||
164 | m_contactResults.Clear(); | ||
165 | |||
166 | return System.Environment.TickCount - time; | ||
167 | } | ||
168 | |||
169 | /// <summary> | ||
170 | /// Method that actually initiates the raycast | ||
171 | /// </summary> | ||
172 | /// <param name="req"></param> | ||
173 | private void RayCast(ODERayCastRequest req) | ||
174 | { | ||
175 | // Create the ray | ||
176 | IntPtr ray = d.CreateRay(m_scene.space, req.length); | ||
177 | d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); | ||
178 | |||
179 | // Collide test | ||
180 | d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback); | ||
181 | |||
182 | // Remove Ray | ||
183 | d.GeomDestroy(ray); | ||
184 | |||
185 | // Define default results | ||
186 | bool hitYN = false; | ||
187 | uint hitConsumerID = 0; | ||
188 | float distance = 999999999999f; | ||
189 | Vector3 closestcontact = new Vector3(99999f, 99999f, 99999f); | ||
190 | Vector3 snormal = Vector3.Zero; | ||
191 | |||
192 | // Find closest contact and object. | ||
193 | lock (m_contactResults) | ||
194 | { | ||
195 | foreach (ContactResult cResult in m_contactResults) | ||
196 | { | ||
197 | if (Vector3.Distance(req.Origin, cResult.Pos) < Vector3.Distance(req.Origin, closestcontact)) | ||
198 | { | ||
199 | closestcontact = cResult.Pos; | ||
200 | hitConsumerID = cResult.ConsumerID; | ||
201 | distance = cResult.Depth; | ||
202 | hitYN = true; | ||
203 | snormal = cResult.Normal; | ||
204 | } | ||
205 | } | ||
206 | |||
207 | m_contactResults.Clear(); | ||
208 | } | ||
209 | |||
210 | // Return results | ||
211 | if (req.callbackMethod != null) | ||
212 | req.callbackMethod(hitYN, closestcontact, hitConsumerID, distance, snormal); | ||
213 | } | ||
214 | |||
215 | /// <summary> | ||
216 | /// Method that actually initiates the raycast | ||
217 | /// </summary> | ||
218 | /// <param name="req"></param> | ||
219 | private void RayCast(ODERayRequest req) | ||
220 | { | ||
221 | // Create the ray | ||
222 | IntPtr ray = d.CreateRay(m_scene.space, req.length); | ||
223 | d.GeomRaySet(ray, req.Origin.X, req.Origin.Y, req.Origin.Z, req.Normal.X, req.Normal.Y, req.Normal.Z); | ||
224 | |||
225 | // Collide test | ||
226 | d.SpaceCollide2(m_scene.space, ray, IntPtr.Zero, nearCallback); | ||
227 | |||
228 | // Remove Ray | ||
229 | d.GeomDestroy(ray); | ||
230 | |||
231 | // Find closest contact and object. | ||
232 | lock (m_contactResults) | ||
233 | { | ||
234 | // Return results | ||
235 | if (req.callbackMethod != null) | ||
236 | req.callbackMethod(m_contactResults); | ||
237 | } | ||
238 | } | ||
239 | |||
240 | // This is the standard Near. Uses space AABBs to speed up detection. | ||
241 | private void near(IntPtr space, IntPtr g1, IntPtr g2) | ||
242 | { | ||
243 | |||
244 | //Don't test against heightfield Geom, or you'll be sorry! | ||
245 | |||
246 | /* | ||
247 | terminate called after throwing an instance of 'std::bad_alloc' | ||
248 | what(): std::bad_alloc | ||
249 | Stacktrace: | ||
250 | |||
251 | at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0x00004> | ||
252 | at (wrapper managed-to-native) Ode.NET.d.Collide (intptr,intptr,int,Ode.NET.d/ContactGeom[],int) <0xffffffff> | ||
253 | at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0x00280> | ||
254 | at (wrapper native-to-managed) OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.near (intptr,intptr,intptr) <0xfff | ||
255 | fffff> | ||
256 | at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0x00004> | ||
257 | at (wrapper managed-to-native) Ode.NET.d.SpaceCollide2 (intptr,intptr,intptr,Ode.NET.d/NearCallback) <0xffffffff> | ||
258 | at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.RayCast (OpenSim.Region.Physics.OdePlugin.ODERayCastRequest) < | ||
259 | 0x00114> | ||
260 | at OpenSim.Region.Physics.OdePlugin.ODERayCastRequestManager.ProcessQueuedRequests () <0x000eb> | ||
261 | at OpenSim.Region.Physics.OdePlugin.OdeScene.Simulate (single) <0x017e6> | ||
262 | at OpenSim.Region.Framework.Scenes.SceneGraph.UpdatePhysics (double) <0x00042> | ||
263 | at OpenSim.Region.Framework.Scenes.Scene.Update () <0x0039e> | ||
264 | at OpenSim.Region.Framework.Scenes.Scene.Heartbeat (object) <0x00019> | ||
265 | at (wrapper runtime-invoke) object.runtime_invoke_void__this___object (object,intptr,intptr,intptr) <0xffffffff> | ||
266 | |||
267 | Native stacktrace: | ||
268 | |||
269 | mono [0x80d2a42] | ||
270 | [0xb7f5840c] | ||
271 | /lib/i686/cmov/libc.so.6(abort+0x188) [0xb7d1a018] | ||
272 | /usr/lib/libstdc++.so.6(_ZN9__gnu_cxx27__verbose_terminate_handlerEv+0x158) [0xb45fc988] | ||
273 | /usr/lib/libstdc++.so.6 [0xb45fa865] | ||
274 | /usr/lib/libstdc++.so.6 [0xb45fa8a2] | ||
275 | /usr/lib/libstdc++.so.6 [0xb45fa9da] | ||
276 | /usr/lib/libstdc++.so.6(_Znwj+0x83) [0xb45fb033] | ||
277 | /usr/lib/libstdc++.so.6(_Znaj+0x1d) [0xb45fb11d] | ||
278 | libode.so(_ZN13dxHeightfield23dCollideHeightfieldZoneEiiiiP6dxGeomiiP12dContactGeomi+0xd04) [0xb46678e4] | ||
279 | libode.so(_Z19dCollideHeightfieldP6dxGeomS0_iP12dContactGeomi+0x54b) [0xb466832b] | ||
280 | libode.so(dCollide+0x102) [0xb46571b2] | ||
281 | [0x95cfdec9] | ||
282 | [0x8ea07fe1] | ||
283 | [0xab260146] | ||
284 | libode.so [0xb465a5c4] | ||
285 | libode.so(_ZN11dxHashSpace8collide2EPvP6dxGeomPFvS0_S2_S2_E+0x75) [0xb465bcf5] | ||
286 | libode.so(dSpaceCollide2+0x177) [0xb465ac67] | ||
287 | [0x95cf978e] | ||
288 | [0x8ea07945] | ||
289 | [0x95cf2bbc] | ||
290 | [0xab2787e7] | ||
291 | [0xab419fb3] | ||
292 | [0xab416657] | ||
293 | [0xab415bda] | ||
294 | [0xb609b08e] | ||
295 | mono(mono_runtime_delegate_invoke+0x34) [0x8192534] | ||
296 | mono [0x81a2f0f] | ||
297 | mono [0x81d28b6] | ||
298 | mono [0x81ea2c6] | ||
299 | /lib/i686/cmov/libpthread.so.0 [0xb7e744c0] | ||
300 | /lib/i686/cmov/libc.so.6(clone+0x5e) [0xb7dcd6de] | ||
301 | */ | ||
302 | |||
303 | // Exclude heightfield geom | ||
304 | |||
305 | if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) | ||
306 | return; | ||
307 | if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass || d.GeomGetClass(g2) == d.GeomClassID.HeightfieldClass) | ||
308 | return; | ||
309 | |||
310 | // Raytest against AABBs of spaces first, then dig into the spaces it hits for actual geoms. | ||
311 | if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) | ||
312 | { | ||
313 | if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) | ||
314 | return; | ||
315 | |||
316 | // Separating static prim geometry spaces. | ||
317 | // We'll be calling near recursivly if one | ||
318 | // of them is a space to find all of the | ||
319 | // contact points in the space | ||
320 | try | ||
321 | { | ||
322 | d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); | ||
323 | } | ||
324 | catch (AccessViolationException) | ||
325 | { | ||
326 | m_log.Warn("[PHYSICS]: Unable to collide test a space"); | ||
327 | return; | ||
328 | } | ||
329 | //Colliding a space or a geom with a space or a geom. so drill down | ||
330 | |||
331 | //Collide all geoms in each space.. | ||
332 | //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback); | ||
333 | //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback); | ||
334 | return; | ||
335 | } | ||
336 | |||
337 | if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) | ||
338 | return; | ||
339 | |||
340 | int count = 0; | ||
341 | try | ||
342 | { | ||
343 | |||
344 | if (g1 == g2) | ||
345 | return; // Can't collide with yourself | ||
346 | |||
347 | lock (contacts) | ||
348 | { | ||
349 | count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf); | ||
350 | } | ||
351 | } | ||
352 | catch (SEHException) | ||
353 | { | ||
354 | m_log.Error("[PHYSICS]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim."); | ||
355 | } | ||
356 | catch (Exception e) | ||
357 | { | ||
358 | m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); | ||
359 | return; | ||
360 | } | ||
361 | |||
362 | PhysicsActor p1 = null; | ||
363 | PhysicsActor p2 = null; | ||
364 | |||
365 | if (g1 != IntPtr.Zero) | ||
366 | m_scene.actor_name_map.TryGetValue(g1, out p1); | ||
367 | |||
368 | if (g2 != IntPtr.Zero) | ||
369 | m_scene.actor_name_map.TryGetValue(g1, out p2); | ||
370 | |||
371 | // Loop over contacts, build results. | ||
372 | for (int i = 0; i < count; i++) | ||
373 | { | ||
374 | if (p1 != null) | ||
375 | { | ||
376 | if (p1 is OdePrim) | ||
377 | { | ||
378 | ContactResult collisionresult = new ContactResult(); | ||
379 | |||
380 | collisionresult.ConsumerID = p1.LocalID; | ||
381 | collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z); | ||
382 | collisionresult.Depth = contacts[i].depth; | ||
383 | collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y, | ||
384 | contacts[i].normal.Z); | ||
385 | lock (m_contactResults) | ||
386 | m_contactResults.Add(collisionresult); | ||
387 | } | ||
388 | } | ||
389 | |||
390 | if (p2 != null) | ||
391 | { | ||
392 | if (p2 is OdePrim) | ||
393 | { | ||
394 | ContactResult collisionresult = new ContactResult(); | ||
395 | |||
396 | collisionresult.ConsumerID = p2.LocalID; | ||
397 | collisionresult.Pos = new Vector3(contacts[i].pos.X, contacts[i].pos.Y, contacts[i].pos.Z); | ||
398 | collisionresult.Depth = contacts[i].depth; | ||
399 | collisionresult.Normal = new Vector3(contacts[i].normal.X, contacts[i].normal.Y, | ||
400 | contacts[i].normal.Z); | ||
401 | |||
402 | lock (m_contactResults) | ||
403 | m_contactResults.Add(collisionresult); | ||
404 | } | ||
405 | } | ||
406 | } | ||
407 | } | ||
408 | |||
409 | /// <summary> | ||
410 | /// Dereference the creator scene so that it can be garbage collected if needed. | ||
411 | /// </summary> | ||
412 | internal void Dispose() | ||
413 | { | ||
414 | m_scene = null; | ||
415 | } | ||
416 | } | ||
417 | |||
418 | public struct ODERayCastRequest | ||
419 | { | ||
420 | public Vector3 Origin; | ||
421 | public Vector3 Normal; | ||
422 | public float length; | ||
423 | public RaycastCallback callbackMethod; | ||
424 | } | ||
425 | |||
426 | public struct ODERayRequest | ||
427 | { | ||
428 | public Vector3 Origin; | ||
429 | public Vector3 Normal; | ||
430 | public int Count; | ||
431 | public float length; | ||
432 | public RayCallback callbackMethod; | ||
433 | } | ||
434 | } \ No newline at end of file | ||