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