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