aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs')
-rw-r--r--OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs2835
1 files changed, 2835 insertions, 0 deletions
diff --git a/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
new file mode 100644
index 0000000..286c7f0
--- /dev/null
+++ b/OpenSim/Region/Physics/UbitOdePlugin/OdeScene.cs
@@ -0,0 +1,2835 @@
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//#define SPAM
29
30using System;
31using System.Collections.Generic;
32using System.Reflection;
33using System.Runtime.InteropServices;
34using System.Threading;
35using System.IO;
36using System.Diagnostics;
37using log4net;
38using Nini.Config;
39using OdeAPI;
40using OpenSim.Framework;
41using OpenSim.Region.Physics.Manager;
42using OpenMetaverse;
43
44namespace OpenSim.Region.Physics.OdePlugin
45{
46 public enum StatusIndicators : int
47 {
48 Generic = 0,
49 Start = 1,
50 End = 2
51 }
52
53 public struct sCollisionData
54 {
55 public uint ColliderLocalId;
56 public uint CollidedWithLocalId;
57 public int NumberOfCollisions;
58 public int CollisionType;
59 public int StatusIndicator;
60 public int lastframe;
61 }
62
63 // colision flags of things others can colide with
64 // rays, sensors, probes removed since can't be colided with
65 // The top space where things are placed provided further selection
66 // ie physical are in active space nonphysical in static
67 // this should be exclusive as possible
68
69 [Flags]
70 public enum CollisionCategories : uint
71 {
72 Disabled = 0,
73 //by 'things' types
74 Space = 0x01,
75 Geom = 0x02, // aka prim/part
76 Character = 0x04,
77 Land = 0x08,
78 Water = 0x010,
79
80 // by state
81 Phantom = 0x01000,
82 VolumeDtc = 0x02000,
83 Selected = 0x04000,
84 NoShape = 0x08000,
85
86
87 All = 0xffffffff
88 }
89
90 /// <summary>
91 /// Material type for a primitive
92 /// </summary>
93 public enum Material : int
94 {
95 /// <summary></summary>
96 Stone = 0,
97 /// <summary></summary>
98 Metal = 1,
99 /// <summary></summary>
100 Glass = 2,
101 /// <summary></summary>
102 Wood = 3,
103 /// <summary></summary>
104 Flesh = 4,
105 /// <summary></summary>
106 Plastic = 5,
107 /// <summary></summary>
108 Rubber = 6,
109
110 light = 7 // compatibility with old viewers
111 }
112
113 public enum changes : int
114 {
115 Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
116 Remove,
117 Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
118 // or removes from a object if arg is null
119 DeLink,
120 Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
121 Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
122 PosOffset, // not in use
123 // arg Vector3 new position in local coords. Changes prim position in object
124 OriOffset, // not in use
125 // arg Vector3 new position in local coords. Changes prim position in object
126 Velocity,
127 AngVelocity,
128 Acceleration,
129 Force,
130 Torque,
131 Momentum,
132
133 AddForce,
134 AddAngForce,
135 AngLock,
136
137 Size,
138 Shape,
139
140 CollidesWater,
141 VolumeDtc,
142
143 Physical,
144 Phantom,
145 Selected,
146 disabled,
147 building,
148
149 VehicleType,
150 VehicleFloatParam,
151 VehicleVectorParam,
152 VehicleRotationParam,
153 VehicleFlags,
154 SetVehicle,
155
156 Null //keep this last used do dim the methods array. does nothing but pulsing the prim
157 }
158
159 public struct ODEchangeitem
160 {
161 public PhysicsActor actor;
162 public OdeCharacter character;
163 public changes what;
164 public Object arg;
165 }
166
167 public class OdeScene : PhysicsScene
168 {
169 private readonly ILog m_log;
170 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
171
172 public bool OdeUbitLib = false;
173// private int threadid = 0;
174 private Random fluidRandomizer = new Random(Environment.TickCount);
175
176 const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
177 const float MaxERP = 0.8f;
178 const float minERP = 0.1f;
179 const float comumContactCFM = 0.0001f;
180
181 float frictionMovementMult = 0.8f;
182
183 float TerrainBounce = 0.1f;
184 float TerrainFriction = 0.3f;
185
186 public float AvatarFriction = 0;// 0.9f * 0.5f;
187
188 private const uint m_regionWidth = Constants.RegionSize;
189 private const uint m_regionHeight = Constants.RegionSize;
190
191 public float ODE_STEPSIZE = 0.020f;
192 public float HalfOdeStep = 0.01f;
193 public int odetimestepMS = 20; // rounded
194 private float metersInSpace = 25.6f;
195 private float m_timeDilation = 1.0f;
196
197 DateTime m_lastframe;
198
199 public float gravityx = 0f;
200 public float gravityy = 0f;
201 public float gravityz = -9.8f;
202
203 private float waterlevel = 0f;
204 private int framecount = 0;
205
206 private IntPtr WaterGeom = IntPtr.Zero;
207 private IntPtr WaterHeightmapData = IntPtr.Zero;
208 private GCHandle WaterMapHandler = new GCHandle();
209
210 public float avPIDD = 2200f; // make it visible
211 public float avPIDP = 900f; // make it visible
212 private float avCapRadius = 0.37f;
213 private float avDensity = 3f;
214 private float avMovementDivisorWalk = 1.3f;
215 private float avMovementDivisorRun = 0.8f;
216 private float minimumGroundFlightOffset = 3f;
217 public float maximumMassObject = 10000.01f;
218
219 public bool meshSculptedPrim = true;
220 public bool forceSimplePrimMeshing = false;
221
222 public float meshSculptLOD = 32;
223 public float MeshSculptphysicalLOD = 32;
224
225 public float geomDefaultDensity = 10.000006836f;
226
227 public int geomContactPointsStartthrottle = 3;
228 public int geomUpdatesPerThrottledUpdate = 15;
229
230 public float bodyPIDD = 35f;
231 public float bodyPIDG = 25;
232
233// public int geomCrossingFailuresBeforeOutofbounds = 6;
234
235 public int bodyFramesAutoDisable = 5;
236
237
238 private d.NearCallback nearCallback;
239
240 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
241 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
242 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
243 private readonly HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
244
245 public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>();
246
247 /// <summary>
248 /// A list of actors that should receive collision events.
249 /// </summary>
250 private readonly List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
251 private readonly List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
252
253 private readonly HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
254 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
255 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
256
257 private float contactsurfacelayer = 0.002f;
258
259 private int contactsPerCollision = 80;
260 internal IntPtr ContactgeomsArray = IntPtr.Zero;
261 private IntPtr GlobalContactsArray = IntPtr.Zero;
262
263 const int maxContactsbeforedeath = 4000;
264 private volatile int m_global_contactcount = 0;
265
266
267 private readonly IntPtr contactgroup;
268
269 public ContactData[] m_materialContactsData = new ContactData[8];
270
271 private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
272 private readonly Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
273 private readonly Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>();
274
275 private int m_physicsiterations = 10;
276 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
277 private readonly PhysicsActor PANull = new NullPhysicsActor();
278 private float step_time = 0.0f;
279
280 public IntPtr world;
281
282 private uint obj2LocalID = 0;
283 private OdeCharacter cc1;
284 private OdePrim cp1;
285 private OdeCharacter cc2;
286 private OdePrim cp2;
287
288 // split the spaces acording to contents type
289 // ActiveSpace contains characters and active prims
290 // StaticSpace contains land and other that is mostly static in enviroment
291 // this can contain subspaces, like the grid in staticspace
292 // as now space only contains this 2 top spaces
293
294 public IntPtr TopSpace; // the global space
295 public IntPtr ActiveSpace; // space for active prims
296 public IntPtr StaticSpace; // space for the static things around
297
298 // some speedup variables
299 private int spaceGridMaxX;
300 private int spaceGridMaxY;
301 private float spacesPerMeter;
302
303 // split static geometry collision into a grid as before
304 private IntPtr[,] staticPrimspace;
305
306 public Object OdeLock;
307 private static Object SimulationLock;
308
309 public IMesher mesher;
310
311 private IConfigSource m_config;
312
313 public bool physics_logging = false;
314 public int physics_logging_interval = 0;
315 public bool physics_logging_append_existing_logfile = false;
316
317 private Vector3 m_worldOffset = Vector3.Zero;
318 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
319 private PhysicsScene m_parentScene = null;
320
321 private ODERayCastRequestManager m_rayCastManager;
322
323
324/* maybe needed if ode uses tls
325 private void checkThread()
326 {
327
328 int th = Thread.CurrentThread.ManagedThreadId;
329 if(th != threadid)
330 {
331 threadid = th;
332 d.AllocateODEDataForThread(~0U);
333 }
334 }
335 */
336 /// <summary>
337 /// Initiailizes the scene
338 /// Sets many properties that ODE requires to be stable
339 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
340 /// </summary>
341 public OdeScene(string sceneIdentifier)
342 {
343 m_log
344 = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
345
346// checkThread();
347 Name = sceneIdentifier;
348
349 OdeLock = new Object();
350 SimulationLock = new Object();
351
352 nearCallback = near;
353
354 m_rayCastManager = new ODERayCastRequestManager(this);
355 lock (OdeLock)
356 {
357 // Create the world and the first space
358 try
359 {
360 world = d.WorldCreate();
361 TopSpace = d.HashSpaceCreate(IntPtr.Zero);
362
363 // now the major subspaces
364 ActiveSpace = d.HashSpaceCreate(TopSpace);
365 StaticSpace = d.HashSpaceCreate(TopSpace);
366 }
367 catch
368 {
369 // i must RtC#FM
370 }
371
372 d.HashSpaceSetLevels(TopSpace, -2, 8);
373 d.HashSpaceSetLevels(ActiveSpace, -2, 8);
374 d.HashSpaceSetLevels(StaticSpace, -2, 8);
375
376 // demote to second level
377 d.SpaceSetSublevel(ActiveSpace, 1);
378 d.SpaceSetSublevel(StaticSpace, 1);
379
380 d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
381 CollisionCategories.Geom |
382 CollisionCategories.Character |
383 CollisionCategories.Phantom |
384 CollisionCategories.VolumeDtc
385 ));
386 d.GeomSetCollideBits(ActiveSpace, 0);
387 d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
388 CollisionCategories.Geom |
389 CollisionCategories.Land |
390 CollisionCategories.Water |
391 CollisionCategories.Phantom |
392 CollisionCategories.VolumeDtc
393 ));
394 d.GeomSetCollideBits(StaticSpace, 0);
395
396 contactgroup = d.JointGroupCreate(0);
397 //contactgroup
398
399 d.WorldSetAutoDisableFlag(world, false);
400 }
401 }
402
403 // Initialize the mesh plugin
404// public override void Initialise(IMesher meshmerizer, IConfigSource config, RegionInfo region )
405 public override void Initialise(IMesher meshmerizer, IConfigSource config)
406 {
407// checkThread();
408 mesher = meshmerizer;
409 m_config = config;
410/*
411 string ode_config = d.GetConfiguration("ODE");
412 if (ode_config != null && ode_config != "")
413 {
414 m_log.WarnFormat("ODE configuration: {0}", ode_config);
415
416 if (ode_config.Contains("ODE_Ubit"))
417 {
418 OdeUbitLib = true;
419 }
420 }
421*/
422 /*
423 if (region != null)
424 {
425 WorldExtents.X = region.RegionSizeX;
426 WorldExtents.Y = region.RegionSizeY;
427 }
428 */
429
430 // Defaults
431
432 int contactsPerCollision = 80;
433
434 if (m_config != null)
435 {
436 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
437 if (physicsconfig != null)
438 {
439 gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
440 gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
441 gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
442
443 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
444
445 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
446
447 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
448 m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", m_physicsiterations);
449
450 avDensity = physicsconfig.GetFloat("av_density", avDensity);
451 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
452 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
453 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", avCapRadius);
454
455 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
456
457 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
458 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
459// geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_failures_before_outofbounds", 5);
460
461 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
462 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
463/*
464 bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", bodyPIDD);
465 bodyPIDG = physicsconfig.GetFloat("body_pid_gain", bodyPIDG);
466*/
467 forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
468 meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", meshSculptedPrim);
469 meshSculptLOD = physicsconfig.GetFloat("mesh_lod", meshSculptLOD);
470 MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", MeshSculptphysicalLOD);
471/*
472 if (Environment.OSVersion.Platform == PlatformID.Unix)
473 {
474 avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", avPIDD);
475 avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", avPIDP);
476 }
477 else
478 {
479
480 avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", avPIDD);
481 avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", avPIDP);
482 }
483*/
484 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
485 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
486 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
487
488 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
489 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
490 }
491 }
492
493 HalfOdeStep = ODE_STEPSIZE * 0.5f;
494 odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f);
495
496 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf);
497 GlobalContactsArray = GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf);
498
499 m_materialContactsData[(int)Material.Stone].mu = 0.8f;
500 m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
501
502 m_materialContactsData[(int)Material.Metal].mu = 0.3f;
503 m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
504
505 m_materialContactsData[(int)Material.Glass].mu = 0.2f;
506 m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
507
508 m_materialContactsData[(int)Material.Wood].mu = 0.6f;
509 m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
510
511 m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
512 m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
513
514 m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
515 m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
516
517 m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
518 m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
519
520 m_materialContactsData[(int)Material.light].mu = 0.0f;
521 m_materialContactsData[(int)Material.light].bounce = 0.0f;
522
523 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
524
525 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
526 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
527
528 d.WorldSetLinearDamping(world, 0.001f);
529 d.WorldSetAngularDamping(world, 0.001f);
530 d.WorldSetAngularDampingThreshold(world, 0f);
531 d.WorldSetLinearDampingThreshold(world, 0f);
532 d.WorldSetMaxAngularSpeed(world, 100f);
533
534 d.WorldSetCFM(world,1e-6f); // a bit harder than default
535 //d.WorldSetCFM(world, 1e-4f); // a bit harder than default
536 d.WorldSetERP(world, 0.6f); // higher than original
537
538 // Set how many steps we go without running collision testing
539 // This is in addition to the step size.
540 // Essentially Steps * m_physicsiterations
541 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
542
543 d.WorldSetContactMaxCorrectingVel(world, 60.0f);
544
545 spacesPerMeter = 1 / metersInSpace;
546 spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeter);
547 spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeter);
548
549 staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
550
551 // create all spaces now
552 int i, j;
553 IntPtr newspace;
554 for (i = 0; i < spaceGridMaxX; i++)
555 for (j = 0; j < spaceGridMaxY; j++)
556 {
557 newspace = d.HashSpaceCreate(StaticSpace);
558 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
559 waitForSpaceUnlock(newspace);
560 d.SpaceSetSublevel(newspace, 2);
561 d.HashSpaceSetLevels(newspace, -2, 8);
562 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
563 CollisionCategories.Geom |
564 CollisionCategories.Land |
565 CollisionCategories.Water |
566 CollisionCategories.Phantom |
567 CollisionCategories.VolumeDtc
568 ));
569 d.GeomSetCollideBits(newspace, 0);
570
571 staticPrimspace[i, j] = newspace;
572 }
573 // let this now be real maximum values
574 spaceGridMaxX--;
575 spaceGridMaxY--;
576 m_lastframe = DateTime.UtcNow;
577 }
578
579 internal void waitForSpaceUnlock(IntPtr space)
580 {
581 //if (space != IntPtr.Zero)
582 //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
583 }
584
585 #region Collision Detection
586
587 // sets a global contact for a joint for contactgeom , and base contact description)
588
589 private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom, float mu, float bounce, float cfm, float erpscale, float dscale)
590 {
591 if (GlobalContactsArray == IntPtr.Zero || m_global_contactcount >= maxContactsbeforedeath)
592 return IntPtr.Zero;
593
594 float erp = contactGeom.depth;
595 erp *= erpscale;
596 if (erp < minERP)
597 erp = minERP;
598 else if (erp > MaxERP)
599 erp = MaxERP;
600
601 float depth = contactGeom.depth * dscale;
602 if (depth > 0.5f)
603 depth = 0.5f;
604
605 d.Contact newcontact = new d.Contact();
606 newcontact.geom.depth = depth;
607 newcontact.geom.g1 = contactGeom.g1;
608 newcontact.geom.g2 = contactGeom.g2;
609 newcontact.geom.pos = contactGeom.pos;
610 newcontact.geom.normal = contactGeom.normal;
611 newcontact.geom.side1 = contactGeom.side1;
612 newcontact.geom.side2 = contactGeom.side2;
613
614 // this needs bounce also
615 newcontact.surface.mode = comumContactFlags;
616 newcontact.surface.mu = mu;
617 newcontact.surface.bounce = bounce;
618 newcontact.surface.soft_cfm = cfm;
619 newcontact.surface.soft_erp = erp;
620
621 IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf));
622 Marshal.StructureToPtr(newcontact, contact, true);
623 return d.JointCreateContactPtr(world, contactgroup, contact);
624 }
625
626 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom)
627 {
628 if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
629 return false;
630
631 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf));
632 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom));
633 return true;
634 }
635
636 /// <summary>
637 /// This is our near callback. A geometry is near a body
638 /// </summary>
639 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
640 /// <param name="g1">a geometry or space</param>
641 /// <param name="g2">another geometry or space</param>
642 ///
643
644 private void near(IntPtr space, IntPtr g1, IntPtr g2)
645 {
646 // no lock here! It's invoked from within Simulate(), which is thread-locked
647
648 if (m_global_contactcount >= maxContactsbeforedeath)
649 return;
650
651 // Test if we're colliding a geom with a space.
652 // If so we have to drill down into the space recursively
653
654 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
655 return;
656
657 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
658 {
659 // We'll be calling near recursivly if one
660 // of them is a space to find all of the
661 // contact points in the space
662 try
663 {
664 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
665 }
666 catch (AccessViolationException)
667 {
668 m_log.Warn("[PHYSICS]: Unable to collide test a space");
669 return;
670 }
671 //here one should check collisions of geoms inside a space
672 // but on each space we only should have geoms that not colide amoung each other
673 // so we don't dig inside spaces
674 return;
675 }
676
677 // get geom bodies to check if we already a joint contact
678 // guess this shouldn't happen now
679 IntPtr b1 = d.GeomGetBody(g1);
680 IntPtr b2 = d.GeomGetBody(g2);
681
682 // d.GeomClassID id = d.GeomGetClass(g1);
683
684 // Figure out how many contact points we have
685 int count = 0;
686 try
687 {
688 // Colliding Geom To Geom
689 // This portion of the function 'was' blatantly ripped off from BoxStack.cs
690
691 if (g1 == g2)
692 return; // Can't collide with yourself
693
694 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
695 return;
696
697 count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
698 }
699 catch (SEHException)
700 {
701 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.");
702// ode.drelease(world);
703 base.TriggerPhysicsBasedRestart();
704 }
705 catch (Exception e)
706 {
707 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
708 return;
709 }
710
711 // id contacts done
712 if (count == 0)
713 return;
714
715 // try get physical actors
716 PhysicsActor p1;
717 PhysicsActor p2;
718
719 if (!actor_name_map.TryGetValue(g1, out p1))
720 {
721 p1 = PANull;
722 }
723
724 if (!actor_name_map.TryGetValue(g2, out p2))
725 {
726 p2 = PANull;
727 }
728
729 // update actors collision score
730 if (p1.CollisionScore >= float.MaxValue - count)
731 p1.CollisionScore = 0;
732 p1.CollisionScore += count;
733
734 if (p2.CollisionScore >= float.MaxValue - count)
735 p2.CollisionScore = 0;
736 p2.CollisionScore += count;
737
738
739 // get first contact
740 d.ContactGeom curContact = new d.ContactGeom();
741 if (!GetCurContactGeom(0, ref curContact))
742 return;
743 // for now it's the one with max depth
744 ContactPoint maxDepthContact = new ContactPoint(
745 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
746 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
747 curContact.depth
748 );
749 // do volume detection case
750 if (
751 (p1.IsVolumeDtc || p2.IsVolumeDtc))
752 {
753 collision_accounting_events(p1, p2, maxDepthContact);
754 return;
755 }
756
757 // big messy collision analises
758
759 Vector3 normoverride = Vector3.Zero; //damm c#
760
761 float mu = 0;
762 float bounce = 0;
763 float cfm = 0.0001f;
764 float erpscale = 1.0f;
765 float dscale = 1.0f;
766 bool IgnoreNegSides = false;
767
768 ContactData contactdata1 = new ContactData(0, 0, false);
769 ContactData contactdata2 = new ContactData(0, 0, false);
770
771 String name = null;
772 bool dop1foot = false;
773 bool dop2foot = false;
774 bool ignore = false;
775 bool AvanormOverride = false;
776
777 switch (p1.PhysicsActorType)
778 {
779 case (int)ActorTypes.Agent:
780 {
781 AvanormOverride = true;
782 Vector3 tmp = p2.Position - p1.Position;
783 normoverride = p2.Velocity - p1.Velocity;
784 mu = normoverride.LengthSquared();
785
786 if (mu > 1e-6)
787 {
788 mu = 1.0f / (float)Math.Sqrt(mu);
789 normoverride *= mu;
790 mu = Vector3.Dot(tmp, normoverride);
791 if (mu > 0)
792 normoverride *= -1;
793 }
794 else
795 {
796 tmp.Normalize();
797 normoverride = -tmp;
798 }
799
800 switch (p2.PhysicsActorType)
801 {
802 case (int)ActorTypes.Agent:
803/*
804 p1.getContactData(ref contactdata1);
805 p2.getContactData(ref contactdata2);
806
807 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
808
809 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
810 mu *= frictionMovementMult;
811*/
812 p1.CollidingObj = true;
813 p2.CollidingObj = true;
814 break;
815 case (int)ActorTypes.Prim:
816/*
817 p1.getContactData(ref contactdata1);
818 p2.getContactData(ref contactdata2);
819
820
821 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
822
823 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
824 mu *= frictionMovementMult;
825 */
826 if (p2.Velocity.LengthSquared() > 0.0f)
827 p2.CollidingObj = true;
828
829 dop1foot = true;
830 break;
831 default:
832 ignore = true; // avatar to terrain and water ignored
833 break;
834 }
835 break;
836 }
837
838 case (int)ActorTypes.Prim:
839 switch (p2.PhysicsActorType)
840 {
841 case (int)ActorTypes.Agent:
842 // p1.getContactData(ref contactdata1);
843 // p2.getContactData(ref contactdata2);
844
845 AvanormOverride = true;
846
847 Vector3 tmp = p2.Position - p1.Position;
848 normoverride = p2.Velocity - p1.Velocity;
849 mu = normoverride.LengthSquared();
850 if (mu > 1e-6)
851 {
852 mu = 1.0f / (float)Math.Sqrt(mu);
853 normoverride *= mu;
854 mu = Vector3.Dot(tmp, normoverride);
855 if (mu > 0)
856 normoverride *= -1;
857 }
858 else
859 {
860 tmp.Normalize();
861 normoverride = -tmp;
862 }
863
864 bounce = 0;
865 mu = 0;
866 cfm = 0.0001f;
867 /*
868 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
869
870 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
871 mu *= frictionMovementMult;
872 */
873 dop2foot = true;
874 if (p1.Velocity.LengthSquared() > 0.0f)
875 p1.CollidingObj = true;
876 break;
877 case (int)ActorTypes.Prim:
878 if ((p1.Velocity - p2.Velocity).LengthSquared() > 0.0f)
879 {
880 p1.CollidingObj = true;
881 p2.CollidingObj = true;
882 }
883 p1.getContactData(ref contactdata1);
884 p2.getContactData(ref contactdata2);
885 bounce = contactdata1.bounce * contactdata2.bounce;
886 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
887
888 cfm = p1.Mass;
889 if (cfm > p2.Mass)
890 cfm = p2.Mass;
891 dscale = 10 / cfm;
892 dscale = (float)Math.Sqrt(dscale);
893 if (dscale > 1.0f)
894 dscale = 1.0f;
895 erpscale = cfm * 0.01f;
896 cfm = 0.0001f / cfm;
897 if (cfm > 0.01f)
898 cfm = 0.01f;
899
900 if ((Math.Abs(p2.Velocity.X - p1.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y - p1.Velocity.Y) > 0.1f))
901 mu *= frictionMovementMult;
902
903 break;
904 default:
905 if (geom_name_map.TryGetValue(g2, out name))
906 {
907 if (name == "Terrain")
908 {
909 p1.getContactData(ref contactdata1);
910 bounce = contactdata1.bounce * TerrainBounce;
911 mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
912 if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f)
913 mu *= frictionMovementMult;
914 p1.CollidingGround = true;
915
916 cfm = p1.Mass;
917 dscale = 10 / cfm;
918 dscale = (float)Math.Sqrt(dscale);
919 if (dscale > 1.0f)
920 dscale = 1.0f;
921 erpscale = cfm * 0.01f;
922 cfm = 0.0001f / cfm;
923 if (cfm > 0.01f)
924 cfm = 0.01f;
925
926 if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass)
927 {
928 if (curContact.side1 > 0)
929 IgnoreNegSides = true;
930 }
931
932 }
933 else if (name == "Water")
934 {
935 ignore = true;
936 }
937 }
938 else
939 ignore = true;
940 break;
941 }
942 break;
943
944 default:
945 if (geom_name_map.TryGetValue(g1, out name))
946 {
947 if (name == "Terrain")
948 {
949 if (p2.PhysicsActorType == (int)ActorTypes.Prim)
950 {
951 p2.CollidingGround = true;
952 p2.getContactData(ref contactdata2);
953 bounce = contactdata2.bounce * TerrainBounce;
954 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
955
956 cfm = p2.Mass;
957 dscale = 10 / cfm;
958 dscale = (float)Math.Sqrt(dscale);
959
960 if (dscale > 1.0f)
961 dscale = 1.0f;
962
963 erpscale = cfm * 0.01f;
964 cfm = 0.0001f / cfm;
965 if (cfm > 0.01f)
966 cfm = 0.01f;
967
968 if (curContact.side1 > 0) // should be 2 ?
969 IgnoreNegSides = true;
970
971 if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f)
972 mu *= frictionMovementMult;
973 }
974 else
975 ignore = true;
976
977 }
978 else if (name == "Water" &&
979 (p2.PhysicsActorType == (int)ActorTypes.Prim || p2.PhysicsActorType == (int)ActorTypes.Agent))
980 {
981 ignore = true;
982 }
983 }
984 else
985 ignore = true;
986 break;
987 }
988
989 if (ignore)
990 return;
991
992 IntPtr Joint;
993
994 int i = 0;
995 while(true)
996 {
997
998 if (IgnoreNegSides && curContact.side1 < 0)
999 {
1000 if (++i >= count)
1001 break;
1002
1003 if (!GetCurContactGeom(i, ref curContact))
1004 break;
1005 }
1006 else
1007
1008 {
1009
1010 if (AvanormOverride)
1011 {
1012 if (curContact.depth > 0.3f)
1013 {
1014 if (dop1foot && (p1.Position.Z - curContact.pos.Z) > (p1.Size.Z - avCapRadius) * 0.5f)
1015 p1.IsColliding = true;
1016 if (dop2foot && (p2.Position.Z - curContact.pos.Z) > (p2.Size.Z - avCapRadius) * 0.5f)
1017 p2.IsColliding = true;
1018 curContact.normal.X = normoverride.X;
1019 curContact.normal.Y = normoverride.Y;
1020 curContact.normal.Z = normoverride.Z;
1021 }
1022
1023 else
1024 {
1025 if (dop1foot)
1026 {
1027 float sz = p1.Size.Z;
1028 Vector3 vtmp = p1.Position;
1029 float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
1030 if (ppos > 0f)
1031 {
1032 if (!p1.Flying)
1033 {
1034 d.AABB aabb;
1035 d.GeomGetAABB(g2, out aabb);
1036 float tmp = vtmp.Z - sz * .25f;
1037
1038 if (aabb.MaxZ < tmp)
1039 {
1040 vtmp.X = curContact.pos.X - vtmp.X;
1041 vtmp.Y = curContact.pos.Y - vtmp.Y;
1042 vtmp.Z = -0.2f;
1043 vtmp.Normalize();
1044 curContact.normal.X = vtmp.X;
1045 curContact.normal.Y = vtmp.Y;
1046 curContact.normal.Z = vtmp.Z;
1047 }
1048 }
1049 }
1050 else
1051 p1.IsColliding = true;
1052
1053 }
1054
1055 if (dop2foot)
1056 {
1057 float sz = p2.Size.Z;
1058 Vector3 vtmp = p2.Position;
1059 float ppos = curContact.pos.Z - vtmp.Z + (sz - avCapRadius) * 0.5f;
1060 if (ppos > 0f)
1061 {
1062 if (!p2.Flying)
1063 {
1064 d.AABB aabb;
1065 d.GeomGetAABB(g1, out aabb);
1066 float tmp = vtmp.Z - sz * .25f;
1067
1068 if (aabb.MaxZ < tmp)
1069 {
1070 vtmp.X = curContact.pos.X - vtmp.X;
1071 vtmp.Y = curContact.pos.Y - vtmp.Y;
1072 vtmp.Z = -0.2f;
1073 vtmp.Normalize();
1074 curContact.normal.X = vtmp.X;
1075 curContact.normal.Y = vtmp.Y;
1076 curContact.normal.Z = vtmp.Z;
1077 }
1078 }
1079 }
1080 else
1081 p2.IsColliding = true;
1082
1083 }
1084 }
1085 }
1086
1087 Joint = CreateContacJoint(ref curContact, mu, bounce, cfm, erpscale, dscale);
1088 d.JointAttach(Joint, b1, b2);
1089
1090 if (++m_global_contactcount >= maxContactsbeforedeath)
1091 break;
1092
1093 if (++i >= count)
1094 break;
1095
1096 if (!GetCurContactGeom(i, ref curContact))
1097 break;
1098
1099 if (curContact.depth > maxDepthContact.PenetrationDepth)
1100 {
1101 maxDepthContact.Position.X = curContact.pos.X;
1102 maxDepthContact.Position.Y = curContact.pos.Y;
1103 maxDepthContact.Position.Z = curContact.pos.Z;
1104 maxDepthContact.SurfaceNormal.X = curContact.normal.X;
1105 maxDepthContact.SurfaceNormal.Y = curContact.normal.Y;
1106 maxDepthContact.SurfaceNormal.Z = curContact.normal.Z;
1107 maxDepthContact.PenetrationDepth = curContact.depth;
1108 }
1109 }
1110 }
1111
1112 collision_accounting_events(p1, p2, maxDepthContact);
1113
1114/*
1115 if (notskipedcount > geomContactPointsStartthrottle)
1116 {
1117 // If there are more then 3 contact points, it's likely
1118 // that we've got a pile of objects, so ...
1119 // We don't want to send out hundreds of terse updates over and over again
1120 // so lets throttle them and send them again after it's somewhat sorted out.
1121 this needs checking so out for now
1122 if (b1 != IntPtr.Zero)
1123 p1.ThrottleUpdates = true;
1124 if (b2 != IntPtr.Zero)
1125 p2.ThrottleUpdates = true;
1126
1127 }
1128 */
1129 }
1130
1131 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
1132 {
1133 obj2LocalID = 0;
1134 bool p1events = p1.SubscribedEvents();
1135 bool p2events = p2.SubscribedEvents();
1136
1137 if (p1.IsVolumeDtc)
1138 p2events = false;
1139 if (p2.IsVolumeDtc)
1140 p1events = false;
1141
1142 if (!(p2events || p1events))
1143 return;
1144
1145 if (p1events)
1146 AddCollisionEventReporting(p1);
1147
1148 if (p2events)
1149 AddCollisionEventReporting(p2);
1150
1151 Vector3 vel = Vector3.Zero;
1152 if (p2 != null && p2.IsPhysical)
1153 vel = p2.Velocity;
1154
1155 if (p1 != null && p1.IsPhysical)
1156 vel -= p1.Velocity;
1157
1158 contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
1159
1160 switch ((ActorTypes)p1.PhysicsActorType)
1161 {
1162 case ActorTypes.Agent:
1163 cc1 = (OdeCharacter)p1;
1164 switch ((ActorTypes)p2.PhysicsActorType)
1165 {
1166 case ActorTypes.Agent:
1167 cc2 = (OdeCharacter)p2;
1168 obj2LocalID = cc2.m_localID;
1169 if (p2events)
1170 cc2.AddCollisionEvent(cc1.m_localID, contact);
1171 break;
1172
1173 case ActorTypes.Prim:
1174 if (p2 is OdePrim)
1175 {
1176 cp2 = (OdePrim)p2;
1177 obj2LocalID = cp2.m_localID;
1178 if (p2events)
1179 cp2.AddCollisionEvent(cc1.m_localID, contact);
1180 }
1181 break;
1182
1183 case ActorTypes.Ground:
1184 case ActorTypes.Unknown:
1185 default:
1186 obj2LocalID = 0;
1187 break;
1188 }
1189 if (p1events)
1190 {
1191 contact.SurfaceNormal = -contact.SurfaceNormal;
1192 cc1.AddCollisionEvent(obj2LocalID, contact);
1193 }
1194 break;
1195
1196 case ActorTypes.Prim:
1197
1198 if (p1 is OdePrim)
1199 {
1200 cp1 = (OdePrim)p1;
1201
1202 // obj1LocalID = cp2.m_localID;
1203 switch ((ActorTypes)p2.PhysicsActorType)
1204 {
1205 case ActorTypes.Agent:
1206 if (p2 is OdeCharacter)
1207 {
1208 cc2 = (OdeCharacter)p2;
1209 obj2LocalID = cc2.m_localID;
1210 if (p2events)
1211 cc2.AddCollisionEvent(cp1.m_localID, contact);
1212 }
1213 break;
1214 case ActorTypes.Prim:
1215
1216 if (p2 is OdePrim)
1217 {
1218 cp2 = (OdePrim)p2;
1219 obj2LocalID = cp2.m_localID;
1220 if (p2events)
1221 cp2.AddCollisionEvent(cp1.m_localID, contact);
1222 }
1223 break;
1224
1225 case ActorTypes.Ground:
1226 case ActorTypes.Unknown:
1227 default:
1228 obj2LocalID = 0;
1229 break;
1230 }
1231 if (p1events)
1232 {
1233 contact.SurfaceNormal = -contact.SurfaceNormal;
1234 cp1.AddCollisionEvent(obj2LocalID, contact);
1235 }
1236 }
1237 break;
1238 case ActorTypes.Ground:
1239 case ActorTypes.Unknown:
1240 default:
1241 switch ((ActorTypes)p2.PhysicsActorType)
1242 {
1243 case ActorTypes.Agent:
1244 if (p2 is OdeCharacter)
1245 {
1246 cc2 = (OdeCharacter)p2;
1247 obj2LocalID = cc2.m_localID;
1248 if (p2events)
1249 cc2.AddCollisionEvent(0, contact);
1250 }
1251 break;
1252 case ActorTypes.Prim:
1253 if (p2 is OdePrim)
1254 {
1255 cp2 = (OdePrim)p2;
1256 obj2LocalID = cp2.m_localID;
1257 if (p2events)
1258 cp2.AddCollisionEvent(0, contact);
1259 }
1260 break;
1261 }
1262 break;
1263 }
1264 }
1265
1266 /// <summary>
1267 /// This is our collision testing routine in ODE
1268 /// </summary>
1269 /// <param name="timeStep"></param>
1270 private void collision_optimized()
1271 {
1272 lock (_characters)
1273 {
1274 try
1275 {
1276 foreach (OdeCharacter chr in _characters)
1277 {
1278 if (chr == null || chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1279 continue;
1280
1281 chr.IsColliding = false;
1282 // chr.CollidingGround = false; not done here
1283 chr.CollidingObj = false;
1284 // do colisions with static space
1285 d.SpaceCollide2(StaticSpace, chr.Shell, IntPtr.Zero, nearCallback);
1286 }
1287 }
1288 catch (AccessViolationException)
1289 {
1290 m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
1291 }
1292
1293 }
1294
1295 lock (_activeprims)
1296 {
1297 foreach (OdePrim aprim in _activeprims)
1298 {
1299 aprim.CollisionScore = 0;
1300 aprim.IsColliding = false;
1301 }
1302 }
1303
1304 // collide active prims with static enviroment
1305 lock (_activegroups)
1306 {
1307 try
1308 {
1309 foreach (OdePrim prm in _activegroups)
1310 {
1311 if (d.BodyIsEnabled(prm.Body) && !prm.m_outbounds)
1312 d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback);
1313 }
1314 }
1315 catch (AccessViolationException)
1316 {
1317 m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space");
1318 }
1319 }
1320 // finally colide active things amoung them
1321 try
1322 {
1323 d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
1324 }
1325 catch (AccessViolationException)
1326 {
1327 m_log.Warn("[PHYSICS]: Unable to collide in Active space");
1328 }
1329// _perloopContact.Clear();
1330 }
1331
1332 #endregion
1333 /// <summary>
1334 /// Add actor to the list that should receive collision events in the simulate loop.
1335 /// </summary>
1336 /// <param name="obj"></param>
1337 public void AddCollisionEventReporting(PhysicsActor obj)
1338 {
1339 if (!_collisionEventPrim.Contains(obj))
1340 _collisionEventPrim.Add(obj);
1341 }
1342
1343 /// <summary>
1344 /// Remove actor from the list that should receive collision events in the simulate loop.
1345 /// </summary>
1346 /// <param name="obj"></param>
1347 public void RemoveCollisionEventReporting(PhysicsActor obj)
1348 {
1349 if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
1350 _collisionEventPrimRemove.Add(obj);
1351 }
1352
1353
1354 #region Add/Remove Entities
1355
1356 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
1357 {
1358 Vector3 pos;
1359 pos.X = position.X;
1360 pos.Y = position.Y;
1361 pos.Z = position.Z;
1362 OdeCharacter newAv = new OdeCharacter(avName, this, pos, size, avPIDD, avPIDP, avCapRadius, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
1363 newAv.Flying = isFlying;
1364 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1365
1366 return newAv;
1367 }
1368
1369 public void AddCharacter(OdeCharacter chr)
1370 {
1371 lock (_characters)
1372 {
1373 if (!_characters.Contains(chr))
1374 {
1375 _characters.Add(chr);
1376 if (chr.bad)
1377 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
1378 }
1379 }
1380 }
1381
1382 public void RemoveCharacter(OdeCharacter chr)
1383 {
1384 lock (_characters)
1385 {
1386 if (_characters.Contains(chr))
1387 {
1388 _characters.Remove(chr);
1389 }
1390 }
1391 }
1392
1393 public void BadCharacter(OdeCharacter chr)
1394 {
1395 lock (_badCharacter)
1396 {
1397 if (!_badCharacter.Contains(chr))
1398 _badCharacter.Add(chr);
1399 }
1400 }
1401
1402 public override void RemoveAvatar(PhysicsActor actor)
1403 {
1404 //m_log.Debug("[PHYSICS]:ODELOCK");
1405 ((OdeCharacter) actor).Destroy();
1406 }
1407
1408 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1409 PrimitiveBaseShape pbs, bool isphysical, uint localID)
1410 {
1411 Vector3 pos = position;
1412 Vector3 siz = size;
1413 Quaternion rot = rotation;
1414
1415 OdePrim newPrim;
1416 lock (OdeLock)
1417 {
1418 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical,false,0,localID);
1419
1420 lock (_prims)
1421 _prims.Add(newPrim);
1422 }
1423 return newPrim;
1424 }
1425
1426 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1427 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, uint localID)
1428 {
1429 Vector3 pos = position;
1430 Vector3 siz = size;
1431 Quaternion rot = rotation;
1432
1433 OdePrim newPrim;
1434 lock (OdeLock)
1435 {
1436 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, 0, localID);
1437
1438 lock (_prims)
1439 _prims.Add(newPrim);
1440 }
1441 return newPrim;
1442 }
1443
1444 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1445 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
1446 {
1447 Vector3 pos = position;
1448 Vector3 siz = size;
1449 Quaternion rot = rotation;
1450
1451 OdePrim newPrim;
1452 lock (OdeLock)
1453 {
1454 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical, isPhantom, shapeType, localID);
1455
1456 lock (_prims)
1457 _prims.Add(newPrim);
1458 }
1459 return newPrim;
1460 }
1461
1462 public void addActivePrim(OdePrim activatePrim)
1463 {
1464 // adds active prim..
1465 lock (_activeprims)
1466 {
1467 if (!_activeprims.Contains(activatePrim))
1468 _activeprims.Add(activatePrim);
1469 }
1470 }
1471
1472 public void addActiveGroups(OdePrim activatePrim)
1473 {
1474 lock (_activegroups)
1475 {
1476 if (!_activegroups.Contains(activatePrim))
1477 _activegroups.Add(activatePrim);
1478 }
1479 }
1480
1481 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1482 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
1483 {
1484 return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, localid);
1485 }
1486
1487
1488 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1489 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
1490 {
1491#if SPAM
1492 m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
1493#endif
1494
1495 return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
1496 }
1497
1498 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1499 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
1500 {
1501#if SPAM
1502 m_log.DebugFormat("[PHYSICS]: Adding physics actor to {0}", primName);
1503#endif
1504
1505 return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
1506 }
1507
1508 public override float TimeDilation
1509 {
1510 get { return m_timeDilation; }
1511 }
1512
1513 public override bool SupportsNINJAJoints
1514 {
1515 get { return false; }
1516 }
1517
1518
1519 public void remActivePrim(OdePrim deactivatePrim)
1520 {
1521 lock (_activeprims)
1522 {
1523 _activeprims.Remove(deactivatePrim);
1524 }
1525 }
1526 public void remActiveGroup(OdePrim deactivatePrim)
1527 {
1528 lock (_activegroups)
1529 {
1530 _activegroups.Remove(deactivatePrim);
1531 }
1532 }
1533
1534 public override void RemovePrim(PhysicsActor prim)
1535 {
1536 // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
1537 // removed in the next physics simulate pass.
1538 if (prim is OdePrim)
1539 {
1540// lock (OdeLock)
1541 {
1542
1543 OdePrim p = (OdePrim)prim;
1544 p.setPrimForRemoval();
1545 }
1546 }
1547 }
1548 /// <summary>
1549 /// This is called from within simulate but outside the locked portion
1550 /// We need to do our own locking here
1551 /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
1552 /// Simulate() -- justincc).
1553 ///
1554 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
1555 ///
1556 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
1557 /// that the space was using.
1558 /// </summary>
1559 /// <param name="prim"></param>
1560 public void RemovePrimThreadLocked(OdePrim prim)
1561 {
1562 //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
1563 lock (prim)
1564 {
1565 RemoveCollisionEventReporting(prim);
1566 lock (_prims)
1567 _prims.Remove(prim);
1568 }
1569
1570 }
1571 #endregion
1572
1573 #region Space Separation Calculation
1574
1575 /// <summary>
1576 /// Called when a static prim moves or becomes static
1577 /// Places the prim in a space one the static sub-spaces grid
1578 /// </summary>
1579 /// <param name="geom">the pointer to the geom that moved</param>
1580 /// <param name="pos">the position that the geom moved to</param>
1581 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
1582 /// <returns>a pointer to the new space it's in</returns>
1583 public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace)
1584 {
1585 // moves a prim into another static sub-space or from another space into a static sub-space
1586
1587 // Called ODEPrim so
1588 // it's already in locked space.
1589
1590 if (geom == IntPtr.Zero) // shouldn't happen
1591 return IntPtr.Zero;
1592
1593 // get the static sub-space for current position
1594 IntPtr newspace = calculateSpaceForGeom(pos);
1595
1596 if (newspace == currentspace) // if we are there all done
1597 return newspace;
1598
1599 // else remove it from its current space
1600 if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom))
1601 {
1602 if (d.GeomIsSpace(currentspace))
1603 {
1604 waitForSpaceUnlock(currentspace);
1605 d.SpaceRemove(currentspace, geom);
1606
1607 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1608 {
1609 d.SpaceDestroy(currentspace);
1610 }
1611 }
1612 else
1613 {
1614 m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
1615 " Geom:" + geom);
1616 }
1617 }
1618 else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space
1619 {
1620 currentspace = d.GeomGetSpace(geom);
1621 if (currentspace != IntPtr.Zero)
1622 {
1623 if (d.GeomIsSpace(currentspace))
1624 {
1625 waitForSpaceUnlock(currentspace);
1626 d.SpaceRemove(currentspace, geom);
1627
1628 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1629 {
1630 d.SpaceDestroy(currentspace);
1631 }
1632
1633 }
1634 }
1635 }
1636
1637 // put the geom in the newspace
1638 waitForSpaceUnlock(newspace);
1639 d.SpaceAdd(newspace, geom);
1640
1641 // let caller know this newspace
1642 return newspace;
1643 }
1644
1645 /// <summary>
1646 /// Calculates the space the prim should be in by its position
1647 /// </summary>
1648 /// <param name="pos"></param>
1649 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
1650 public IntPtr calculateSpaceForGeom(Vector3 pos)
1651 {
1652 int x, y;
1653 x = (int)(pos.X * spacesPerMeter);
1654 if (x < 0)
1655 x = 0;
1656 else if (x > spaceGridMaxX)
1657 x = spaceGridMaxX;
1658
1659 y = (int)(pos.Y * spacesPerMeter);
1660 if (y < 0)
1661 y = 0;
1662 else if (y >spaceGridMaxY)
1663 y = spaceGridMaxY;
1664
1665 IntPtr tmpSpace = staticPrimspace[x, y];
1666 return tmpSpace;
1667 }
1668
1669 #endregion
1670
1671 /// <summary>
1672 /// Routine to figure out if we need to mesh this prim with our mesher
1673 /// </summary>
1674 /// <param name="pbs"></param>
1675 /// <returns></returns>
1676 public bool needsMeshing(PrimitiveBaseShape pbs)
1677 {
1678 // check sculpts or meshs
1679 if (pbs.SculptEntry)
1680 {
1681 if (meshSculptedPrim)
1682 return true;
1683
1684 if (pbs.SculptType == (byte)SculptType.Mesh) // always do meshs
1685 return true;
1686
1687 return false;
1688 }
1689
1690 if (forceSimplePrimMeshing)
1691 return true;
1692
1693 // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
1694
1695 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
1696 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
1697 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
1698 {
1699
1700 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
1701 && pbs.ProfileHollow == 0
1702 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
1703 && pbs.PathBegin == 0 && pbs.PathEnd == 0
1704 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
1705 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
1706 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
1707 {
1708#if SPAM
1709 m_log.Warn("NonMesh");
1710#endif
1711 return false;
1712 }
1713 }
1714
1715 // following code doesn't give meshs to boxes and spheres ever
1716 // and it's odd.. so for now just return true if asked to force meshs
1717 // hopefully mesher will fail if doesn't suport so things still get basic boxes
1718
1719 int iPropertiesNotSupportedDefault = 0;
1720
1721 if (pbs.ProfileHollow != 0)
1722 iPropertiesNotSupportedDefault++;
1723
1724 if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
1725 iPropertiesNotSupportedDefault++;
1726
1727 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
1728 iPropertiesNotSupportedDefault++;
1729
1730 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
1731 iPropertiesNotSupportedDefault++;
1732
1733 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
1734 iPropertiesNotSupportedDefault++;
1735
1736 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
1737 iPropertiesNotSupportedDefault++;
1738
1739 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
1740 iPropertiesNotSupportedDefault++;
1741
1742 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
1743 iPropertiesNotSupportedDefault++;
1744
1745 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
1746 iPropertiesNotSupportedDefault++;
1747
1748 // test for torus
1749 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
1750 {
1751 if (pbs.PathCurve == (byte)Extrusion.Curve1)
1752 {
1753 iPropertiesNotSupportedDefault++;
1754 }
1755 }
1756 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
1757 {
1758 if (pbs.PathCurve == (byte)Extrusion.Straight)
1759 {
1760 iPropertiesNotSupportedDefault++;
1761 }
1762
1763 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
1764 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1765 {
1766 iPropertiesNotSupportedDefault++;
1767 }
1768 }
1769 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
1770 {
1771 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
1772 {
1773 iPropertiesNotSupportedDefault++;
1774 }
1775 }
1776 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
1777 {
1778 if (pbs.PathCurve == (byte)Extrusion.Straight)
1779 {
1780 iPropertiesNotSupportedDefault++;
1781 }
1782 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
1783 {
1784 iPropertiesNotSupportedDefault++;
1785 }
1786 }
1787
1788 if (iPropertiesNotSupportedDefault == 0)
1789 {
1790#if SPAM
1791 m_log.Warn("NonMesh");
1792#endif
1793 return false;
1794 }
1795#if SPAM
1796 m_log.Debug("Mesh");
1797#endif
1798 return true;
1799 }
1800
1801 /// <summary>
1802 /// Called to queue a change to a actor
1803 /// to use in place of old taint mechanism so changes do have a time sequence
1804 /// </summary>
1805
1806 public void AddChange(PhysicsActor actor, changes what, Object arg)
1807 {
1808 ODEchangeitem item = new ODEchangeitem();
1809 item.actor = actor;
1810 item.what = what;
1811 item.arg = arg;
1812 ChangesQueue.Enqueue(item);
1813 }
1814
1815 /// <summary>
1816 /// Called after our prim properties are set Scale, position etc.
1817 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
1818 /// This assures us that we have no race conditions
1819 /// </summary>
1820 /// <param name="prim"></param>
1821 public override void AddPhysicsActorTaint(PhysicsActor prim)
1822 {
1823 }
1824
1825 /// <summary>
1826 /// This is our main simulate loop
1827 /// It's thread locked by a Mutex in the scene.
1828 /// It holds Collisions, it instructs ODE to step through the physical reactions
1829 /// It moves the objects around in memory
1830 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
1831 /// </summary>
1832 /// <param name="timeStep"></param>
1833 /// <returns></returns>
1834 public override float Simulate(float timeStep)
1835 {
1836
1837 DateTime now = DateTime.UtcNow;
1838 TimeSpan SinceLastFrame = now - m_lastframe;
1839 m_lastframe = now;
1840 timeStep = (float)SinceLastFrame.TotalSeconds;
1841
1842 // acumulate time so we can reduce error
1843 step_time += timeStep;
1844
1845 if (step_time < HalfOdeStep)
1846 return 0;
1847
1848 if (framecount < 0)
1849 framecount = 0;
1850
1851 framecount++;
1852
1853 int curphysiteractions;
1854
1855 // if in trouble reduce step resolution
1856 if (step_time >= m_SkipFramesAtms)
1857 curphysiteractions = m_physicsiterations / 2;
1858 else
1859 curphysiteractions = m_physicsiterations;
1860
1861 int nodeframes = 0;
1862
1863// checkThread();
1864
1865 lock (SimulationLock)
1866 lock(OdeLock)
1867 {
1868 // adjust number of iterations per step
1869 try
1870 {
1871 d.WorldSetQuickStepNumIterations(world, curphysiteractions);
1872 }
1873 catch (StackOverflowException)
1874 {
1875 m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
1876// ode.drelease(world);
1877 base.TriggerPhysicsBasedRestart();
1878 }
1879
1880 while (step_time > HalfOdeStep && nodeframes < 10) //limit number of steps so we don't say here for ever
1881 {
1882 try
1883 {
1884 // clear pointer/counter to contacts to pass into joints
1885 m_global_contactcount = 0;
1886
1887 ODEchangeitem item;
1888
1889 if(ChangesQueue.Count >0)
1890 {
1891 int ttmpstart = Util.EnvironmentTickCount();
1892 int ttmp;
1893
1894 while(ChangesQueue.Dequeue(out item))
1895 {
1896 if (item.actor != null)
1897 {
1898 try
1899 {
1900 if (item.actor is OdeCharacter)
1901 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
1902 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
1903 RemovePrimThreadLocked((OdePrim)item.actor);
1904 }
1905 catch
1906 {
1907 m_log.Warn("[PHYSICS]: doChange failed for a actor");
1908 };
1909 }
1910 ttmp = Util.EnvironmentTickCountSubtract(ttmpstart);
1911 if (ttmp > 20)
1912 break;
1913 }
1914 }
1915
1916 // Move characters
1917 lock (_characters)
1918 {
1919 List<OdeCharacter> defects = new List<OdeCharacter>();
1920 foreach (OdeCharacter actor in _characters)
1921 {
1922 if (actor != null)
1923 actor.Move(ODE_STEPSIZE, defects);
1924 }
1925 if (defects.Count != 0)
1926 {
1927 foreach (OdeCharacter defect in defects)
1928 {
1929 RemoveCharacter(defect);
1930 }
1931 }
1932 }
1933
1934 // Move other active objects
1935 lock (_activegroups)
1936 {
1937 foreach (OdePrim aprim in _activegroups)
1938 {
1939 aprim.Move();
1940 }
1941 }
1942
1943 //if ((framecount % m_randomizeWater) == 0)
1944 // randomizeWater(waterlevel);
1945
1946 m_rayCastManager.ProcessQueuedRequests();
1947
1948 collision_optimized();
1949
1950 foreach (PhysicsActor obj in _collisionEventPrim)
1951 {
1952 if (obj == null)
1953 continue;
1954
1955 switch ((ActorTypes)obj.PhysicsActorType)
1956 {
1957 case ActorTypes.Agent:
1958 OdeCharacter cobj = (OdeCharacter)obj;
1959 cobj.AddCollisionFrameTime((int)(odetimestepMS));
1960 cobj.SendCollisions();
1961 break;
1962
1963 case ActorTypes.Prim:
1964 OdePrim pobj = (OdePrim)obj;
1965 if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds))
1966 {
1967 pobj.AddCollisionFrameTime((int)(odetimestepMS));
1968 pobj.SendCollisions();
1969 }
1970 break;
1971 }
1972 }
1973
1974 foreach (PhysicsActor obj in _collisionEventPrimRemove)
1975 _collisionEventPrim.Remove(obj);
1976
1977 _collisionEventPrimRemove.Clear();
1978
1979 // do a ode simulation step
1980 d.WorldQuickStep(world, ODE_STEPSIZE);
1981 d.JointGroupEmpty(contactgroup);
1982
1983 // update managed ideia of physical data and do updates to core
1984 /*
1985 lock (_characters)
1986 {
1987 foreach (OdeCharacter actor in _characters)
1988 {
1989 if (actor != null)
1990 {
1991 if (actor.bad)
1992 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
1993
1994 actor.UpdatePositionAndVelocity();
1995 }
1996 }
1997 }
1998 */
1999
2000 lock (_activegroups)
2001 {
2002 {
2003 foreach (OdePrim actor in _activegroups)
2004 {
2005 if (actor.IsPhysical)
2006 {
2007 actor.UpdatePositionAndVelocity();
2008 }
2009 }
2010 }
2011 }
2012 }
2013 catch (Exception e)
2014 {
2015 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
2016// ode.dunlock(world);
2017 }
2018
2019
2020 step_time -= ODE_STEPSIZE;
2021 nodeframes++;
2022 }
2023
2024 lock (_badCharacter)
2025 {
2026 if (_badCharacter.Count > 0)
2027 {
2028 foreach (OdeCharacter chr in _badCharacter)
2029 {
2030 RemoveCharacter(chr);
2031 }
2032
2033 _badCharacter.Clear();
2034 }
2035 }
2036
2037 int nactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
2038 int nstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
2039 int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
2040 int nbodies = d.NTotalBodies;
2041 int ngeoms = d.NTotalGeoms;
2042
2043 // Finished with all sim stepping. If requested, dump world state to file for debugging.
2044 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
2045 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
2046 if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
2047 {
2048 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
2049 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
2050
2051 if (physics_logging_append_existing_logfile)
2052 {
2053 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
2054 TextWriter fwriter = File.AppendText(fname);
2055 fwriter.WriteLine(header);
2056 fwriter.Close();
2057 }
2058
2059 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
2060 }
2061
2062 // think time dilation as to do with dinamic step size that we dont' have
2063 // even so tell something to world
2064 if (nodeframes < 10) // we did the requested loops
2065 m_timeDilation = 1.0f;
2066 else if (step_time > 0)
2067 {
2068 m_timeDilation = timeStep / step_time;
2069 if (m_timeDilation > 1)
2070 m_timeDilation = 1;
2071 if (step_time > m_SkipFramesAtms)
2072 step_time = 0;
2073 }
2074 }
2075
2076// return nodeframes * ODE_STEPSIZE; // return real simulated time
2077 return 1000 * nodeframes; // return steps for now * 1000 to keep core happy
2078 }
2079
2080 /// <summary>
2081 public override void GetResults()
2082 {
2083 }
2084
2085 public override bool IsThreaded
2086 {
2087 // for now we won't be multithreaded
2088 get { return (false); }
2089 }
2090
2091 public float GetTerrainHeightAtXY(float x, float y)
2092 {
2093
2094
2095 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
2096 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
2097
2098
2099 IntPtr heightFieldGeom = IntPtr.Zero;
2100
2101 // get region map
2102 if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom))
2103 return 0f;
2104
2105 if (heightFieldGeom == IntPtr.Zero)
2106 return 0f;
2107
2108 if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
2109 return 0f;
2110
2111 // TerrainHeightField for ODE as offset 1m
2112 x += 1f - offsetX;
2113 y += 1f - offsetY;
2114
2115 // make position fit into array
2116 if (x < 0)
2117 x = 0;
2118 if (y < 0)
2119 y = 0;
2120
2121 // integer indexs
2122 int ix;
2123 int iy;
2124 // interpolators offset
2125 float dx;
2126 float dy;
2127
2128 int regsize = (int)Constants.RegionSize + 3; // map size see setterrain number of samples
2129
2130 if (OdeUbitLib)
2131 {
2132 if (x < regsize - 1)
2133 {
2134 ix = (int)x;
2135 dx = x - (float)ix;
2136 }
2137 else // out world use external height
2138 {
2139 ix = regsize - 2;
2140 dx = 0;
2141 }
2142 if (y < regsize - 1)
2143 {
2144 iy = (int)y;
2145 dy = y - (float)iy;
2146 }
2147 else
2148 {
2149 iy = regsize - 2;
2150 dy = 0;
2151 }
2152 }
2153
2154 else
2155 {
2156 // we still have square fixed size regions
2157 // also flip x and y because of how map is done for ODE fliped axis
2158 // so ix,iy,dx and dy are inter exchanged
2159 if (x < regsize - 1)
2160 {
2161 iy = (int)x;
2162 dy = x - (float)iy;
2163 }
2164 else // out world use external height
2165 {
2166 iy = regsize - 2;
2167 dy = 0;
2168 }
2169 if (y < regsize - 1)
2170 {
2171 ix = (int)y;
2172 dx = y - (float)ix;
2173 }
2174 else
2175 {
2176 ix = regsize - 2;
2177 dx = 0;
2178 }
2179 }
2180
2181 float h0;
2182 float h1;
2183 float h2;
2184
2185 iy *= regsize;
2186 iy += ix; // all indexes have iy + ix
2187
2188 float[] heights = TerrainHeightFieldHeights[heightFieldGeom];
2189 /*
2190 if ((dx + dy) <= 1.0f)
2191 {
2192 h0 = ((float)heights[iy]); // 0,0 vertice
2193 h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
2194 h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
2195 }
2196 else
2197 {
2198 h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
2199 h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
2200 h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
2201 }
2202 */
2203 h0 = ((float)heights[iy]); // 0,0 vertice
2204
2205 if ((dy > dx))
2206 {
2207 iy += regsize;
2208 h2 = (float)heights[iy]; // 0,1 vertice
2209 h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
2210 h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
2211 }
2212 else
2213 {
2214 iy++;
2215 h2 = (float)heights[iy]; // vertice 1,0
2216 h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
2217 h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
2218 }
2219
2220 return h0 + h1 + h2;
2221 }
2222
2223
2224 public override void SetTerrain(float[] heightMap)
2225 {
2226 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
2227 {
2228 if (m_parentScene is OdeScene)
2229 {
2230 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
2231 }
2232 }
2233 else
2234 {
2235 SetTerrain(heightMap, m_worldOffset);
2236 }
2237 }
2238
2239 public override void CombineTerrain(float[] heightMap, Vector3 pOffset)
2240 {
2241 SetTerrain(heightMap, pOffset);
2242 }
2243
2244 public void SetTerrain(float[] heightMap, Vector3 pOffset)
2245 {
2246 if (OdeUbitLib)
2247 UbitSetTerrain(heightMap, pOffset);
2248 else
2249 OriSetTerrain(heightMap, pOffset);
2250 }
2251
2252 public void OriSetTerrain(float[] heightMap, Vector3 pOffset)
2253 {
2254 // assumes 1m size grid and constante size square regions
2255 // needs to know about sims around in future
2256
2257 float[] _heightmap;
2258
2259 uint heightmapWidth = Constants.RegionSize + 2;
2260 uint heightmapHeight = Constants.RegionSize + 2;
2261
2262 uint heightmapWidthSamples = heightmapWidth + 1;
2263 uint heightmapHeightSamples = heightmapHeight + 1;
2264
2265 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2266
2267 const float scale = 1.0f;
2268 const float offset = 0.0f;
2269 const float thickness = 10f;
2270 const int wrap = 0;
2271
2272 uint regionsize = Constants.RegionSize;
2273
2274 float hfmin = float.MaxValue;
2275 float hfmax = float.MinValue;
2276 float val;
2277 uint xx;
2278 uint yy;
2279
2280 uint maxXXYY = regionsize - 1;
2281 // flipping map adding one margin all around so things don't fall in edges
2282
2283 uint xt = 0;
2284 xx = 0;
2285
2286 for (uint x = 0; x < heightmapWidthSamples; x++)
2287 {
2288 if (x > 1 && xx < maxXXYY)
2289 xx++;
2290 yy = 0;
2291 for (uint y = 0; y < heightmapHeightSamples; y++)
2292 {
2293 if (y > 1 && y < maxXXYY)
2294 yy += regionsize;
2295
2296 val = heightMap[yy + xx];
2297 if (val < 0.0f)
2298 val = 0.0f; // no neg terrain as in chode
2299 _heightmap[xt + y] = val;
2300
2301 if (hfmin > val)
2302 hfmin = val;
2303 if (hfmax < val)
2304 hfmax = val;
2305 }
2306 xt += heightmapHeightSamples;
2307 }
2308 lock (OdeLock)
2309 {
2310 IntPtr GroundGeom = IntPtr.Zero;
2311 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2312 {
2313 RegionTerrain.Remove(pOffset);
2314 if (GroundGeom != IntPtr.Zero)
2315 {
2316 d.GeomDestroy(GroundGeom);
2317
2318 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2319 {
2320 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2321 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2322 TerrainHeightFieldHeights.Remove(GroundGeom);
2323 }
2324 }
2325 }
2326 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2327
2328 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2329
2330 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight,
2331 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2332 offset, thickness, wrap);
2333
2334 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2335 GroundGeom = d.CreateHeightfield(StaticSpace, HeightmapData, 1);
2336 if (GroundGeom != IntPtr.Zero)
2337 {
2338 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
2339 d.GeomSetCollideBits(GroundGeom, 0);
2340
2341 }
2342 geom_name_map[GroundGeom] = "Terrain";
2343
2344 d.Matrix3 R = new d.Matrix3();
2345
2346 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2347 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2348
2349
2350 q1 = q1 * q2;
2351
2352 Vector3 v3;
2353 float angle;
2354 q1.GetAxisAngle(out v3, out angle);
2355
2356 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2357 d.GeomSetRotation(GroundGeom, ref R);
2358 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2359 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
2360// TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap);
2361 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2362 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2363
2364 }
2365 }
2366
2367 public void UbitSetTerrain(float[] heightMap, Vector3 pOffset)
2368 {
2369 // assumes 1m size grid and constante size square regions
2370 // needs to know about sims around in future
2371
2372 float[] _heightmap;
2373
2374 uint heightmapWidth = Constants.RegionSize + 2;
2375 uint heightmapHeight = Constants.RegionSize + 2;
2376
2377 uint heightmapWidthSamples = heightmapWidth + 1;
2378 uint heightmapHeightSamples = heightmapHeight + 1;
2379
2380 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2381
2382
2383 uint regionsize = Constants.RegionSize;
2384
2385 float hfmin = float.MaxValue;
2386// float hfmax = float.MinValue;
2387 float val;
2388
2389
2390 uint maxXXYY = regionsize - 1;
2391 // adding one margin all around so things don't fall in edges
2392
2393 uint xx;
2394 uint yy = 0;
2395 uint yt = 0;
2396
2397 for (uint y = 0; y < heightmapHeightSamples; y++)
2398 {
2399 if (y > 1 && y < maxXXYY)
2400 yy += regionsize;
2401 xx = 0;
2402 for (uint x = 0; x < heightmapWidthSamples; x++)
2403 {
2404 if (x > 1 && x < maxXXYY)
2405 xx++;
2406
2407 val = heightMap[yy + xx];
2408 if (val < 0.0f)
2409 val = 0.0f; // no neg terrain as in chode
2410 _heightmap[yt + x] = val;
2411
2412 if (hfmin > val)
2413 hfmin = val;
2414// if (hfmax < val)
2415// hfmax = val;
2416 }
2417 yt += heightmapWidthSamples;
2418 }
2419 lock (OdeLock)
2420 {
2421 IntPtr GroundGeom = IntPtr.Zero;
2422 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2423 {
2424 RegionTerrain.Remove(pOffset);
2425 if (GroundGeom != IntPtr.Zero)
2426 {
2427 d.GeomDestroy(GroundGeom);
2428
2429 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2430 {
2431 if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated)
2432 TerrainHeightFieldHeightsHandlers[GroundGeom].Free();
2433 TerrainHeightFieldHeightsHandlers.Remove(GroundGeom);
2434 TerrainHeightFieldHeights.Remove(GroundGeom);
2435 }
2436 }
2437 }
2438 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2439
2440 const int wrap = 0;
2441 float thickness = hfmin;
2442 if (thickness < 0)
2443 thickness = 1;
2444
2445 GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2446
2447 d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f,
2448 (int)heightmapWidthSamples, (int)heightmapHeightSamples,
2449 thickness, wrap);
2450
2451// d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2452 GroundGeom = d.CreateUbitTerrain(StaticSpace, HeightmapData, 1);
2453 if (GroundGeom != IntPtr.Zero)
2454 {
2455 d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land));
2456 d.GeomSetCollideBits(GroundGeom, 0);
2457
2458 }
2459 geom_name_map[GroundGeom] = "Terrain";
2460
2461 d.GeomSetPosition(GroundGeom, pOffset.X + (float)Constants.RegionSize * 0.5f, pOffset.Y + (float)Constants.RegionSize * 0.5f, 0);
2462 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
2463 // TerrainHeightFieldHeights.Add(GroundGeom, ODElandMap);
2464 TerrainHeightFieldHeights.Add(GroundGeom, _heightmap);
2465 TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler);
2466 }
2467 }
2468
2469
2470 public override void DeleteTerrain()
2471 {
2472 }
2473
2474 public float GetWaterLevel()
2475 {
2476 return waterlevel;
2477 }
2478
2479 public override bool SupportsCombining()
2480 {
2481 return true;
2482 }
2483/*
2484 public override void UnCombine(PhysicsScene pScene)
2485 {
2486 IntPtr localGround = IntPtr.Zero;
2487// float[] localHeightfield;
2488 bool proceed = false;
2489 List<IntPtr> geomDestroyList = new List<IntPtr>();
2490
2491 lock (OdeLock)
2492 {
2493 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
2494 {
2495 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
2496 {
2497 if (geom == localGround)
2498 {
2499// localHeightfield = TerrainHeightFieldHeights[geom];
2500 proceed = true;
2501 }
2502 else
2503 {
2504 geomDestroyList.Add(geom);
2505 }
2506 }
2507
2508 if (proceed)
2509 {
2510 m_worldOffset = Vector3.Zero;
2511 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
2512 m_parentScene = null;
2513
2514 foreach (IntPtr g in geomDestroyList)
2515 {
2516 // removingHeightField needs to be done or the garbage collector will
2517 // collect the terrain data before we tell ODE to destroy it causing
2518 // memory corruption
2519 if (TerrainHeightFieldHeights.ContainsKey(g))
2520 {
2521// float[] removingHeightField = TerrainHeightFieldHeights[g];
2522 TerrainHeightFieldHeights.Remove(g);
2523
2524 if (RegionTerrain.ContainsKey(g))
2525 {
2526 RegionTerrain.Remove(g);
2527 }
2528
2529 d.GeomDestroy(g);
2530 //removingHeightField = new float[0];
2531 }
2532 }
2533
2534 }
2535 else
2536 {
2537 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
2538 }
2539 }
2540 }
2541 }
2542*/
2543 public override void SetWaterLevel(float baseheight)
2544 {
2545 waterlevel = baseheight;
2546 randomizeWater(waterlevel);
2547 }
2548
2549 public void randomizeWater(float baseheight)
2550 {
2551 const uint heightmapWidth = Constants.RegionSize + 2;
2552 const uint heightmapHeight = Constants.RegionSize + 2;
2553 const uint heightmapWidthSamples = heightmapWidth + 1;
2554 const uint heightmapHeightSamples = heightmapHeight + 1;
2555
2556 const float scale = 1.0f;
2557 const float offset = 0.0f;
2558 const int wrap = 0;
2559
2560 float[] _watermap = new float[heightmapWidthSamples * heightmapWidthSamples];
2561
2562 float maxheigh = float.MinValue;
2563 float minheigh = float.MaxValue;
2564 float val;
2565 for (int i = 0; i < (heightmapWidthSamples * heightmapHeightSamples); i++)
2566 {
2567
2568 val = (baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f);
2569 _watermap[i] = val;
2570 if (maxheigh < val)
2571 maxheigh = val;
2572 if (minheigh > val)
2573 minheigh = val;
2574 }
2575
2576 float thickness = minheigh;
2577
2578 lock (OdeLock)
2579 {
2580 if (WaterGeom != IntPtr.Zero)
2581 {
2582 d.GeomDestroy(WaterGeom);
2583 d.GeomHeightfieldDataDestroy(WaterHeightmapData);
2584 WaterGeom = IntPtr.Zero;
2585 WaterHeightmapData = IntPtr.Zero;
2586 if(WaterMapHandler.IsAllocated)
2587 WaterMapHandler.Free();
2588 }
2589
2590 WaterHeightmapData = d.GeomHeightfieldDataCreate();
2591
2592 WaterMapHandler = GCHandle.Alloc(_watermap, GCHandleType.Pinned);
2593
2594 d.GeomHeightfieldDataBuildSingle(WaterHeightmapData, WaterMapHandler.AddrOfPinnedObject(), 0, heightmapWidth, heightmapHeight,
2595 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
2596 offset, thickness, wrap);
2597 d.GeomHeightfieldDataSetBounds(WaterHeightmapData, minheigh, maxheigh);
2598 WaterGeom = d.CreateHeightfield(StaticSpace, WaterHeightmapData, 1);
2599 if (WaterGeom != IntPtr.Zero)
2600 {
2601 d.GeomSetCategoryBits(WaterGeom, (uint)(CollisionCategories.Water));
2602 d.GeomSetCollideBits(WaterGeom, 0);
2603
2604 geom_name_map[WaterGeom] = "Water";
2605
2606 d.Matrix3 R = new d.Matrix3();
2607
2608 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2609 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2610
2611 q1 = q1 * q2;
2612 Vector3 v3;
2613 float angle;
2614 q1.GetAxisAngle(out v3, out angle);
2615
2616 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2617 d.GeomSetRotation(WaterGeom, ref R);
2618 d.GeomSetPosition(WaterGeom, (float)Constants.RegionSize * 0.5f, (float)Constants.RegionSize * 0.5f, 0);
2619 }
2620 }
2621 }
2622
2623 public override void Dispose()
2624 {
2625 m_rayCastManager.Dispose();
2626 m_rayCastManager = null;
2627
2628 lock (OdeLock)
2629 {
2630 lock (_prims)
2631 {
2632 foreach (OdePrim prm in _prims)
2633 {
2634 RemovePrim(prm);
2635 }
2636 }
2637
2638 if (TerrainHeightFieldHeightsHandlers.Count > 0)
2639 {
2640 foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values)
2641 {
2642 if (gch.IsAllocated)
2643 gch.Free();
2644 }
2645 }
2646
2647 if (WaterGeom != IntPtr.Zero)
2648 {
2649 d.GeomDestroy(WaterGeom);
2650 WaterGeom = IntPtr.Zero;
2651 if (WaterHeightmapData != IntPtr.Zero)
2652 d.GeomHeightfieldDataDestroy(WaterHeightmapData);
2653 WaterHeightmapData = IntPtr.Zero;
2654
2655 if (WaterMapHandler.IsAllocated)
2656 WaterMapHandler.Free();
2657 }
2658
2659
2660 if (ContactgeomsArray != IntPtr.Zero)
2661 Marshal.FreeHGlobal(ContactgeomsArray);
2662 if (GlobalContactsArray != IntPtr.Zero)
2663 Marshal.FreeHGlobal(GlobalContactsArray);
2664
2665
2666 d.WorldDestroy(world);
2667 //d.CloseODE();
2668 }
2669 }
2670
2671 public override Dictionary<uint, float> GetTopColliders()
2672 {
2673 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
2674 int cnt = 0;
2675 lock (_prims)
2676 {
2677 foreach (OdePrim prm in _prims)
2678 {
2679 if (prm.CollisionScore > 0)
2680 {
2681 returncolliders.Add(prm.m_localID, prm.CollisionScore);
2682 cnt++;
2683 prm.CollisionScore = 0f;
2684 if (cnt > 25)
2685 {
2686 break;
2687 }
2688 }
2689 }
2690 }
2691 return returncolliders;
2692 }
2693
2694 public override bool SupportsRayCast()
2695 {
2696 return true;
2697 }
2698
2699 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2700 {
2701 if (retMethod != null)
2702 {
2703 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
2704 }
2705 }
2706
2707 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2708 {
2709 if (retMethod != null)
2710 {
2711 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2712 }
2713 }
2714
2715 // don't like this
2716 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
2717 {
2718 ContactResult[] ourResults = null;
2719 RayCallback retMethod = delegate(List<ContactResult> results)
2720 {
2721 ourResults = new ContactResult[results.Count];
2722 results.CopyTo(ourResults, 0);
2723 };
2724 int waitTime = 0;
2725 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2726 while (ourResults == null && waitTime < 1000)
2727 {
2728 Thread.Sleep(1);
2729 waitTime++;
2730 }
2731 if (ourResults == null)
2732 return new List<ContactResult>();
2733 return new List<ContactResult>(ourResults);
2734 }
2735
2736 public override bool SuportsRaycastWorldFiltered()
2737 {
2738 return true;
2739 }
2740
2741 public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
2742 {
2743 object SyncObject = new object();
2744 List<ContactResult> ourresults = new List<ContactResult>();
2745
2746 RayCallback retMethod = delegate(List<ContactResult> results)
2747 {
2748 lock (SyncObject)
2749 {
2750 ourresults = results;
2751 Monitor.PulseAll(SyncObject);
2752 }
2753 };
2754
2755 lock (SyncObject)
2756 {
2757 m_rayCastManager.QueueRequest(position, direction, length, Count,filter, retMethod);
2758 if (!Monitor.Wait(SyncObject, 500))
2759 return null;
2760 else
2761 return ourresults;
2762 }
2763 }
2764
2765 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
2766 {
2767 if (retMethod != null && actor !=null)
2768 {
2769 IntPtr geom;
2770 if (actor is OdePrim)
2771 geom = ((OdePrim)actor).prim_geom;
2772 else if (actor is OdeCharacter)
2773 geom = ((OdePrim)actor).prim_geom;
2774 else
2775 return;
2776 if (geom == IntPtr.Zero)
2777 return;
2778 m_rayCastManager.QueueRequest(geom, position, direction, length, retMethod);
2779 }
2780 }
2781
2782 public override void RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
2783 {
2784 if (retMethod != null && actor != null)
2785 {
2786 IntPtr geom;
2787 if (actor is OdePrim)
2788 geom = ((OdePrim)actor).prim_geom;
2789 else if (actor is OdeCharacter)
2790 geom = ((OdePrim)actor).prim_geom;
2791 else
2792 return;
2793 if (geom == IntPtr.Zero)
2794 return;
2795
2796 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2797 }
2798 }
2799
2800 // don't like this
2801 public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count)
2802 {
2803 if (actor != null)
2804 {
2805 IntPtr geom;
2806 if (actor is OdePrim)
2807 geom = ((OdePrim)actor).prim_geom;
2808 else if (actor is OdeCharacter)
2809 geom = ((OdePrim)actor).prim_geom;
2810 else
2811 return new List<ContactResult>();
2812 if (geom == IntPtr.Zero)
2813 return new List<ContactResult>();
2814
2815 ContactResult[] ourResults = null;
2816 RayCallback retMethod = delegate(List<ContactResult> results)
2817 {
2818 ourResults = new ContactResult[results.Count];
2819 results.CopyTo(ourResults, 0);
2820 };
2821 int waitTime = 0;
2822 m_rayCastManager.QueueRequest(geom,position, direction, length, Count, retMethod);
2823 while (ourResults == null && waitTime < 1000)
2824 {
2825 Thread.Sleep(1);
2826 waitTime++;
2827 }
2828 if (ourResults == null)
2829 return new List<ContactResult>();
2830 return new List<ContactResult>(ourResults);
2831 }
2832 return new List<ContactResult>();
2833 }
2834 }
2835}
generated by cgit v1.1 at 2024-11-15 03:49:19 +0000