aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs3859
1 files changed, 3859 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..ab084fd
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -0,0 +1,3859 @@
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
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 }
850 else
851 {
852 p2.IsColliding = true;
853 }
854
855 //if ((framecount % m_returncollisions) == 0)
856
857 switch (p1.PhysicsActorType)
858 {
859 case (int)ActorTypes.Agent:
860 p2.CollidingObj = true;
861 break;
862 case (int)ActorTypes.Prim:
863 if (p2.Velocity.LengthSquared() > 0.0f)
864 p2.CollidingObj = true;
865 break;
866 case (int)ActorTypes.Unknown:
867 p2.CollidingGround = true;
868 break;
869 default:
870 p2.CollidingGround = true;
871 break;
872 }
873
874 // we don't want prim or avatar to explode
875
876 #region InterPenetration Handling - Unintended physics explosions
877# region disabled code1
878
879 if (curContact.depth >= 0.08f)
880 {
881 //This is disabled at the moment only because it needs more tweaking
882 //It will eventually be uncommented
883 /*
884 if (contact.depth >= 1.00f)
885 {
886 //m_log.Debug("[PHYSICS]: " + contact.depth.ToString());
887 }
888
889 //If you interpenetrate a prim with an agent
890 if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
891 p1.PhysicsActorType == (int) ActorTypes.Prim) ||
892 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
893 p2.PhysicsActorType == (int) ActorTypes.Prim))
894 {
895
896 //contact.depth = contact.depth * 4.15f;
897 /*
898 if (p2.PhysicsActorType == (int) ActorTypes.Agent)
899 {
900 p2.CollidingObj = true;
901 contact.depth = 0.003f;
902 p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f);
903 OdeCharacter character = (OdeCharacter) p2;
904 character.SetPidStatus(true);
905 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));
906
907 }
908 else
909 {
910
911 //contact.depth = 0.0000000f;
912 }
913 if (p1.PhysicsActorType == (int) ActorTypes.Agent)
914 {
915
916 p1.CollidingObj = true;
917 contact.depth = 0.003f;
918 p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f);
919 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));
920 OdeCharacter character = (OdeCharacter)p1;
921 character.SetPidStatus(true);
922 }
923 else
924 {
925
926 //contact.depth = 0.0000000f;
927 }
928
929
930
931 }
932*/
933 // If you interpenetrate a prim with another prim
934 /*
935 if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)
936 {
937 #region disabledcode2
938 //OdePrim op1 = (OdePrim)p1;
939 //OdePrim op2 = (OdePrim)p2;
940 //op1.m_collisionscore++;
941 //op2.m_collisionscore++;
942
943 //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000)
944 //{
945 //op1.m_taintdisable = true;
946 //AddPhysicsActorTaint(p1);
947 //op2.m_taintdisable = true;
948 //AddPhysicsActorTaint(p2);
949 //}
950
951 //if (contact.depth >= 0.25f)
952 //{
953 // Don't collide, one or both prim will expld.
954
955 //op1.m_interpenetrationcount++;
956 //op2.m_interpenetrationcount++;
957 //interpenetrations_before_disable = 200;
958 //if (op1.m_interpenetrationcount >= interpenetrations_before_disable)
959 //{
960 //op1.m_taintdisable = true;
961 //AddPhysicsActorTaint(p1);
962 //}
963 //if (op2.m_interpenetrationcount >= interpenetrations_before_disable)
964 //{
965 // op2.m_taintdisable = true;
966 //AddPhysicsActorTaint(p2);
967 //}
968
969 //contact.depth = contact.depth / 8f;
970 //contact.normal = new d.Vector3(0, 0, 1);
971 //}
972 //if (op1.m_disabled || op2.m_disabled)
973 //{
974 //Manually disabled objects stay disabled
975 //contact.depth = 0f;
976 //}
977 #endregion
978 }
979 */
980#endregion
981 if (curContact.depth >= 1.00f)
982 {
983 //m_log.Info("[P]: " + contact.depth.ToString());
984 if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
985 p1.PhysicsActorType == (int) ActorTypes.Unknown) ||
986 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
987 p2.PhysicsActorType == (int) ActorTypes.Unknown))
988 {
989 if (p2.PhysicsActorType == (int) ActorTypes.Agent)
990 {
991 if (p2 is OdeCharacter)
992 {
993 OdeCharacter character = (OdeCharacter) p2;
994
995 //p2.CollidingObj = true;
996 curContact.depth = 0.00000003f;
997 p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f);
998 curContact.pos =
999 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
1000 curContact.pos.Y + (p1.Size.Y/2),
1001 curContact.pos.Z + (p1.Size.Z/2));
1002 character.SetPidStatus(true);
1003 }
1004 }
1005
1006
1007 if (p1.PhysicsActorType == (int) ActorTypes.Agent)
1008 {
1009 if (p1 is OdeCharacter)
1010 {
1011 OdeCharacter character = (OdeCharacter) p1;
1012
1013 //p2.CollidingObj = true;
1014 curContact.depth = 0.00000003f;
1015 p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f);
1016 curContact.pos =
1017 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
1018 curContact.pos.Y + (p1.Size.Y/2),
1019 curContact.pos.Z + (p1.Size.Z/2));
1020 character.SetPidStatus(true);
1021 }
1022 }
1023 }
1024 }
1025 }
1026
1027 #endregion
1028
1029 // Logic for collision handling
1030 // Note, that if *all* contacts are skipped (VolumeDetect)
1031 // The prim still detects (and forwards) collision events but
1032 // appears to be phantom for the world
1033 Boolean skipThisContact = false;
1034
1035 if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect))
1036 skipThisContact = true; // No collision on volume detect prims
1037
1038 if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
1039 skipThisContact = true; // No collision on volume detect prims
1040
1041 if (!skipThisContact && curContact.depth < 0f)
1042 skipThisContact = true;
1043
1044 if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType))
1045 skipThisContact = true;
1046
1047 const int maxContactsbeforedeath = 4000;
1048 joint = IntPtr.Zero;
1049
1050 if (!skipThisContact)
1051 {
1052 // If we're colliding against terrain
1053 if (name1 == "Terrain" || name2 == "Terrain")
1054 {
1055 // If we're moving
1056 if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
1057 (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
1058 {
1059 // Use the movement terrain contact
1060 AvatarMovementTerrainContact.geom = curContact;
1061 _perloopContact.Add(curContact);
1062 if (m_global_contactcount < maxContactsbeforedeath)
1063 {
1064 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact);
1065 m_global_contactcount++;
1066 }
1067 }
1068 else
1069 {
1070 if (p2.PhysicsActorType == (int)ActorTypes.Agent)
1071 {
1072 // Use the non moving terrain contact
1073 TerrainContact.geom = curContact;
1074 _perloopContact.Add(curContact);
1075 if (m_global_contactcount < maxContactsbeforedeath)
1076 {
1077 joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
1078 m_global_contactcount++;
1079 }
1080 }
1081 else
1082 {
1083 if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim)
1084 {
1085 // prim prim contact
1086 // int pj294950 = 0;
1087 int movintYN = 0;
1088 int material = (int) Material.Wood;
1089 // prim terrain contact
1090 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
1091 {
1092 movintYN = 1;
1093 }
1094
1095 if (p2 is OdePrim)
1096 material = ((OdePrim)p2).m_material;
1097
1098 //m_log.DebugFormat("Material: {0}", material);
1099 m_materialContacts[material, movintYN].geom = curContact;
1100 _perloopContact.Add(curContact);
1101
1102 if (m_global_contactcount < maxContactsbeforedeath)
1103 {
1104 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1105 m_global_contactcount++;
1106
1107 }
1108
1109 }
1110 else
1111 {
1112
1113 int movintYN = 0;
1114 // prim terrain contact
1115 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
1116 {
1117 movintYN = 1;
1118 }
1119
1120 int material = (int)Material.Wood;
1121
1122 if (p2 is OdePrim)
1123 material = ((OdePrim)p2).m_material;
1124 //m_log.DebugFormat("Material: {0}", material);
1125 m_materialContacts[material, movintYN].geom = curContact;
1126 _perloopContact.Add(curContact);
1127
1128 if (m_global_contactcount < maxContactsbeforedeath)
1129 {
1130 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1131 m_global_contactcount++;
1132
1133 }
1134 }
1135 }
1136 }
1137 //if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1138 //{
1139 //m_log.Debug("[PHYSICS]: prim contacting with ground");
1140 //}
1141 }
1142 else if (name1 == "Water" || name2 == "Water")
1143 {
1144 /*
1145 if ((p2.PhysicsActorType == (int) ActorTypes.Prim))
1146 {
1147 }
1148 else
1149 {
1150 }
1151 */
1152 //WaterContact.surface.soft_cfm = 0.0000f;
1153 //WaterContact.surface.soft_erp = 0.00000f;
1154 if (curContact.depth > 0.1f)
1155 {
1156 curContact.depth *= 52;
1157 //contact.normal = new d.Vector3(0, 0, 1);
1158 //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f);
1159 }
1160 WaterContact.geom = curContact;
1161 _perloopContact.Add(curContact);
1162 if (m_global_contactcount < maxContactsbeforedeath)
1163 {
1164 joint = d.JointCreateContact(world, contactgroup, ref WaterContact);
1165 m_global_contactcount++;
1166 }
1167 //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth);
1168 }
1169 else
1170 {
1171 // we're colliding with prim or avatar
1172 // check if we're moving
1173 if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
1174 {
1175 if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
1176 {
1177 // Use the Movement prim contact
1178 AvatarMovementprimContact.geom = curContact;
1179 _perloopContact.Add(curContact);
1180 if (m_global_contactcount < maxContactsbeforedeath)
1181 {
1182 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact);
1183 m_global_contactcount++;
1184 }
1185 }
1186 else
1187 {
1188 // Use the non movement contact
1189 contact.geom = curContact;
1190 _perloopContact.Add(curContact);
1191
1192 if (m_global_contactcount < maxContactsbeforedeath)
1193 {
1194 joint = d.JointCreateContact(world, contactgroup, ref contact);
1195 m_global_contactcount++;
1196 }
1197 }
1198 }
1199 else if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1200 {
1201 //p1.PhysicsActorType
1202 int material = (int)Material.Wood;
1203
1204 if (p2 is OdePrim)
1205 material = ((OdePrim)p2).m_material;
1206
1207 //m_log.DebugFormat("Material: {0}", material);
1208 m_materialContacts[material, 0].geom = curContact;
1209 _perloopContact.Add(curContact);
1210
1211 if (m_global_contactcount < maxContactsbeforedeath)
1212 {
1213 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, 0]);
1214 m_global_contactcount++;
1215
1216 }
1217 }
1218 }
1219
1220 if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide!
1221 {
1222 d.JointAttach(joint, b1, b2);
1223 m_global_contactcount++;
1224 }
1225
1226 }
1227 collision_accounting_events(p1, p2, maxDepthContact);
1228 if (count > geomContactPointsStartthrottle)
1229 {
1230 // If there are more then 3 contact points, it's likely
1231 // that we've got a pile of objects, so ...
1232 // We don't want to send out hundreds of terse updates over and over again
1233 // so lets throttle them and send them again after it's somewhat sorted out.
1234 p2.ThrottleUpdates = true;
1235 }
1236 //m_log.Debug(count.ToString());
1237 //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
1238 }
1239 }
1240
1241 private bool checkDupe(d.ContactGeom contactGeom, int atype)
1242 {
1243 bool result = false;
1244 //return result;
1245 if (!m_filterCollisions)
1246 return false;
1247
1248 ActorTypes at = (ActorTypes)atype;
1249 lock (_perloopContact)
1250 {
1251 foreach (d.ContactGeom contact in _perloopContact)
1252 {
1253 //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2))
1254 //{
1255 // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2)
1256 if (at == ActorTypes.Agent)
1257 {
1258 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)
1259 {
1260
1261 if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f)
1262 {
1263 //contactGeom.depth *= .00005f;
1264 //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1265 // 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));
1266 result = true;
1267 break;
1268 }
1269 else
1270 {
1271 //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1272 }
1273 }
1274 else
1275 {
1276 //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));
1277 //int i = 0;
1278 }
1279 }
1280 else if (at == ActorTypes.Prim)
1281 {
1282 //d.AABB aabb1 = new d.AABB();
1283 //d.AABB aabb2 = new d.AABB();
1284
1285 //d.GeomGetAABB(contactGeom.g2, out aabb2);
1286 //d.GeomGetAABB(contactGeom.g1, out aabb1);
1287 //aabb1.
1288 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)
1289 {
1290 if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z)
1291 {
1292 if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f)
1293 {
1294 result = true;
1295 break;
1296 }
1297 }
1298 //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1299 //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));
1300 }
1301
1302 }
1303
1304 //}
1305
1306 }
1307 }
1308 return result;
1309 }
1310
1311 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
1312 {
1313 // obj1LocalID = 0;
1314 //returncollisions = false;
1315 obj2LocalID = 0;
1316 //ctype = 0;
1317 //cStartStop = 0;
1318 if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
1319 return;
1320
1321 switch ((ActorTypes)p2.PhysicsActorType)
1322 {
1323 case ActorTypes.Agent:
1324 cc2 = (OdeCharacter)p2;
1325
1326 // obj1LocalID = cc2.m_localID;
1327 switch ((ActorTypes)p1.PhysicsActorType)
1328 {
1329 case ActorTypes.Agent:
1330 cc1 = (OdeCharacter)p1;
1331 obj2LocalID = cc1.m_localID;
1332 cc1.AddCollisionEvent(cc2.m_localID, contact);
1333 //ctype = (int)CollisionCategories.Character;
1334
1335 //if (cc1.CollidingObj)
1336 //cStartStop = (int)StatusIndicators.Generic;
1337 //else
1338 //cStartStop = (int)StatusIndicators.Start;
1339
1340 //returncollisions = true;
1341 break;
1342 case ActorTypes.Prim:
1343 if (p1 is OdePrim)
1344 {
1345 cp1 = (OdePrim) p1;
1346 obj2LocalID = cp1.m_localID;
1347 cp1.AddCollisionEvent(cc2.m_localID, contact);
1348 }
1349 //ctype = (int)CollisionCategories.Geom;
1350
1351 //if (cp1.CollidingObj)
1352 //cStartStop = (int)StatusIndicators.Generic;
1353 //else
1354 //cStartStop = (int)StatusIndicators.Start;
1355
1356 //returncollisions = true;
1357 break;
1358
1359 case ActorTypes.Ground:
1360 case ActorTypes.Unknown:
1361 obj2LocalID = 0;
1362 //ctype = (int)CollisionCategories.Land;
1363 //returncollisions = true;
1364 break;
1365 }
1366
1367 cc2.AddCollisionEvent(obj2LocalID, contact);
1368 break;
1369 case ActorTypes.Prim:
1370
1371 if (p2 is OdePrim)
1372 {
1373 cp2 = (OdePrim) p2;
1374
1375 // obj1LocalID = cp2.m_localID;
1376 switch ((ActorTypes) p1.PhysicsActorType)
1377 {
1378 case ActorTypes.Agent:
1379 if (p1 is OdeCharacter)
1380 {
1381 cc1 = (OdeCharacter) p1;
1382 obj2LocalID = cc1.m_localID;
1383 cc1.AddCollisionEvent(cp2.m_localID, contact);
1384 //ctype = (int)CollisionCategories.Character;
1385
1386 //if (cc1.CollidingObj)
1387 //cStartStop = (int)StatusIndicators.Generic;
1388 //else
1389 //cStartStop = (int)StatusIndicators.Start;
1390 //returncollisions = true;
1391 }
1392 break;
1393 case ActorTypes.Prim:
1394
1395 if (p1 is OdePrim)
1396 {
1397 cp1 = (OdePrim) p1;
1398 obj2LocalID = cp1.m_localID;
1399 cp1.AddCollisionEvent(cp2.m_localID, contact);
1400 //ctype = (int)CollisionCategories.Geom;
1401
1402 //if (cp1.CollidingObj)
1403 //cStartStop = (int)StatusIndicators.Generic;
1404 //else
1405 //cStartStop = (int)StatusIndicators.Start;
1406
1407 //returncollisions = true;
1408 }
1409 break;
1410
1411 case ActorTypes.Ground:
1412 case ActorTypes.Unknown:
1413 obj2LocalID = 0;
1414 //ctype = (int)CollisionCategories.Land;
1415
1416 //returncollisions = true;
1417 break;
1418 }
1419
1420 cp2.AddCollisionEvent(obj2LocalID, contact);
1421 }
1422 break;
1423 }
1424 //if (returncollisions)
1425 //{
1426
1427 //lock (m_storedCollisions)
1428 //{
1429 //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString();
1430 //if (m_storedCollisions.ContainsKey(cDictKey))
1431 //{
1432 //sCollisionData objd = m_storedCollisions[cDictKey];
1433 //objd.NumberOfCollisions += 1;
1434 //objd.lastframe = framecount;
1435 //m_storedCollisions[cDictKey] = objd;
1436 //}
1437 //else
1438 //{
1439 //sCollisionData objd = new sCollisionData();
1440 //objd.ColliderLocalId = obj1LocalID;
1441 //objd.CollidedWithLocalId = obj2LocalID;
1442 //objd.CollisionType = ctype;
1443 //objd.NumberOfCollisions = 1;
1444 //objd.lastframe = framecount;
1445 //objd.StatusIndicator = cStartStop;
1446 //m_storedCollisions.Add(cDictKey, objd);
1447 //}
1448 //}
1449 // }
1450 }
1451
1452 public int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount)
1453 {
1454 /* String name1 = null;
1455 String name2 = null;
1456
1457 if (!geom_name_map.TryGetValue(trimesh, out name1))
1458 {
1459 name1 = "null";
1460 }
1461 if (!geom_name_map.TryGetValue(refObject, out name2))
1462 {
1463 name2 = "null";
1464 }
1465
1466 m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2);
1467 */
1468 return 1;
1469 }
1470
1471 public int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex)
1472 {
1473 String name1 = null;
1474 String name2 = null;
1475
1476 if (!geom_name_map.TryGetValue(trimesh, out name1))
1477 {
1478 name1 = "null";
1479 }
1480
1481 if (!geom_name_map.TryGetValue(refObject, out name2))
1482 {
1483 name2 = "null";
1484 }
1485
1486 // m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex);
1487
1488 d.Vector3 v0 = new d.Vector3();
1489 d.Vector3 v1 = new d.Vector3();
1490 d.Vector3 v2 = new d.Vector3();
1491
1492 d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2);
1493 // 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);
1494
1495 return 1;
1496 }
1497
1498 /// <summary>
1499 /// This is our collision testing routine in ODE
1500 /// </summary>
1501 /// <param name="timeStep"></param>
1502 private void collision_optimized(float timeStep)
1503 {
1504 _perloopContact.Clear();
1505
1506 lock (_characters)
1507 {
1508 foreach (OdeCharacter chr in _characters)
1509 {
1510 // Reset the collision values to false
1511 // since we don't know if we're colliding yet
1512
1513 // For some reason this can happen. Don't ask...
1514 //
1515 if (chr == null)
1516 continue;
1517
1518 if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1519 continue;
1520
1521 chr.IsColliding = false;
1522 chr.CollidingGround = false;
1523 chr.CollidingObj = false;
1524
1525 // test the avatar's geometry for collision with the space
1526 // This will return near and the space that they are the closest to
1527 // And we'll run this again against the avatar and the space segment
1528 // This will return with a bunch of possible objects in the space segment
1529 // and we'll run it again on all of them.
1530 try
1531 {
1532 d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback);
1533 }
1534 catch (AccessViolationException)
1535 {
1536 m_log.Warn("[PHYSICS]: Unable to space collide");
1537 }
1538 //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
1539 //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
1540 //{
1541 //chr.Position.Z = terrainheight + 10.0f;
1542 //forcedZ = true;
1543 //}
1544 }
1545 }
1546
1547 lock (_activeprims)
1548 {
1549 List<OdePrim> removeprims = null;
1550 foreach (OdePrim chr in _activeprims)
1551 {
1552 if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled))
1553 {
1554 try
1555 {
1556 lock (chr)
1557 {
1558 if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false)
1559 {
1560 d.SpaceCollide2(space, chr.prim_geom, IntPtr.Zero, nearCallback);
1561 }
1562 else
1563 {
1564 if (removeprims == null)
1565 {
1566 removeprims = new List<OdePrim>();
1567 }
1568 removeprims.Add(chr);
1569 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!");
1570 }
1571 }
1572 }
1573 catch (AccessViolationException)
1574 {
1575 m_log.Warn("[PHYSICS]: Unable to space collide");
1576 }
1577 }
1578 }
1579 if (removeprims != null)
1580 {
1581 foreach (OdePrim chr in removeprims)
1582 {
1583 _activeprims.Remove(chr);
1584 }
1585 }
1586 }
1587
1588 _perloopContact.Clear();
1589 }
1590
1591 #endregion
1592
1593 public override void Combine(PhysicsScene pScene, Vector3 offset, Vector3 extents)
1594 {
1595 m_worldOffset = offset;
1596 WorldExtents = new Vector2(extents.X, extents.Y);
1597 m_parentScene = pScene;
1598
1599 }
1600
1601 // Recovered for use by fly height. Kitto Flora
1602 public float GetTerrainHeightAtXY(float x, float y)
1603 {
1604
1605 int offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1606 int offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize;
1607
1608 IntPtr heightFieldGeom = IntPtr.Zero;
1609
1610 if (RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom))
1611 {
1612 if (heightFieldGeom != IntPtr.Zero)
1613 {
1614 if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
1615 {
1616
1617 int index;
1618
1619
1620 if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y ||
1621 (int)x < 0.001f || (int)y < 0.001f)
1622 return 0;
1623
1624 x = x - offsetX;
1625 y = y - offsetY;
1626
1627 index = (int)((int)x * ((int)Constants.RegionSize + 2) + (int)y);
1628
1629 if (index < TerrainHeightFieldHeights[heightFieldGeom].Length)
1630 {
1631 //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]);
1632 return (float)TerrainHeightFieldHeights[heightFieldGeom][index];
1633 }
1634
1635 else
1636 return 0f;
1637 }
1638 else
1639 {
1640 return 0f;
1641 }
1642
1643 }
1644 else
1645 {
1646 return 0f;
1647 }
1648
1649 }
1650 else
1651 {
1652 return 0f;
1653 }
1654
1655
1656 }
1657// End recovered. Kitto Flora
1658
1659 public void addCollisionEventReporting(PhysicsActor obj)
1660 {
1661 lock (_collisionEventPrim)
1662 {
1663 if (!_collisionEventPrim.Contains(obj))
1664 _collisionEventPrim.Add(obj);
1665 }
1666 }
1667
1668 public void remCollisionEventReporting(PhysicsActor obj)
1669 {
1670 lock (_collisionEventPrim)
1671 {
1672 if (!_collisionEventPrim.Contains(obj))
1673 _collisionEventPrim.Remove(obj);
1674 }
1675 }
1676
1677 #region Add/Remove Entities
1678
1679 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 size, bool isFlying)
1680 {
1681 Vector3 pos;
1682 pos.X = position.X;
1683 pos.Y = position.Y;
1684 pos.Z = position.Z;
1685 OdeCharacter newAv = new OdeCharacter(avName, this, pos, ode, size, avPIDD, avPIDP, avCapRadius, avStandupTensor, avDensity, avHeightFudgeFactor, avMovementDivisorWalk, avMovementDivisorRun);
1686 newAv.Flying = isFlying;
1687 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1688
1689 return newAv;
1690 }
1691
1692 public void AddCharacter(OdeCharacter chr)
1693 {
1694 lock (_characters)
1695 {
1696 if (!_characters.Contains(chr))
1697 {
1698 _characters.Add(chr);
1699 if (chr.bad)
1700 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
1701 }
1702 }
1703 }
1704
1705 public void RemoveCharacter(OdeCharacter chr)
1706 {
1707 lock (_characters)
1708 {
1709 if (_characters.Contains(chr))
1710 {
1711 _characters.Remove(chr);
1712 }
1713 }
1714 }
1715 public void BadCharacter(OdeCharacter chr)
1716 {
1717 lock (_badCharacter)
1718 {
1719 if (!_badCharacter.Contains(chr))
1720 _badCharacter.Add(chr);
1721 }
1722 }
1723
1724 public override void RemoveAvatar(PhysicsActor actor)
1725 {
1726 //m_log.Debug("[PHYSICS]:ODELOCK");
1727 ((OdeCharacter) actor).Destroy();
1728
1729 }
1730
1731 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1732 IMesh mesh, PrimitiveBaseShape pbs, bool isphysical)
1733 {
1734
1735 Vector3 pos = position;
1736 Vector3 siz = size;
1737 Quaternion rot = rotation;
1738
1739 OdePrim newPrim;
1740 lock (OdeLock)
1741 {
1742 newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode);
1743
1744 lock (_prims)
1745 _prims.Add(newPrim);
1746 }
1747
1748 return newPrim;
1749 }
1750
1751 public void addActivePrim(OdePrim activatePrim)
1752 {
1753 // adds active prim.. (ones that should be iterated over in collisions_optimized
1754 lock (_activeprims)
1755 {
1756 if (!_activeprims.Contains(activatePrim))
1757 _activeprims.Add(activatePrim);
1758 //else
1759 // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
1760 }
1761 }
1762
1763 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1764 Vector3 size, Quaternion rotation) //To be removed
1765 {
1766 return AddPrimShape(primName, pbs, position, size, rotation, false);
1767 }
1768
1769 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1770 Vector3 size, Quaternion rotation, bool isPhysical)
1771 {
1772 PhysicsActor result;
1773 IMesh mesh = null;
1774
1775 if (needsMeshing(pbs))
1776 mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical);
1777
1778 result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical);
1779
1780 return result;
1781 }
1782
1783 public override float TimeDilation
1784 {
1785 get { return m_timeDilation; }
1786 }
1787
1788 public override bool SupportsNINJAJoints
1789 {
1790 get { return m_NINJA_physics_joints_enabled; }
1791 }
1792
1793 // internal utility function: must be called within a lock (OdeLock)
1794 private void InternalAddActiveJoint(PhysicsJoint joint)
1795 {
1796 activeJoints.Add(joint);
1797 SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
1798 }
1799
1800 // internal utility function: must be called within a lock (OdeLock)
1801 private void InternalAddPendingJoint(OdePhysicsJoint joint)
1802 {
1803 pendingJoints.Add(joint);
1804 SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
1805 }
1806
1807 // internal utility function: must be called within a lock (OdeLock)
1808 private void InternalRemovePendingJoint(PhysicsJoint joint)
1809 {
1810 pendingJoints.Remove(joint);
1811 SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene);
1812 }
1813
1814 // internal utility function: must be called within a lock (OdeLock)
1815 private void InternalRemoveActiveJoint(PhysicsJoint joint)
1816 {
1817 activeJoints.Remove(joint);
1818 SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
1819 }
1820
1821 public override void DumpJointInfo()
1822 {
1823 string hdr = "[NINJA] JOINTINFO: ";
1824 foreach (PhysicsJoint j in pendingJoints)
1825 {
1826 m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1827 }
1828 m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
1829 foreach (string jointName in SOPName_to_pendingJoint.Keys)
1830 {
1831 m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
1832 }
1833 m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
1834 foreach (PhysicsJoint j in activeJoints)
1835 {
1836 m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1837 }
1838 m_log.Debug(hdr + activeJoints.Count + " total active joints");
1839 foreach (string jointName in SOPName_to_activeJoint.Keys)
1840 {
1841 m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
1842 }
1843 m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
1844
1845 m_log.Debug(hdr + " Per-body joint connectivity information follows.");
1846 m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
1847 foreach (string actorName in joints_connecting_actor.Keys)
1848 {
1849 m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it");
1850 foreach (PhysicsJoint j in joints_connecting_actor[actorName])
1851 {
1852 m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1853 }
1854 m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
1855 }
1856 }
1857
1858 public override void RequestJointDeletion(string ObjectNameInScene)
1859 {
1860 lock (externalJointRequestsLock)
1861 {
1862 if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously
1863 {
1864 requestedJointsToBeDeleted.Add(ObjectNameInScene);
1865 }
1866 }
1867 }
1868
1869 private void DeleteRequestedJoints()
1870 {
1871 List<string> myRequestedJointsToBeDeleted;
1872 lock (externalJointRequestsLock)
1873 {
1874 // make a local copy of the shared list for processing (threading issues)
1875 myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
1876 }
1877
1878 foreach (string jointName in myRequestedJointsToBeDeleted)
1879 {
1880 lock (OdeLock)
1881 {
1882 //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
1883 if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
1884 {
1885 OdePhysicsJoint joint = null;
1886 if (SOPName_to_activeJoint.ContainsKey(jointName))
1887 {
1888 joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
1889 InternalRemoveActiveJoint(joint);
1890 }
1891 else if (SOPName_to_pendingJoint.ContainsKey(jointName))
1892 {
1893 joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
1894 InternalRemovePendingJoint(joint);
1895 }
1896
1897 if (joint != null)
1898 {
1899 //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
1900 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
1901 {
1902 string bodyName = joint.BodyNames[iBodyName];
1903 if (bodyName != "NULL")
1904 {
1905 joints_connecting_actor[bodyName].Remove(joint);
1906 if (joints_connecting_actor[bodyName].Count == 0)
1907 {
1908 joints_connecting_actor.Remove(bodyName);
1909 }
1910 }
1911 }
1912
1913 DoJointDeactivated(joint);
1914 if (joint.jointID != IntPtr.Zero)
1915 {
1916 d.JointDestroy(joint.jointID);
1917 joint.jointID = IntPtr.Zero;
1918 //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
1919 }
1920 else
1921 {
1922 //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
1923 }
1924 }
1925 else
1926 {
1927 // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
1928 }
1929 }
1930 else
1931 {
1932 // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
1933 }
1934 }
1935 }
1936
1937 // remove processed joints from the shared list
1938 lock (externalJointRequestsLock)
1939 {
1940 foreach (string jointName in myRequestedJointsToBeDeleted)
1941 {
1942 requestedJointsToBeDeleted.Remove(jointName);
1943 }
1944 }
1945 }
1946
1947 // for pending joints we don't know if their associated bodies exist yet or not.
1948 // the joint is actually created during processing of the taints
1949 private void CreateRequestedJoints()
1950 {
1951 List<PhysicsJoint> myRequestedJointsToBeCreated;
1952 lock (externalJointRequestsLock)
1953 {
1954 // make a local copy of the shared list for processing (threading issues)
1955 myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
1956 }
1957
1958 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
1959 {
1960 lock (OdeLock)
1961 {
1962 if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
1963 {
1964 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);
1965 continue;
1966 }
1967 if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
1968 {
1969 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);
1970 continue;
1971 }
1972
1973 InternalAddPendingJoint(joint as OdePhysicsJoint);
1974
1975 if (joint.BodyNames.Count >= 2)
1976 {
1977 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
1978 {
1979 string bodyName = joint.BodyNames[iBodyName];
1980 if (bodyName != "NULL")
1981 {
1982 if (!joints_connecting_actor.ContainsKey(bodyName))
1983 {
1984 joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
1985 }
1986 joints_connecting_actor[bodyName].Add(joint);
1987 }
1988 }
1989 }
1990 }
1991 }
1992
1993 // remove processed joints from shared list
1994 lock (externalJointRequestsLock)
1995 {
1996 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
1997 {
1998 requestedJointsToBeCreated.Remove(joint);
1999 }
2000 }
2001
2002 }
2003
2004 // public function to add an request for joint creation
2005 // this joint will just be added to a waiting list that is NOT processed during the main
2006 // Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
2007
2008 public override PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, Vector3 position,
2009 Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
2010
2011 {
2012
2013 OdePhysicsJoint joint = new OdePhysicsJoint();
2014 joint.ObjectNameInScene = objectNameInScene;
2015 joint.Type = jointType;
2016 joint.Position = position;
2017 joint.Rotation = rotation;
2018 joint.RawParams = parms;
2019 joint.BodyNames = new List<string>(bodyNames);
2020 joint.TrackedBodyName = trackedBodyName;
2021 joint.LocalRotation = localRotation;
2022 joint.jointID = IntPtr.Zero;
2023 joint.ErrorMessageCount = 0;
2024
2025 lock (externalJointRequestsLock)
2026 {
2027 if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice
2028 {
2029 requestedJointsToBeCreated.Add(joint);
2030 }
2031 }
2032 return joint;
2033 }
2034
2035 private void RemoveAllJointsConnectedToActor(PhysicsActor actor)
2036 {
2037 //m_log.Debug("RemoveAllJointsConnectedToActor: start");
2038 if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
2039 {
2040
2041 List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
2042 //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)
2043 foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
2044 {
2045 jointsToRemove.Add(j);
2046 }
2047 foreach (PhysicsJoint j in jointsToRemove)
2048 {
2049 //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
2050 RequestJointDeletion(j.ObjectNameInScene);
2051 //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
2052 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)
2053 }
2054 }
2055 }
2056
2057 public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
2058 {
2059 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start");
2060 lock (OdeLock)
2061 {
2062 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
2063 RemoveAllJointsConnectedToActor(actor);
2064 }
2065 }
2066
2067 // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2068 public override Vector3 GetJointAnchor(PhysicsJoint joint)
2069 {
2070 Debug.Assert(joint.IsInPhysicsEngine);
2071 d.Vector3 pos = new d.Vector3();
2072
2073 if (!(joint is OdePhysicsJoint))
2074 {
2075 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
2076 }
2077 else
2078 {
2079 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2080 switch (odeJoint.Type)
2081 {
2082 case PhysicsJointType.Ball:
2083 d.JointGetBallAnchor(odeJoint.jointID, out pos);
2084 break;
2085 case PhysicsJointType.Hinge:
2086 d.JointGetHingeAnchor(odeJoint.jointID, out pos);
2087 break;
2088 }
2089 }
2090 return new Vector3(pos.X, pos.Y, pos.Z);
2091 }
2092
2093 // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2094 // WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
2095 // appears to be unreliable. Fortunately we can compute the joint axis ourselves by
2096 // keeping track of the joint's original orientation relative to one of the involved bodies.
2097 public override Vector3 GetJointAxis(PhysicsJoint joint)
2098 {
2099 Debug.Assert(joint.IsInPhysicsEngine);
2100 d.Vector3 axis = new d.Vector3();
2101
2102 if (!(joint is OdePhysicsJoint))
2103 {
2104 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
2105 }
2106 else
2107 {
2108 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2109 switch (odeJoint.Type)
2110 {
2111 case PhysicsJointType.Ball:
2112 DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
2113 break;
2114 case PhysicsJointType.Hinge:
2115 d.JointGetHingeAxis(odeJoint.jointID, out axis);
2116 break;
2117 }
2118 }
2119 return new Vector3(axis.X, axis.Y, axis.Z);
2120 }
2121
2122
2123 public void remActivePrim(OdePrim deactivatePrim)
2124 {
2125 lock (_activeprims)
2126 {
2127 _activeprims.Remove(deactivatePrim);
2128 }
2129 }
2130
2131 public override void RemovePrim(PhysicsActor prim)
2132 {
2133 if (prim is OdePrim)
2134 {
2135 lock (OdeLock)
2136 {
2137 OdePrim p = (OdePrim) prim;
2138
2139 p.setPrimForRemoval();
2140 AddPhysicsActorTaint(prim);
2141 //RemovePrimThreadLocked(p);
2142 }
2143 }
2144 }
2145
2146 /// <summary>
2147 /// This is called from within simulate but outside the locked portion
2148 /// We need to do our own locking here
2149 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
2150 ///
2151 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
2152 /// that the space was using.
2153 /// </summary>
2154 /// <param name="prim"></param>
2155 public void RemovePrimThreadLocked(OdePrim prim)
2156 {
2157//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
2158 lock (prim)
2159 {
2160 remCollisionEventReporting(prim);
2161 lock (ode)
2162 {
2163 if (prim.prim_geom != IntPtr.Zero)
2164 {
2165 prim.ResetTaints();
2166
2167 if (prim.IsPhysical)
2168 {
2169 prim.disableBody();
2170 if (prim.childPrim)
2171 {
2172 prim.childPrim = false;
2173 prim.Body = IntPtr.Zero;
2174 prim.m_disabled = true;
2175 prim.IsPhysical = false;
2176 }
2177
2178
2179 }
2180 // we don't want to remove the main space
2181
2182 // If the geometry is in the targetspace, remove it from the target space
2183 //m_log.Warn(prim.m_targetSpace);
2184
2185 //if (prim.m_targetSpace != IntPtr.Zero)
2186 //{
2187 //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom))
2188 //{
2189
2190 //if (d.GeomIsSpace(prim.m_targetSpace))
2191 //{
2192 //waitForSpaceUnlock(prim.m_targetSpace);
2193 //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom);
2194 prim.m_targetSpace = IntPtr.Zero;
2195 //}
2196 //else
2197 //{
2198 // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2199 //((OdePrim)prim).m_targetSpace.ToString());
2200 //}
2201
2202 //}
2203 //}
2204 //m_log.Warn(prim.prim_geom);
2205 try
2206 {
2207 if (prim.prim_geom != IntPtr.Zero)
2208 {
2209
2210//string tPA;
2211//geom_name_map.TryGetValue(prim.prim_geom, out tPA);
2212//Console.WriteLine("**** Remove {0}", tPA);
2213 if(geom_name_map.ContainsKey(prim.prim_geom)) geom_name_map.Remove(prim.prim_geom);
2214 if(actor_name_map.ContainsKey(prim.prim_geom)) actor_name_map.Remove(prim.prim_geom);
2215 d.GeomDestroy(prim.prim_geom);
2216 prim.prim_geom = IntPtr.Zero;
2217 }
2218 else
2219 {
2220 m_log.Warn("[PHYSICS]: Unable to remove prim from physics scene");
2221 }
2222 }
2223 catch (AccessViolationException)
2224 {
2225 m_log.Info("[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed.");
2226 }
2227 lock (_prims)
2228 _prims.Remove(prim);
2229
2230 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2231 //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
2232 //{
2233 //if (prim.m_targetSpace != null)
2234 //{
2235 //if (d.GeomIsSpace(prim.m_targetSpace))
2236 //{
2237 //waitForSpaceUnlock(prim.m_targetSpace);
2238 //d.SpaceRemove(space, prim.m_targetSpace);
2239 // free up memory used by the space.
2240 //d.SpaceDestroy(prim.m_targetSpace);
2241 //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
2242 //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
2243 //}
2244 //else
2245 //{
2246 //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2247 //((OdePrim) prim).m_targetSpace.ToString());
2248 //}
2249 //}
2250 //}
2251
2252 if (SupportsNINJAJoints)
2253 {
2254 RemoveAllJointsConnectedToActorThreadLocked(prim);
2255 }
2256 }
2257 }
2258 }
2259 }
2260
2261 #endregion
2262
2263 #region Space Separation Calculation
2264
2265 /// <summary>
2266 /// Takes a space pointer and zeros out the array we're using to hold the spaces
2267 /// </summary>
2268 /// <param name="pSpace"></param>
2269 public void resetSpaceArrayItemToZero(IntPtr pSpace)
2270 {
2271 for (int x = 0; x < staticPrimspace.GetLength(0); x++)
2272 {
2273 for (int y = 0; y < staticPrimspace.GetLength(1); y++)
2274 {
2275 if (staticPrimspace[x, y] == pSpace)
2276 staticPrimspace[x, y] = IntPtr.Zero;
2277 }
2278 }
2279 }
2280
2281 public void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY)
2282 {
2283 staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero;
2284 }
2285
2286 /// <summary>
2287 /// Called when a static prim moves. Allocates a space for the prim based on its position
2288 /// </summary>
2289 /// <param name="geom">the pointer to the geom that moved</param>
2290 /// <param name="pos">the position that the geom moved to</param>
2291 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
2292 /// <returns>a pointer to the new space it's in</returns>
2293 public IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace)
2294 {
2295 // Called from setting the Position and Size of an ODEPrim so
2296 // it's already in locked space.
2297
2298 // we don't want to remove the main space
2299 // we don't need to test physical here because this function should
2300 // never be called if the prim is physical(active)
2301
2302 // All physical prim end up in the root space
2303 //Thread.Sleep(20);
2304 if (currentspace != space)
2305 {
2306 //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString());
2307 //if (currentspace == IntPtr.Zero)
2308 //{
2309 //int adfadf = 0;
2310 //}
2311 if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero)
2312 {
2313 if (d.GeomIsSpace(currentspace))
2314 {
2315 waitForSpaceUnlock(currentspace);
2316 d.SpaceRemove(currentspace, geom);
2317 }
2318 else
2319 {
2320 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" + currentspace +
2321 " Geom:" + geom);
2322 }
2323 }
2324 else
2325 {
2326 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2327 if (sGeomIsIn != IntPtr.Zero)
2328 {
2329 if (d.GeomIsSpace(currentspace))
2330 {
2331 waitForSpaceUnlock(sGeomIsIn);
2332 d.SpaceRemove(sGeomIsIn, geom);
2333 }
2334 else
2335 {
2336 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2337 sGeomIsIn + " Geom:" + geom);
2338 }
2339 }
2340 }
2341
2342 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2343 if (d.SpaceGetNumGeoms(currentspace) == 0)
2344 {
2345 if (currentspace != IntPtr.Zero)
2346 {
2347 if (d.GeomIsSpace(currentspace))
2348 {
2349 waitForSpaceUnlock(currentspace);
2350 waitForSpaceUnlock(space);
2351 d.SpaceRemove(space, currentspace);
2352 // free up memory used by the space.
2353
2354 //d.SpaceDestroy(currentspace);
2355 resetSpaceArrayItemToZero(currentspace);
2356 }
2357 else
2358 {
2359 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2360 currentspace + " Geom:" + geom);
2361 }
2362 }
2363 }
2364 }
2365 else
2366 {
2367 // this is a physical object that got disabled. ;.;
2368 if (currentspace != IntPtr.Zero && geom != IntPtr.Zero)
2369 {
2370 if (d.SpaceQuery(currentspace, geom))
2371 {
2372 if (d.GeomIsSpace(currentspace))
2373 {
2374 waitForSpaceUnlock(currentspace);
2375 d.SpaceRemove(currentspace, geom);
2376 }
2377 else
2378 {
2379 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2380 currentspace + " Geom:" + geom);
2381 }
2382 }
2383 else
2384 {
2385 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2386 if (sGeomIsIn != IntPtr.Zero)
2387 {
2388 if (d.GeomIsSpace(sGeomIsIn))
2389 {
2390 waitForSpaceUnlock(sGeomIsIn);
2391 d.SpaceRemove(sGeomIsIn, geom);
2392 }
2393 else
2394 {
2395 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2396 sGeomIsIn + " Geom:" + geom);
2397 }
2398 }
2399 }
2400 }
2401 }
2402
2403 // The routines in the Position and Size sections do the 'inserting' into the space,
2404 // so all we have to do is make sure that the space that we're putting the prim into
2405 // is in the 'main' space.
2406 int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos);
2407 IntPtr newspace = calculateSpaceForGeom(pos);
2408
2409 if (newspace == IntPtr.Zero)
2410 {
2411 newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
2412 d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh);
2413 }
2414
2415 return newspace;
2416 }
2417
2418 /// <summary>
2419 /// Creates a new space at X Y
2420 /// </summary>
2421 /// <param name="iprimspaceArrItemX"></param>
2422 /// <param name="iprimspaceArrItemY"></param>
2423 /// <returns>A pointer to the created space</returns>
2424 public IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
2425 {
2426 // creating a new space for prim and inserting it into main space.
2427 staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero);
2428 d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space);
2429 waitForSpaceUnlock(space);
2430 d.SpaceSetSublevel(space, 1);
2431 d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
2432 return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
2433 }
2434
2435 /// <summary>
2436 /// Calculates the space the prim should be in by its position
2437 /// </summary>
2438 /// <param name="pos"></param>
2439 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
2440 public IntPtr calculateSpaceForGeom(Vector3 pos)
2441 {
2442 int[] xyspace = calculateSpaceArrayItemFromPos(pos);
2443 //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
2444 return staticPrimspace[xyspace[0], xyspace[1]];
2445 }
2446
2447 /// <summary>
2448 /// Holds the space allocation logic
2449 /// </summary>
2450 /// <param name="pos"></param>
2451 /// <returns>an array item based on the position</returns>
2452 public int[] calculateSpaceArrayItemFromPos(Vector3 pos)
2453 {
2454 int[] returnint = new int[2];
2455
2456 returnint[0] = (int) (pos.X/metersInSpace);
2457
2458 if (returnint[0] > ((int) (259f/metersInSpace)))
2459 returnint[0] = ((int) (259f/metersInSpace));
2460 if (returnint[0] < 0)
2461 returnint[0] = 0;
2462
2463 returnint[1] = (int) (pos.Y/metersInSpace);
2464 if (returnint[1] > ((int) (259f/metersInSpace)))
2465 returnint[1] = ((int) (259f/metersInSpace));
2466 if (returnint[1] < 0)
2467 returnint[1] = 0;
2468
2469 return returnint;
2470 }
2471
2472 #endregion
2473
2474 /// <summary>
2475 /// Routine to figure out if we need to mesh this prim with our mesher
2476 /// </summary>
2477 /// <param name="pbs"></param>
2478 /// <returns></returns>
2479 public bool needsMeshing(PrimitiveBaseShape pbs)
2480 {
2481 // most of this is redundant now as the mesher will return null if it cant mesh a prim
2482 // but we still need to check for sculptie meshing being enabled so this is the most
2483 // convenient place to do it for now...
2484
2485 // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
2486 // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
2487 int iPropertiesNotSupportedDefault = 0;
2488
2489 if (pbs.SculptEntry && !meshSculptedPrim)
2490 {
2491#if SPAM
2492 m_log.Warn("NonMesh");
2493#endif
2494 return false;
2495 }
2496
2497 // 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
2498 if (!forceSimplePrimMeshing)
2499 {
2500 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
2501 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
2502 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
2503 {
2504
2505 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
2506 && pbs.ProfileHollow == 0
2507 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
2508 && pbs.PathBegin == 0 && pbs.PathEnd == 0
2509 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
2510 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
2511 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
2512 {
2513#if SPAM
2514 m_log.Warn("NonMesh");
2515#endif
2516 return false;
2517 }
2518 }
2519 }
2520
2521 if (pbs.ProfileHollow != 0)
2522 iPropertiesNotSupportedDefault++;
2523
2524 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
2525 iPropertiesNotSupportedDefault++;
2526
2527 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
2528 iPropertiesNotSupportedDefault++;
2529
2530 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
2531 iPropertiesNotSupportedDefault++;
2532
2533 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
2534 iPropertiesNotSupportedDefault++;
2535
2536 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
2537 iPropertiesNotSupportedDefault++;
2538
2539 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))
2540 iPropertiesNotSupportedDefault++;
2541
2542 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
2543 iPropertiesNotSupportedDefault++;
2544
2545 // test for torus
2546 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
2547 {
2548 if (pbs.PathCurve == (byte)Extrusion.Curve1)
2549 {
2550 iPropertiesNotSupportedDefault++;
2551 }
2552 }
2553 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
2554 {
2555 if (pbs.PathCurve == (byte)Extrusion.Straight)
2556 {
2557 iPropertiesNotSupportedDefault++;
2558 }
2559
2560 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
2561 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2562 {
2563 iPropertiesNotSupportedDefault++;
2564 }
2565 }
2566 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
2567 {
2568 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
2569 {
2570 iPropertiesNotSupportedDefault++;
2571 }
2572 }
2573 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
2574 {
2575 if (pbs.PathCurve == (byte)Extrusion.Straight)
2576 {
2577 iPropertiesNotSupportedDefault++;
2578 }
2579 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2580 {
2581 iPropertiesNotSupportedDefault++;
2582 }
2583 }
2584
2585
2586 if (iPropertiesNotSupportedDefault == 0)
2587 {
2588#if SPAM
2589 m_log.Warn("NonMesh");
2590#endif
2591 return false;
2592 }
2593#if SPAM
2594 m_log.Debug("Mesh");
2595#endif
2596 return true;
2597 }
2598
2599 /// <summary>
2600 /// Called after our prim properties are set Scale, position etc.
2601 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
2602 /// This assures us that we have no race conditions
2603 /// </summary>
2604 /// <param name="prim"></param>
2605 public override void AddPhysicsActorTaint(PhysicsActor prim)
2606 {
2607
2608 if (prim is OdePrim)
2609 {
2610 OdePrim taintedprim = ((OdePrim) prim);
2611 lock (_taintedPrimLock)
2612 {
2613 if (!(_taintedPrimH.Contains(taintedprim)))
2614 {
2615//Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.m_primName);
2616 _taintedPrimH.Add(taintedprim); // HashSet for searching
2617 _taintedPrimL.Add(taintedprim); // List for ordered readout
2618 }
2619 }
2620 return;
2621 }
2622 else if (prim is OdeCharacter)
2623 {
2624 OdeCharacter taintedchar = ((OdeCharacter)prim);
2625 lock (_taintedActors)
2626 {
2627 if (!(_taintedActors.Contains(taintedchar)))
2628 {
2629 _taintedActors.Add(taintedchar);
2630 if (taintedchar.bad)
2631 m_log.DebugFormat("[PHYSICS]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid);
2632 }
2633 }
2634 }
2635 }
2636
2637 /// <summary>
2638 /// This is our main simulate loop
2639 /// It's thread locked by a Mutex in the scene.
2640 /// It holds Collisions, it instructs ODE to step through the physical reactions
2641 /// It moves the objects around in memory
2642 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
2643 /// </summary>
2644 /// <param name="timeStep"></param>
2645 /// <returns></returns>
2646 public override float Simulate(float timeStep)
2647 {
2648 if (framecount >= int.MaxValue)
2649 framecount = 0;
2650 //if (m_worldOffset != Vector3.Zero)
2651 // return 0;
2652
2653 framecount++;
2654
2655 DateTime now = DateTime.UtcNow;
2656 TimeSpan SinceLastFrame = now - m_lastframe;
2657 m_lastframe = now;
2658 float realtime = (float)SinceLastFrame.TotalSeconds;
2659// Console.WriteLine("ts={0} rt={1}", timeStep, realtime);
2660 timeStep = realtime;
2661
2662 // float fps = 1.0f / realtime;
2663 float fps = 0.0f; // number of ODE steps in this Simulate step
2664 //m_log.Info(timeStep.ToString());
2665 step_time += timeStep;
2666
2667 // If We're loaded down by something else,
2668 // or debugging with the Visual Studio project on pause
2669 // skip a few frames to catch up gracefully.
2670 // without shooting the physicsactors all over the place
2671
2672 if (step_time >= m_SkipFramesAtms)
2673 {
2674 // Instead of trying to catch up, it'll do 5 physics frames only
2675 step_time = ODE_STEPSIZE;
2676 m_physicsiterations = 5;
2677 }
2678 else
2679 {
2680 m_physicsiterations = 10;
2681 }
2682
2683 if (SupportsNINJAJoints)
2684 {
2685 DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2686 CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2687 }
2688
2689 lock (OdeLock)
2690 {
2691 // Process 10 frames if the sim is running normal..
2692 // process 5 frames if the sim is running slow
2693 //try
2694 //{
2695 //d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
2696 //}
2697 //catch (StackOverflowException)
2698 //{
2699 // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
2700 // ode.drelease(world);
2701 //base.TriggerPhysicsBasedRestart();
2702 //}
2703
2704 int i = 0;
2705
2706 // Figure out the Frames Per Second we're going at.
2707 //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size
2708
2709 // fps = (step_time / ODE_STEPSIZE) * 1000;
2710 // HACK: Using a time dilation of 1.0 to debug rubberbanding issues
2711 //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f);
2712
2713 // step_time = 0.09375f;
2714
2715 while (step_time > 0.0f)
2716 {
2717 //lock (ode)
2718 //{
2719 //if (!ode.lockquery())
2720 //{
2721 // ode.dlock(world);
2722 try
2723 {
2724 // Insert, remove Characters
2725 bool processedtaints = false;
2726
2727 lock (_taintedActors)
2728 {
2729 if (_taintedActors.Count > 0)
2730 {
2731 foreach (OdeCharacter character in _taintedActors)
2732 {
2733
2734 character.ProcessTaints(ODE_STEPSIZE);
2735
2736 processedtaints = true;
2737 //character.m_collisionscore = 0;
2738 }
2739
2740 if (processedtaints)
2741 _taintedActors.Clear();
2742 }
2743 } // end lock _taintedActors
2744
2745 // Modify other objects in the scene.
2746 processedtaints = false;
2747
2748 lock (_taintedPrimLock)
2749 {
2750 foreach (OdePrim prim in _taintedPrimL)
2751 {
2752 if (prim.m_taintremove)
2753 {
2754 //Console.WriteLine("Simulate calls RemovePrimThreadLocked");
2755 RemovePrimThreadLocked(prim);
2756 }
2757 else
2758 {
2759 //Console.WriteLine("Simulate calls ProcessTaints");
2760 prim.ProcessTaints(ODE_STEPSIZE);
2761 }
2762 processedtaints = true;
2763 prim.m_collisionscore = 0;
2764
2765 // This loop can block up the Heartbeat for a very long time on large regions.
2766 // We need to let the Watchdog know that the Heartbeat is not dead
2767 // NOTE: This is currently commented out, but if things like OAR loading are
2768 // timing the heartbeat out we will need to uncomment it
2769 //Watchdog.UpdateThread();
2770 }
2771
2772 if (SupportsNINJAJoints)
2773 {
2774 // Create pending joints, if possible
2775
2776 // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
2777 // a joint requires specifying the body id of both involved bodies
2778 if (pendingJoints.Count > 0)
2779 {
2780 List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
2781 //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
2782 foreach (PhysicsJoint joint in pendingJoints)
2783 {
2784 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
2785 string[] jointParams = joint.RawParams.Split(" ".ToCharArray(),
2786 System.StringSplitOptions.RemoveEmptyEntries);
2787 List<IntPtr> jointBodies = new List<IntPtr>();
2788 bool allJointBodiesAreReady = true;
2789 foreach (string jointParam in jointParams)
2790 {
2791 if (jointParam == "NULL")
2792 {
2793 //DoJointErrorMessage(joint, "attaching NULL joint to world");
2794 jointBodies.Add(IntPtr.Zero);
2795 }
2796 else
2797 {
2798 //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
2799 bool foundPrim = false;
2800 lock (_prims)
2801 {
2802 foreach (OdePrim prim in _prims) // FIXME: inefficient
2803 {
2804 if (prim.SOPName == jointParam)
2805 {
2806 //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
2807 if (prim.IsPhysical && prim.Body != IntPtr.Zero)
2808 {
2809 jointBodies.Add(prim.Body);
2810 foundPrim = true;
2811 break;
2812 }
2813 else
2814 {
2815 DoJointErrorMessage(joint, "prim name " + jointParam +
2816 " exists but is not (yet) physical; deferring joint creation. " +
2817 "IsPhysical property is " + prim.IsPhysical +
2818 " and body is " + prim.Body);
2819 foundPrim = false;
2820 break;
2821 }
2822 }
2823 }
2824 }
2825 if (foundPrim)
2826 {
2827 // all is fine
2828 }
2829 else
2830 {
2831 allJointBodiesAreReady = false;
2832 break;
2833 }
2834 }
2835 }
2836 if (allJointBodiesAreReady)
2837 {
2838 //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
2839 if (jointBodies[0] == jointBodies[1])
2840 {
2841 DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
2842 }
2843 else
2844 {
2845 switch (joint.Type)
2846 {
2847 case PhysicsJointType.Ball:
2848 {
2849 IntPtr odeJoint;
2850 //DoJointErrorMessage(joint, "ODE creating ball joint ");
2851 odeJoint = d.JointCreateBall(world, IntPtr.Zero);
2852 //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2853 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2854 //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
2855 d.JointSetBallAnchor(odeJoint,
2856 joint.Position.X,
2857 joint.Position.Y,
2858 joint.Position.Z);
2859 //DoJointErrorMessage(joint, "ODE joint setting OK");
2860 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
2861 //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
2862 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
2863 //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
2864
2865 if (joint is OdePhysicsJoint)
2866 {
2867 ((OdePhysicsJoint)joint).jointID = odeJoint;
2868 }
2869 else
2870 {
2871 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2872 }
2873 }
2874 break;
2875 case PhysicsJointType.Hinge:
2876 {
2877 IntPtr odeJoint;
2878 //DoJointErrorMessage(joint, "ODE creating hinge joint ");
2879 odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
2880 //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2881 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2882 //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
2883 d.JointSetHingeAnchor(odeJoint,
2884 joint.Position.X,
2885 joint.Position.Y,
2886 joint.Position.Z);
2887 // We use the orientation of the x-axis of the joint's coordinate frame
2888 // as the axis for the hinge.
2889
2890 // Therefore, we must get the joint's coordinate frame based on the
2891 // joint.Rotation field, which originates from the orientation of the
2892 // joint's proxy object in the scene.
2893
2894 // The joint's coordinate frame is defined as the transformation matrix
2895 // that converts a vector from joint-local coordinates into world coordinates.
2896 // World coordinates are defined as the XYZ coordinate system of the sim,
2897 // as shown in the top status-bar of the viewer.
2898
2899 // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
2900 // and use that as the hinge axis.
2901
2902 //joint.Rotation.Normalize();
2903 Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
2904
2905 // Now extract the X axis of the joint's coordinate frame.
2906
2907 // Do not try to use proxyFrame.AtAxis or you will become mired in the
2908 // tar pit of transposed, inverted, and generally messed-up orientations.
2909 // (In other words, Matrix4.AtAxis() is borked.)
2910 // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
2911
2912 // Instead, compute the X axis of the coordinate frame by transforming
2913 // the (1,0,0) vector. At least that works.
2914
2915 //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
2916 Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
2917 //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
2918 //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
2919 d.JointSetHingeAxis(odeJoint,
2920 jointAxis.X,
2921 jointAxis.Y,
2922 jointAxis.Z);
2923 //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
2924 if (joint is OdePhysicsJoint)
2925 {
2926 ((OdePhysicsJoint)joint).jointID = odeJoint;
2927 }
2928 else
2929 {
2930 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2931 }
2932 }
2933 break;
2934 }
2935 successfullyProcessedPendingJoints.Add(joint);
2936 }
2937 }
2938 else
2939 {
2940 DoJointErrorMessage(joint, "joint could not yet be created; still pending");
2941 }
2942 }
2943 foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
2944 {
2945 //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
2946 //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
2947 InternalRemovePendingJoint(successfullyProcessedJoint);
2948 //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
2949 InternalAddActiveJoint(successfullyProcessedJoint);
2950 //DoJointErrorMessage(successfullyProcessedJoint, "done");
2951 }
2952 }
2953 } // end SupportsNINJAJoints
2954
2955 if (processedtaints)
2956//Console.WriteLine("Simulate calls Clear of _taintedPrim list");
2957 _taintedPrimH.Clear(); // ??? if this only ???
2958 _taintedPrimL.Clear();
2959 } // end lock _taintedPrimLock
2960
2961 // Move characters
2962 lock (_characters)
2963 {
2964 List<OdeCharacter> defects = new List<OdeCharacter>();
2965 foreach (OdeCharacter actor in _characters)
2966 {
2967 if (actor != null)
2968 actor.Move(ODE_STEPSIZE, defects);
2969 }
2970 if (0 != defects.Count)
2971 {
2972 foreach (OdeCharacter defect in defects)
2973 {
2974 RemoveCharacter(defect);
2975 }
2976 }
2977 } // end lock _characters
2978
2979 // Move other active objects
2980 lock (_activeprims)
2981 {
2982 foreach (OdePrim prim in _activeprims)
2983 {
2984 prim.m_collisionscore = 0;
2985 prim.Move(ODE_STEPSIZE);
2986 }
2987 } // end lock _activeprims
2988
2989 //if ((framecount % m_randomizeWater) == 0)
2990 // randomizeWater(waterlevel);
2991
2992 //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
2993 m_rayCastManager.ProcessQueuedRequests();
2994
2995 collision_optimized(ODE_STEPSIZE);
2996
2997 lock (_collisionEventPrim)
2998 {
2999 foreach (PhysicsActor obj in _collisionEventPrim)
3000 {
3001 if (obj == null)
3002 continue;
3003
3004 switch ((ActorTypes)obj.PhysicsActorType)
3005 {
3006 case ActorTypes.Agent:
3007 OdeCharacter cobj = (OdeCharacter)obj;
3008 cobj.AddCollisionFrameTime(100);
3009 cobj.SendCollisions();
3010 break;
3011 case ActorTypes.Prim:
3012 OdePrim pobj = (OdePrim)obj;
3013 pobj.SendCollisions();
3014 break;
3015 }
3016 }
3017 } // end lock _collisionEventPrim
3018
3019 //if (m_global_contactcount > 5)
3020 //{
3021 // m_log.DebugFormat("[PHYSICS]: Contacts:{0}", m_global_contactcount);
3022 //}
3023
3024 m_global_contactcount = 0;
3025
3026 d.WorldQuickStep(world, ODE_STEPSIZE);
3027 d.JointGroupEmpty(contactgroup);
3028 fps++;
3029 //ode.dunlock(world);
3030 } // end try
3031 catch (Exception e)
3032 {
3033 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
3034 ode.dunlock(world);
3035 }
3036
3037 step_time -= ODE_STEPSIZE;
3038 i++;
3039 //}
3040 //else
3041 //{
3042 //fps = 0;
3043 //}
3044 //}
3045 } // end while (step_time > 0.0f)
3046
3047 lock (_characters)
3048 {
3049 foreach (OdeCharacter actor in _characters)
3050 {
3051 if (actor != null)
3052 {
3053 if (actor.bad)
3054 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
3055 actor.UpdatePositionAndVelocity();
3056 }
3057 }
3058 }
3059
3060 lock (_badCharacter)
3061 {
3062 if (_badCharacter.Count > 0)
3063 {
3064 foreach (OdeCharacter chr in _badCharacter)
3065 {
3066 RemoveCharacter(chr);
3067 }
3068 _badCharacter.Clear();
3069 }
3070 }
3071
3072 lock (_activeprims)
3073 {
3074 //if (timeStep < 0.2f)
3075 {
3076 foreach (OdePrim actor in _activeprims)
3077 {
3078 if (actor.IsPhysical && (d.BodyIsEnabled(actor.Body) || !actor._zeroFlag))
3079 {
3080 actor.UpdatePositionAndVelocity();
3081
3082 if (SupportsNINJAJoints)
3083 {
3084 // If an actor moved, move its joint proxy objects as well.
3085 // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
3086 // for this purpose but it is never called! So we just do the joint
3087 // movement code here.
3088
3089 if (actor.SOPName != null &&
3090 joints_connecting_actor.ContainsKey(actor.SOPName) &&
3091 joints_connecting_actor[actor.SOPName] != null &&
3092 joints_connecting_actor[actor.SOPName].Count > 0)
3093 {
3094 foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
3095 {
3096 if (affectedJoint.IsInPhysicsEngine)
3097 {
3098 DoJointMoved(affectedJoint);
3099 }
3100 else
3101 {
3102 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);
3103 }
3104 }
3105 }
3106 }
3107 }
3108 }
3109 }
3110 } // end lock _activeprims
3111
3112 //DumpJointInfo();
3113
3114 // Finished with all sim stepping. If requested, dump world state to file for debugging.
3115 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
3116 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
3117 if (physics_logging && (physics_logging_interval>0) && (framecount % physics_logging_interval == 0))
3118 {
3119 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
3120 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
3121
3122 if (physics_logging_append_existing_logfile)
3123 {
3124 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
3125 TextWriter fwriter = File.AppendText(fname);
3126 fwriter.WriteLine(header);
3127 fwriter.Close();
3128 }
3129 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
3130 }
3131 } // end lock OdeLock
3132
3133 return fps * 1000.0f; //NB This is a FRAME COUNT, not a time! AND is divide by 1000 in SimStatusReporter!
3134 } // end Simulate
3135
3136 public override void GetResults()
3137 {
3138 }
3139
3140 public override bool IsThreaded
3141 {
3142 // for now we won't be multithreaded
3143 get { return (false); }
3144 }
3145
3146 #region ODE Specific Terrain Fixes
3147 public float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
3148 {
3149 float[] returnarr = new float[262144];
3150 float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
3151
3152 // Filling out the array into its multi-dimensional components
3153 for (int y = 0; y < WorldExtents.Y; y++)
3154 {
3155 for (int x = 0; x < WorldExtents.X; x++)
3156 {
3157 resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
3158 }
3159 }
3160
3161 // Resize using Nearest Neighbour
3162
3163 // This particular way is quick but it only works on a multiple of the original
3164
3165 // The idea behind this method can be described with the following diagrams
3166 // second pass and third pass happen in the same loop really.. just separated
3167 // them to show what this does.
3168
3169 // First Pass
3170 // ResultArr:
3171 // 1,1,1,1,1,1
3172 // 1,1,1,1,1,1
3173 // 1,1,1,1,1,1
3174 // 1,1,1,1,1,1
3175 // 1,1,1,1,1,1
3176 // 1,1,1,1,1,1
3177
3178 // Second Pass
3179 // ResultArr2:
3180 // 1,,1,,1,,1,,1,,1,
3181 // ,,,,,,,,,,
3182 // 1,,1,,1,,1,,1,,1,
3183 // ,,,,,,,,,,
3184 // 1,,1,,1,,1,,1,,1,
3185 // ,,,,,,,,,,
3186 // 1,,1,,1,,1,,1,,1,
3187 // ,,,,,,,,,,
3188 // 1,,1,,1,,1,,1,,1,
3189 // ,,,,,,,,,,
3190 // 1,,1,,1,,1,,1,,1,
3191
3192 // Third pass fills in the blanks
3193 // ResultArr2:
3194 // 1,1,1,1,1,1,1,1,1,1,1,1
3195 // 1,1,1,1,1,1,1,1,1,1,1,1
3196 // 1,1,1,1,1,1,1,1,1,1,1,1
3197 // 1,1,1,1,1,1,1,1,1,1,1,1
3198 // 1,1,1,1,1,1,1,1,1,1,1,1
3199 // 1,1,1,1,1,1,1,1,1,1,1,1
3200 // 1,1,1,1,1,1,1,1,1,1,1,1
3201 // 1,1,1,1,1,1,1,1,1,1,1,1
3202 // 1,1,1,1,1,1,1,1,1,1,1,1
3203 // 1,1,1,1,1,1,1,1,1,1,1,1
3204 // 1,1,1,1,1,1,1,1,1,1,1,1
3205
3206 // X,Y = .
3207 // X+1,y = ^
3208 // X,Y+1 = *
3209 // X+1,Y+1 = #
3210
3211 // Filling in like this;
3212 // .*
3213 // ^#
3214 // 1st .
3215 // 2nd *
3216 // 3rd ^
3217 // 4th #
3218 // on single loop.
3219
3220 float[,] resultarr2 = new float[512, 512];
3221 for (int y = 0; y < WorldExtents.Y; y++)
3222 {
3223 for (int x = 0; x < WorldExtents.X; x++)
3224 {
3225 resultarr2[y * 2, x * 2] = resultarr[y, x];
3226
3227 if (y < WorldExtents.Y)
3228 {
3229 resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
3230 }
3231 if (x < WorldExtents.X)
3232 {
3233 resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
3234 }
3235 if (x < WorldExtents.X && y < WorldExtents.Y)
3236 {
3237 resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
3238 }
3239 }
3240 }
3241
3242 //Flatten out the array
3243 int i = 0;
3244 for (int y = 0; y < 512; y++)
3245 {
3246 for (int x = 0; x < 512; x++)
3247 {
3248 if (resultarr2[y, x] <= 0)
3249 returnarr[i] = 0.0000001f;
3250 else
3251 returnarr[i] = resultarr2[y, x];
3252
3253 i++;
3254 }
3255 }
3256
3257 return returnarr;
3258 }
3259
3260 public float[] ResizeTerrain512Interpolation(float[] heightMap)
3261 {
3262 float[] returnarr = new float[262144];
3263 float[,] resultarr = new float[512,512];
3264
3265 // Filling out the array into its multi-dimensional components
3266 for (int y = 0; y < 256; y++)
3267 {
3268 for (int x = 0; x < 256; x++)
3269 {
3270 resultarr[y, x] = heightMap[y * 256 + x];
3271 }
3272 }
3273
3274 // Resize using interpolation
3275
3276 // This particular way is quick but it only works on a multiple of the original
3277
3278 // The idea behind this method can be described with the following diagrams
3279 // second pass and third pass happen in the same loop really.. just separated
3280 // them to show what this does.
3281
3282 // First Pass
3283 // ResultArr:
3284 // 1,1,1,1,1,1
3285 // 1,1,1,1,1,1
3286 // 1,1,1,1,1,1
3287 // 1,1,1,1,1,1
3288 // 1,1,1,1,1,1
3289 // 1,1,1,1,1,1
3290
3291 // Second Pass
3292 // ResultArr2:
3293 // 1,,1,,1,,1,,1,,1,
3294 // ,,,,,,,,,,
3295 // 1,,1,,1,,1,,1,,1,
3296 // ,,,,,,,,,,
3297 // 1,,1,,1,,1,,1,,1,
3298 // ,,,,,,,,,,
3299 // 1,,1,,1,,1,,1,,1,
3300 // ,,,,,,,,,,
3301 // 1,,1,,1,,1,,1,,1,
3302 // ,,,,,,,,,,
3303 // 1,,1,,1,,1,,1,,1,
3304
3305 // Third pass fills in the blanks
3306 // ResultArr2:
3307 // 1,1,1,1,1,1,1,1,1,1,1,1
3308 // 1,1,1,1,1,1,1,1,1,1,1,1
3309 // 1,1,1,1,1,1,1,1,1,1,1,1
3310 // 1,1,1,1,1,1,1,1,1,1,1,1
3311 // 1,1,1,1,1,1,1,1,1,1,1,1
3312 // 1,1,1,1,1,1,1,1,1,1,1,1
3313 // 1,1,1,1,1,1,1,1,1,1,1,1
3314 // 1,1,1,1,1,1,1,1,1,1,1,1
3315 // 1,1,1,1,1,1,1,1,1,1,1,1
3316 // 1,1,1,1,1,1,1,1,1,1,1,1
3317 // 1,1,1,1,1,1,1,1,1,1,1,1
3318
3319 // X,Y = .
3320 // X+1,y = ^
3321 // X,Y+1 = *
3322 // X+1,Y+1 = #
3323
3324 // Filling in like this;
3325 // .*
3326 // ^#
3327 // 1st .
3328 // 2nd *
3329 // 3rd ^
3330 // 4th #
3331 // on single loop.
3332
3333 float[,] resultarr2 = new float[512,512];
3334 for (int y = 0; y < (int)Constants.RegionSize; y++)
3335 {
3336 for (int x = 0; x < (int)Constants.RegionSize; x++)
3337 {
3338 resultarr2[y*2, x*2] = resultarr[y, x];
3339
3340 if (y < (int)Constants.RegionSize)
3341 {
3342 if (y + 1 < (int)Constants.RegionSize)
3343 {
3344 if (x + 1 < (int)Constants.RegionSize)
3345 {
3346 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
3347 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3348 }
3349 else
3350 {
3351 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
3352 }
3353 }
3354 else
3355 {
3356 resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
3357 }
3358 }
3359 if (x < (int)Constants.RegionSize)
3360 {
3361 if (x + 1 < (int)Constants.RegionSize)
3362 {
3363 if (y + 1 < (int)Constants.RegionSize)
3364 {
3365 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3366 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3367 }
3368 else
3369 {
3370 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
3371 }
3372 }
3373 else
3374 {
3375 resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
3376 }
3377 }
3378 if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
3379 {
3380 if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
3381 {
3382 resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3383 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3384 }
3385 else
3386 {
3387 resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
3388 }
3389 }
3390 }
3391 }
3392 //Flatten out the array
3393 int i = 0;
3394 for (int y = 0; y < 512; y++)
3395 {
3396 for (int x = 0; x < 512; x++)
3397 {
3398 if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
3399 {
3400 m_log.Warn("[PHYSICS]: Non finite heightfield element detected. Setting it to 0");
3401 resultarr2[y, x] = 0;
3402 }
3403 returnarr[i] = resultarr2[y, x];
3404 i++;
3405 }
3406 }
3407
3408 return returnarr;
3409 }
3410
3411 #endregion
3412
3413 public override void SetTerrain(float[] heightMap)
3414 {
3415 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
3416 {
3417 if (m_parentScene is OdeScene)
3418 {
3419 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
3420 }
3421 }
3422 else
3423 {
3424 SetTerrain(heightMap, m_worldOffset);
3425 }
3426 }
3427
3428 public void SetTerrain(float[] heightMap, Vector3 pOffset)
3429 {
3430
3431 uint regionsize = (uint) Constants.RegionSize; // visible region size eg. 256(M)
3432
3433 uint heightmapWidth = regionsize + 1; // ODE map size 257 x 257 (Meters) (1 extra
3434 uint heightmapHeight = regionsize + 1;
3435
3436 uint heightmapWidthSamples = (uint)regionsize + 2; // Sample file size, 258 x 258 samples
3437 uint heightmapHeightSamples = (uint)regionsize + 2;
3438
3439 // Array of height samples for ODE
3440 float[] _heightmap;
3441 _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 258 x 258
3442
3443 // Other ODE parameters
3444 const float scale = 1.0f;
3445 const float offset = 0.0f;
3446 const float thickness = 2.0f; // Was 0.2f, Larger appears to prevent Av fall-through
3447 const int wrap = 0;
3448
3449 float hfmin = 2000f;
3450 float hfmax = -2000f;
3451 float minele = 0.0f; // Dont allow -ve heights
3452
3453 uint x = 0;
3454 uint y = 0;
3455 uint xx = 0;
3456 uint yy = 0;
3457
3458 // load the height samples array from the heightMap
3459 for ( x = 0; x < heightmapWidthSamples; x++) // 0 to 257
3460 {
3461 for ( y = 0; y < heightmapHeightSamples; y++) // 0 to 257
3462 {
3463 xx = x - 1;
3464 if (xx < 0) xx = 0;
3465 if (xx > (regionsize - 1)) xx = regionsize - 1;
3466
3467 yy = y - 1;
3468 if (yy < 0) yy = 0;
3469 if (yy > (regionsize - 1)) yy = regionsize - 1;
3470 // Input xx = 0 0 1 2 ..... 254 255 255 256 total in
3471 // Output x = 0 1 2 3 ..... 255 256 257 258 total out
3472 float val= heightMap[(yy * regionsize) + xx]; // input from heightMap, <0-255 * 256> <0-255>
3473 if (val < minele) val = minele;
3474 _heightmap[x * (regionsize + 2) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257>
3475 hfmin = (val < hfmin) ? val : hfmin;
3476 hfmax = (val > hfmax) ? val : hfmax;
3477 }
3478 }
3479
3480 lock (OdeLock)
3481 {
3482 IntPtr GroundGeom = IntPtr.Zero;
3483 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
3484 {
3485 RegionTerrain.Remove(pOffset);
3486 if (GroundGeom != IntPtr.Zero)
3487 {
3488 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
3489 {
3490 TerrainHeightFieldHeights.Remove(GroundGeom);
3491 }
3492 d.SpaceRemove(space, GroundGeom);
3493 d.GeomDestroy(GroundGeom);
3494 }
3495 }
3496 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
3497 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0,
3498 heightmapWidth, heightmapHeight, (int)heightmapWidthSamples,
3499 (int)heightmapHeightSamples, scale, offset, thickness, wrap);
3500 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
3501 GroundGeom = d.CreateHeightfield(space, HeightmapData, 1);
3502 if (GroundGeom != IntPtr.Zero)
3503 {
3504 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
3505 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
3506 }
3507 geom_name_map[GroundGeom] = "Terrain";
3508
3509 d.Matrix3 R = new d.Matrix3();
3510
3511 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
3512 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
3513 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
3514
3515 q1 = q1 * q2;
3516 //q1 = q1 * q3;
3517 Vector3 v3;
3518 float angle;
3519 q1.GetAxisAngle(out v3, out angle);
3520
3521 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
3522 d.GeomSetRotation(GroundGeom, ref R);
3523 d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)) - 0.5f, (pOffset.Y + (regionsize * 0.5f)) - 0.5f, 0);
3524 IntPtr testGround = IntPtr.Zero;
3525 if (RegionTerrain.TryGetValue(pOffset, out testGround))
3526 {
3527 RegionTerrain.Remove(pOffset);
3528 }
3529 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
3530 TerrainHeightFieldHeights.Add(GroundGeom,_heightmap);
3531 }
3532 }
3533
3534 public override void DeleteTerrain()
3535 {
3536 }
3537
3538 public float GetWaterLevel()
3539 {
3540 return waterlevel;
3541 }
3542
3543 public override bool SupportsCombining()
3544 {
3545 return true;
3546 }
3547
3548 public override void UnCombine(PhysicsScene pScene)
3549 {
3550 IntPtr localGround = IntPtr.Zero;
3551// float[] localHeightfield;
3552 bool proceed = false;
3553 List<IntPtr> geomDestroyList = new List<IntPtr>();
3554
3555 lock (OdeLock)
3556 {
3557 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
3558 {
3559 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
3560 {
3561 if (geom == localGround)
3562 {
3563// localHeightfield = TerrainHeightFieldHeights[geom];
3564 proceed = true;
3565 }
3566 else
3567 {
3568 geomDestroyList.Add(geom);
3569 }
3570 }
3571
3572 if (proceed)
3573 {
3574 m_worldOffset = Vector3.Zero;
3575 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
3576 m_parentScene = null;
3577
3578 foreach (IntPtr g in geomDestroyList)
3579 {
3580 // removingHeightField needs to be done or the garbage collector will
3581 // collect the terrain data before we tell ODE to destroy it causing
3582 // memory corruption
3583 if (TerrainHeightFieldHeights.ContainsKey(g))
3584 {
3585// float[] removingHeightField = TerrainHeightFieldHeights[g];
3586 TerrainHeightFieldHeights.Remove(g);
3587
3588 if (RegionTerrain.ContainsKey(g))
3589 {
3590 RegionTerrain.Remove(g);
3591 }
3592
3593 d.GeomDestroy(g);
3594 //removingHeightField = new float[0];
3595 }
3596 }
3597
3598 }
3599 else
3600 {
3601 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
3602
3603 }
3604 }
3605 }
3606 }
3607
3608 public override void SetWaterLevel(float baseheight)
3609 {
3610 waterlevel = baseheight;
3611 randomizeWater(waterlevel);
3612 }
3613
3614 public void randomizeWater(float baseheight)
3615 {
3616 const uint heightmapWidth = m_regionWidth + 2;
3617 const uint heightmapHeight = m_regionHeight + 2;
3618 const uint heightmapWidthSamples = m_regionWidth + 2;
3619 const uint heightmapHeightSamples = m_regionHeight + 2;
3620 const float scale = 1.0f;
3621 const float offset = 0.0f;
3622 const float thickness = 2.9f;
3623 const int wrap = 0;
3624
3625 for (int i = 0; i < (258 * 258); i++)
3626 {
3627 _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
3628 // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
3629 }
3630
3631 lock (OdeLock)
3632 {
3633 if (WaterGeom != IntPtr.Zero)
3634 {
3635 d.SpaceRemove(space, WaterGeom);
3636 }
3637 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
3638 d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
3639 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
3640 offset, thickness, wrap);
3641 d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
3642 WaterGeom = d.CreateHeightfield(space, HeightmapData, 1);
3643 if (WaterGeom != IntPtr.Zero)
3644 {
3645 d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
3646 d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
3647
3648 }
3649 geom_name_map[WaterGeom] = "Water";
3650
3651 d.Matrix3 R = new d.Matrix3();
3652
3653 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
3654 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
3655 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
3656
3657 q1 = q1 * q2;
3658 //q1 = q1 * q3;
3659 Vector3 v3;
3660 float angle;
3661 q1.GetAxisAngle(out v3, out angle);
3662
3663 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
3664 d.GeomSetRotation(WaterGeom, ref R);
3665 d.GeomSetPosition(WaterGeom, 128, 128, 0);
3666
3667 }
3668
3669 }
3670
3671 public override void Dispose()
3672 {
3673 m_rayCastManager.Dispose();
3674 m_rayCastManager = null;
3675
3676 lock (OdeLock)
3677 {
3678 lock (_prims)
3679 {
3680 foreach (OdePrim prm in _prims)
3681 {
3682 RemovePrim(prm);
3683 }
3684 }
3685
3686 //foreach (OdeCharacter act in _characters)
3687 //{
3688 //RemoveAvatar(act);
3689 //}
3690 d.WorldDestroy(world);
3691 //d.CloseODE();
3692 }
3693 }
3694 public override Dictionary<uint, float> GetTopColliders()
3695 {
3696 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
3697 int cnt = 0;
3698 lock (_prims)
3699 {
3700 foreach (OdePrim prm in _prims)
3701 {
3702 if (prm.CollisionScore > 0)
3703 {
3704 returncolliders.Add(prm.m_localID, prm.CollisionScore);
3705 cnt++;
3706 prm.CollisionScore = 0f;
3707 if (cnt > 25)
3708 {
3709 break;
3710 }
3711 }
3712 }
3713 }
3714 return returncolliders;
3715 }
3716
3717 public override bool SupportsRayCast()
3718 {
3719 return true;
3720 }
3721
3722 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
3723 {
3724 if (retMethod != null)
3725 {
3726 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
3727 }
3728 }
3729
3730#if USE_DRAWSTUFF
3731 // Keyboard callback
3732 public void command(int cmd)
3733 {
3734 IntPtr geom;
3735 d.Mass mass;
3736 d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
3737
3738
3739
3740 Char ch = Char.ToLower((Char)cmd);
3741 switch ((Char)ch)
3742 {
3743 case 'w':
3744 try
3745 {
3746 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));
3747
3748 xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
3749 ds.SetViewpoint(ref xyz, ref hpr);
3750 }
3751 catch (ArgumentException)
3752 { hpr.X = 0; }
3753 break;
3754
3755 case 'a':
3756 hpr.X++;
3757 ds.SetViewpoint(ref xyz, ref hpr);
3758 break;
3759
3760 case 's':
3761 try
3762 {
3763 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));
3764
3765 xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
3766 ds.SetViewpoint(ref xyz, ref hpr);
3767 }
3768 catch (ArgumentException)
3769 { hpr.X = 0; }
3770 break;
3771 case 'd':
3772 hpr.X--;
3773 ds.SetViewpoint(ref xyz, ref hpr);
3774 break;
3775 case 'r':
3776 xyz.Z++;
3777 ds.SetViewpoint(ref xyz, ref hpr);
3778 break;
3779 case 'f':
3780 xyz.Z--;
3781 ds.SetViewpoint(ref xyz, ref hpr);
3782 break;
3783 case 'e':
3784 xyz.Y++;
3785 ds.SetViewpoint(ref xyz, ref hpr);
3786 break;
3787 case 'q':
3788 xyz.Y--;
3789 ds.SetViewpoint(ref xyz, ref hpr);
3790 break;
3791 }
3792 }
3793
3794 public void step(int pause)
3795 {
3796
3797 ds.SetColor(1.0f, 1.0f, 0.0f);
3798 ds.SetTexture(ds.Texture.Wood);
3799 lock (_prims)
3800 {
3801 foreach (OdePrim prm in _prims)
3802 {
3803 //IntPtr body = d.GeomGetBody(prm.prim_geom);
3804 if (prm.prim_geom != IntPtr.Zero)
3805 {
3806 d.Vector3 pos;
3807 d.GeomCopyPosition(prm.prim_geom, out pos);
3808 //d.BodyCopyPosition(body, out pos);
3809
3810 d.Matrix3 R;
3811 d.GeomCopyRotation(prm.prim_geom, out R);
3812 //d.BodyCopyRotation(body, out R);
3813
3814
3815 d.Vector3 sides = new d.Vector3();
3816 sides.X = prm.Size.X;
3817 sides.Y = prm.Size.Y;
3818 sides.Z = prm.Size.Z;
3819
3820 ds.DrawBox(ref pos, ref R, ref sides);
3821 }
3822 }
3823 }
3824 ds.SetColor(1.0f, 0.0f, 0.0f);
3825 lock (_characters)
3826 {
3827 foreach (OdeCharacter chr in _characters)
3828 {
3829 if (chr.Shell != IntPtr.Zero)
3830 {
3831 IntPtr body = d.GeomGetBody(chr.Shell);
3832
3833 d.Vector3 pos;
3834 d.GeomCopyPosition(chr.Shell, out pos);
3835 //d.BodyCopyPosition(body, out pos);
3836
3837 d.Matrix3 R;
3838 d.GeomCopyRotation(chr.Shell, out R);
3839 //d.BodyCopyRotation(body, out R);
3840
3841 ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
3842 d.Vector3 sides = new d.Vector3();
3843 sides.X = 0.5f;
3844 sides.Y = 0.5f;
3845 sides.Z = 0.5f;
3846
3847 ds.DrawBox(ref pos, ref R, ref sides);
3848 }
3849 }
3850 }
3851 }
3852
3853 public void start(int unused)
3854 {
3855 ds.SetViewpoint(ref xyz, ref hpr);
3856 }
3857#endif
3858 }
3859}
generated by cgit v1.1 at 2024-09-25 20:27:27 +0000