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