aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
diff options
context:
space:
mode:
Diffstat (limited to '')
-rw-r--r--OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs3853
1 files changed, 3853 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..79e2986
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -0,0 +1,3853 @@
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 d.GeomDestroy(prim.prim_geom);
2210 prim.prim_geom = IntPtr.Zero;
2211 }
2212 else
2213 {
2214 m_log.Warn("[PHYSICS]: Unable to remove prim from physics scene");
2215 }
2216 }
2217 catch (AccessViolationException)
2218 {
2219 m_log.Info("[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed.");
2220 }
2221 lock (_prims)
2222 _prims.Remove(prim);
2223
2224 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2225 //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
2226 //{
2227 //if (prim.m_targetSpace != null)
2228 //{
2229 //if (d.GeomIsSpace(prim.m_targetSpace))
2230 //{
2231 //waitForSpaceUnlock(prim.m_targetSpace);
2232 //d.SpaceRemove(space, prim.m_targetSpace);
2233 // free up memory used by the space.
2234 //d.SpaceDestroy(prim.m_targetSpace);
2235 //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
2236 //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
2237 //}
2238 //else
2239 //{
2240 //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2241 //((OdePrim) prim).m_targetSpace.ToString());
2242 //}
2243 //}
2244 //}
2245
2246 if (SupportsNINJAJoints)
2247 {
2248 RemoveAllJointsConnectedToActorThreadLocked(prim);
2249 }
2250 }
2251 }
2252 }
2253 }
2254
2255 #endregion
2256
2257 #region Space Separation Calculation
2258
2259 /// <summary>
2260 /// Takes a space pointer and zeros out the array we're using to hold the spaces
2261 /// </summary>
2262 /// <param name="pSpace"></param>
2263 public void resetSpaceArrayItemToZero(IntPtr pSpace)
2264 {
2265 for (int x = 0; x < staticPrimspace.GetLength(0); x++)
2266 {
2267 for (int y = 0; y < staticPrimspace.GetLength(1); y++)
2268 {
2269 if (staticPrimspace[x, y] == pSpace)
2270 staticPrimspace[x, y] = IntPtr.Zero;
2271 }
2272 }
2273 }
2274
2275 public void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY)
2276 {
2277 staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero;
2278 }
2279
2280 /// <summary>
2281 /// Called when a static prim moves. Allocates a space for the prim based on its position
2282 /// </summary>
2283 /// <param name="geom">the pointer to the geom that moved</param>
2284 /// <param name="pos">the position that the geom moved to</param>
2285 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
2286 /// <returns>a pointer to the new space it's in</returns>
2287 public IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace)
2288 {
2289 // Called from setting the Position and Size of an ODEPrim so
2290 // it's already in locked space.
2291
2292 // we don't want to remove the main space
2293 // we don't need to test physical here because this function should
2294 // never be called if the prim is physical(active)
2295
2296 // All physical prim end up in the root space
2297 //Thread.Sleep(20);
2298 if (currentspace != space)
2299 {
2300 //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString());
2301 //if (currentspace == IntPtr.Zero)
2302 //{
2303 //int adfadf = 0;
2304 //}
2305 if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero)
2306 {
2307 if (d.GeomIsSpace(currentspace))
2308 {
2309 waitForSpaceUnlock(currentspace);
2310 d.SpaceRemove(currentspace, geom);
2311 }
2312 else
2313 {
2314 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" + currentspace +
2315 " Geom:" + geom);
2316 }
2317 }
2318 else
2319 {
2320 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2321 if (sGeomIsIn != IntPtr.Zero)
2322 {
2323 if (d.GeomIsSpace(currentspace))
2324 {
2325 waitForSpaceUnlock(sGeomIsIn);
2326 d.SpaceRemove(sGeomIsIn, geom);
2327 }
2328 else
2329 {
2330 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2331 sGeomIsIn + " Geom:" + geom);
2332 }
2333 }
2334 }
2335
2336 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2337 if (d.SpaceGetNumGeoms(currentspace) == 0)
2338 {
2339 if (currentspace != IntPtr.Zero)
2340 {
2341 if (d.GeomIsSpace(currentspace))
2342 {
2343 waitForSpaceUnlock(currentspace);
2344 waitForSpaceUnlock(space);
2345 d.SpaceRemove(space, currentspace);
2346 // free up memory used by the space.
2347
2348 //d.SpaceDestroy(currentspace);
2349 resetSpaceArrayItemToZero(currentspace);
2350 }
2351 else
2352 {
2353 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2354 currentspace + " Geom:" + geom);
2355 }
2356 }
2357 }
2358 }
2359 else
2360 {
2361 // this is a physical object that got disabled. ;.;
2362 if (currentspace != IntPtr.Zero && geom != IntPtr.Zero)
2363 {
2364 if (d.SpaceQuery(currentspace, geom))
2365 {
2366 if (d.GeomIsSpace(currentspace))
2367 {
2368 waitForSpaceUnlock(currentspace);
2369 d.SpaceRemove(currentspace, geom);
2370 }
2371 else
2372 {
2373 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2374 currentspace + " Geom:" + geom);
2375 }
2376 }
2377 else
2378 {
2379 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2380 if (sGeomIsIn != IntPtr.Zero)
2381 {
2382 if (d.GeomIsSpace(sGeomIsIn))
2383 {
2384 waitForSpaceUnlock(sGeomIsIn);
2385 d.SpaceRemove(sGeomIsIn, geom);
2386 }
2387 else
2388 {
2389 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2390 sGeomIsIn + " Geom:" + geom);
2391 }
2392 }
2393 }
2394 }
2395 }
2396
2397 // The routines in the Position and Size sections do the 'inserting' into the space,
2398 // so all we have to do is make sure that the space that we're putting the prim into
2399 // is in the 'main' space.
2400 int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos);
2401 IntPtr newspace = calculateSpaceForGeom(pos);
2402
2403 if (newspace == IntPtr.Zero)
2404 {
2405 newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
2406 d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh);
2407 }
2408
2409 return newspace;
2410 }
2411
2412 /// <summary>
2413 /// Creates a new space at X Y
2414 /// </summary>
2415 /// <param name="iprimspaceArrItemX"></param>
2416 /// <param name="iprimspaceArrItemY"></param>
2417 /// <returns>A pointer to the created space</returns>
2418 public IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
2419 {
2420 // creating a new space for prim and inserting it into main space.
2421 staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero);
2422 d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space);
2423 waitForSpaceUnlock(space);
2424 d.SpaceSetSublevel(space, 1);
2425 d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
2426 return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
2427 }
2428
2429 /// <summary>
2430 /// Calculates the space the prim should be in by its position
2431 /// </summary>
2432 /// <param name="pos"></param>
2433 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
2434 public IntPtr calculateSpaceForGeom(Vector3 pos)
2435 {
2436 int[] xyspace = calculateSpaceArrayItemFromPos(pos);
2437 //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
2438 return staticPrimspace[xyspace[0], xyspace[1]];
2439 }
2440
2441 /// <summary>
2442 /// Holds the space allocation logic
2443 /// </summary>
2444 /// <param name="pos"></param>
2445 /// <returns>an array item based on the position</returns>
2446 public int[] calculateSpaceArrayItemFromPos(Vector3 pos)
2447 {
2448 int[] returnint = new int[2];
2449
2450 returnint[0] = (int) (pos.X/metersInSpace);
2451
2452 if (returnint[0] > ((int) (259f/metersInSpace)))
2453 returnint[0] = ((int) (259f/metersInSpace));
2454 if (returnint[0] < 0)
2455 returnint[0] = 0;
2456
2457 returnint[1] = (int) (pos.Y/metersInSpace);
2458 if (returnint[1] > ((int) (259f/metersInSpace)))
2459 returnint[1] = ((int) (259f/metersInSpace));
2460 if (returnint[1] < 0)
2461 returnint[1] = 0;
2462
2463 return returnint;
2464 }
2465
2466 #endregion
2467
2468 /// <summary>
2469 /// Routine to figure out if we need to mesh this prim with our mesher
2470 /// </summary>
2471 /// <param name="pbs"></param>
2472 /// <returns></returns>
2473 public bool needsMeshing(PrimitiveBaseShape pbs)
2474 {
2475 // most of this is redundant now as the mesher will return null if it cant mesh a prim
2476 // but we still need to check for sculptie meshing being enabled so this is the most
2477 // convenient place to do it for now...
2478
2479 // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
2480 // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
2481 int iPropertiesNotSupportedDefault = 0;
2482
2483 if (pbs.SculptEntry && !meshSculptedPrim)
2484 {
2485#if SPAM
2486 m_log.Warn("NonMesh");
2487#endif
2488 return false;
2489 }
2490
2491 // 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
2492 if (!forceSimplePrimMeshing)
2493 {
2494 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
2495 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
2496 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
2497 {
2498
2499 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
2500 && pbs.ProfileHollow == 0
2501 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
2502 && pbs.PathBegin == 0 && pbs.PathEnd == 0
2503 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
2504 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
2505 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
2506 {
2507#if SPAM
2508 m_log.Warn("NonMesh");
2509#endif
2510 return false;
2511 }
2512 }
2513 }
2514
2515 if (pbs.ProfileHollow != 0)
2516 iPropertiesNotSupportedDefault++;
2517
2518 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
2519 iPropertiesNotSupportedDefault++;
2520
2521 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
2522 iPropertiesNotSupportedDefault++;
2523
2524 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
2525 iPropertiesNotSupportedDefault++;
2526
2527 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
2528 iPropertiesNotSupportedDefault++;
2529
2530 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
2531 iPropertiesNotSupportedDefault++;
2532
2533 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))
2534 iPropertiesNotSupportedDefault++;
2535
2536 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
2537 iPropertiesNotSupportedDefault++;
2538
2539 // test for torus
2540 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
2541 {
2542 if (pbs.PathCurve == (byte)Extrusion.Curve1)
2543 {
2544 iPropertiesNotSupportedDefault++;
2545 }
2546 }
2547 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
2548 {
2549 if (pbs.PathCurve == (byte)Extrusion.Straight)
2550 {
2551 iPropertiesNotSupportedDefault++;
2552 }
2553
2554 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
2555 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2556 {
2557 iPropertiesNotSupportedDefault++;
2558 }
2559 }
2560 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
2561 {
2562 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
2563 {
2564 iPropertiesNotSupportedDefault++;
2565 }
2566 }
2567 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
2568 {
2569 if (pbs.PathCurve == (byte)Extrusion.Straight)
2570 {
2571 iPropertiesNotSupportedDefault++;
2572 }
2573 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2574 {
2575 iPropertiesNotSupportedDefault++;
2576 }
2577 }
2578
2579
2580 if (iPropertiesNotSupportedDefault == 0)
2581 {
2582#if SPAM
2583 m_log.Warn("NonMesh");
2584#endif
2585 return false;
2586 }
2587#if SPAM
2588 m_log.Debug("Mesh");
2589#endif
2590 return true;
2591 }
2592
2593 /// <summary>
2594 /// Called after our prim properties are set Scale, position etc.
2595 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
2596 /// This assures us that we have no race conditions
2597 /// </summary>
2598 /// <param name="prim"></param>
2599 public override void AddPhysicsActorTaint(PhysicsActor prim)
2600 {
2601
2602 if (prim is OdePrim)
2603 {
2604 OdePrim taintedprim = ((OdePrim) prim);
2605 lock (_taintedPrimLock)
2606 {
2607 if (!(_taintedPrimH.Contains(taintedprim)))
2608 {
2609//Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.m_primName);
2610 _taintedPrimH.Add(taintedprim); // HashSet for searching
2611 _taintedPrimL.Add(taintedprim); // List for ordered readout
2612 }
2613 }
2614 return;
2615 }
2616 else if (prim is OdeCharacter)
2617 {
2618 OdeCharacter taintedchar = ((OdeCharacter)prim);
2619 lock (_taintedActors)
2620 {
2621 if (!(_taintedActors.Contains(taintedchar)))
2622 {
2623 _taintedActors.Add(taintedchar);
2624 if (taintedchar.bad)
2625 m_log.DebugFormat("[PHYSICS]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid);
2626 }
2627 }
2628 }
2629 }
2630
2631 /// <summary>
2632 /// This is our main simulate loop
2633 /// It's thread locked by a Mutex in the scene.
2634 /// It holds Collisions, it instructs ODE to step through the physical reactions
2635 /// It moves the objects around in memory
2636 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
2637 /// </summary>
2638 /// <param name="timeStep"></param>
2639 /// <returns></returns>
2640 public override float Simulate(float timeStep)
2641 {
2642 if (framecount >= int.MaxValue)
2643 framecount = 0;
2644 //if (m_worldOffset != Vector3.Zero)
2645 // return 0;
2646
2647 framecount++;
2648
2649 DateTime now = DateTime.UtcNow;
2650 TimeSpan SinceLastFrame = now - m_lastframe;
2651 m_lastframe = now;
2652 float realtime = (float)SinceLastFrame.TotalSeconds;
2653// Console.WriteLine("ts={0} rt={1}", timeStep, realtime);
2654 timeStep = realtime;
2655
2656 // float fps = 1.0f / realtime;
2657 float fps = 0.0f; // number of ODE steps in this Simulate step
2658 //m_log.Info(timeStep.ToString());
2659 step_time += timeStep;
2660
2661 // If We're loaded down by something else,
2662 // or debugging with the Visual Studio project on pause
2663 // skip a few frames to catch up gracefully.
2664 // without shooting the physicsactors all over the place
2665
2666 if (step_time >= m_SkipFramesAtms)
2667 {
2668 // Instead of trying to catch up, it'll do 5 physics frames only
2669 step_time = ODE_STEPSIZE;
2670 m_physicsiterations = 5;
2671 }
2672 else
2673 {
2674 m_physicsiterations = 10;
2675 }
2676
2677 if (SupportsNINJAJoints)
2678 {
2679 DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2680 CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2681 }
2682
2683 lock (OdeLock)
2684 {
2685 // Process 10 frames if the sim is running normal..
2686 // process 5 frames if the sim is running slow
2687 //try
2688 //{
2689 //d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
2690 //}
2691 //catch (StackOverflowException)
2692 //{
2693 // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
2694 // ode.drelease(world);
2695 //base.TriggerPhysicsBasedRestart();
2696 //}
2697
2698 int i = 0;
2699
2700 // Figure out the Frames Per Second we're going at.
2701 //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size
2702
2703 // fps = (step_time / ODE_STEPSIZE) * 1000;
2704 // HACK: Using a time dilation of 1.0 to debug rubberbanding issues
2705 //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f);
2706
2707 // step_time = 0.09375f;
2708
2709 while (step_time > 0.0f)
2710 {
2711 //lock (ode)
2712 //{
2713 //if (!ode.lockquery())
2714 //{
2715 // ode.dlock(world);
2716 try
2717 {
2718 // Insert, remove Characters
2719 bool processedtaints = false;
2720
2721 lock (_taintedActors)
2722 {
2723 if (_taintedActors.Count > 0)
2724 {
2725 foreach (OdeCharacter character in _taintedActors)
2726 {
2727
2728 character.ProcessTaints(ODE_STEPSIZE);
2729
2730 processedtaints = true;
2731 //character.m_collisionscore = 0;
2732 }
2733
2734 if (processedtaints)
2735 _taintedActors.Clear();
2736 }
2737 } // end lock _taintedActors
2738
2739 // Modify other objects in the scene.
2740 processedtaints = false;
2741
2742 lock (_taintedPrimLock)
2743 {
2744 foreach (OdePrim prim in _taintedPrimL)
2745 {
2746 if (prim.m_taintremove)
2747 {
2748 //Console.WriteLine("Simulate calls RemovePrimThreadLocked");
2749 RemovePrimThreadLocked(prim);
2750 }
2751 else
2752 {
2753 //Console.WriteLine("Simulate calls ProcessTaints");
2754 prim.ProcessTaints(ODE_STEPSIZE);
2755 }
2756 processedtaints = true;
2757 prim.m_collisionscore = 0;
2758
2759 // This loop can block up the Heartbeat for a very long time on large regions.
2760 // We need to let the Watchdog know that the Heartbeat is not dead
2761 // NOTE: This is currently commented out, but if things like OAR loading are
2762 // timing the heartbeat out we will need to uncomment it
2763 //Watchdog.UpdateThread();
2764 }
2765
2766 if (SupportsNINJAJoints)
2767 {
2768 // Create pending joints, if possible
2769
2770 // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
2771 // a joint requires specifying the body id of both involved bodies
2772 if (pendingJoints.Count > 0)
2773 {
2774 List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
2775 //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
2776 foreach (PhysicsJoint joint in pendingJoints)
2777 {
2778 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
2779 string[] jointParams = joint.RawParams.Split(" ".ToCharArray(),
2780 System.StringSplitOptions.RemoveEmptyEntries);
2781 List<IntPtr> jointBodies = new List<IntPtr>();
2782 bool allJointBodiesAreReady = true;
2783 foreach (string jointParam in jointParams)
2784 {
2785 if (jointParam == "NULL")
2786 {
2787 //DoJointErrorMessage(joint, "attaching NULL joint to world");
2788 jointBodies.Add(IntPtr.Zero);
2789 }
2790 else
2791 {
2792 //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
2793 bool foundPrim = false;
2794 lock (_prims)
2795 {
2796 foreach (OdePrim prim in _prims) // FIXME: inefficient
2797 {
2798 if (prim.SOPName == jointParam)
2799 {
2800 //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
2801 if (prim.IsPhysical && prim.Body != IntPtr.Zero)
2802 {
2803 jointBodies.Add(prim.Body);
2804 foundPrim = true;
2805 break;
2806 }
2807 else
2808 {
2809 DoJointErrorMessage(joint, "prim name " + jointParam +
2810 " exists but is not (yet) physical; deferring joint creation. " +
2811 "IsPhysical property is " + prim.IsPhysical +
2812 " and body is " + prim.Body);
2813 foundPrim = false;
2814 break;
2815 }
2816 }
2817 }
2818 }
2819 if (foundPrim)
2820 {
2821 // all is fine
2822 }
2823 else
2824 {
2825 allJointBodiesAreReady = false;
2826 break;
2827 }
2828 }
2829 }
2830 if (allJointBodiesAreReady)
2831 {
2832 //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
2833 if (jointBodies[0] == jointBodies[1])
2834 {
2835 DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
2836 }
2837 else
2838 {
2839 switch (joint.Type)
2840 {
2841 case PhysicsJointType.Ball:
2842 {
2843 IntPtr odeJoint;
2844 //DoJointErrorMessage(joint, "ODE creating ball joint ");
2845 odeJoint = d.JointCreateBall(world, IntPtr.Zero);
2846 //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2847 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2848 //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
2849 d.JointSetBallAnchor(odeJoint,
2850 joint.Position.X,
2851 joint.Position.Y,
2852 joint.Position.Z);
2853 //DoJointErrorMessage(joint, "ODE joint setting OK");
2854 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
2855 //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
2856 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
2857 //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
2858
2859 if (joint is OdePhysicsJoint)
2860 {
2861 ((OdePhysicsJoint)joint).jointID = odeJoint;
2862 }
2863 else
2864 {
2865 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2866 }
2867 }
2868 break;
2869 case PhysicsJointType.Hinge:
2870 {
2871 IntPtr odeJoint;
2872 //DoJointErrorMessage(joint, "ODE creating hinge joint ");
2873 odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
2874 //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2875 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2876 //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
2877 d.JointSetHingeAnchor(odeJoint,
2878 joint.Position.X,
2879 joint.Position.Y,
2880 joint.Position.Z);
2881 // We use the orientation of the x-axis of the joint's coordinate frame
2882 // as the axis for the hinge.
2883
2884 // Therefore, we must get the joint's coordinate frame based on the
2885 // joint.Rotation field, which originates from the orientation of the
2886 // joint's proxy object in the scene.
2887
2888 // The joint's coordinate frame is defined as the transformation matrix
2889 // that converts a vector from joint-local coordinates into world coordinates.
2890 // World coordinates are defined as the XYZ coordinate system of the sim,
2891 // as shown in the top status-bar of the viewer.
2892
2893 // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
2894 // and use that as the hinge axis.
2895
2896 //joint.Rotation.Normalize();
2897 Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
2898
2899 // Now extract the X axis of the joint's coordinate frame.
2900
2901 // Do not try to use proxyFrame.AtAxis or you will become mired in the
2902 // tar pit of transposed, inverted, and generally messed-up orientations.
2903 // (In other words, Matrix4.AtAxis() is borked.)
2904 // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
2905
2906 // Instead, compute the X axis of the coordinate frame by transforming
2907 // the (1,0,0) vector. At least that works.
2908
2909 //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
2910 Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
2911 //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
2912 //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
2913 d.JointSetHingeAxis(odeJoint,
2914 jointAxis.X,
2915 jointAxis.Y,
2916 jointAxis.Z);
2917 //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
2918 if (joint is OdePhysicsJoint)
2919 {
2920 ((OdePhysicsJoint)joint).jointID = odeJoint;
2921 }
2922 else
2923 {
2924 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2925 }
2926 }
2927 break;
2928 }
2929 successfullyProcessedPendingJoints.Add(joint);
2930 }
2931 }
2932 else
2933 {
2934 DoJointErrorMessage(joint, "joint could not yet be created; still pending");
2935 }
2936 }
2937 foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
2938 {
2939 //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
2940 //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
2941 InternalRemovePendingJoint(successfullyProcessedJoint);
2942 //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
2943 InternalAddActiveJoint(successfullyProcessedJoint);
2944 //DoJointErrorMessage(successfullyProcessedJoint, "done");
2945 }
2946 }
2947 } // end SupportsNINJAJoints
2948
2949 if (processedtaints)
2950//Console.WriteLine("Simulate calls Clear of _taintedPrim list");
2951 _taintedPrimH.Clear(); // ??? if this only ???
2952 _taintedPrimL.Clear();
2953 } // end lock _taintedPrimLock
2954
2955 // Move characters
2956 lock (_characters)
2957 {
2958 List<OdeCharacter> defects = new List<OdeCharacter>();
2959 foreach (OdeCharacter actor in _characters)
2960 {
2961 if (actor != null)
2962 actor.Move(ODE_STEPSIZE, defects);
2963 }
2964 if (0 != defects.Count)
2965 {
2966 foreach (OdeCharacter defect in defects)
2967 {
2968 RemoveCharacter(defect);
2969 }
2970 }
2971 } // end lock _characters
2972
2973 // Move other active objects
2974 lock (_activeprims)
2975 {
2976 foreach (OdePrim prim in _activeprims)
2977 {
2978 prim.m_collisionscore = 0;
2979 prim.Move(ODE_STEPSIZE);
2980 }
2981 } // end lock _activeprims
2982
2983 //if ((framecount % m_randomizeWater) == 0)
2984 // randomizeWater(waterlevel);
2985
2986 //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
2987 m_rayCastManager.ProcessQueuedRequests();
2988
2989 collision_optimized(ODE_STEPSIZE);
2990
2991 lock (_collisionEventPrim)
2992 {
2993 foreach (PhysicsActor obj in _collisionEventPrim)
2994 {
2995 if (obj == null)
2996 continue;
2997
2998 switch ((ActorTypes)obj.PhysicsActorType)
2999 {
3000 case ActorTypes.Agent:
3001 OdeCharacter cobj = (OdeCharacter)obj;
3002 cobj.AddCollisionFrameTime(100);
3003 cobj.SendCollisions();
3004 break;
3005 case ActorTypes.Prim:
3006 OdePrim pobj = (OdePrim)obj;
3007 pobj.SendCollisions();
3008 break;
3009 }
3010 }
3011 } // end lock _collisionEventPrim
3012
3013 //if (m_global_contactcount > 5)
3014 //{
3015 // m_log.DebugFormat("[PHYSICS]: Contacts:{0}", m_global_contactcount);
3016 //}
3017
3018 m_global_contactcount = 0;
3019
3020 d.WorldQuickStep(world, ODE_STEPSIZE);
3021 d.JointGroupEmpty(contactgroup);
3022 fps++;
3023 //ode.dunlock(world);
3024 } // end try
3025 catch (Exception e)
3026 {
3027 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
3028 ode.dunlock(world);
3029 }
3030
3031 step_time -= ODE_STEPSIZE;
3032 i++;
3033 //}
3034 //else
3035 //{
3036 //fps = 0;
3037 //}
3038 //}
3039 } // end while (step_time > 0.0f)
3040
3041 lock (_characters)
3042 {
3043 foreach (OdeCharacter actor in _characters)
3044 {
3045 if (actor != null)
3046 {
3047 if (actor.bad)
3048 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
3049 actor.UpdatePositionAndVelocity();
3050 }
3051 }
3052 }
3053
3054 lock (_badCharacter)
3055 {
3056 if (_badCharacter.Count > 0)
3057 {
3058 foreach (OdeCharacter chr in _badCharacter)
3059 {
3060 RemoveCharacter(chr);
3061 }
3062 _badCharacter.Clear();
3063 }
3064 }
3065
3066 lock (_activeprims)
3067 {
3068 //if (timeStep < 0.2f)
3069 {
3070 foreach (OdePrim actor in _activeprims)
3071 {
3072 if (actor.IsPhysical && (d.BodyIsEnabled(actor.Body) || !actor._zeroFlag))
3073 {
3074 actor.UpdatePositionAndVelocity();
3075
3076 if (SupportsNINJAJoints)
3077 {
3078 // If an actor moved, move its joint proxy objects as well.
3079 // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
3080 // for this purpose but it is never called! So we just do the joint
3081 // movement code here.
3082
3083 if (actor.SOPName != null &&
3084 joints_connecting_actor.ContainsKey(actor.SOPName) &&
3085 joints_connecting_actor[actor.SOPName] != null &&
3086 joints_connecting_actor[actor.SOPName].Count > 0)
3087 {
3088 foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
3089 {
3090 if (affectedJoint.IsInPhysicsEngine)
3091 {
3092 DoJointMoved(affectedJoint);
3093 }
3094 else
3095 {
3096 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);
3097 }
3098 }
3099 }
3100 }
3101 }
3102 }
3103 }
3104 } // end lock _activeprims
3105
3106 //DumpJointInfo();
3107
3108 // Finished with all sim stepping. If requested, dump world state to file for debugging.
3109 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
3110 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
3111 if (physics_logging && (physics_logging_interval>0) && (framecount % physics_logging_interval == 0))
3112 {
3113 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
3114 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
3115
3116 if (physics_logging_append_existing_logfile)
3117 {
3118 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
3119 TextWriter fwriter = File.AppendText(fname);
3120 fwriter.WriteLine(header);
3121 fwriter.Close();
3122 }
3123 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
3124 }
3125 } // end lock OdeLock
3126
3127 return fps * 1000.0f; //NB This is a FRAME COUNT, not a time! AND is divide by 1000 in SimStatusReporter!
3128 } // end Simulate
3129
3130 public override void GetResults()
3131 {
3132 }
3133
3134 public override bool IsThreaded
3135 {
3136 // for now we won't be multithreaded
3137 get { return (false); }
3138 }
3139
3140 #region ODE Specific Terrain Fixes
3141 public float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
3142 {
3143 float[] returnarr = new float[262144];
3144 float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
3145
3146 // Filling out the array into its multi-dimensional components
3147 for (int y = 0; y < WorldExtents.Y; y++)
3148 {
3149 for (int x = 0; x < WorldExtents.X; x++)
3150 {
3151 resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
3152 }
3153 }
3154
3155 // Resize using Nearest Neighbour
3156
3157 // This particular way is quick but it only works on a multiple of the original
3158
3159 // The idea behind this method can be described with the following diagrams
3160 // second pass and third pass happen in the same loop really.. just separated
3161 // them to show what this does.
3162
3163 // First Pass
3164 // ResultArr:
3165 // 1,1,1,1,1,1
3166 // 1,1,1,1,1,1
3167 // 1,1,1,1,1,1
3168 // 1,1,1,1,1,1
3169 // 1,1,1,1,1,1
3170 // 1,1,1,1,1,1
3171
3172 // Second Pass
3173 // ResultArr2:
3174 // 1,,1,,1,,1,,1,,1,
3175 // ,,,,,,,,,,
3176 // 1,,1,,1,,1,,1,,1,
3177 // ,,,,,,,,,,
3178 // 1,,1,,1,,1,,1,,1,
3179 // ,,,,,,,,,,
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 // Third pass fills in the blanks
3187 // ResultArr2:
3188 // 1,1,1,1,1,1,1,1,1,1,1,1
3189 // 1,1,1,1,1,1,1,1,1,1,1,1
3190 // 1,1,1,1,1,1,1,1,1,1,1,1
3191 // 1,1,1,1,1,1,1,1,1,1,1,1
3192 // 1,1,1,1,1,1,1,1,1,1,1,1
3193 // 1,1,1,1,1,1,1,1,1,1,1,1
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
3200 // X,Y = .
3201 // X+1,y = ^
3202 // X,Y+1 = *
3203 // X+1,Y+1 = #
3204
3205 // Filling in like this;
3206 // .*
3207 // ^#
3208 // 1st .
3209 // 2nd *
3210 // 3rd ^
3211 // 4th #
3212 // on single loop.
3213
3214 float[,] resultarr2 = new float[512, 512];
3215 for (int y = 0; y < WorldExtents.Y; y++)
3216 {
3217 for (int x = 0; x < WorldExtents.X; x++)
3218 {
3219 resultarr2[y * 2, x * 2] = resultarr[y, x];
3220
3221 if (y < WorldExtents.Y)
3222 {
3223 resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
3224 }
3225 if (x < WorldExtents.X)
3226 {
3227 resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
3228 }
3229 if (x < WorldExtents.X && y < WorldExtents.Y)
3230 {
3231 resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
3232 }
3233 }
3234 }
3235
3236 //Flatten out the array
3237 int i = 0;
3238 for (int y = 0; y < 512; y++)
3239 {
3240 for (int x = 0; x < 512; x++)
3241 {
3242 if (resultarr2[y, x] <= 0)
3243 returnarr[i] = 0.0000001f;
3244 else
3245 returnarr[i] = resultarr2[y, x];
3246
3247 i++;
3248 }
3249 }
3250
3251 return returnarr;
3252 }
3253
3254 public float[] ResizeTerrain512Interpolation(float[] heightMap)
3255 {
3256 float[] returnarr = new float[262144];
3257 float[,] resultarr = new float[512,512];
3258
3259 // Filling out the array into its multi-dimensional components
3260 for (int y = 0; y < 256; y++)
3261 {
3262 for (int x = 0; x < 256; x++)
3263 {
3264 resultarr[y, x] = heightMap[y * 256 + x];
3265 }
3266 }
3267
3268 // Resize using interpolation
3269
3270 // This particular way is quick but it only works on a multiple of the original
3271
3272 // The idea behind this method can be described with the following diagrams
3273 // second pass and third pass happen in the same loop really.. just separated
3274 // them to show what this does.
3275
3276 // First Pass
3277 // ResultArr:
3278 // 1,1,1,1,1,1
3279 // 1,1,1,1,1,1
3280 // 1,1,1,1,1,1
3281 // 1,1,1,1,1,1
3282 // 1,1,1,1,1,1
3283 // 1,1,1,1,1,1
3284
3285 // Second Pass
3286 // ResultArr2:
3287 // 1,,1,,1,,1,,1,,1,
3288 // ,,,,,,,,,,
3289 // 1,,1,,1,,1,,1,,1,
3290 // ,,,,,,,,,,
3291 // 1,,1,,1,,1,,1,,1,
3292 // ,,,,,,,,,,
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 // Third pass fills in the blanks
3300 // ResultArr2:
3301 // 1,1,1,1,1,1,1,1,1,1,1,1
3302 // 1,1,1,1,1,1,1,1,1,1,1,1
3303 // 1,1,1,1,1,1,1,1,1,1,1,1
3304 // 1,1,1,1,1,1,1,1,1,1,1,1
3305 // 1,1,1,1,1,1,1,1,1,1,1,1
3306 // 1,1,1,1,1,1,1,1,1,1,1,1
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
3313 // X,Y = .
3314 // X+1,y = ^
3315 // X,Y+1 = *
3316 // X+1,Y+1 = #
3317
3318 // Filling in like this;
3319 // .*
3320 // ^#
3321 // 1st .
3322 // 2nd *
3323 // 3rd ^
3324 // 4th #
3325 // on single loop.
3326
3327 float[,] resultarr2 = new float[512,512];
3328 for (int y = 0; y < (int)Constants.RegionSize; y++)
3329 {
3330 for (int x = 0; x < (int)Constants.RegionSize; x++)
3331 {
3332 resultarr2[y*2, x*2] = resultarr[y, x];
3333
3334 if (y < (int)Constants.RegionSize)
3335 {
3336 if (y + 1 < (int)Constants.RegionSize)
3337 {
3338 if (x + 1 < (int)Constants.RegionSize)
3339 {
3340 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
3341 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3342 }
3343 else
3344 {
3345 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
3346 }
3347 }
3348 else
3349 {
3350 resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
3351 }
3352 }
3353 if (x < (int)Constants.RegionSize)
3354 {
3355 if (x + 1 < (int)Constants.RegionSize)
3356 {
3357 if (y + 1 < (int)Constants.RegionSize)
3358 {
3359 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3360 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3361 }
3362 else
3363 {
3364 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
3365 }
3366 }
3367 else
3368 {
3369 resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
3370 }
3371 }
3372 if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
3373 {
3374 if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
3375 {
3376 resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3377 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3378 }
3379 else
3380 {
3381 resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
3382 }
3383 }
3384 }
3385 }
3386 //Flatten out the array
3387 int i = 0;
3388 for (int y = 0; y < 512; y++)
3389 {
3390 for (int x = 0; x < 512; x++)
3391 {
3392 if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
3393 {
3394 m_log.Warn("[PHYSICS]: Non finite heightfield element detected. Setting it to 0");
3395 resultarr2[y, x] = 0;
3396 }
3397 returnarr[i] = resultarr2[y, x];
3398 i++;
3399 }
3400 }
3401
3402 return returnarr;
3403 }
3404
3405 #endregion
3406
3407 public override void SetTerrain(float[] heightMap)
3408 {
3409 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
3410 {
3411 if (m_parentScene is OdeScene)
3412 {
3413 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
3414 }
3415 }
3416 else
3417 {
3418 SetTerrain(heightMap, m_worldOffset);
3419 }
3420 }
3421
3422 public void SetTerrain(float[] heightMap, Vector3 pOffset)
3423 {
3424
3425 uint regionsize = (uint) Constants.RegionSize; // visible region size eg. 256(M)
3426
3427 uint heightmapWidth = regionsize + 1; // ODE map size 257 x 257 (Meters) (1 extra
3428 uint heightmapHeight = regionsize + 1;
3429
3430 uint heightmapWidthSamples = (uint)regionsize + 2; // Sample file size, 258 x 258 samples
3431 uint heightmapHeightSamples = (uint)regionsize + 2;
3432
3433 // Array of height samples for ODE
3434 float[] _heightmap;
3435 _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 258 x 258
3436
3437 // Other ODE parameters
3438 const float scale = 1.0f;
3439 const float offset = 0.0f;
3440 const float thickness = 2.0f; // Was 0.2f, Larger appears to prevent Av fall-through
3441 const int wrap = 0;
3442
3443 float hfmin = 2000f;
3444 float hfmax = -2000f;
3445 float minele = 0.0f; // Dont allow -ve heights
3446
3447 uint x = 0;
3448 uint y = 0;
3449 uint xx = 0;
3450 uint yy = 0;
3451
3452 // load the height samples array from the heightMap
3453 for ( x = 0; x < heightmapWidthSamples; x++) // 0 to 257
3454 {
3455 for ( y = 0; y < heightmapHeightSamples; y++) // 0 to 257
3456 {
3457 xx = x - 1;
3458 if (xx < 0) xx = 0;
3459 if (xx > (regionsize - 1)) xx = regionsize - 1;
3460
3461 yy = y - 1;
3462 if (yy < 0) yy = 0;
3463 if (yy > (regionsize - 1)) yy = regionsize - 1;
3464 // Input xx = 0 0 1 2 ..... 254 255 255 256 total in
3465 // Output x = 0 1 2 3 ..... 255 256 257 258 total out
3466 float val= heightMap[(yy * regionsize) + xx]; // input from heightMap, <0-255 * 256> <0-255>
3467 if (val < minele) val = minele;
3468 _heightmap[x * (regionsize + 2) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257>
3469 hfmin = (val < hfmin) ? val : hfmin;
3470 hfmax = (val > hfmax) ? val : hfmax;
3471 }
3472 }
3473
3474 lock (OdeLock)
3475 {
3476 IntPtr GroundGeom = IntPtr.Zero;
3477 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
3478 {
3479 RegionTerrain.Remove(pOffset);
3480 if (GroundGeom != IntPtr.Zero)
3481 {
3482 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
3483 {
3484 TerrainHeightFieldHeights.Remove(GroundGeom);
3485 }
3486 d.SpaceRemove(space, GroundGeom);
3487 d.GeomDestroy(GroundGeom);
3488 }
3489 }
3490 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
3491 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0,
3492 heightmapWidth, heightmapHeight, (int)heightmapWidthSamples,
3493 (int)heightmapHeightSamples, scale, offset, thickness, wrap);
3494 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
3495 GroundGeom = d.CreateHeightfield(space, HeightmapData, 1);
3496 if (GroundGeom != IntPtr.Zero)
3497 {
3498 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
3499 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
3500 }
3501 geom_name_map[GroundGeom] = "Terrain";
3502
3503 d.Matrix3 R = new d.Matrix3();
3504
3505 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
3506 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
3507 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
3508
3509 q1 = q1 * q2;
3510 //q1 = q1 * q3;
3511 Vector3 v3;
3512 float angle;
3513 q1.GetAxisAngle(out v3, out angle);
3514
3515 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
3516 d.GeomSetRotation(GroundGeom, ref R);
3517 d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)) - 0.5f, (pOffset.Y + (regionsize * 0.5f)) - 0.5f, 0);
3518 IntPtr testGround = IntPtr.Zero;
3519 if (RegionTerrain.TryGetValue(pOffset, out testGround))
3520 {
3521 RegionTerrain.Remove(pOffset);
3522 }
3523 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
3524 TerrainHeightFieldHeights.Add(GroundGeom,_heightmap);
3525 }
3526 }
3527
3528 public override void DeleteTerrain()
3529 {
3530 }
3531
3532 public float GetWaterLevel()
3533 {
3534 return waterlevel;
3535 }
3536
3537 public override bool SupportsCombining()
3538 {
3539 return true;
3540 }
3541
3542 public override void UnCombine(PhysicsScene pScene)
3543 {
3544 IntPtr localGround = IntPtr.Zero;
3545// float[] localHeightfield;
3546 bool proceed = false;
3547 List<IntPtr> geomDestroyList = new List<IntPtr>();
3548
3549 lock (OdeLock)
3550 {
3551 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
3552 {
3553 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
3554 {
3555 if (geom == localGround)
3556 {
3557// localHeightfield = TerrainHeightFieldHeights[geom];
3558 proceed = true;
3559 }
3560 else
3561 {
3562 geomDestroyList.Add(geom);
3563 }
3564 }
3565
3566 if (proceed)
3567 {
3568 m_worldOffset = Vector3.Zero;
3569 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
3570 m_parentScene = null;
3571
3572 foreach (IntPtr g in geomDestroyList)
3573 {
3574 // removingHeightField needs to be done or the garbage collector will
3575 // collect the terrain data before we tell ODE to destroy it causing
3576 // memory corruption
3577 if (TerrainHeightFieldHeights.ContainsKey(g))
3578 {
3579// float[] removingHeightField = TerrainHeightFieldHeights[g];
3580 TerrainHeightFieldHeights.Remove(g);
3581
3582 if (RegionTerrain.ContainsKey(g))
3583 {
3584 RegionTerrain.Remove(g);
3585 }
3586
3587 d.GeomDestroy(g);
3588 //removingHeightField = new float[0];
3589 }
3590 }
3591
3592 }
3593 else
3594 {
3595 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
3596
3597 }
3598 }
3599 }
3600 }
3601
3602 public override void SetWaterLevel(float baseheight)
3603 {
3604 waterlevel = baseheight;
3605 randomizeWater(waterlevel);
3606 }
3607
3608 public void randomizeWater(float baseheight)
3609 {
3610 const uint heightmapWidth = m_regionWidth + 2;
3611 const uint heightmapHeight = m_regionHeight + 2;
3612 const uint heightmapWidthSamples = m_regionWidth + 2;
3613 const uint heightmapHeightSamples = m_regionHeight + 2;
3614 const float scale = 1.0f;
3615 const float offset = 0.0f;
3616 const float thickness = 2.9f;
3617 const int wrap = 0;
3618
3619 for (int i = 0; i < (258 * 258); i++)
3620 {
3621 _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
3622 // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
3623 }
3624
3625 lock (OdeLock)
3626 {
3627 if (WaterGeom != IntPtr.Zero)
3628 {
3629 d.SpaceRemove(space, WaterGeom);
3630 }
3631 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
3632 d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
3633 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
3634 offset, thickness, wrap);
3635 d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
3636 WaterGeom = d.CreateHeightfield(space, HeightmapData, 1);
3637 if (WaterGeom != IntPtr.Zero)
3638 {
3639 d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
3640 d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
3641
3642 }
3643 geom_name_map[WaterGeom] = "Water";
3644
3645 d.Matrix3 R = new d.Matrix3();
3646
3647 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
3648 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
3649 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
3650
3651 q1 = q1 * q2;
3652 //q1 = q1 * q3;
3653 Vector3 v3;
3654 float angle;
3655 q1.GetAxisAngle(out v3, out angle);
3656
3657 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
3658 d.GeomSetRotation(WaterGeom, ref R);
3659 d.GeomSetPosition(WaterGeom, 128, 128, 0);
3660
3661 }
3662
3663 }
3664
3665 public override void Dispose()
3666 {
3667 m_rayCastManager.Dispose();
3668 m_rayCastManager = null;
3669
3670 lock (OdeLock)
3671 {
3672 lock (_prims)
3673 {
3674 foreach (OdePrim prm in _prims)
3675 {
3676 RemovePrim(prm);
3677 }
3678 }
3679
3680 //foreach (OdeCharacter act in _characters)
3681 //{
3682 //RemoveAvatar(act);
3683 //}
3684 d.WorldDestroy(world);
3685 //d.CloseODE();
3686 }
3687 }
3688 public override Dictionary<uint, float> GetTopColliders()
3689 {
3690 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
3691 int cnt = 0;
3692 lock (_prims)
3693 {
3694 foreach (OdePrim prm in _prims)
3695 {
3696 if (prm.CollisionScore > 0)
3697 {
3698 returncolliders.Add(prm.m_localID, prm.CollisionScore);
3699 cnt++;
3700 prm.CollisionScore = 0f;
3701 if (cnt > 25)
3702 {
3703 break;
3704 }
3705 }
3706 }
3707 }
3708 return returncolliders;
3709 }
3710
3711 public override bool SupportsRayCast()
3712 {
3713 return true;
3714 }
3715
3716 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
3717 {
3718 if (retMethod != null)
3719 {
3720 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
3721 }
3722 }
3723
3724#if USE_DRAWSTUFF
3725 // Keyboard callback
3726 public void command(int cmd)
3727 {
3728 IntPtr geom;
3729 d.Mass mass;
3730 d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
3731
3732
3733
3734 Char ch = Char.ToLower((Char)cmd);
3735 switch ((Char)ch)
3736 {
3737 case 'w':
3738 try
3739 {
3740 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));
3741
3742 xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
3743 ds.SetViewpoint(ref xyz, ref hpr);
3744 }
3745 catch (ArgumentException)
3746 { hpr.X = 0; }
3747 break;
3748
3749 case 'a':
3750 hpr.X++;
3751 ds.SetViewpoint(ref xyz, ref hpr);
3752 break;
3753
3754 case 's':
3755 try
3756 {
3757 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));
3758
3759 xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
3760 ds.SetViewpoint(ref xyz, ref hpr);
3761 }
3762 catch (ArgumentException)
3763 { hpr.X = 0; }
3764 break;
3765 case 'd':
3766 hpr.X--;
3767 ds.SetViewpoint(ref xyz, ref hpr);
3768 break;
3769 case 'r':
3770 xyz.Z++;
3771 ds.SetViewpoint(ref xyz, ref hpr);
3772 break;
3773 case 'f':
3774 xyz.Z--;
3775 ds.SetViewpoint(ref xyz, ref hpr);
3776 break;
3777 case 'e':
3778 xyz.Y++;
3779 ds.SetViewpoint(ref xyz, ref hpr);
3780 break;
3781 case 'q':
3782 xyz.Y--;
3783 ds.SetViewpoint(ref xyz, ref hpr);
3784 break;
3785 }
3786 }
3787
3788 public void step(int pause)
3789 {
3790
3791 ds.SetColor(1.0f, 1.0f, 0.0f);
3792 ds.SetTexture(ds.Texture.Wood);
3793 lock (_prims)
3794 {
3795 foreach (OdePrim prm in _prims)
3796 {
3797 //IntPtr body = d.GeomGetBody(prm.prim_geom);
3798 if (prm.prim_geom != IntPtr.Zero)
3799 {
3800 d.Vector3 pos;
3801 d.GeomCopyPosition(prm.prim_geom, out pos);
3802 //d.BodyCopyPosition(body, out pos);
3803
3804 d.Matrix3 R;
3805 d.GeomCopyRotation(prm.prim_geom, out R);
3806 //d.BodyCopyRotation(body, out R);
3807
3808
3809 d.Vector3 sides = new d.Vector3();
3810 sides.X = prm.Size.X;
3811 sides.Y = prm.Size.Y;
3812 sides.Z = prm.Size.Z;
3813
3814 ds.DrawBox(ref pos, ref R, ref sides);
3815 }
3816 }
3817 }
3818 ds.SetColor(1.0f, 0.0f, 0.0f);
3819 lock (_characters)
3820 {
3821 foreach (OdeCharacter chr in _characters)
3822 {
3823 if (chr.Shell != IntPtr.Zero)
3824 {
3825 IntPtr body = d.GeomGetBody(chr.Shell);
3826
3827 d.Vector3 pos;
3828 d.GeomCopyPosition(chr.Shell, out pos);
3829 //d.BodyCopyPosition(body, out pos);
3830
3831 d.Matrix3 R;
3832 d.GeomCopyRotation(chr.Shell, out R);
3833 //d.BodyCopyRotation(body, out R);
3834
3835 ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
3836 d.Vector3 sides = new d.Vector3();
3837 sides.X = 0.5f;
3838 sides.Y = 0.5f;
3839 sides.Z = 0.5f;
3840
3841 ds.DrawBox(ref pos, ref R, ref sides);
3842 }
3843 }
3844 }
3845 }
3846
3847 public void start(int unused)
3848 {
3849 ds.SetViewpoint(ref xyz, ref hpr);
3850 }
3851#endif
3852 }
3853}
generated by cgit v1.1 at 2024-12-19 16:10:16 +0000