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