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