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