aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/PhysicsModules/Ode/OdeScene.cs')
-rw-r--r--OpenSim/Region/PhysicsModules/Ode/OdeScene.cs3541
1 files changed, 3541 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs b/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
new file mode 100644
index 0000000..d15568e
--- /dev/null
+++ b/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
@@ -0,0 +1,3541 @@
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// changes for varsize regions
29// note that raycasts need to have limited range
30// (even in normal regions)
31// or application thread stack may just blowup
32// see RayCast(ODERayCastRequest req)
33
34using System;
35using System.Collections.Generic;
36using System.Diagnostics;
37using System.IO;
38using System.Linq;
39using System.Reflection;
40using System.Runtime.ExceptionServices;
41using System.Runtime.InteropServices;
42using System.Threading;
43using log4net;
44using Nini.Config;
45using Mono.Addins;
46using OpenMetaverse;
47using OpenSim.Framework;
48using OpenSim.Region.PhysicsModules.SharedBase;
49using OpenSim.Region.Framework.Scenes;
50using OpenSim.Region.Framework.Interfaces;
51
52namespace OpenSim.Region.PhysicsModule.ODE
53{
54 public enum StatusIndicators : int
55 {
56 Generic = 0,
57 Start = 1,
58 End = 2
59 }
60
61 [Flags]
62 public enum CollisionCategories : int
63 {
64 Disabled = 0,
65 Geom = 0x00000001,
66 Body = 0x00000002,
67 Space = 0x00000004,
68 Character = 0x00000008,
69 Land = 0x00000010,
70 Water = 0x00000020,
71 Wind = 0x00000040,
72 Sensor = 0x00000080,
73 Selected = 0x00000100
74 }
75
76 /// <summary>
77 /// Material type for a primitive
78 /// </summary>
79 public enum Material : int
80 {
81 /// <summary></summary>
82 Stone = 0,
83 /// <summary></summary>
84 Metal = 1,
85 /// <summary></summary>
86 Glass = 2,
87 /// <summary></summary>
88 Wood = 3,
89 /// <summary></summary>
90 Flesh = 4,
91 /// <summary></summary>
92 Plastic = 5,
93 /// <summary></summary>
94 Rubber = 6
95 }
96
97 public class OdeScene : PhysicsScene
98 {
99 private readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString());
100
101 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
102
103 /// <summary>
104 /// Provide a sync object so that only one thread calls d.Collide() at a time across all OdeScene instances.
105 /// </summary>
106 /// <remarks>
107 /// With ODE as of r1755 (though also tested on r1860), only one thread can call d.Collide() at a
108 /// time, even where physics objects are in entirely different ODE worlds. This is because generating contacts
109 /// uses a static cache at the ODE level.
110 ///
111 /// Without locking, simulators running multiple regions will eventually crash with a native stack trace similar
112 /// to
113 ///
114 /// mono() [0x489171]
115 /// mono() [0x4d154f]
116 /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f6ded592c60]
117 /// .../opensim/bin/libode-x86_64.so(_ZN6Opcode11OBBCollider8_CollideEPKNS_14AABBNoLeafNodeE+0xd7a) [0x7f6dd822628a]
118 ///
119 /// ODE provides an experimental option to cache in thread local storage but compiling ODE with this option
120 /// causes OpenSimulator to immediately crash with a native stack trace similar to
121 ///
122 /// mono() [0x489171]
123 /// mono() [0x4d154f]
124 /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f03c9849c60]
125 /// .../opensim/bin/libode-x86_64.so(_Z12dCollideCCTLP6dxGeomS0_iP12dContactGeomi+0x92) [0x7f03b44bcf82]
126 /// </remarks>
127 internal static Object UniversalColliderSyncObject = new Object();
128 internal static Object SimulationLock = new Object();
129
130 /// <summary>
131 /// Is stats collecting enabled for this ODE scene?
132 /// </summary>
133 public bool CollectStats { get; set; }
134
135 /// <summary>
136 /// Statistics for this scene.
137 /// </summary>
138 private Dictionary<string, float> m_stats = new Dictionary<string, float>();
139
140 /// <summary>
141 /// Stat name for total number of avatars in this ODE scene.
142 /// </summary>
143 public const string ODETotalAvatarsStatName = "ODETotalAvatars";
144
145 /// <summary>
146 /// Stat name for total number of prims in this ODE scene.
147 /// </summary>
148 public const string ODETotalPrimsStatName = "ODETotalPrims";
149
150 /// <summary>
151 /// Stat name for total number of prims with active physics in this ODE scene.
152 /// </summary>
153 public const string ODEActivePrimsStatName = "ODEActivePrims";
154
155 /// <summary>
156 /// Stat name for the total time spent in ODE frame processing.
157 /// </summary>
158 /// <remarks>
159 /// A sanity check for the main scene loop physics time.
160 /// </remarks>
161 public const string ODETotalFrameMsStatName = "ODETotalFrameMS";
162
163 /// <summary>
164 /// Stat name for time spent processing avatar taints per frame
165 /// </summary>
166 public const string ODEAvatarTaintMsStatName = "ODEAvatarTaintFrameMS";
167
168 /// <summary>
169 /// Stat name for time spent processing prim taints per frame
170 /// </summary>
171 public const string ODEPrimTaintMsStatName = "ODEPrimTaintFrameMS";
172
173 /// <summary>
174 /// Stat name for time spent calculating avatar forces per frame.
175 /// </summary>
176 public const string ODEAvatarForcesFrameMsStatName = "ODEAvatarForcesFrameMS";
177
178 /// <summary>
179 /// Stat name for time spent calculating prim forces per frame
180 /// </summary>
181 public const string ODEPrimForcesFrameMsStatName = "ODEPrimForcesFrameMS";
182
183 /// <summary>
184 /// Stat name for time spent fulfilling raycasting requests per frame
185 /// </summary>
186 public const string ODERaycastingFrameMsStatName = "ODERaycastingFrameMS";
187
188 /// <summary>
189 /// Stat name for time spent in native code that actually steps through the simulation.
190 /// </summary>
191 public const string ODENativeStepFrameMsStatName = "ODENativeStepFrameMS";
192
193 /// <summary>
194 /// Stat name for the number of milliseconds that ODE spends in native space collision code.
195 /// </summary>
196 public const string ODENativeSpaceCollisionFrameMsStatName = "ODENativeSpaceCollisionFrameMS";
197
198 /// <summary>
199 /// Stat name for milliseconds that ODE spends in native geom collision code.
200 /// </summary>
201 public const string ODENativeGeomCollisionFrameMsStatName = "ODENativeGeomCollisionFrameMS";
202
203 /// <summary>
204 /// Time spent in collision processing that is not spent in native space or geom collision code.
205 /// </summary>
206 public const string ODEOtherCollisionFrameMsStatName = "ODEOtherCollisionFrameMS";
207
208 /// <summary>
209 /// Stat name for time spent notifying listeners of collisions
210 /// </summary>
211 public const string ODECollisionNotificationFrameMsStatName = "ODECollisionNotificationFrameMS";
212
213 /// <summary>
214 /// Stat name for milliseconds spent updating avatar position and velocity
215 /// </summary>
216 public const string ODEAvatarUpdateFrameMsStatName = "ODEAvatarUpdateFrameMS";
217
218 /// <summary>
219 /// Stat name for the milliseconds spent updating prim position and velocity
220 /// </summary>
221 public const string ODEPrimUpdateFrameMsStatName = "ODEPrimUpdateFrameMS";
222
223 /// <summary>
224 /// Stat name for avatar collisions with another entity.
225 /// </summary>
226 public const string ODEAvatarContactsStatsName = "ODEAvatarContacts";
227
228 /// <summary>
229 /// Stat name for prim collisions with another entity.
230 /// </summary>
231 public const string ODEPrimContactsStatName = "ODEPrimContacts";
232
233 /// <summary>
234 /// Used to hold tick numbers for stat collection purposes.
235 /// </summary>
236 private int m_nativeCollisionStartTick;
237
238 /// <summary>
239 /// A messy way to tell if we need to avoid adding a collision time because this was already done in the callback.
240 /// </summary>
241 private bool m_inCollisionTiming;
242
243 /// <summary>
244 /// A temporary holder for the number of avatar collisions in a frame, so we can work out how many object
245 /// collisions occured using the _perloopcontact if stats collection is enabled.
246 /// </summary>
247 private int m_tempAvatarCollisionsThisFrame;
248
249 /// <summary>
250 /// Used in calculating physics frame time dilation
251 /// </summary>
252 private int tickCountFrameRun;
253
254 /// <summary>
255 /// Used in calculating physics frame time dilation
256 /// </summary>
257 private int latertickcount;
258
259 private Random fluidRandomizer = new Random(Environment.TickCount);
260
261 private uint m_regionWidth = Constants.RegionSize;
262 private uint m_regionHeight = Constants.RegionSize;
263
264 private float ODE_STEPSIZE = 0.0178f;
265 private float metersInSpace = 29.9f;
266 private float m_timeDilation = 1.0f;
267
268 public float gravityx = 0f;
269 public float gravityy = 0f;
270 public float gravityz = -9.8f;
271
272 public float AvatarTerminalVelocity { get; set; }
273
274 private float contactsurfacelayer = 0.001f;
275
276 private int HashspaceLow = -5;
277 private int HashspaceHigh = 12;
278
279 private float waterlevel = 0f;
280 private int framecount = 0;
281 //private int m_returncollisions = 10;
282
283 private IntPtr contactgroup;
284
285// internal IntPtr WaterGeom;
286
287 private float nmTerrainContactFriction = 255.0f;
288 private float nmTerrainContactBounce = 0.1f;
289 private float nmTerrainContactERP = 0.1025f;
290
291 private float mTerrainContactFriction = 75f;
292 private float mTerrainContactBounce = 0.1f;
293 private float mTerrainContactERP = 0.05025f;
294
295 private float nmAvatarObjectContactFriction = 250f;
296 private float nmAvatarObjectContactBounce = 0.1f;
297
298 private float mAvatarObjectContactFriction = 75f;
299 private float mAvatarObjectContactBounce = 0.1f;
300
301 private float avPIDD = 3200f;
302 private float avPIDP = 1400f;
303 private float avCapRadius = 0.37f;
304 private float avStandupTensor = 2000000f;
305
306 /// <summary>
307 /// true = old compatibility mode with leaning capsule; false = new corrected mode
308 /// </summary>
309 /// <remarks>
310 /// Even when set to false, the capsule still tilts but this is done in a different way.
311 /// </remarks>
312 public bool IsAvCapsuleTilted { get; private set; }
313
314 private float avDensity = 80f;
315 private float avMovementDivisorWalk = 1.3f;
316 private float avMovementDivisorRun = 0.8f;
317 private float minimumGroundFlightOffset = 3f;
318 public float maximumMassObject = 10000.01f;
319
320 public bool meshSculptedPrim = true;
321 public bool forceSimplePrimMeshing = false;
322
323 public float meshSculptLOD = 32;
324 public float MeshSculptphysicalLOD = 16;
325
326 public float geomDefaultDensity = 10.000006836f;
327
328 public int geomContactPointsStartthrottle = 3;
329 public int geomUpdatesPerThrottledUpdate = 15;
330 private const int avatarExpectedContacts = 3;
331
332 public float bodyPIDD = 35f;
333 public float bodyPIDG = 25;
334
335 public int bodyFramesAutoDisable = 20;
336
337 private bool m_filterCollisions = true;
338
339 private d.NearCallback nearCallback;
340 public d.TriCallback triCallback;
341 public d.TriArrayCallback triArrayCallback;
342
343 /// <summary>
344 /// Avatars in the physics scene.
345 /// </summary>
346 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
347
348 /// <summary>
349 /// Prims in the physics scene.
350 /// </summary>
351 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
352
353 /// <summary>
354 /// Prims in the physics scene that are subject to physics, not just collisions.
355 /// </summary>
356 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
357
358 /// <summary>
359 /// Prims that the simulator has created/deleted/updated and so need updating in ODE.
360 /// </summary>
361 private readonly HashSet<OdePrim> _taintedPrims = new HashSet<OdePrim>();
362
363 /// <summary>
364 /// Record a character that has taints to be processed.
365 /// </summary>
366 private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>();
367
368 /// <summary>
369 /// Keep record of contacts in the physics loop so that we can remove duplicates.
370 /// </summary>
371 private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>();
372
373 /// <summary>
374 /// A dictionary of actors that should receive collision events.
375 /// </summary>
376 private readonly Dictionary<uint, PhysicsActor> m_collisionEventActors = new Dictionary<uint, PhysicsActor>();
377
378 /// <summary>
379 /// A dictionary of collision event changes that are waiting to be processed.
380 /// </summary>
381 private readonly Dictionary<uint, PhysicsActor> m_collisionEventActorsChanges = new Dictionary<uint, PhysicsActor>();
382
383 /// <summary>
384 /// Maps a unique geometry id (a memory location) to a physics actor name.
385 /// </summary>
386 /// <remarks>
387 /// Only actors participating in collisions have geometries. This has to be maintained separately from
388 /// actor_name_map because terrain and water currently don't conceptually have a physics actor of their own
389 /// apart from the singleton PANull
390 /// </remarks>
391 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
392
393 /// <summary>
394 /// Maps a unique geometry id (a memory location) to a physics actor.
395 /// </summary>
396 /// <remarks>
397 /// Only actors participating in collisions have geometries.
398 /// </remarks>
399 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
400
401 /// <summary>
402 /// Defects list to remove characters that no longer have finite positions due to some other bug.
403 /// </summary>
404 /// <remarks>
405 /// Used repeatedly in Simulate() but initialized once here.
406 /// </remarks>
407 private readonly List<OdeCharacter> defects = new List<OdeCharacter>();
408
409 private bool m_NINJA_physics_joints_enabled = false;
410 //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>();
411 private readonly Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>();
412 private d.ContactGeom[] contacts;
413
414 /// <summary>
415 /// Lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
416 /// </summary>
417 private readonly List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>();
418
419 /// <summary>
420 /// can lock for longer. accessed only by OdeScene.
421 /// </summary>
422 private readonly List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>();
423
424 /// <summary>
425 /// can lock for longer. accessed only by OdeScene.
426 /// </summary>
427 private readonly List<PhysicsJoint> activeJoints = new List<PhysicsJoint>();
428
429 /// <summary>
430 /// lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active
431 /// </summary>
432 private readonly List<string> requestedJointsToBeDeleted = new List<string>();
433
434 private Object externalJointRequestsLock = new Object();
435 private readonly Dictionary<String, PhysicsJoint> SOPName_to_activeJoint = new Dictionary<String, PhysicsJoint>();
436 private readonly Dictionary<String, PhysicsJoint> SOPName_to_pendingJoint = new Dictionary<String, PhysicsJoint>();
437 private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
438 private readonly Dictionary<IntPtr,float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
439
440 private d.Contact contact;
441 private d.Contact TerrainContact;
442 private d.Contact AvatarMovementprimContact;
443 private d.Contact AvatarMovementTerrainContact;
444 private d.Contact WaterContact;
445 private d.Contact[,] m_materialContacts;
446
447 private int m_physicsiterations = 10;
448 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
449 private readonly PhysicsActor PANull = new NullPhysicsActor();
450 private float step_time = 0.0f;
451 public IntPtr world;
452 private uint obj2LocalID = 0;
453 private OdeCharacter cc1;
454 private OdePrim cp1;
455 private OdeCharacter cc2;
456 private OdePrim cp2;
457 private int p1ExpectedPoints = 0;
458 private int p2ExpectedPoints = 0;
459
460 public IntPtr space;
461
462 // split static geometry collision handling into spaces of 30 meters
463 public IntPtr[,] staticPrimspace;
464
465 /// <summary>
466 /// Used to lock the entire physics scene. Locked during the main part of Simulate()
467 /// </summary>
468 internal Object OdeLock = new Object();
469
470 private bool _worldInitialized = false;
471
472 public IMesher mesher;
473
474 private IConfigSource m_config;
475
476 public bool physics_logging = false;
477 public int physics_logging_interval = 0;
478 public bool physics_logging_append_existing_logfile = false;
479
480 private bool avplanted = false;
481 private bool av_av_collisions_off = false;
482
483 public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f);
484 public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
485
486 private volatile int m_global_contactcount = 0;
487
488 private Vector3 m_worldOffset = Vector3.Zero;
489 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
490 private PhysicsScene m_parentScene = null;
491
492 float spacesPerMeterX;
493 float spacesPerMeterY;
494 int spaceGridMaxX;
495 int spaceGridMaxY;
496
497 private ODERayCastRequestManager m_rayCastManager;
498
499 public Scene m_frameWorkScene = null;
500
501 public OdeScene(Scene pscene, IConfigSource psourceconfig, string pname, string pversion)
502 {
503 m_config = psourceconfig;
504 m_frameWorkScene = pscene;
505
506 EngineType = pname;
507 PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName;
508 EngineName = pname + " " + pversion;
509
510 pscene.RegisterModuleInterface<PhysicsScene>(this);
511 Vector3 extent = new Vector3(pscene.RegionInfo.RegionSizeX, pscene.RegionInfo.RegionSizeY, pscene.RegionInfo.RegionSizeZ);
512 Initialise(extent);
513 InitialiseFromConfig(m_config);
514
515 // This may not be that good since terrain may not be avaiable at this point
516 base.Initialise(pscene.PhysicsRequestAsset,
517 (pscene.Heightmap != null ? pscene.Heightmap.GetFloatsSerialised() : new float[(int)(extent.X * extent.Y)]),
518 (float)pscene.RegionInfo.RegionSettings.WaterHeight);
519
520 }
521
522 public void RegionLoaded()
523 {
524 mesher = m_frameWorkScene.RequestModuleInterface<IMesher>();
525 if (mesher == null)
526 m_log.WarnFormat("[ODE SCENE]: No mesher in {0}. Things will not work well.", PhysicsSceneName);
527
528 m_frameWorkScene.PhysicsEnabled = true;
529 }
530
531 /// <summary>
532 /// Initiailizes the scene
533 /// Sets many properties that ODE requires to be stable
534 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
535 /// </summary>
536 private void Initialise(Vector3 regionExtent)
537 {
538 WorldExtents.X = regionExtent.X;
539 m_regionWidth = (uint)regionExtent.X;
540 WorldExtents.Y = regionExtent.Y;
541 m_regionHeight = (uint)regionExtent.Y;
542
543 nearCallback = near;
544 m_rayCastManager = new ODERayCastRequestManager(this);
545
546 // Create the world and the first space
547 world = d.WorldCreate();
548 space = d.HashSpaceCreate(IntPtr.Zero);
549
550 contactgroup = d.JointGroupCreate(0);
551
552 d.WorldSetAutoDisableFlag(world, false);
553 }
554
555 // Initialize from configs
556 private void InitialiseFromConfig(IConfigSource config)
557 {
558 InitializeExtraStats();
559
560 m_config = config;
561 // Defaults
562
563 avPIDD = 2200.0f;
564 avPIDP = 900.0f;
565 avStandupTensor = 550000f;
566
567 int contactsPerCollision = 80;
568
569 if (m_config != null)
570 {
571 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
572 if (physicsconfig != null)
573 {
574 CollectStats = physicsconfig.GetBoolean("collect_stats", false);
575
576 gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
577 gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
578 gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
579
580 float avatarTerminalVelocity = physicsconfig.GetFloat("avatar_terminal_velocity", 54f);
581 AvatarTerminalVelocity = Util.Clamp<float>(avatarTerminalVelocity, 0, 255f);
582 if (AvatarTerminalVelocity != avatarTerminalVelocity)
583 {
584 m_log.WarnFormat(
585 "[ODE SCENE]: avatar_terminal_velocity of {0} is invalid. Clamping to {1}",
586 avatarTerminalVelocity, AvatarTerminalVelocity);
587 }
588
589 HashspaceLow = physicsconfig.GetInt("world_hashspace_level_low", -5);
590 HashspaceHigh = physicsconfig.GetInt("world_hashspace_level_high", 12);
591
592 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f);
593
594 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f);
595
596 nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f);
597 nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f);
598 nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f);
599
600 mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f);
601 mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.05f);
602 mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f);
603
604 nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f);
605 nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f);
606
607 mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f);
608 mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f);
609
610 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
611 m_physicsiterations = physicsconfig.GetInt("world_solver_iterations", 10);
612
613 avDensity = physicsconfig.GetFloat("av_density", 80f);
614// avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f);
615 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f);
616 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f);
617 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
618 avplanted = physicsconfig.GetBoolean("av_planted", false);
619 av_av_collisions_off = physicsconfig.GetBoolean("av_av_collisions_off", false);
620
621 IsAvCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false);
622
623 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
624
625 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5);
626 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
627
628 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
629 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
630
631 bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
632 bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
633
634 forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
635 meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
636 meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
637 MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
638 m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
639
640 avPIDD = physicsconfig.GetFloat("av_pid_derivative", 2200.0f);
641 avPIDP = physicsconfig.GetFloat("av_pid_proportional", 900.0f);
642 avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor", 550000f);
643
644 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
645 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
646 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
647
648// m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
649 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f);
650 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
651 }
652 }
653
654 contacts = new d.ContactGeom[contactsPerCollision];
655
656 spacesPerMeterX = 1.0f / metersInSpace;
657 spacesPerMeterY = 1.0f / metersInSpace;
658
659 spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX);
660 spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY);
661
662 // note: limit number of spaces
663 if (spaceGridMaxX > 24)
664 {
665 spaceGridMaxX = 24;
666 spacesPerMeterX = spaceGridMaxX / WorldExtents.X;
667 }
668 if (spaceGridMaxY > 24)
669 {
670 spaceGridMaxY = 24;
671 spacesPerMeterY = spaceGridMaxY / WorldExtents.Y;
672 }
673
674 staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
675
676 // make this index limits
677 spaceGridMaxX--;
678 spaceGridMaxY--;
679
680 // Centeral contact friction and bounce
681 // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why
682 // an avatar falls through in Z but not in X or Y when walking on a prim.
683 contact.surface.mode |= d.ContactFlags.SoftERP;
684 contact.surface.mu = nmAvatarObjectContactFriction;
685 contact.surface.bounce = nmAvatarObjectContactBounce;
686 contact.surface.soft_cfm = 0.010f;
687 contact.surface.soft_erp = 0.010f;
688
689 // Terrain contact friction and Bounce
690 // This is the *non* moving version. Use this when an avatar
691 // isn't moving to keep it in place better
692 TerrainContact.surface.mode |= d.ContactFlags.SoftERP;
693 TerrainContact.surface.mu = nmTerrainContactFriction;
694 TerrainContact.surface.bounce = nmTerrainContactBounce;
695 TerrainContact.surface.soft_erp = nmTerrainContactERP;
696
697 WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM);
698 WaterContact.surface.mu = 0f; // No friction
699 WaterContact.surface.bounce = 0.0f; // No bounce
700 WaterContact.surface.soft_cfm = 0.010f;
701 WaterContact.surface.soft_erp = 0.010f;
702
703 // Prim contact friction and bounce
704 // THis is the *non* moving version of friction and bounce
705 // Use this when an avatar comes in contact with a prim
706 // and is moving
707 AvatarMovementprimContact.surface.mu = mAvatarObjectContactFriction;
708 AvatarMovementprimContact.surface.bounce = mAvatarObjectContactBounce;
709
710 // Terrain contact friction bounce and various error correcting calculations
711 // Use this when an avatar is in contact with the terrain and moving.
712 AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP;
713 AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction;
714 AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce;
715 AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP;
716
717 /*
718 <summary></summary>
719 Stone = 0,
720 /// <summary></summary>
721 Metal = 1,
722 /// <summary></summary>
723 Glass = 2,
724 /// <summary></summary>
725 Wood = 3,
726 /// <summary></summary>
727 Flesh = 4,
728 /// <summary></summary>
729 Plastic = 5,
730 /// <summary></summary>
731 Rubber = 6
732 */
733
734 m_materialContacts = new d.Contact[7,2];
735
736 m_materialContacts[(int)Material.Stone, 0] = new d.Contact();
737 m_materialContacts[(int)Material.Stone, 0].surface.mode |= d.ContactFlags.SoftERP;
738 m_materialContacts[(int)Material.Stone, 0].surface.mu = nmAvatarObjectContactFriction;
739 m_materialContacts[(int)Material.Stone, 0].surface.bounce = nmAvatarObjectContactBounce;
740 m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.010f;
741 m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.010f;
742
743 m_materialContacts[(int)Material.Stone, 1] = new d.Contact();
744 m_materialContacts[(int)Material.Stone, 1].surface.mode |= d.ContactFlags.SoftERP;
745 m_materialContacts[(int)Material.Stone, 1].surface.mu = mAvatarObjectContactFriction;
746 m_materialContacts[(int)Material.Stone, 1].surface.bounce = mAvatarObjectContactBounce;
747 m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.010f;
748 m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.010f;
749
750 m_materialContacts[(int)Material.Metal, 0] = new d.Contact();
751 m_materialContacts[(int)Material.Metal, 0].surface.mode |= d.ContactFlags.SoftERP;
752 m_materialContacts[(int)Material.Metal, 0].surface.mu = nmAvatarObjectContactFriction;
753 m_materialContacts[(int)Material.Metal, 0].surface.bounce = nmAvatarObjectContactBounce;
754 m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.010f;
755 m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.010f;
756
757 m_materialContacts[(int)Material.Metal, 1] = new d.Contact();
758 m_materialContacts[(int)Material.Metal, 1].surface.mode |= d.ContactFlags.SoftERP;
759 m_materialContacts[(int)Material.Metal, 1].surface.mu = mAvatarObjectContactFriction;
760 m_materialContacts[(int)Material.Metal, 1].surface.bounce = mAvatarObjectContactBounce;
761 m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.010f;
762 m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.010f;
763
764 m_materialContacts[(int)Material.Glass, 0] = new d.Contact();
765 m_materialContacts[(int)Material.Glass, 0].surface.mode |= d.ContactFlags.SoftERP;
766 m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f;
767 m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.5f;
768 m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.010f;
769 m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.010f;
770
771 /*
772 private float nmAvatarObjectContactFriction = 250f;
773 private float nmAvatarObjectContactBounce = 0.1f;
774
775 private float mAvatarObjectContactFriction = 75f;
776 private float mAvatarObjectContactBounce = 0.1f;
777 */
778 m_materialContacts[(int)Material.Glass, 1] = new d.Contact();
779 m_materialContacts[(int)Material.Glass, 1].surface.mode |= d.ContactFlags.SoftERP;
780 m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f;
781 m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.5f;
782 m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.010f;
783 m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.010f;
784
785 m_materialContacts[(int)Material.Wood, 0] = new d.Contact();
786 m_materialContacts[(int)Material.Wood, 0].surface.mode |= d.ContactFlags.SoftERP;
787 m_materialContacts[(int)Material.Wood, 0].surface.mu = nmAvatarObjectContactFriction;
788 m_materialContacts[(int)Material.Wood, 0].surface.bounce = nmAvatarObjectContactBounce;
789 m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.010f;
790 m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.010f;
791
792 m_materialContacts[(int)Material.Wood, 1] = new d.Contact();
793 m_materialContacts[(int)Material.Wood, 1].surface.mode |= d.ContactFlags.SoftERP;
794 m_materialContacts[(int)Material.Wood, 1].surface.mu = mAvatarObjectContactFriction;
795 m_materialContacts[(int)Material.Wood, 1].surface.bounce = mAvatarObjectContactBounce;
796 m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.010f;
797 m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.010f;
798
799 m_materialContacts[(int)Material.Flesh, 0] = new d.Contact();
800 m_materialContacts[(int)Material.Flesh, 0].surface.mode |= d.ContactFlags.SoftERP;
801 m_materialContacts[(int)Material.Flesh, 0].surface.mu = nmAvatarObjectContactFriction;
802 m_materialContacts[(int)Material.Flesh, 0].surface.bounce = nmAvatarObjectContactBounce;
803 m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.010f;
804 m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.010f;
805
806 m_materialContacts[(int)Material.Flesh, 1] = new d.Contact();
807 m_materialContacts[(int)Material.Flesh, 1].surface.mode |= d.ContactFlags.SoftERP;
808 m_materialContacts[(int)Material.Flesh, 1].surface.mu = mAvatarObjectContactFriction;
809 m_materialContacts[(int)Material.Flesh, 1].surface.bounce = mAvatarObjectContactBounce;
810 m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.010f;
811 m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.010f;
812
813 m_materialContacts[(int)Material.Plastic, 0] = new d.Contact();
814 m_materialContacts[(int)Material.Plastic, 0].surface.mode |= d.ContactFlags.SoftERP;
815 m_materialContacts[(int)Material.Plastic, 0].surface.mu = nmAvatarObjectContactFriction;
816 m_materialContacts[(int)Material.Plastic, 0].surface.bounce = nmAvatarObjectContactBounce;
817 m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.010f;
818 m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.010f;
819
820 m_materialContacts[(int)Material.Plastic, 1] = new d.Contact();
821 m_materialContacts[(int)Material.Plastic, 1].surface.mode |= d.ContactFlags.SoftERP;
822 m_materialContacts[(int)Material.Plastic, 1].surface.mu = mAvatarObjectContactFriction;
823 m_materialContacts[(int)Material.Plastic, 1].surface.bounce = mAvatarObjectContactBounce;
824 m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.010f;
825 m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.010f;
826
827 m_materialContacts[(int)Material.Rubber, 0] = new d.Contact();
828 m_materialContacts[(int)Material.Rubber, 0].surface.mode |= d.ContactFlags.SoftERP;
829 m_materialContacts[(int)Material.Rubber, 0].surface.mu = nmAvatarObjectContactFriction;
830 m_materialContacts[(int)Material.Rubber, 0].surface.bounce = nmAvatarObjectContactBounce;
831 m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.010f;
832 m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.010f;
833
834 m_materialContacts[(int)Material.Rubber, 1] = new d.Contact();
835 m_materialContacts[(int)Material.Rubber, 1].surface.mode |= d.ContactFlags.SoftERP;
836 m_materialContacts[(int)Material.Rubber, 1].surface.mu = mAvatarObjectContactFriction;
837 m_materialContacts[(int)Material.Rubber, 1].surface.bounce = mAvatarObjectContactBounce;
838 m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f;
839 m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f;
840
841 d.HashSpaceSetLevels(space, HashspaceLow, HashspaceHigh);
842
843 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
844
845 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
846 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
847
848 d.WorldSetLinearDamping(world, 256f);
849 d.WorldSetAngularDamping(world, 256f);
850 d.WorldSetAngularDampingThreshold(world, 256f);
851 d.WorldSetLinearDampingThreshold(world, 256f);
852 d.WorldSetMaxAngularSpeed(world, 256f);
853
854 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
855 //d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
856
857 for (int i = 0; i < staticPrimspace.GetLength(0); i++)
858 {
859 for (int j = 0; j < staticPrimspace.GetLength(1); j++)
860 {
861 staticPrimspace[i, j] = IntPtr.Zero;
862 }
863 }
864
865 _worldInitialized = true;
866 }
867
868 #region Collision Detection
869
870 /// <summary>
871 /// Collides two geometries.
872 /// </summary>
873 /// <returns></returns>
874 /// <param name='geom1'></param>
875 /// <param name='geom2'>/param>
876 /// <param name='maxContacts'></param>
877 /// <param name='contactsArray'></param>
878 /// <param name='contactGeomSize'></param>
879 private int CollideGeoms(
880 IntPtr geom1, IntPtr geom2, int maxContacts, d.ContactGeom[] contactsArray, int contactGeomSize)
881 {
882 int count;
883
884 lock (OdeScene.UniversalColliderSyncObject)
885 {
886 // We do this inside the lock so that we don't count any delay in acquiring it
887 if (CollectStats)
888 m_nativeCollisionStartTick = Util.EnvironmentTickCount();
889
890 count = d.Collide(geom1, geom2, maxContacts, contactsArray, contactGeomSize);
891 }
892
893 // We do this outside the lock so that any waiting threads aren't held up, though the effect is probably
894 // negligable
895 if (CollectStats)
896 m_stats[ODENativeGeomCollisionFrameMsStatName]
897 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
898
899 return count;
900 }
901
902 /// <summary>
903 /// Collide two spaces or a space and a geometry.
904 /// </summary>
905 /// <param name='space1'></param>
906 /// <param name='space2'>/param>
907 /// <param name='data'></param>
908 private void CollideSpaces(IntPtr space1, IntPtr space2, IntPtr data)
909 {
910 if (CollectStats)
911 {
912 m_inCollisionTiming = true;
913 m_nativeCollisionStartTick = Util.EnvironmentTickCount();
914 }
915
916 d.SpaceCollide2(space1, space2, data, nearCallback);
917
918 if (CollectStats && m_inCollisionTiming)
919 {
920 m_stats[ODENativeSpaceCollisionFrameMsStatName]
921 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
922 m_inCollisionTiming = false;
923 }
924 }
925
926 /// <summary>
927 /// This is our near callback. A geometry is near a body
928 /// </summary>
929 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
930 /// <param name="g1">a geometry or space</param>
931 /// <param name="g2">another geometry or space</param>
932 private void near(IntPtr space, IntPtr g1, IntPtr g2)
933 {
934 if (CollectStats && m_inCollisionTiming)
935 {
936 m_stats[ODENativeSpaceCollisionFrameMsStatName]
937 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
938 m_inCollisionTiming = false;
939 }
940
941// m_log.DebugFormat("[PHYSICS]: Colliding {0} and {1} in {2}", g1, g2, space);
942 // no lock here! It's invoked from within Simulate(), which is thread-locked
943
944 // Test if we're colliding a geom with a space.
945 // If so we have to drill down into the space recursively
946
947 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
948 {
949 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
950 return;
951
952 // Separating static prim geometry spaces.
953 // We'll be calling near recursivly if one
954 // of them is a space to find all of the
955 // contact points in the space
956 try
957 {
958 CollideSpaces(g1, g2, IntPtr.Zero);
959 }
960 catch (AccessViolationException)
961 {
962 m_log.Error("[ODE SCENE]: Unable to collide test a space");
963 return;
964 }
965 //Colliding a space or a geom with a space or a geom. so drill down
966
967 //Collide all geoms in each space..
968 //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
969 //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
970 return;
971 }
972
973 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
974 return;
975
976 IntPtr b1 = d.GeomGetBody(g1);
977 IntPtr b2 = d.GeomGetBody(g2);
978
979 // d.GeomClassID id = d.GeomGetClass(g1);
980
981 String name1 = null;
982 String name2 = null;
983
984 if (!geom_name_map.TryGetValue(g1, out name1))
985 {
986 name1 = "null";
987 }
988 if (!geom_name_map.TryGetValue(g2, out name2))
989 {
990 name2 = "null";
991 }
992
993 // Figure out how many contact points we have
994 int count = 0;
995
996 try
997 {
998 // Colliding Geom To Geom
999 // This portion of the function 'was' blatantly ripped off from BoxStack.cs
1000
1001 if (g1 == g2)
1002 return; // Can't collide with yourself
1003
1004 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
1005 return;
1006
1007 count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.unmanagedSizeOf);
1008
1009 // All code after this is only relevant if we have any collisions
1010 if (count <= 0)
1011 return;
1012
1013 if (count > contacts.Length)
1014 m_log.Error("[ODE SCENE]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length);
1015 }
1016 catch (SEHException)
1017 {
1018 m_log.Error(
1019 "[ODE SCENE]: 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.");
1020 base.TriggerPhysicsBasedRestart();
1021 }
1022 catch (Exception e)
1023 {
1024 m_log.ErrorFormat("[ODE SCENE]: Unable to collide test an object: {0}", e.Message);
1025 return;
1026 }
1027
1028 PhysicsActor p1;
1029 PhysicsActor p2;
1030
1031 p1ExpectedPoints = 0;
1032 p2ExpectedPoints = 0;
1033
1034 if (!actor_name_map.TryGetValue(g1, out p1))
1035 {
1036 p1 = PANull;
1037 }
1038
1039 if (!actor_name_map.TryGetValue(g2, out p2))
1040 {
1041 p2 = PANull;
1042 }
1043
1044 ContactPoint maxDepthContact = new ContactPoint();
1045 if (p1.CollisionScore + count >= float.MaxValue)
1046 p1.CollisionScore = 0;
1047 p1.CollisionScore += count;
1048
1049 if (p2.CollisionScore + count >= float.MaxValue)
1050 p2.CollisionScore = 0;
1051 p2.CollisionScore += count;
1052
1053 for (int i = 0; i < count; i++)
1054 {
1055 d.ContactGeom curContact = contacts[i];
1056
1057 if (curContact.depth > maxDepthContact.PenetrationDepth)
1058 {
1059 maxDepthContact = new ContactPoint(
1060 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
1061 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
1062 curContact.depth
1063 );
1064 }
1065
1066 //m_log.Warn("[CCOUNT]: " + count);
1067 IntPtr joint;
1068 // If we're colliding with terrain, use 'TerrainContact' instead of contact.
1069 // allows us to have different settings
1070
1071 // We only need to test p2 for 'jump crouch purposes'
1072 if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim)
1073 {
1074 // Testing if the collision is at the feet of the avatar
1075
1076 //m_log.DebugFormat("[PHYSICS]: {0} - {1} - {2} - {3}", curContact.pos.Z, p2.Position.Z, (p2.Position.Z - curContact.pos.Z), (p2.Size.Z * 0.6f));
1077 if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f))
1078 p2.IsColliding = true;
1079 }
1080 else
1081 {
1082 p2.IsColliding = true;
1083 }
1084
1085 //if ((framecount % m_returncollisions) == 0)
1086
1087 switch (p1.PhysicsActorType)
1088 {
1089 case (int)ActorTypes.Agent:
1090 p1ExpectedPoints = avatarExpectedContacts;
1091 p2.CollidingObj = true;
1092 break;
1093 case (int)ActorTypes.Prim:
1094 if (p1 != null && p1 is OdePrim)
1095 p1ExpectedPoints = ((OdePrim) p1).ExpectedCollisionContacts;
1096
1097 if (p2.Velocity.LengthSquared() > 0.0f)
1098 p2.CollidingObj = true;
1099 break;
1100 case (int)ActorTypes.Unknown:
1101 p2.CollidingGround = true;
1102 break;
1103 default:
1104 p2.CollidingGround = true;
1105 break;
1106 }
1107
1108 // we don't want prim or avatar to explode
1109
1110 #region InterPenetration Handling - Unintended physics explosions
1111
1112 if (curContact.depth >= 0.08f)
1113 {
1114 if (curContact.depth >= 1.00f)
1115 {
1116 //m_log.Info("[P]: " + contact.depth.ToString());
1117 if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
1118 p1.PhysicsActorType == (int) ActorTypes.Unknown) ||
1119 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
1120 p2.PhysicsActorType == (int) ActorTypes.Unknown))
1121 {
1122 if (p2.PhysicsActorType == (int) ActorTypes.Agent)
1123 {
1124 if (p2 is OdeCharacter)
1125 {
1126 OdeCharacter character = (OdeCharacter) p2;
1127
1128 //p2.CollidingObj = true;
1129 curContact.depth = 0.00000003f;
1130 p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f);
1131 curContact.pos =
1132 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
1133 curContact.pos.Y + (p1.Size.Y/2),
1134 curContact.pos.Z + (p1.Size.Z/2));
1135 character.SetPidStatus(true);
1136 }
1137 }
1138
1139 if (p1.PhysicsActorType == (int) ActorTypes.Agent)
1140 {
1141 if (p1 is OdeCharacter)
1142 {
1143 OdeCharacter character = (OdeCharacter) p1;
1144
1145 //p2.CollidingObj = true;
1146 curContact.depth = 0.00000003f;
1147 p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f);
1148 curContact.pos =
1149 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
1150 curContact.pos.Y + (p1.Size.Y/2),
1151 curContact.pos.Z + (p1.Size.Z/2));
1152 character.SetPidStatus(true);
1153 }
1154 }
1155 }
1156 }
1157 }
1158
1159 #endregion
1160
1161 // Logic for collision handling
1162 // Note, that if *all* contacts are skipped (VolumeDetect)
1163 // The prim still detects (and forwards) collision events but
1164 // appears to be phantom for the world
1165 Boolean skipThisContact = false;
1166
1167 if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect))
1168 skipThisContact = true; // No collision on volume detect prims
1169
1170 if (av_av_collisions_off)
1171 if ((p1 is OdeCharacter) && (p2 is OdeCharacter))
1172 skipThisContact = true;
1173
1174 if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
1175 skipThisContact = true; // No collision on volume detect prims
1176
1177 if (!skipThisContact && curContact.depth < 0f)
1178 skipThisContact = true;
1179
1180 if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType))
1181 skipThisContact = true;
1182
1183 const int maxContactsbeforedeath = 4000;
1184 joint = IntPtr.Zero;
1185
1186 if (!skipThisContact)
1187 {
1188 _perloopContact.Add(curContact);
1189
1190 if (name1 == "Terrain" || name2 == "Terrain")
1191 {
1192 if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
1193 (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
1194 {
1195 p2ExpectedPoints = avatarExpectedContacts;
1196 // Avatar is moving on terrain, use the movement terrain contact
1197 AvatarMovementTerrainContact.geom = curContact;
1198
1199 if (m_global_contactcount < maxContactsbeforedeath)
1200 {
1201 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact);
1202 m_global_contactcount++;
1203 }
1204 }
1205 else
1206 {
1207 if (p2.PhysicsActorType == (int)ActorTypes.Agent)
1208 {
1209 p2ExpectedPoints = avatarExpectedContacts;
1210 // Avatar is standing on terrain, use the non moving terrain contact
1211 TerrainContact.geom = curContact;
1212
1213 if (m_global_contactcount < maxContactsbeforedeath)
1214 {
1215 joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
1216 m_global_contactcount++;
1217 }
1218 }
1219 else
1220 {
1221 if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim)
1222 {
1223 // prim prim contact
1224 // int pj294950 = 0;
1225 int movintYN = 0;
1226 int material = (int) Material.Wood;
1227 // prim terrain contact
1228 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
1229 {
1230 movintYN = 1;
1231 }
1232
1233 if (p2 is OdePrim)
1234 {
1235 material = ((OdePrim) p2).m_material;
1236 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1237 }
1238
1239 // Unnessesary because p1 is defined above
1240 //if (p1 is OdePrim)
1241 // {
1242 // p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts;
1243 // }
1244 //m_log.DebugFormat("Material: {0}", material);
1245
1246 m_materialContacts[material, movintYN].geom = curContact;
1247
1248 if (m_global_contactcount < maxContactsbeforedeath)
1249 {
1250 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1251 m_global_contactcount++;
1252 }
1253 }
1254 else
1255 {
1256 int movintYN = 0;
1257 // prim terrain contact
1258 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
1259 {
1260 movintYN = 1;
1261 }
1262
1263 int material = (int)Material.Wood;
1264
1265 if (p2 is OdePrim)
1266 {
1267 material = ((OdePrim)p2).m_material;
1268 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1269 }
1270
1271 //m_log.DebugFormat("Material: {0}", material);
1272 m_materialContacts[material, movintYN].geom = curContact;
1273
1274 if (m_global_contactcount < maxContactsbeforedeath)
1275 {
1276 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1277 m_global_contactcount++;
1278 }
1279 }
1280 }
1281 }
1282 //if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1283 //{
1284 //m_log.Debug("[PHYSICS]: prim contacting with ground");
1285 //}
1286 }
1287 else if (name1 == "Water" || name2 == "Water")
1288 {
1289 /*
1290 if ((p2.PhysicsActorType == (int) ActorTypes.Prim))
1291 {
1292 }
1293 else
1294 {
1295 }
1296 */
1297 //WaterContact.surface.soft_cfm = 0.0000f;
1298 //WaterContact.surface.soft_erp = 0.00000f;
1299 if (curContact.depth > 0.1f)
1300 {
1301 curContact.depth *= 52;
1302 //contact.normal = new d.Vector3(0, 0, 1);
1303 //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f);
1304 }
1305
1306 WaterContact.geom = curContact;
1307
1308 if (m_global_contactcount < maxContactsbeforedeath)
1309 {
1310 joint = d.JointCreateContact(world, contactgroup, ref WaterContact);
1311 m_global_contactcount++;
1312 }
1313 //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth);
1314 }
1315 else
1316 {
1317 if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
1318 {
1319 p2ExpectedPoints = avatarExpectedContacts;
1320 if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
1321 {
1322 // Avatar is moving on a prim, use the Movement prim contact
1323 AvatarMovementprimContact.geom = curContact;
1324
1325 if (m_global_contactcount < maxContactsbeforedeath)
1326 {
1327 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact);
1328 m_global_contactcount++;
1329 }
1330 }
1331 else
1332 {
1333 // Avatar is standing still on a prim, use the non movement contact
1334 contact.geom = curContact;
1335
1336 if (m_global_contactcount < maxContactsbeforedeath)
1337 {
1338 joint = d.JointCreateContact(world, contactgroup, ref contact);
1339 m_global_contactcount++;
1340 }
1341 }
1342 }
1343 else if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1344 {
1345 //p1.PhysicsActorType
1346 int material = (int)Material.Wood;
1347
1348 if (p2 is OdePrim)
1349 {
1350 material = ((OdePrim)p2).m_material;
1351 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1352 }
1353
1354 //m_log.DebugFormat("Material: {0}", material);
1355 m_materialContacts[material, 0].geom = curContact;
1356
1357 if (m_global_contactcount < maxContactsbeforedeath)
1358 {
1359 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, 0]);
1360 m_global_contactcount++;
1361 }
1362 }
1363 }
1364
1365 if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide!
1366 {
1367 d.JointAttach(joint, b1, b2);
1368 m_global_contactcount++;
1369 }
1370 }
1371
1372 collision_accounting_events(p1, p2, maxDepthContact);
1373
1374 if (count > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (geomContactPointsStartthrottle))
1375 {
1376 // If there are more then 3 contact points, it's likely
1377 // that we've got a pile of objects, so ...
1378 // We don't want to send out hundreds of terse updates over and over again
1379 // so lets throttle them and send them again after it's somewhat sorted out.
1380 p2.ThrottleUpdates = true;
1381 }
1382 //m_log.Debug(count.ToString());
1383 //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
1384 }
1385 }
1386
1387 private bool checkDupe(d.ContactGeom contactGeom, int atype)
1388 {
1389 if (!m_filterCollisions)
1390 return false;
1391
1392 bool result = false;
1393
1394 ActorTypes at = (ActorTypes)atype;
1395
1396 foreach (d.ContactGeom contact in _perloopContact)
1397 {
1398 //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2))
1399 //{
1400 // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2)
1401 if (at == ActorTypes.Agent)
1402 {
1403 if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f)
1404 && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f)
1405 && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)))
1406 {
1407 if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f)
1408 {
1409 result = true;
1410 break;
1411 }
1412 }
1413 }
1414 else if (at == ActorTypes.Prim)
1415 {
1416 if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)))
1417 {
1418 if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z)
1419 {
1420 if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f)
1421 {
1422 result = true;
1423 break;
1424 }
1425 }
1426 //m_log.DebugFormat("[Collision]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1427 //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
1428 }
1429 }
1430 }
1431
1432 return result;
1433 }
1434
1435 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
1436 {
1437 // obj1LocalID = 0;
1438 //returncollisions = false;
1439 obj2LocalID = 0;
1440 //ctype = 0;
1441 //cStartStop = 0;
1442// if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
1443// return;
1444 bool p1events = p1.SubscribedEvents();
1445 bool p2events = p2.SubscribedEvents();
1446
1447 if (p1.IsVolumeDtc)
1448 p2events = false;
1449 if (p2.IsVolumeDtc)
1450 p1events = false;
1451
1452 if (!p2events && !p1events)
1453 return;
1454
1455 Vector3 vel = Vector3.Zero;
1456 if (p2 != null && p2.IsPhysical)
1457 vel = p2.Velocity;
1458
1459 if (p1 != null && p1.IsPhysical)
1460 vel -= p1.Velocity;
1461
1462 contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
1463
1464 switch ((ActorTypes)p2.PhysicsActorType)
1465 {
1466 case ActorTypes.Agent:
1467 cc2 = (OdeCharacter)p2;
1468
1469 // obj1LocalID = cc2.m_localID;
1470 switch ((ActorTypes)p1.PhysicsActorType)
1471 {
1472 case ActorTypes.Agent:
1473 cc1 = (OdeCharacter)p1;
1474 obj2LocalID = cc1.LocalID;
1475 cc1.AddCollisionEvent(cc2.LocalID, contact);
1476 break;
1477
1478 case ActorTypes.Prim:
1479 if (p1 is OdePrim)
1480 {
1481 cp1 = (OdePrim) p1;
1482 obj2LocalID = cp1.LocalID;
1483 cp1.AddCollisionEvent(cc2.LocalID, contact);
1484 }
1485 break;
1486
1487 case ActorTypes.Ground:
1488 case ActorTypes.Unknown:
1489 obj2LocalID = 0;
1490 break;
1491 }
1492
1493 cc2.AddCollisionEvent(obj2LocalID, contact);
1494 break;
1495
1496 case ActorTypes.Prim:
1497
1498 if (p2 is OdePrim)
1499 {
1500 cp2 = (OdePrim) p2;
1501
1502 // obj1LocalID = cp2.m_localID;
1503 switch ((ActorTypes) p1.PhysicsActorType)
1504 {
1505 case ActorTypes.Agent:
1506 if (p1 is OdeCharacter)
1507 {
1508 cc1 = (OdeCharacter) p1;
1509 obj2LocalID = cc1.LocalID;
1510 cc1.AddCollisionEvent(cp2.LocalID, contact);
1511 }
1512 break;
1513 case ActorTypes.Prim:
1514
1515 if (p1 is OdePrim)
1516 {
1517 cp1 = (OdePrim) p1;
1518 obj2LocalID = cp1.LocalID;
1519 cp1.AddCollisionEvent(cp2.LocalID, contact);
1520 }
1521 break;
1522
1523 case ActorTypes.Ground:
1524 case ActorTypes.Unknown:
1525 obj2LocalID = 0;
1526 break;
1527 }
1528
1529 cp2.AddCollisionEvent(obj2LocalID, contact);
1530 }
1531 break;
1532 }
1533 }
1534 /// <summary>
1535 /// This is our collision testing routine in ODE
1536 /// </summary>
1537 private void collision_optimized()
1538 {
1539 _perloopContact.Clear();
1540
1541 foreach (OdeCharacter chr in _characters)
1542 {
1543 // Reset the collision values to false
1544 // since we don't know if we're colliding yet
1545 if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1546 continue;
1547
1548 chr.IsColliding = false;
1549 chr.CollidingGround = false;
1550 chr.CollidingObj = false;
1551
1552 // Test the avatar's geometry for collision with the space
1553 // This will return near and the space that they are the closest to
1554 // And we'll run this again against the avatar and the space segment
1555 // This will return with a bunch of possible objects in the space segment
1556 // and we'll run it again on all of them.
1557 try
1558 {
1559 CollideSpaces(space, chr.Shell, IntPtr.Zero);
1560 }
1561 catch (AccessViolationException)
1562 {
1563 m_log.ErrorFormat("[ODE SCENE]: Unable to space collide {0}", PhysicsSceneName);
1564 }
1565
1566 //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
1567 //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
1568 //{
1569 //chr.Position.Z = terrainheight + 10.0f;
1570 //forcedZ = true;
1571 //}
1572 }
1573
1574 if (CollectStats)
1575 {
1576 m_tempAvatarCollisionsThisFrame = _perloopContact.Count;
1577 m_stats[ODEAvatarContactsStatsName] += m_tempAvatarCollisionsThisFrame;
1578 }
1579
1580 List<OdePrim> removeprims = null;
1581 foreach (OdePrim chr in _activeprims)
1582 {
1583 if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled))
1584 {
1585 try
1586 {
1587 lock (chr)
1588 {
1589 if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false)
1590 {
1591 CollideSpaces(space, chr.prim_geom, IntPtr.Zero);
1592 }
1593 else
1594 {
1595 if (removeprims == null)
1596 {
1597 removeprims = new List<OdePrim>();
1598 }
1599 removeprims.Add(chr);
1600 m_log.Error(
1601 "[ODE SCENE]: unable to collide test active prim against space. The space was zero, the geom was zero or it was in the process of being removed. Removed it from the active prim list. This needs to be fixed!");
1602 }
1603 }
1604 }
1605 catch (AccessViolationException)
1606 {
1607 m_log.Error("[ODE SCENE]: Unable to space collide");
1608 }
1609 }
1610 }
1611
1612 if (CollectStats)
1613 m_stats[ODEPrimContactsStatName] += _perloopContact.Count - m_tempAvatarCollisionsThisFrame;
1614
1615 if (removeprims != null)
1616 {
1617 foreach (OdePrim chr in removeprims)
1618 {
1619 _activeprims.Remove(chr);
1620 }
1621 }
1622 }
1623
1624 #endregion
1625
1626 // Recovered for use by fly height. Kitto Flora
1627 internal float GetTerrainHeightAtXY(float x, float y)
1628 {
1629 IntPtr heightFieldGeom = IntPtr.Zero;
1630 int offsetX = 0;
1631 int offsetY = 0;
1632
1633 if(RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom))
1634 {
1635 if (heightFieldGeom != IntPtr.Zero)
1636 {
1637 if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
1638 {
1639
1640 int index;
1641
1642
1643 if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y ||
1644 (int)x < 0.001f || (int)y < 0.001f)
1645 return 0;
1646
1647 x = x - offsetX + 1f;
1648 y = y - offsetY + 1f;
1649
1650 // map is rotated
1651 index = (int)x * ((int)m_regionHeight + 3) + (int)y;
1652
1653 if (index < TerrainHeightFieldHeights[heightFieldGeom].Length)
1654 {
1655 //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]);
1656 return (float)TerrainHeightFieldHeights[heightFieldGeom][index];
1657 }
1658
1659 else
1660 return 0f;
1661 }
1662 else
1663 {
1664 return 0f;
1665 }
1666
1667 }
1668 else
1669 {
1670 return 0f;
1671 }
1672
1673 }
1674 else
1675 {
1676 return 0f;
1677 }
1678 }
1679// End recovered. Kitto Flora
1680
1681 /// <summary>
1682 /// Add actor to the list that should receive collision events in the simulate loop.
1683 /// </summary>
1684 /// <param name="obj"></param>
1685 internal void AddCollisionEventReporting(PhysicsActor obj)
1686 {
1687// m_log.DebugFormat("[PHYSICS]: Adding {0} {1} to collision event reporting", obj.SOPName, obj.LocalID);
1688
1689 lock (m_collisionEventActorsChanges)
1690 m_collisionEventActorsChanges[obj.LocalID] = obj;
1691 }
1692
1693 /// <summary>
1694 /// Remove actor from the list that should receive collision events in the simulate loop.
1695 /// </summary>
1696 /// <param name="obj"></param>
1697 internal void RemoveCollisionEventReporting(PhysicsActor obj)
1698 {
1699// m_log.DebugFormat("[PHYSICS]: Removing {0} {1} from collision event reporting", obj.SOPName, obj.LocalID);
1700
1701 lock (m_collisionEventActorsChanges)
1702 m_collisionEventActorsChanges[obj.LocalID] = null;
1703 }
1704
1705 #region Add/Remove Entities
1706
1707 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
1708 {
1709 d.AllocateODEDataForThread(0);
1710
1711 OdeCharacter newAv
1712 = new OdeCharacter(
1713 avName, this, position, velocity, size, avPIDD, avPIDP,
1714 avCapRadius, avStandupTensor, avDensity,
1715 avMovementDivisorWalk, avMovementDivisorRun);
1716
1717 newAv.Flying = isFlying;
1718 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1719 newAv.m_avatarplanted = avplanted;
1720
1721 return newAv;
1722 }
1723
1724 public override void RemoveAvatar(PhysicsActor actor)
1725 {
1726// m_log.DebugFormat(
1727// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}",
1728// actor.Name, actor.LocalID, Name);
1729
1730 lock (OdeLock)
1731 {
1732 d.AllocateODEDataForThread(0);
1733
1734 ((OdeCharacter) actor).Destroy();
1735 }
1736 }
1737
1738 internal void AddCharacter(OdeCharacter chr)
1739 {
1740 chr.m_avatarplanted = avplanted;
1741 if (!_characters.Contains(chr))
1742 {
1743 _characters.Add(chr);
1744
1745// m_log.DebugFormat(
1746// "[ODE SCENE]: Adding physics character {0} {1} to physics scene {2}. Count now {3}",
1747// chr.Name, chr.LocalID, Name, _characters.Count);
1748
1749 if (chr.bad)
1750 m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to characters list", chr.m_uuid);
1751 }
1752 else
1753 {
1754 m_log.ErrorFormat(
1755 "[ODE SCENE]: Tried to add character {0} {1} but they are already in the set!",
1756 chr.Name, chr.LocalID);
1757 }
1758 }
1759
1760 internal void RemoveCharacter(OdeCharacter chr)
1761 {
1762 if (_characters.Contains(chr))
1763 {
1764 _characters.Remove(chr);
1765
1766// m_log.DebugFormat(
1767// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}. Count now {3}",
1768// chr.Name, chr.LocalID, Name, _characters.Count);
1769 }
1770 else
1771 {
1772 m_log.ErrorFormat(
1773 "[ODE SCENE]: Tried to remove character {0} {1} but they are not in the list!",
1774 chr.Name, chr.LocalID);
1775 }
1776 }
1777
1778 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1779 PrimitiveBaseShape pbs, bool isphysical, uint localID)
1780 {
1781 Vector3 pos = position;
1782 Vector3 siz = size;
1783 Quaternion rot = rotation;
1784
1785
1786 OdePrim newPrim;
1787 lock (OdeLock)
1788 {
1789 d.AllocateODEDataForThread(0);
1790 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical);
1791
1792 lock (_prims)
1793 _prims.Add(newPrim);
1794 }
1795 newPrim.LocalID = localID;
1796 return newPrim;
1797 }
1798
1799 /// <summary>
1800 /// Make this prim subject to physics.
1801 /// </summary>
1802 /// <param name="prim"></param>
1803 internal void ActivatePrim(OdePrim prim)
1804 {
1805 // adds active prim.. (ones that should be iterated over in collisions_optimized
1806 if (!_activeprims.Contains(prim))
1807 _activeprims.Add(prim);
1808 //else
1809 // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
1810 }
1811
1812 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1813 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
1814 {
1815// m_log.DebugFormat("[ODE SCENE]: Adding physics prim {0} {1} to physics scene {2}", primName, localid, Name);
1816
1817 return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
1818 }
1819
1820 public override float TimeDilation
1821 {
1822 get { return m_timeDilation; }
1823 }
1824
1825 public override bool SupportsNINJAJoints
1826 {
1827 get { return m_NINJA_physics_joints_enabled; }
1828 }
1829
1830 // internal utility function: must be called within a lock (OdeLock)
1831 private void InternalAddActiveJoint(PhysicsJoint joint)
1832 {
1833 activeJoints.Add(joint);
1834 SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
1835 }
1836
1837 // internal utility function: must be called within a lock (OdeLock)
1838 private void InternalAddPendingJoint(OdePhysicsJoint joint)
1839 {
1840 pendingJoints.Add(joint);
1841 SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
1842 }
1843
1844 // internal utility function: must be called within a lock (OdeLock)
1845 private void InternalRemovePendingJoint(PhysicsJoint joint)
1846 {
1847 pendingJoints.Remove(joint);
1848 SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene);
1849 }
1850
1851 // internal utility function: must be called within a lock (OdeLock)
1852 private void InternalRemoveActiveJoint(PhysicsJoint joint)
1853 {
1854 activeJoints.Remove(joint);
1855 SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
1856 }
1857
1858 public override void DumpJointInfo()
1859 {
1860 string hdr = "[NINJA] JOINTINFO: ";
1861 foreach (PhysicsJoint j in pendingJoints)
1862 {
1863 m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1864 }
1865 m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
1866 foreach (string jointName in SOPName_to_pendingJoint.Keys)
1867 {
1868 m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
1869 }
1870 m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
1871 foreach (PhysicsJoint j in activeJoints)
1872 {
1873 m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1874 }
1875 m_log.Debug(hdr + activeJoints.Count + " total active joints");
1876 foreach (string jointName in SOPName_to_activeJoint.Keys)
1877 {
1878 m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
1879 }
1880 m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
1881
1882 m_log.Debug(hdr + " Per-body joint connectivity information follows.");
1883 m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
1884 foreach (string actorName in joints_connecting_actor.Keys)
1885 {
1886 m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it");
1887 foreach (PhysicsJoint j in joints_connecting_actor[actorName])
1888 {
1889 m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1890 }
1891 m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
1892 }
1893 }
1894
1895 public override void RequestJointDeletion(string ObjectNameInScene)
1896 {
1897 lock (externalJointRequestsLock)
1898 {
1899 if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously
1900 {
1901 requestedJointsToBeDeleted.Add(ObjectNameInScene);
1902 }
1903 }
1904 }
1905
1906 private void DeleteRequestedJoints()
1907 {
1908 List<string> myRequestedJointsToBeDeleted;
1909 lock (externalJointRequestsLock)
1910 {
1911 // make a local copy of the shared list for processing (threading issues)
1912 myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
1913 }
1914
1915 foreach (string jointName in myRequestedJointsToBeDeleted)
1916 {
1917 lock (OdeLock)
1918 {
1919 //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
1920 if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
1921 {
1922 OdePhysicsJoint joint = null;
1923 if (SOPName_to_activeJoint.ContainsKey(jointName))
1924 {
1925 joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
1926 InternalRemoveActiveJoint(joint);
1927 }
1928 else if (SOPName_to_pendingJoint.ContainsKey(jointName))
1929 {
1930 joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
1931 InternalRemovePendingJoint(joint);
1932 }
1933
1934 if (joint != null)
1935 {
1936 //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
1937 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
1938 {
1939 string bodyName = joint.BodyNames[iBodyName];
1940 if (bodyName != "NULL")
1941 {
1942 joints_connecting_actor[bodyName].Remove(joint);
1943 if (joints_connecting_actor[bodyName].Count == 0)
1944 {
1945 joints_connecting_actor.Remove(bodyName);
1946 }
1947 }
1948 }
1949
1950 DoJointDeactivated(joint);
1951 if (joint.jointID != IntPtr.Zero)
1952 {
1953 d.JointDestroy(joint.jointID);
1954 joint.jointID = IntPtr.Zero;
1955 //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
1956 }
1957 else
1958 {
1959 //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
1960 }
1961 }
1962 else
1963 {
1964 // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
1965 }
1966 }
1967 else
1968 {
1969 // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
1970 }
1971 }
1972 }
1973
1974 // remove processed joints from the shared list
1975 lock (externalJointRequestsLock)
1976 {
1977 foreach (string jointName in myRequestedJointsToBeDeleted)
1978 {
1979 requestedJointsToBeDeleted.Remove(jointName);
1980 }
1981 }
1982 }
1983
1984 // for pending joints we don't know if their associated bodies exist yet or not.
1985 // the joint is actually created during processing of the taints
1986 private void CreateRequestedJoints()
1987 {
1988 List<PhysicsJoint> myRequestedJointsToBeCreated;
1989 lock (externalJointRequestsLock)
1990 {
1991 // make a local copy of the shared list for processing (threading issues)
1992 myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
1993 }
1994
1995 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
1996 {
1997 lock (OdeLock)
1998 {
1999 if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
2000 {
2001 DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already pending joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
2002 continue;
2003 }
2004 if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
2005 {
2006 DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already active joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
2007 continue;
2008 }
2009
2010 InternalAddPendingJoint(joint as OdePhysicsJoint);
2011
2012 if (joint.BodyNames.Count >= 2)
2013 {
2014 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
2015 {
2016 string bodyName = joint.BodyNames[iBodyName];
2017 if (bodyName != "NULL")
2018 {
2019 if (!joints_connecting_actor.ContainsKey(bodyName))
2020 {
2021 joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
2022 }
2023 joints_connecting_actor[bodyName].Add(joint);
2024 }
2025 }
2026 }
2027 }
2028 }
2029
2030 // remove processed joints from shared list
2031 lock (externalJointRequestsLock)
2032 {
2033 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
2034 {
2035 requestedJointsToBeCreated.Remove(joint);
2036 }
2037 }
2038 }
2039
2040 /// <summary>
2041 /// Add a request for joint creation.
2042 /// </summary>
2043 /// <remarks>
2044 /// this joint will just be added to a waiting list that is NOT processed during the main
2045 /// Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
2046 /// </remarks>
2047 /// <param name="objectNameInScene"></param>
2048 /// <param name="jointType"></param>
2049 /// <param name="position"></param>
2050 /// <param name="rotation"></param>
2051 /// <param name="parms"></param>
2052 /// <param name="bodyNames"></param>
2053 /// <param name="trackedBodyName"></param>
2054 /// <param name="localRotation"></param>
2055 /// <returns></returns>
2056 public override PhysicsJoint RequestJointCreation(
2057 string objectNameInScene, PhysicsJointType jointType, Vector3 position,
2058 Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
2059 {
2060 OdePhysicsJoint joint = new OdePhysicsJoint();
2061 joint.ObjectNameInScene = objectNameInScene;
2062 joint.Type = jointType;
2063 joint.Position = position;
2064 joint.Rotation = rotation;
2065 joint.RawParams = parms;
2066 joint.BodyNames = new List<string>(bodyNames);
2067 joint.TrackedBodyName = trackedBodyName;
2068 joint.LocalRotation = localRotation;
2069 joint.jointID = IntPtr.Zero;
2070 joint.ErrorMessageCount = 0;
2071
2072 lock (externalJointRequestsLock)
2073 {
2074 if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice
2075 {
2076 requestedJointsToBeCreated.Add(joint);
2077 }
2078 }
2079
2080 return joint;
2081 }
2082
2083 private void RemoveAllJointsConnectedToActor(PhysicsActor actor)
2084 {
2085 //m_log.Debug("RemoveAllJointsConnectedToActor: start");
2086 if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
2087 {
2088 List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
2089 //TODO: merge these 2 loops (originally it was needed to avoid altering a list being iterated over, but it is no longer needed due to the joint request queue mechanism)
2090 foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
2091 {
2092 jointsToRemove.Add(j);
2093 }
2094 foreach (PhysicsJoint j in jointsToRemove)
2095 {
2096 //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
2097 RequestJointDeletion(j.ObjectNameInScene);
2098 //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
2099 j.TrackedBodyName = null; // *IMMEDIATELY* prevent any further movement of this joint (else a deleted actor might cause spurious tracking motion of the joint for a few frames, leading to the joint proxy object disappearing)
2100 }
2101 }
2102 }
2103
2104 public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
2105 {
2106 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start");
2107 lock (OdeLock)
2108 {
2109 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
2110 RemoveAllJointsConnectedToActor(actor);
2111 }
2112 }
2113
2114 // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2115 public override Vector3 GetJointAnchor(PhysicsJoint joint)
2116 {
2117 Debug.Assert(joint.IsInPhysicsEngine);
2118 d.Vector3 pos = new d.Vector3();
2119
2120 if (!(joint is OdePhysicsJoint))
2121 {
2122 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
2123 }
2124 else
2125 {
2126 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2127 switch (odeJoint.Type)
2128 {
2129 case PhysicsJointType.Ball:
2130 d.JointGetBallAnchor(odeJoint.jointID, out pos);
2131 break;
2132 case PhysicsJointType.Hinge:
2133 d.JointGetHingeAnchor(odeJoint.jointID, out pos);
2134 break;
2135 }
2136 }
2137 return new Vector3(pos.X, pos.Y, pos.Z);
2138 }
2139
2140 /// <summary>
2141 /// Get joint axis.
2142 /// </summary>
2143 /// <remarks>
2144 /// normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2145 /// WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
2146 /// appears to be unreliable. Fortunately we can compute the joint axis ourselves by
2147 /// keeping track of the joint's original orientation relative to one of the involved bodies.
2148 /// </remarks>
2149 /// <param name="joint"></param>
2150 /// <returns></returns>
2151 public override Vector3 GetJointAxis(PhysicsJoint joint)
2152 {
2153 Debug.Assert(joint.IsInPhysicsEngine);
2154 d.Vector3 axis = new d.Vector3();
2155
2156 if (!(joint is OdePhysicsJoint))
2157 {
2158 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
2159 }
2160 else
2161 {
2162 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2163 switch (odeJoint.Type)
2164 {
2165 case PhysicsJointType.Ball:
2166 DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
2167 break;
2168 case PhysicsJointType.Hinge:
2169 d.JointGetHingeAxis(odeJoint.jointID, out axis);
2170 break;
2171 }
2172 }
2173 return new Vector3(axis.X, axis.Y, axis.Z);
2174 }
2175
2176 /// <summary>
2177 /// Stop this prim being subject to physics
2178 /// </summary>
2179 /// <param name="prim"></param>
2180 internal void DeactivatePrim(OdePrim prim)
2181 {
2182 _activeprims.Remove(prim);
2183 }
2184
2185 public override void RemovePrim(PhysicsActor prim)
2186 {
2187 // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
2188 // removed in the next physics simulate pass.
2189 if (prim is OdePrim)
2190 {
2191 lock (OdeLock)
2192 {
2193 OdePrim p = (OdePrim) prim;
2194
2195 p.setPrimForRemoval();
2196 AddPhysicsActorTaint(prim);
2197 }
2198 }
2199 }
2200
2201 /// <summary>
2202 /// This is called from within simulate but outside the locked portion
2203 /// We need to do our own locking here
2204 /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
2205 /// Simulate() -- justincc).
2206 ///
2207 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
2208 ///
2209 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
2210 /// that the space was using.
2211 /// </summary>
2212 /// <param name="prim"></param>
2213 internal void RemovePrimThreadLocked(OdePrim prim)
2214 {
2215// m_log.DebugFormat("[ODE SCENE]: Removing physical prim {0} {1}", prim.Name, prim.LocalID);
2216
2217 lock (prim)
2218 {
2219 RemoveCollisionEventReporting(prim);
2220
2221 if (prim.prim_geom != IntPtr.Zero)
2222 {
2223 prim.ResetTaints();
2224
2225 if (prim.IsPhysical)
2226 {
2227 prim.disableBody();
2228 if (prim.childPrim)
2229 {
2230 prim.childPrim = false;
2231 prim.Body = IntPtr.Zero;
2232 prim.m_disabled = true;
2233 prim.IsPhysical = false;
2234 }
2235
2236
2237 }
2238 prim.m_targetSpace = IntPtr.Zero;
2239 if (!prim.RemoveGeom())
2240 m_log.Warn("[ODE SCENE]: Unable to remove prim from physics scene");
2241
2242 lock (_prims)
2243 _prims.Remove(prim);
2244
2245
2246 if (SupportsNINJAJoints)
2247 RemoveAllJointsConnectedToActorThreadLocked(prim);
2248 }
2249 }
2250 }
2251
2252 #endregion
2253
2254 #region Space Separation Calculation
2255
2256 /// <summary>
2257 /// Takes a space pointer and zeros out the array we're using to hold the spaces
2258 /// </summary>
2259 /// <param name="pSpace"></param>
2260 private void resetSpaceArrayItemToZero(IntPtr pSpace)
2261 {
2262 for (int x = 0; x < staticPrimspace.GetLength(0); x++)
2263 {
2264 for (int y = 0; y < staticPrimspace.GetLength(1); y++)
2265 {
2266 if (staticPrimspace[x, y] == pSpace)
2267 staticPrimspace[x, y] = IntPtr.Zero;
2268 }
2269 }
2270 }
2271
2272// private void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY)
2273// {
2274// staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero;
2275// }
2276
2277 /// <summary>
2278 /// Called when a static prim moves. Allocates a space for the prim based on its position
2279 /// </summary>
2280 /// <param name="geom">the pointer to the geom that moved</param>
2281 /// <param name="pos">the position that the geom moved to</param>
2282 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
2283 /// <returns>a pointer to the new space it's in</returns>
2284 internal IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace)
2285 {
2286 // Called from setting the Position and Size of an ODEPrim so
2287 // it's already in locked space.
2288
2289 // we don't want to remove the main space
2290 // we don't need to test physical here because this function should
2291 // never be called if the prim is physical(active)
2292
2293 // All physical prim end up in the root space
2294 //Thread.Sleep(20);
2295 if (currentspace != space)
2296 {
2297 //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString());
2298 //if (currentspace == IntPtr.Zero)
2299 //{
2300 //int adfadf = 0;
2301 //}
2302 if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero)
2303 {
2304 if (d.GeomIsSpace(currentspace))
2305 {
2306// waitForSpaceUnlock(currentspace);
2307 d.SpaceRemove(currentspace, geom);
2308 }
2309 else
2310 {
2311 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + currentspace +
2312 " Geom:" + geom);
2313 }
2314 }
2315 else
2316 {
2317 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2318 if (sGeomIsIn != IntPtr.Zero)
2319 {
2320 if (d.GeomIsSpace(currentspace))
2321 {
2322// waitForSpaceUnlock(sGeomIsIn);
2323 d.SpaceRemove(sGeomIsIn, geom);
2324 }
2325 else
2326 {
2327 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2328 sGeomIsIn + " Geom:" + geom);
2329 }
2330 }
2331 }
2332
2333 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2334 if (d.SpaceGetNumGeoms(currentspace) == 0)
2335 {
2336 if (currentspace != IntPtr.Zero)
2337 {
2338 if (d.GeomIsSpace(currentspace))
2339 {
2340 d.SpaceRemove(space, currentspace);
2341 // free up memory used by the space.
2342
2343 resetSpaceArrayItemToZero(currentspace);
2344 }
2345 else
2346 {
2347 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2348 currentspace + " Geom:" + geom);
2349 }
2350 }
2351 }
2352 }
2353 else
2354 {
2355 // this is a physical object that got disabled. ;.;
2356 if (currentspace != IntPtr.Zero && geom != IntPtr.Zero)
2357 {
2358 if (d.SpaceQuery(currentspace, geom))
2359 {
2360 if (d.GeomIsSpace(currentspace))
2361 {
2362// waitForSpaceUnlock(currentspace);
2363 d.SpaceRemove(currentspace, geom);
2364 }
2365 else
2366 {
2367 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2368 currentspace + " Geom:" + geom);
2369 }
2370 }
2371 else
2372 {
2373 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2374 if (sGeomIsIn != IntPtr.Zero)
2375 {
2376 if (d.GeomIsSpace(sGeomIsIn))
2377 {
2378// waitForSpaceUnlock(sGeomIsIn);
2379 d.SpaceRemove(sGeomIsIn, geom);
2380 }
2381 else
2382 {
2383 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2384 sGeomIsIn + " Geom:" + geom);
2385 }
2386 }
2387 }
2388 }
2389 }
2390
2391 // The routines in the Position and Size sections do the 'inserting' into the space,
2392 // so all we have to do is make sure that the space that we're putting the prim into
2393 // is in the 'main' space.
2394 int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos);
2395 IntPtr newspace = calculateSpaceForGeom(pos);
2396
2397 if (newspace == IntPtr.Zero)
2398 {
2399 newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
2400 d.HashSpaceSetLevels(newspace, HashspaceLow, HashspaceHigh);
2401 }
2402
2403 return newspace;
2404 }
2405
2406 /// <summary>
2407 /// Creates a new space at X Y
2408 /// </summary>
2409 /// <param name="iprimspaceArrItemX"></param>
2410 /// <param name="iprimspaceArrItemY"></param>
2411 /// <returns>A pointer to the created space</returns>
2412 internal IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
2413 {
2414 // creating a new space for prim and inserting it into main space.
2415 staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero);
2416 d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space);
2417// waitForSpaceUnlock(space);
2418 d.SpaceSetSublevel(space, 1);
2419 d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
2420
2421 return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
2422 }
2423
2424 /// <summary>
2425 /// Calculates the space the prim should be in by its position
2426 /// </summary>
2427 /// <param name="pos"></param>
2428 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
2429 internal IntPtr calculateSpaceForGeom(Vector3 pos)
2430 {
2431 int[] xyspace = calculateSpaceArrayItemFromPos(pos);
2432 //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
2433 return staticPrimspace[xyspace[0], xyspace[1]];
2434 }
2435
2436 /// <summary>
2437 /// Holds the space allocation logic
2438 /// </summary>
2439 /// <param name="pos"></param>
2440 /// <returns>an array item based on the position</returns>
2441 internal int[] calculateSpaceArrayItemFromPos(Vector3 pos)
2442 {
2443 int[] returnint = new int[2];
2444
2445 returnint[0] = (int) (pos.X * spacesPerMeterX);
2446
2447 if (returnint[0] > spaceGridMaxX)
2448 returnint[0] = spaceGridMaxX;
2449 if (returnint[0] < 0)
2450 returnint[0] = 0;
2451
2452 returnint[1] = (int)(pos.Y * spacesPerMeterY);
2453 if (returnint[1] > spaceGridMaxY)
2454 returnint[1] = spaceGridMaxY;
2455 if (returnint[1] < 0)
2456 returnint[1] = 0;
2457
2458 return returnint;
2459 }
2460
2461 #endregion
2462
2463 /// <summary>
2464 /// Routine to figure out if we need to mesh this prim with our mesher
2465 /// </summary>
2466 /// <param name="pbs"></param>
2467 /// <returns></returns>
2468 internal bool needsMeshing(PrimitiveBaseShape pbs)
2469 {
2470 // most of this is redundant now as the mesher will return null if it cant mesh a prim
2471 // but we still need to check for sculptie meshing being enabled so this is the most
2472 // convenient place to do it for now...
2473
2474 // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
2475 // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
2476 int iPropertiesNotSupportedDefault = 0;
2477
2478 if (pbs.SculptEntry && !meshSculptedPrim)
2479 {
2480#if SPAM
2481 m_log.Warn("NonMesh");
2482#endif
2483 return false;
2484 }
2485
2486 // 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
2487 if (!forceSimplePrimMeshing && !pbs.SculptEntry)
2488 {
2489 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
2490 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
2491 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
2492 {
2493
2494 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
2495 && pbs.ProfileHollow == 0
2496 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
2497 && pbs.PathBegin == 0 && pbs.PathEnd == 0
2498 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
2499 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
2500 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
2501 {
2502#if SPAM
2503 m_log.Warn("NonMesh");
2504#endif
2505 return false;
2506 }
2507 }
2508 }
2509
2510 if (pbs.ProfileHollow != 0)
2511 iPropertiesNotSupportedDefault++;
2512
2513 if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
2514 iPropertiesNotSupportedDefault++;
2515
2516 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
2517 iPropertiesNotSupportedDefault++;
2518
2519 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
2520 iPropertiesNotSupportedDefault++;
2521
2522 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
2523 iPropertiesNotSupportedDefault++;
2524
2525 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
2526 iPropertiesNotSupportedDefault++;
2527
2528 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
2529 iPropertiesNotSupportedDefault++;
2530
2531 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))
2532 iPropertiesNotSupportedDefault++;
2533
2534 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
2535 iPropertiesNotSupportedDefault++;
2536
2537 // test for torus
2538 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
2539 {
2540 if (pbs.PathCurve == (byte)Extrusion.Curve1)
2541 {
2542 iPropertiesNotSupportedDefault++;
2543 }
2544 }
2545 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
2546 {
2547 if (pbs.PathCurve == (byte)Extrusion.Straight)
2548 {
2549 iPropertiesNotSupportedDefault++;
2550 }
2551
2552 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
2553 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2554 {
2555 iPropertiesNotSupportedDefault++;
2556 }
2557 }
2558 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
2559 {
2560 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
2561 {
2562 iPropertiesNotSupportedDefault++;
2563 }
2564 }
2565 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
2566 {
2567 if (pbs.PathCurve == (byte)Extrusion.Straight)
2568 {
2569 iPropertiesNotSupportedDefault++;
2570 }
2571 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2572 {
2573 iPropertiesNotSupportedDefault++;
2574 }
2575 }
2576
2577 if (pbs.SculptEntry && meshSculptedPrim)
2578 iPropertiesNotSupportedDefault++;
2579
2580 if (iPropertiesNotSupportedDefault == 0)
2581 {
2582#if SPAM
2583 m_log.Warn("NonMesh");
2584#endif
2585 return false;
2586 }
2587#if SPAM
2588 m_log.Debug("Mesh");
2589#endif
2590 return true;
2591 }
2592
2593 /// <summary>
2594 /// Called after our prim properties are set Scale, position etc.
2595 /// </summary>
2596 /// <remarks>
2597 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
2598 /// This assures us that we have no race conditions
2599 /// </remarks>
2600 /// <param name="actor"></param>
2601 public override void AddPhysicsActorTaint(PhysicsActor actor)
2602 {
2603 if (actor is OdePrim)
2604 {
2605 OdePrim taintedprim = ((OdePrim)actor);
2606 lock (_taintedPrims)
2607 _taintedPrims.Add(taintedprim);
2608 }
2609 else if (actor is OdeCharacter)
2610 {
2611 OdeCharacter taintedchar = ((OdeCharacter)actor);
2612 lock (_taintedActors)
2613 {
2614 _taintedActors.Add(taintedchar);
2615 if (taintedchar.bad)
2616 m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid);
2617 }
2618 }
2619 }
2620
2621 // does all pending changes generated during region load process
2622 public override void ProcessPreSimulation()
2623 {
2624 lock (OdeLock)
2625 {
2626 if (world == IntPtr.Zero)
2627 {
2628 _taintedPrims.Clear();;
2629 return;
2630 }
2631
2632 int donechanges = 0;
2633 if (_taintedPrims.Count > 0)
2634 {
2635
2636 m_log.InfoFormat("[Ode] start processing pending actor operations");
2637 int tstart = Util.EnvironmentTickCount();
2638
2639 d.AllocateODEDataForThread(0);
2640
2641 lock (_taintedPrims)
2642 {
2643 foreach (OdePrim prim in _taintedPrims)
2644 {
2645 if (prim.m_taintremove)
2646 RemovePrimThreadLocked(prim);
2647 else
2648 prim.ProcessTaints();
2649
2650 prim.m_collisionscore = 0;
2651 donechanges++;
2652 }
2653 _taintedPrims.Clear();
2654 }
2655
2656 int time = Util.EnvironmentTickCountSubtract(tstart);
2657 m_log.InfoFormat("[Ode] finished {0} operations in {1}ms", donechanges, time);
2658 }
2659 m_log.InfoFormat("[Ode] {0} prim actors loaded",_prims.Count);
2660 }
2661 }
2662
2663
2664 /// <summary>
2665 /// This is our main simulate loop
2666 /// </summary>
2667 /// <remarks>
2668 /// It's thread locked by a Mutex in the scene.
2669 /// It holds Collisions, it instructs ODE to step through the physical reactions
2670 /// It moves the objects around in memory
2671 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
2672 /// </remarks>
2673 /// <param name="timeStep"></param>
2674 /// <returns>The number of frames simulated over that period.</returns>
2675 public override float Simulate(float timeStep)
2676 {
2677 if (!_worldInitialized)
2678 return 1.0f;
2679
2680 int startFrameTick = CollectStats ? Util.EnvironmentTickCount() : 0;
2681 int tempTick = 0, tempTick2 = 0;
2682
2683 if (framecount >= int.MaxValue)
2684 framecount = 0;
2685
2686 framecount++;
2687
2688 float fps = 0;
2689
2690 step_time += timeStep;
2691
2692 float HalfOdeStep = ODE_STEPSIZE * 0.5f;
2693 if (step_time < HalfOdeStep)
2694 return 0;
2695
2696
2697 // We change _collisionEventPrimChanges to avoid locking _collisionEventPrim itself and causing potential
2698 // deadlock if the collision event tries to lock something else later on which is already locked by a
2699 // caller that is adding or removing the collision event.
2700 lock (m_collisionEventActorsChanges)
2701 {
2702 foreach (KeyValuePair<uint, PhysicsActor> kvp in m_collisionEventActorsChanges)
2703 {
2704 if (kvp.Value == null)
2705 m_collisionEventActors.Remove(kvp.Key);
2706 else
2707 m_collisionEventActors[kvp.Key] = kvp.Value;
2708 }
2709
2710 m_collisionEventActorsChanges.Clear();
2711 }
2712
2713 if (SupportsNINJAJoints)
2714 {
2715 DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2716 CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2717 }
2718
2719
2720 lock (OdeLock)
2721 {
2722 d.AllocateODEDataForThread(~0U);
2723
2724 while (step_time > HalfOdeStep)
2725 {
2726 try
2727 {
2728 if (CollectStats)
2729 tempTick = Util.EnvironmentTickCount();
2730
2731 lock (_taintedActors)
2732 {
2733 foreach (OdeCharacter character in _taintedActors)
2734 character.ProcessTaints();
2735
2736 _taintedActors.Clear();
2737 }
2738
2739 if (CollectStats)
2740 {
2741 tempTick2 = Util.EnvironmentTickCount();
2742 m_stats[ODEAvatarTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2743 tempTick = tempTick2;
2744 }
2745
2746 lock (_taintedPrims)
2747 {
2748 foreach (OdePrim prim in _taintedPrims)
2749 {
2750 if (prim.m_taintremove)
2751 {
2752// Console.WriteLine("Simulate calls RemovePrimThreadLocked for {0}", prim.Name);
2753 RemovePrimThreadLocked(prim);
2754 }
2755 else
2756 {
2757// Console.WriteLine("Simulate calls ProcessTaints for {0}", prim.Name);
2758 prim.ProcessTaints();
2759 }
2760
2761 prim.m_collisionscore = 0;
2762
2763 // This loop can block up the Heartbeat for a very long time on large regions.
2764 // We need to let the Watchdog know that the Heartbeat is not dead
2765 // NOTE: This is currently commented out, but if things like OAR loading are
2766 // timing the heartbeat out we will need to uncomment it
2767 //Watchdog.UpdateThread();
2768 }
2769
2770 if (SupportsNINJAJoints)
2771 SimulatePendingNINJAJoints();
2772
2773 _taintedPrims.Clear();
2774 }
2775
2776 if (CollectStats)
2777 {
2778 tempTick2 = Util.EnvironmentTickCount();
2779 m_stats[ODEPrimTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2780 tempTick = tempTick2;
2781 }
2782
2783 // Move characters
2784 foreach (OdeCharacter actor in _characters)
2785 actor.Move(defects);
2786
2787 if (defects.Count != 0)
2788 {
2789 foreach (OdeCharacter actor in defects)
2790 {
2791 m_log.ErrorFormat(
2792 "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when moving",
2793 actor.Name, actor.LocalID, PhysicsSceneName);
2794
2795 RemoveCharacter(actor);
2796 actor.DestroyOdeStructures();
2797 }
2798
2799 defects.Clear();
2800 }
2801
2802 if (CollectStats)
2803 {
2804 tempTick2 = Util.EnvironmentTickCount();
2805 m_stats[ODEAvatarForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2806 tempTick = tempTick2;
2807 }
2808
2809 // Move other active objects
2810 foreach (OdePrim prim in _activeprims)
2811 {
2812 prim.m_collisionscore = 0;
2813 prim.Move(timeStep);
2814 }
2815
2816 if (CollectStats)
2817 {
2818 tempTick2 = Util.EnvironmentTickCount();
2819 m_stats[ODEPrimForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2820 tempTick = tempTick2;
2821 }
2822
2823 m_rayCastManager.ProcessQueuedRequests();
2824
2825 if (CollectStats)
2826 {
2827 tempTick2 = Util.EnvironmentTickCount();
2828 m_stats[ODERaycastingFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2829 tempTick = tempTick2;
2830 }
2831
2832 collision_optimized();
2833
2834 if (CollectStats)
2835 {
2836 tempTick2 = Util.EnvironmentTickCount();
2837 m_stats[ODEOtherCollisionFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2838 tempTick = tempTick2;
2839 }
2840
2841 foreach (PhysicsActor obj in m_collisionEventActors.Values)
2842 {
2843 // m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID);
2844
2845 switch ((ActorTypes)obj.PhysicsActorType)
2846 {
2847 case ActorTypes.Agent:
2848 OdeCharacter cobj = (OdeCharacter)obj;
2849 cobj.AddCollisionFrameTime(100);
2850 cobj.SendCollisions();
2851 break;
2852
2853 case ActorTypes.Prim:
2854 OdePrim pobj = (OdePrim)obj;
2855 pobj.SendCollisions();
2856 break;
2857 }
2858 }
2859
2860// if (m_global_contactcount > 0)
2861// m_log.DebugFormat(
2862// "[PHYSICS]: Collision contacts to process this frame = {0}", m_global_contactcount);
2863
2864 m_global_contactcount = 0;
2865
2866 if (CollectStats)
2867 {
2868 tempTick2 = Util.EnvironmentTickCount();
2869 m_stats[ODECollisionNotificationFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2870 tempTick = tempTick2;
2871 }
2872
2873 lock(SimulationLock)
2874 d.WorldQuickStep(world, ODE_STEPSIZE);
2875
2876 if (CollectStats)
2877 m_stats[ODENativeStepFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick);
2878
2879 d.JointGroupEmpty(contactgroup);
2880 }
2881 catch (Exception e)
2882 {
2883 m_log.ErrorFormat("[ODE SCENE]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
2884 }
2885
2886 step_time -= ODE_STEPSIZE;
2887 fps += ODE_STEPSIZE;
2888 }
2889
2890 if (CollectStats)
2891 tempTick = Util.EnvironmentTickCount();
2892
2893 foreach (OdeCharacter actor in _characters)
2894 {
2895 if (actor.bad)
2896 m_log.ErrorFormat("[ODE SCENE]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
2897
2898 actor.UpdatePositionAndVelocity(defects);
2899 }
2900
2901 if (defects.Count != 0)
2902 {
2903 foreach (OdeCharacter actor in defects)
2904 {
2905 m_log.ErrorFormat(
2906 "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when updating position and velocity",
2907 actor.Name, actor.LocalID, PhysicsSceneName);
2908
2909 RemoveCharacter(actor);
2910 actor.DestroyOdeStructures();
2911 }
2912
2913 defects.Clear();
2914 }
2915
2916 if (CollectStats)
2917 {
2918 tempTick2 = Util.EnvironmentTickCount();
2919 m_stats[ODEAvatarUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2920 tempTick = tempTick2;
2921 }
2922
2923 //if (timeStep < 0.2f)
2924
2925 foreach (OdePrim prim in _activeprims)
2926 {
2927 if (prim.IsPhysical && (d.BodyIsEnabled(prim.Body) || !prim._zeroFlag))
2928 {
2929 prim.UpdatePositionAndVelocity();
2930
2931 if (SupportsNINJAJoints)
2932 SimulateActorPendingJoints(prim);
2933 }
2934 }
2935
2936 if (CollectStats)
2937 m_stats[ODEPrimUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick);
2938
2939 //DumpJointInfo();
2940
2941 // Finished with all sim stepping. If requested, dump world state to file for debugging.
2942 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
2943 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
2944 if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0))
2945 {
2946 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
2947 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
2948
2949 if (physics_logging_append_existing_logfile)
2950 {
2951 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
2952 TextWriter fwriter = File.AppendText(fname);
2953 fwriter.WriteLine(header);
2954 fwriter.Close();
2955 }
2956
2957 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
2958 }
2959
2960 latertickcount = Util.EnvironmentTickCountSubtract(tickCountFrameRun);
2961
2962 // OpenSimulator above does 10 fps. 10 fps = means that the main thread loop and physics
2963 // has a max of 100 ms to run theoretically.
2964 // If the main loop stalls, it calls Simulate later which makes the tick count ms larger.
2965 // If Physics stalls, it takes longer which makes the tick count ms larger.
2966
2967 if (latertickcount < 100)
2968 {
2969 m_timeDilation = 1.0f;
2970 }
2971 else
2972 {
2973 m_timeDilation = 100f / latertickcount;
2974 //m_timeDilation = Math.Min((Math.Max(100 - (Util.EnvironmentTickCount() - tickCountFrameRun), 1) / 100f), 1.0f);
2975 }
2976
2977 tickCountFrameRun = Util.EnvironmentTickCount();
2978
2979 if (CollectStats)
2980 m_stats[ODETotalFrameMsStatName] += Util.EnvironmentTickCountSubtract(startFrameTick);
2981 }
2982
2983 fps *= 1.0f/timeStep;
2984 return fps;
2985 }
2986
2987 /// <summary>
2988 /// Simulate pending NINJA joints.
2989 /// </summary>
2990 /// <remarks>
2991 /// Called by the main Simulate() loop if NINJA joints are active. Should not be called from anywhere else.
2992 /// </remarks>
2993 private void SimulatePendingNINJAJoints()
2994 {
2995 // Create pending joints, if possible
2996
2997 // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
2998 // a joint requires specifying the body id of both involved bodies
2999 if (pendingJoints.Count > 0)
3000 {
3001 List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
3002 //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
3003 foreach (PhysicsJoint joint in pendingJoints)
3004 {
3005 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
3006 string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries);
3007 List<IntPtr> jointBodies = new List<IntPtr>();
3008 bool allJointBodiesAreReady = true;
3009 foreach (string jointParam in jointParams)
3010 {
3011 if (jointParam == "NULL")
3012 {
3013 //DoJointErrorMessage(joint, "attaching NULL joint to world");
3014 jointBodies.Add(IntPtr.Zero);
3015 }
3016 else
3017 {
3018 //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
3019 bool foundPrim = false;
3020 lock (_prims)
3021 {
3022 foreach (OdePrim prim in _prims) // FIXME: inefficient
3023 {
3024 if (prim.SOPName == jointParam)
3025 {
3026 //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
3027 if (prim.IsPhysical && prim.Body != IntPtr.Zero)
3028 {
3029 jointBodies.Add(prim.Body);
3030 foundPrim = true;
3031 break;
3032 }
3033 else
3034 {
3035 DoJointErrorMessage(joint, "prim name " + jointParam +
3036 " exists but is not (yet) physical; deferring joint creation. " +
3037 "IsPhysical property is " + prim.IsPhysical +
3038 " and body is " + prim.Body);
3039 foundPrim = false;
3040 break;
3041 }
3042 }
3043 }
3044 }
3045 if (foundPrim)
3046 {
3047 // all is fine
3048 }
3049 else
3050 {
3051 allJointBodiesAreReady = false;
3052 break;
3053 }
3054 }
3055 }
3056
3057 if (allJointBodiesAreReady)
3058 {
3059 //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
3060 if (jointBodies[0] == jointBodies[1])
3061 {
3062 DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
3063 }
3064 else
3065 {
3066 switch (joint.Type)
3067 {
3068 case PhysicsJointType.Ball:
3069 {
3070 IntPtr odeJoint;
3071 //DoJointErrorMessage(joint, "ODE creating ball joint ");
3072 odeJoint = d.JointCreateBall(world, IntPtr.Zero);
3073 //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
3074 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
3075 //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
3076 d.JointSetBallAnchor(odeJoint,
3077 joint.Position.X,
3078 joint.Position.Y,
3079 joint.Position.Z);
3080 //DoJointErrorMessage(joint, "ODE joint setting OK");
3081 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
3082 //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
3083 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
3084 //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
3085
3086 if (joint is OdePhysicsJoint)
3087 {
3088 ((OdePhysicsJoint)joint).jointID = odeJoint;
3089 }
3090 else
3091 {
3092 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
3093 }
3094 }
3095 break;
3096 case PhysicsJointType.Hinge:
3097 {
3098 IntPtr odeJoint;
3099 //DoJointErrorMessage(joint, "ODE creating hinge joint ");
3100 odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
3101 //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
3102 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
3103 //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
3104 d.JointSetHingeAnchor(odeJoint,
3105 joint.Position.X,
3106 joint.Position.Y,
3107 joint.Position.Z);
3108 // We use the orientation of the x-axis of the joint's coordinate frame
3109 // as the axis for the hinge.
3110
3111 // Therefore, we must get the joint's coordinate frame based on the
3112 // joint.Rotation field, which originates from the orientation of the
3113 // joint's proxy object in the scene.
3114
3115 // The joint's coordinate frame is defined as the transformation matrix
3116 // that converts a vector from joint-local coordinates into world coordinates.
3117 // World coordinates are defined as the XYZ coordinate system of the sim,
3118 // as shown in the top status-bar of the viewer.
3119
3120 // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
3121 // and use that as the hinge axis.
3122
3123 //joint.Rotation.Normalize();
3124 Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
3125
3126 // Now extract the X axis of the joint's coordinate frame.
3127
3128 // Do not try to use proxyFrame.AtAxis or you will become mired in the
3129 // tar pit of transposed, inverted, and generally messed-up orientations.
3130 // (In other words, Matrix4.AtAxis() is borked.)
3131 // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
3132
3133 // Instead, compute the X axis of the coordinate frame by transforming
3134 // the (1,0,0) vector. At least that works.
3135
3136 //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
3137 Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
3138 //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
3139 //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
3140 d.JointSetHingeAxis(odeJoint,
3141 jointAxis.X,
3142 jointAxis.Y,
3143 jointAxis.Z);
3144 //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
3145 if (joint is OdePhysicsJoint)
3146 {
3147 ((OdePhysicsJoint)joint).jointID = odeJoint;
3148 }
3149 else
3150 {
3151 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
3152 }
3153 }
3154 break;
3155 }
3156 successfullyProcessedPendingJoints.Add(joint);
3157 }
3158 }
3159 else
3160 {
3161 DoJointErrorMessage(joint, "joint could not yet be created; still pending");
3162 }
3163 }
3164
3165 foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
3166 {
3167 //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
3168 //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
3169 InternalRemovePendingJoint(successfullyProcessedJoint);
3170 //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
3171 InternalAddActiveJoint(successfullyProcessedJoint);
3172 //DoJointErrorMessage(successfullyProcessedJoint, "done");
3173 }
3174 }
3175 }
3176
3177 /// <summary>
3178 /// Simulate the joint proxies of a NINJA actor.
3179 /// </summary>
3180 /// <remarks>
3181 /// Called as part of the Simulate() loop if NINJA physics is active. Must only be called from there.
3182 /// </remarks>
3183 /// <param name="actor"></param>
3184 private void SimulateActorPendingJoints(OdePrim actor)
3185 {
3186 // If an actor moved, move its joint proxy objects as well.
3187 // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
3188 // for this purpose but it is never called! So we just do the joint
3189 // movement code here.
3190
3191 if (actor.SOPName != null &&
3192 joints_connecting_actor.ContainsKey(actor.SOPName) &&
3193 joints_connecting_actor[actor.SOPName] != null &&
3194 joints_connecting_actor[actor.SOPName].Count > 0)
3195 {
3196 foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
3197 {
3198 if (affectedJoint.IsInPhysicsEngine)
3199 {
3200 DoJointMoved(affectedJoint);
3201 }
3202 else
3203 {
3204 DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams);
3205 }
3206 }
3207 }
3208 }
3209
3210 public override void GetResults()
3211 {
3212 }
3213
3214 public override bool IsThreaded
3215 {
3216 // for now we won't be multithreaded
3217 get { return false; }
3218 }
3219
3220 public override void SetTerrain(float[] heightMap)
3221 {
3222 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
3223 {
3224 if (m_parentScene is OdeScene)
3225 {
3226 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
3227 }
3228 }
3229 else
3230 {
3231 SetTerrain(heightMap, m_worldOffset);
3232 }
3233 }
3234
3235 private void SetTerrain(float[] heightMap, Vector3 pOffset)
3236 {
3237 int startTime = Util.EnvironmentTickCount();
3238 m_log.DebugFormat("[ODE SCENE]: Setting terrain for {0} with offset {1}", PhysicsSceneName, pOffset);
3239
3240
3241 float[] _heightmap;
3242
3243 // ok im lasy this are just a aliases
3244 uint regionsizeX = m_regionWidth;
3245 uint regionsizeY = m_regionHeight;
3246
3247 // map is rotated
3248 uint heightmapWidth = regionsizeY + 2;
3249 uint heightmapHeight = regionsizeX + 2;
3250
3251 uint heightmapWidthSamples = heightmapWidth + 1;
3252 uint heightmapHeightSamples = heightmapHeight + 1;
3253
3254 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
3255
3256 const float scale = 1.0f;
3257 const float offset = 0.0f;
3258 const float thickness = 10f;
3259 const int wrap = 0;
3260
3261
3262 float hfmin = float.MaxValue;
3263 float hfmax = float.MinValue;
3264 float val;
3265 uint xx;
3266 uint yy;
3267
3268 uint maxXX = regionsizeX - 1;
3269 uint maxYY = regionsizeY - 1;
3270
3271 // flipping map adding one margin all around so things don't fall in edges
3272
3273 uint xt = 0;
3274 xx = 0;
3275
3276
3277 for (uint x = 0; x < heightmapWidthSamples; x++)
3278 {
3279 if (x > 1 && xx < maxXX)
3280 xx++;
3281 yy = 0;
3282 for (uint y = 0; y < heightmapHeightSamples; y++)
3283 {
3284 if (y > 1 && y < maxYY)
3285 yy += regionsizeX;
3286
3287 val = heightMap[yy + xx];
3288 if (val < 0.0f)
3289 val = 0.0f;
3290 _heightmap[xt + y] = val;
3291
3292 if (hfmin > val)
3293 hfmin = val;
3294 if (hfmax < val)
3295 hfmax = val;
3296 }
3297 xt += heightmapHeightSamples;
3298 }
3299
3300 lock (OdeLock)
3301 {
3302 d.AllocateODEDataForThread(~0U);
3303
3304 IntPtr GroundGeom = IntPtr.Zero;
3305 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
3306 {
3307 RegionTerrain.Remove(pOffset);
3308 if (GroundGeom != IntPtr.Zero)
3309 {
3310 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
3311 {
3312 TerrainHeightFieldHeights.Remove(GroundGeom);
3313 }
3314 d.SpaceRemove(space, GroundGeom);
3315 d.GeomDestroy(GroundGeom);
3316 }
3317
3318 }
3319 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
3320 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0,
3321 heightmapWidth, heightmapHeight,
3322 (int)heightmapWidthSamples,
3323 (int)heightmapHeightSamples,
3324 scale, offset, thickness, wrap);
3325
3326 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
3327 GroundGeom = d.CreateHeightfield(space, HeightmapData, 1);
3328 if (GroundGeom != IntPtr.Zero)
3329 {
3330 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
3331 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
3332
3333 }
3334 geom_name_map[GroundGeom] = "Terrain";
3335
3336 d.Matrix3 R = new d.Matrix3();
3337
3338 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
3339 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
3340
3341 q1 = q1 * q2;
3342 Vector3 v3;
3343 float angle;
3344 q1.GetAxisAngle(out v3, out angle);
3345
3346 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
3347 d.GeomSetRotation(GroundGeom, ref R);
3348 d.GeomSetPosition(GroundGeom, pOffset.X + regionsizeX * 0.5f, pOffset.Y + regionsizeY * 0.5f, 0.0f);
3349 IntPtr testGround = IntPtr.Zero;
3350 if (RegionTerrain.TryGetValue(pOffset, out testGround))
3351 {
3352 RegionTerrain.Remove(pOffset);
3353 }
3354 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
3355 TerrainHeightFieldHeights.Add(GroundGeom,_heightmap);
3356 }
3357
3358 m_log.DebugFormat(
3359 "[ODE SCENE]: Setting terrain for {0} took {1}ms", PhysicsSceneName, Util.EnvironmentTickCountSubtract(startTime));
3360 }
3361
3362 public override void DeleteTerrain()
3363 {
3364 }
3365
3366 internal float GetWaterLevel()
3367 {
3368 return waterlevel;
3369 }
3370
3371 public override void SetWaterLevel(float baseheight)
3372 {
3373 waterlevel = baseheight;
3374 }
3375
3376 [HandleProcessCorruptedStateExceptions]
3377 public override void Dispose()
3378 {
3379 lock(SimulationLock)
3380 lock(OdeLock)
3381 {
3382 if(world == IntPtr.Zero)
3383 return;
3384
3385 _worldInitialized = false;
3386
3387 d.AllocateODEDataForThread(~0U);
3388
3389 if (m_rayCastManager != null)
3390 {
3391 m_rayCastManager.Dispose();
3392 m_rayCastManager = null;
3393 }
3394
3395 lock (_prims)
3396 {
3397 foreach (OdePrim prm in _prims)
3398 {
3399 RemovePrim(prm);
3400 }
3401 }
3402
3403 //foreach (OdeCharacter act in _characters)
3404 //{
3405 //RemoveAvatar(act);
3406 //}
3407 IntPtr GroundGeom = IntPtr.Zero;
3408 if (RegionTerrain.TryGetValue(m_worldOffset, out GroundGeom))
3409 {
3410 RegionTerrain.Remove(m_worldOffset);
3411 if (GroundGeom != IntPtr.Zero)
3412 {
3413 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
3414 TerrainHeightFieldHeights.Remove(GroundGeom);
3415 d.GeomDestroy(GroundGeom);
3416 }
3417 }
3418
3419 try
3420 {
3421 d.WorldDestroy(world);
3422 world = IntPtr.Zero;
3423 }
3424 catch (AccessViolationException e)
3425 {
3426 m_log.ErrorFormat("[ODE SCENE]: exception {0}", e.Message);
3427 }
3428 }
3429 }
3430
3431 private int compareByCollisionsDesc(OdePrim A, OdePrim B)
3432 {
3433 return -A.CollisionScore.CompareTo(B.CollisionScore);
3434 }
3435
3436 public override Dictionary<uint, float> GetTopColliders()
3437 {
3438 Dictionary<uint, float> topColliders;
3439
3440 lock (_prims)
3441 {
3442 List<OdePrim> orderedPrims = new List<OdePrim>(_prims);
3443 orderedPrims.Sort(compareByCollisionsDesc);
3444 topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
3445
3446 foreach (OdePrim p in _prims)
3447 p.CollisionScore = 0;
3448 }
3449
3450 return topColliders;
3451 }
3452
3453 public override bool SupportsRayCast()
3454 {
3455 return true;
3456 }
3457
3458 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
3459 {
3460 if (retMethod != null)
3461 {
3462 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
3463 }
3464 }
3465
3466 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
3467 {
3468 if (retMethod != null)
3469 {
3470 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
3471 }
3472 }
3473
3474 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
3475 {
3476 ContactResult[] ourResults = null;
3477 RayCallback retMethod = delegate(List<ContactResult> results)
3478 {
3479 ourResults = new ContactResult[results.Count];
3480 results.CopyTo(ourResults, 0);
3481 };
3482 int waitTime = 0;
3483 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
3484 while (ourResults == null && waitTime < 1000)
3485 {
3486 Thread.Sleep(1);
3487 waitTime++;
3488 }
3489 if (ourResults == null)
3490 return new List<ContactResult> ();
3491 return new List<ContactResult>(ourResults);
3492 }
3493
3494 public override Dictionary<string, float> GetStats()
3495 {
3496 if (!CollectStats)
3497 return null;
3498
3499 Dictionary<string, float> returnStats;
3500
3501 lock (OdeLock)
3502 {
3503 returnStats = new Dictionary<string, float>(m_stats);
3504
3505 // FIXME: This is a SUPER DUMB HACK until we can establish stats that aren't subject to a division by
3506 // 3 from the SimStatsReporter.
3507 returnStats[ODETotalAvatarsStatName] = _characters.Count * 3;
3508 returnStats[ODETotalPrimsStatName] = _prims.Count * 3;
3509 returnStats[ODEActivePrimsStatName] = _activeprims.Count * 3;
3510
3511 InitializeExtraStats();
3512 }
3513
3514 returnStats[ODEOtherCollisionFrameMsStatName]
3515 = returnStats[ODEOtherCollisionFrameMsStatName]
3516 - returnStats[ODENativeSpaceCollisionFrameMsStatName]
3517 - returnStats[ODENativeGeomCollisionFrameMsStatName];
3518
3519 return returnStats;
3520 }
3521
3522 private void InitializeExtraStats()
3523 {
3524 m_stats[ODETotalFrameMsStatName] = 0;
3525 m_stats[ODEAvatarTaintMsStatName] = 0;
3526 m_stats[ODEPrimTaintMsStatName] = 0;
3527 m_stats[ODEAvatarForcesFrameMsStatName] = 0;
3528 m_stats[ODEPrimForcesFrameMsStatName] = 0;
3529 m_stats[ODERaycastingFrameMsStatName] = 0;
3530 m_stats[ODENativeStepFrameMsStatName] = 0;
3531 m_stats[ODENativeSpaceCollisionFrameMsStatName] = 0;
3532 m_stats[ODENativeGeomCollisionFrameMsStatName] = 0;
3533 m_stats[ODEOtherCollisionFrameMsStatName] = 0;
3534 m_stats[ODECollisionNotificationFrameMsStatName] = 0;
3535 m_stats[ODEAvatarContactsStatsName] = 0;
3536 m_stats[ODEPrimContactsStatName] = 0;
3537 m_stats[ODEAvatarUpdateFrameMsStatName] = 0;
3538 m_stats[ODEPrimUpdateFrameMsStatName] = 0;
3539 }
3540 }
3541}
generated by cgit v1.1 at 2024-11-07 02:46:37 +0000