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