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