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