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.cs3886
1 files changed, 3886 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..05f6fae
--- /dev/null
+++ b/OpenSim/Region/Physics/ChOdePlugin/OdePlugin.cs
@@ -0,0 +1,3886 @@
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, 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, PhysicsActor parent,
1743 PrimitiveBaseShape pbs, uint localid, byte[] sdata)
1744 {
1745 Vector3 pos = position;
1746
1747 OdePrim newPrim;
1748 lock (OdeLock)
1749 {
1750 newPrim = new OdePrim(name, this, pos, parent, pbs, ode, localid, sdata);
1751 lock (_prims)
1752 _prims.Add(newPrim);
1753 }
1754
1755 return newPrim;
1756 }
1757*/
1758
1759 public void addActivePrim(OdePrim activatePrim)
1760 {
1761 // adds active prim.. (ones that should be iterated over in collisions_optimized
1762 lock (_activeprims)
1763 {
1764 if (!_activeprims.Contains(activatePrim))
1765 _activeprims.Add(activatePrim);
1766 //else
1767 // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
1768 }
1769 }
1770
1771 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1772 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
1773 {
1774 PhysicsActor result;
1775 IMesh mesh = null;
1776
1777 if (needsMeshing(pbs))
1778 mesh = mesher.CreateMesh(primName, pbs, size, (int)LevelOfDetail.High, true);
1779
1780 result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical, localid);
1781
1782 return result;
1783 }
1784/*
1785 public override PhysicsActor AddPrimShape(string primName, PhysicsActor parent, PrimitiveBaseShape pbs, Vector3 position,
1786 uint localid, byte[] sdata)
1787 {
1788 PhysicsActor result;
1789
1790 result = AddPrim(primName, position, parent,
1791 pbs, localid, sdata);
1792
1793 return result;
1794 }
1795*/
1796 public override float TimeDilation
1797 {
1798 get { return m_timeDilation; }
1799 }
1800
1801 public override bool SupportsNINJAJoints
1802 {
1803 get { return m_NINJA_physics_joints_enabled; }
1804 }
1805
1806 // internal utility function: must be called within a lock (OdeLock)
1807 private void InternalAddActiveJoint(PhysicsJoint joint)
1808 {
1809 activeJoints.Add(joint);
1810 SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
1811 }
1812
1813 // internal utility function: must be called within a lock (OdeLock)
1814 private void InternalAddPendingJoint(OdePhysicsJoint joint)
1815 {
1816 pendingJoints.Add(joint);
1817 SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
1818 }
1819
1820 // internal utility function: must be called within a lock (OdeLock)
1821 private void InternalRemovePendingJoint(PhysicsJoint joint)
1822 {
1823 pendingJoints.Remove(joint);
1824 SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene);
1825 }
1826
1827 // internal utility function: must be called within a lock (OdeLock)
1828 private void InternalRemoveActiveJoint(PhysicsJoint joint)
1829 {
1830 activeJoints.Remove(joint);
1831 SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
1832 }
1833
1834 public override void DumpJointInfo()
1835 {
1836 string hdr = "[NINJA] JOINTINFO: ";
1837 foreach (PhysicsJoint j in pendingJoints)
1838 {
1839 m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1840 }
1841 m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
1842 foreach (string jointName in SOPName_to_pendingJoint.Keys)
1843 {
1844 m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
1845 }
1846 m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
1847 foreach (PhysicsJoint j in activeJoints)
1848 {
1849 m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1850 }
1851 m_log.Debug(hdr + activeJoints.Count + " total active joints");
1852 foreach (string jointName in SOPName_to_activeJoint.Keys)
1853 {
1854 m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
1855 }
1856 m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
1857
1858 m_log.Debug(hdr + " Per-body joint connectivity information follows.");
1859 m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
1860 foreach (string actorName in joints_connecting_actor.Keys)
1861 {
1862 m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it");
1863 foreach (PhysicsJoint j in joints_connecting_actor[actorName])
1864 {
1865 m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1866 }
1867 m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
1868 }
1869 }
1870
1871 public override void RequestJointDeletion(string ObjectNameInScene)
1872 {
1873 lock (externalJointRequestsLock)
1874 {
1875 if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously
1876 {
1877 requestedJointsToBeDeleted.Add(ObjectNameInScene);
1878 }
1879 }
1880 }
1881
1882 private void DeleteRequestedJoints()
1883 {
1884 List<string> myRequestedJointsToBeDeleted;
1885 lock (externalJointRequestsLock)
1886 {
1887 // make a local copy of the shared list for processing (threading issues)
1888 myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
1889 }
1890
1891 foreach (string jointName in myRequestedJointsToBeDeleted)
1892 {
1893 lock (OdeLock)
1894 {
1895 //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
1896 if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
1897 {
1898 OdePhysicsJoint joint = null;
1899 if (SOPName_to_activeJoint.ContainsKey(jointName))
1900 {
1901 joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
1902 InternalRemoveActiveJoint(joint);
1903 }
1904 else if (SOPName_to_pendingJoint.ContainsKey(jointName))
1905 {
1906 joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
1907 InternalRemovePendingJoint(joint);
1908 }
1909
1910 if (joint != null)
1911 {
1912 //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
1913 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
1914 {
1915 string bodyName = joint.BodyNames[iBodyName];
1916 if (bodyName != "NULL")
1917 {
1918 joints_connecting_actor[bodyName].Remove(joint);
1919 if (joints_connecting_actor[bodyName].Count == 0)
1920 {
1921 joints_connecting_actor.Remove(bodyName);
1922 }
1923 }
1924 }
1925
1926 DoJointDeactivated(joint);
1927 if (joint.jointID != IntPtr.Zero)
1928 {
1929 d.JointDestroy(joint.jointID);
1930 joint.jointID = IntPtr.Zero;
1931 //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
1932 }
1933 else
1934 {
1935 //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
1936 }
1937 }
1938 else
1939 {
1940 // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
1941 }
1942 }
1943 else
1944 {
1945 // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
1946 }
1947 }
1948 }
1949
1950 // remove processed joints from the shared list
1951 lock (externalJointRequestsLock)
1952 {
1953 foreach (string jointName in myRequestedJointsToBeDeleted)
1954 {
1955 requestedJointsToBeDeleted.Remove(jointName);
1956 }
1957 }
1958 }
1959
1960 // for pending joints we don't know if their associated bodies exist yet or not.
1961 // the joint is actually created during processing of the taints
1962 private void CreateRequestedJoints()
1963 {
1964 List<PhysicsJoint> myRequestedJointsToBeCreated;
1965 lock (externalJointRequestsLock)
1966 {
1967 // make a local copy of the shared list for processing (threading issues)
1968 myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
1969 }
1970
1971 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
1972 {
1973 lock (OdeLock)
1974 {
1975 if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
1976 {
1977 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);
1978 continue;
1979 }
1980 if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
1981 {
1982 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);
1983 continue;
1984 }
1985
1986 InternalAddPendingJoint(joint as OdePhysicsJoint);
1987
1988 if (joint.BodyNames.Count >= 2)
1989 {
1990 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
1991 {
1992 string bodyName = joint.BodyNames[iBodyName];
1993 if (bodyName != "NULL")
1994 {
1995 if (!joints_connecting_actor.ContainsKey(bodyName))
1996 {
1997 joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
1998 }
1999 joints_connecting_actor[bodyName].Add(joint);
2000 }
2001 }
2002 }
2003 }
2004 }
2005
2006 // remove processed joints from shared list
2007 lock (externalJointRequestsLock)
2008 {
2009 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
2010 {
2011 requestedJointsToBeCreated.Remove(joint);
2012 }
2013 }
2014
2015 }
2016
2017 // public function to add an request for joint creation
2018 // this joint will just be added to a waiting list that is NOT processed during the main
2019 // Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
2020
2021 public override PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, Vector3 position,
2022 Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
2023
2024 {
2025
2026 OdePhysicsJoint joint = new OdePhysicsJoint();
2027 joint.ObjectNameInScene = objectNameInScene;
2028 joint.Type = jointType;
2029 joint.Position = position;
2030 joint.Rotation = rotation;
2031 joint.RawParams = parms;
2032 joint.BodyNames = new List<string>(bodyNames);
2033 joint.TrackedBodyName = trackedBodyName;
2034 joint.LocalRotation = localRotation;
2035 joint.jointID = IntPtr.Zero;
2036 joint.ErrorMessageCount = 0;
2037
2038 lock (externalJointRequestsLock)
2039 {
2040 if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice
2041 {
2042 requestedJointsToBeCreated.Add(joint);
2043 }
2044 }
2045 return joint;
2046 }
2047
2048 private void RemoveAllJointsConnectedToActor(PhysicsActor actor)
2049 {
2050 //m_log.Debug("RemoveAllJointsConnectedToActor: start");
2051 if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
2052 {
2053
2054 List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
2055 //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)
2056 foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
2057 {
2058 jointsToRemove.Add(j);
2059 }
2060 foreach (PhysicsJoint j in jointsToRemove)
2061 {
2062 //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
2063 RequestJointDeletion(j.ObjectNameInScene);
2064 //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
2065 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)
2066 }
2067 }
2068 }
2069
2070 public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
2071 {
2072 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start");
2073 lock (OdeLock)
2074 {
2075 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
2076 RemoveAllJointsConnectedToActor(actor);
2077 }
2078 }
2079
2080 // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2081 public override Vector3 GetJointAnchor(PhysicsJoint joint)
2082 {
2083 Debug.Assert(joint.IsInPhysicsEngine);
2084 d.Vector3 pos = new d.Vector3();
2085
2086 if (!(joint is OdePhysicsJoint))
2087 {
2088 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
2089 }
2090 else
2091 {
2092 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2093 switch (odeJoint.Type)
2094 {
2095 case PhysicsJointType.Ball:
2096 d.JointGetBallAnchor(odeJoint.jointID, out pos);
2097 break;
2098 case PhysicsJointType.Hinge:
2099 d.JointGetHingeAnchor(odeJoint.jointID, out pos);
2100 break;
2101 }
2102 }
2103 return new Vector3(pos.X, pos.Y, pos.Z);
2104 }
2105
2106 // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2107 // WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
2108 // appears to be unreliable. Fortunately we can compute the joint axis ourselves by
2109 // keeping track of the joint's original orientation relative to one of the involved bodies.
2110 public override Vector3 GetJointAxis(PhysicsJoint joint)
2111 {
2112 Debug.Assert(joint.IsInPhysicsEngine);
2113 d.Vector3 axis = new d.Vector3();
2114
2115 if (!(joint is OdePhysicsJoint))
2116 {
2117 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
2118 }
2119 else
2120 {
2121 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2122 switch (odeJoint.Type)
2123 {
2124 case PhysicsJointType.Ball:
2125 DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
2126 break;
2127 case PhysicsJointType.Hinge:
2128 d.JointGetHingeAxis(odeJoint.jointID, out axis);
2129 break;
2130 }
2131 }
2132 return new Vector3(axis.X, axis.Y, axis.Z);
2133 }
2134
2135
2136 public void remActivePrim(OdePrim deactivatePrim)
2137 {
2138 lock (_activeprims)
2139 {
2140 _activeprims.Remove(deactivatePrim);
2141 }
2142 }
2143
2144 public override void RemovePrim(PhysicsActor prim)
2145 {
2146 if (prim is OdePrim)
2147 {
2148 lock (OdeLock)
2149 {
2150 OdePrim p = (OdePrim) prim;
2151
2152 p.setPrimForRemoval();
2153 AddPhysicsActorTaint(prim);
2154 //RemovePrimThreadLocked(p);
2155 }
2156 }
2157 }
2158
2159 /// <summary>
2160 /// This is called from within simulate but outside the locked portion
2161 /// We need to do our own locking here
2162 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
2163 ///
2164 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
2165 /// that the space was using.
2166 /// </summary>
2167 /// <param name="prim"></param>
2168 public void RemovePrimThreadLocked(OdePrim prim)
2169 {
2170//Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
2171 lock (prim)
2172 {
2173 remCollisionEventReporting(prim);
2174 lock (ode)
2175 {
2176 if (prim.prim_geom != IntPtr.Zero)
2177 {
2178 prim.ResetTaints();
2179
2180 try
2181 {
2182 if (prim._triMeshData != IntPtr.Zero)
2183 {
2184 d.GeomTriMeshDataDestroy(prim._triMeshData);
2185 prim._triMeshData = IntPtr.Zero;
2186 }
2187 }
2188 catch { };
2189
2190 if (prim.IsPhysical)
2191 {
2192 prim.disableBody();
2193 if (prim.childPrim)
2194 {
2195 prim.childPrim = false;
2196 prim.Body = IntPtr.Zero;
2197 prim.m_disabled = true;
2198 prim.IsPhysical = false;
2199 }
2200
2201 }
2202 // we don't want to remove the main space
2203
2204 // If the geometry is in the targetspace, remove it from the target space
2205 //m_log.Warn(prim.m_targetSpace);
2206
2207 //if (prim.m_targetSpace != IntPtr.Zero)
2208 //{
2209 //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom))
2210 //{
2211
2212 //if (d.GeomIsSpace(prim.m_targetSpace))
2213 //{
2214 //waitForSpaceUnlock(prim.m_targetSpace);
2215 //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom);
2216 prim.m_targetSpace = IntPtr.Zero;
2217 //}
2218 //else
2219 //{
2220 // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2221 //((OdePrim)prim).m_targetSpace.ToString());
2222 //}
2223
2224 //}
2225 //}
2226 //m_log.Warn(prim.prim_geom);
2227 try
2228 {
2229 if (prim.prim_geom != IntPtr.Zero)
2230 {
2231
2232//string tPA;
2233//geom_name_map.TryGetValue(prim.prim_geom, out tPA);
2234//Console.WriteLine("**** Remove {0}", tPA);
2235 if(geom_name_map.ContainsKey(prim.prim_geom)) geom_name_map.Remove(prim.prim_geom);
2236 if(actor_name_map.ContainsKey(prim.prim_geom)) actor_name_map.Remove(prim.prim_geom);
2237 d.GeomDestroy(prim.prim_geom);
2238 prim.prim_geom = IntPtr.Zero;
2239 }
2240 else
2241 {
2242 m_log.Warn("[PHYSICS]: Unable to remove prim from physics scene");
2243 }
2244 }
2245 catch (AccessViolationException)
2246 {
2247 m_log.Info("[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed.");
2248 }
2249 lock (_prims)
2250 _prims.Remove(prim);
2251
2252 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2253 //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
2254 //{
2255 //if (prim.m_targetSpace != null)
2256 //{
2257 //if (d.GeomIsSpace(prim.m_targetSpace))
2258 //{
2259 //waitForSpaceUnlock(prim.m_targetSpace);
2260 //d.SpaceRemove(space, prim.m_targetSpace);
2261 // free up memory used by the space.
2262 //d.SpaceDestroy(prim.m_targetSpace);
2263 //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
2264 //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
2265 //}
2266 //else
2267 //{
2268 //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
2269 //((OdePrim) prim).m_targetSpace.ToString());
2270 //}
2271 //}
2272 //}
2273
2274 if (SupportsNINJAJoints)
2275 {
2276 RemoveAllJointsConnectedToActorThreadLocked(prim);
2277 }
2278 }
2279 }
2280 }
2281 }
2282
2283 #endregion
2284
2285 #region Space Separation Calculation
2286
2287 /// <summary>
2288 /// Takes a space pointer and zeros out the array we're using to hold the spaces
2289 /// </summary>
2290 /// <param name="pSpace"></param>
2291 public void resetSpaceArrayItemToZero(IntPtr pSpace)
2292 {
2293 for (int x = 0; x < staticPrimspace.GetLength(0); x++)
2294 {
2295 for (int y = 0; y < staticPrimspace.GetLength(1); y++)
2296 {
2297 if (staticPrimspace[x, y] == pSpace)
2298 staticPrimspace[x, y] = IntPtr.Zero;
2299 }
2300 }
2301 }
2302
2303 public void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY)
2304 {
2305 staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero;
2306 }
2307
2308 /// <summary>
2309 /// Called when a static prim moves. Allocates a space for the prim based on its position
2310 /// </summary>
2311 /// <param name="geom">the pointer to the geom that moved</param>
2312 /// <param name="pos">the position that the geom moved to</param>
2313 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
2314 /// <returns>a pointer to the new space it's in</returns>
2315 public IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace)
2316 {
2317 // Called from setting the Position and Size of an ODEPrim so
2318 // it's already in locked space.
2319
2320 // we don't want to remove the main space
2321 // we don't need to test physical here because this function should
2322 // never be called if the prim is physical(active)
2323
2324 // All physical prim end up in the root space
2325 //Thread.Sleep(20);
2326 if (currentspace != space)
2327 {
2328 //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString());
2329 //if (currentspace == IntPtr.Zero)
2330 //{
2331 //int adfadf = 0;
2332 //}
2333 if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero)
2334 {
2335 if (d.GeomIsSpace(currentspace))
2336 {
2337 waitForSpaceUnlock(currentspace);
2338 d.SpaceRemove(currentspace, geom);
2339 }
2340 else
2341 {
2342 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" + currentspace +
2343 " Geom:" + geom);
2344 }
2345 }
2346 else
2347 {
2348 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2349 if (sGeomIsIn != IntPtr.Zero)
2350 {
2351 if (d.GeomIsSpace(currentspace))
2352 {
2353 waitForSpaceUnlock(sGeomIsIn);
2354 d.SpaceRemove(sGeomIsIn, geom);
2355 }
2356 else
2357 {
2358 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2359 sGeomIsIn + " Geom:" + geom);
2360 }
2361 }
2362 }
2363
2364 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2365 if (d.SpaceGetNumGeoms(currentspace) == 0)
2366 {
2367 if (currentspace != IntPtr.Zero)
2368 {
2369 if (d.GeomIsSpace(currentspace))
2370 {
2371 waitForSpaceUnlock(currentspace);
2372 waitForSpaceUnlock(space);
2373 d.SpaceRemove(space, currentspace);
2374 // free up memory used by the space.
2375
2376 //d.SpaceDestroy(currentspace);
2377 resetSpaceArrayItemToZero(currentspace);
2378 }
2379 else
2380 {
2381 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2382 currentspace + " Geom:" + geom);
2383 }
2384 }
2385 }
2386 }
2387 else
2388 {
2389 // this is a physical object that got disabled. ;.;
2390 if (currentspace != IntPtr.Zero && geom != IntPtr.Zero)
2391 {
2392 if (d.SpaceQuery(currentspace, geom))
2393 {
2394 if (d.GeomIsSpace(currentspace))
2395 {
2396 waitForSpaceUnlock(currentspace);
2397 d.SpaceRemove(currentspace, geom);
2398 }
2399 else
2400 {
2401 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2402 currentspace + " Geom:" + geom);
2403 }
2404 }
2405 else
2406 {
2407 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2408 if (sGeomIsIn != IntPtr.Zero)
2409 {
2410 if (d.GeomIsSpace(sGeomIsIn))
2411 {
2412 waitForSpaceUnlock(sGeomIsIn);
2413 d.SpaceRemove(sGeomIsIn, geom);
2414 }
2415 else
2416 {
2417 m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
2418 sGeomIsIn + " Geom:" + geom);
2419 }
2420 }
2421 }
2422 }
2423 }
2424
2425 // The routines in the Position and Size sections do the 'inserting' into the space,
2426 // so all we have to do is make sure that the space that we're putting the prim into
2427 // is in the 'main' space.
2428 int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos);
2429 IntPtr newspace = calculateSpaceForGeom(pos);
2430
2431 if (newspace == IntPtr.Zero)
2432 {
2433 newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
2434 d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh);
2435 }
2436
2437 return newspace;
2438 }
2439
2440 /// <summary>
2441 /// Creates a new space at X Y
2442 /// </summary>
2443 /// <param name="iprimspaceArrItemX"></param>
2444 /// <param name="iprimspaceArrItemY"></param>
2445 /// <returns>A pointer to the created space</returns>
2446 public IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
2447 {
2448 // creating a new space for prim and inserting it into main space.
2449 staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero);
2450 d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space);
2451 waitForSpaceUnlock(space);
2452 d.SpaceSetSublevel(space, 1);
2453 d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
2454 return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
2455 }
2456
2457 /// <summary>
2458 /// Calculates the space the prim should be in by its position
2459 /// </summary>
2460 /// <param name="pos"></param>
2461 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
2462 public IntPtr calculateSpaceForGeom(Vector3 pos)
2463 {
2464 int[] xyspace = calculateSpaceArrayItemFromPos(pos);
2465 //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
2466 return staticPrimspace[xyspace[0], xyspace[1]];
2467 }
2468
2469 /// <summary>
2470 /// Holds the space allocation logic
2471 /// </summary>
2472 /// <param name="pos"></param>
2473 /// <returns>an array item based on the position</returns>
2474 public int[] calculateSpaceArrayItemFromPos(Vector3 pos)
2475 {
2476 int[] returnint = new int[2];
2477
2478 returnint[0] = (int) (pos.X/metersInSpace);
2479
2480 if (returnint[0] > ((int) (259f/metersInSpace)))
2481 returnint[0] = ((int) (259f/metersInSpace));
2482 if (returnint[0] < 0)
2483 returnint[0] = 0;
2484
2485 returnint[1] = (int) (pos.Y/metersInSpace);
2486 if (returnint[1] > ((int) (259f/metersInSpace)))
2487 returnint[1] = ((int) (259f/metersInSpace));
2488 if (returnint[1] < 0)
2489 returnint[1] = 0;
2490
2491 return returnint;
2492 }
2493
2494 #endregion
2495
2496 /// <summary>
2497 /// Routine to figure out if we need to mesh this prim with our mesher
2498 /// </summary>
2499 /// <param name="pbs"></param>
2500 /// <returns></returns>
2501 public bool needsMeshing(PrimitiveBaseShape pbs)
2502 {
2503 // most of this is redundant now as the mesher will return null if it cant mesh a prim
2504 // but we still need to check for sculptie meshing being enabled so this is the most
2505 // convenient place to do it for now...
2506
2507 // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
2508 // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
2509 int iPropertiesNotSupportedDefault = 0;
2510
2511 if (pbs.SculptEntry && !meshSculptedPrim)
2512 {
2513#if SPAM
2514 m_log.Warn("NonMesh");
2515#endif
2516 return false;
2517 }
2518
2519 // 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
2520 if (!forceSimplePrimMeshing && !pbs.SculptEntry)
2521 {
2522 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
2523 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
2524 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
2525 {
2526
2527 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
2528 && pbs.ProfileHollow == 0
2529 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
2530 && pbs.PathBegin == 0 && pbs.PathEnd == 0
2531 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
2532 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
2533 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
2534 {
2535#if SPAM
2536 m_log.Warn("NonMesh");
2537#endif
2538 return false;
2539 }
2540 }
2541 }
2542
2543 if (forceSimplePrimMeshing)
2544 return true;
2545
2546 if (pbs.ProfileHollow != 0)
2547 iPropertiesNotSupportedDefault++;
2548
2549 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
2550 iPropertiesNotSupportedDefault++;
2551
2552 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
2553 iPropertiesNotSupportedDefault++;
2554
2555 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
2556 iPropertiesNotSupportedDefault++;
2557
2558 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
2559 iPropertiesNotSupportedDefault++;
2560
2561 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
2562 iPropertiesNotSupportedDefault++;
2563
2564 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))
2565 iPropertiesNotSupportedDefault++;
2566
2567 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
2568 iPropertiesNotSupportedDefault++;
2569
2570 // test for torus
2571 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
2572 {
2573 if (pbs.PathCurve == (byte)Extrusion.Curve1)
2574 {
2575 iPropertiesNotSupportedDefault++;
2576 }
2577 }
2578 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
2579 {
2580 if (pbs.PathCurve == (byte)Extrusion.Straight)
2581 {
2582 iPropertiesNotSupportedDefault++;
2583 }
2584
2585 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
2586 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2587 {
2588 iPropertiesNotSupportedDefault++;
2589 }
2590 }
2591 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
2592 {
2593 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
2594 {
2595 iPropertiesNotSupportedDefault++;
2596 }
2597 }
2598 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
2599 {
2600 if (pbs.PathCurve == (byte)Extrusion.Straight)
2601 {
2602 iPropertiesNotSupportedDefault++;
2603 }
2604 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2605 {
2606 iPropertiesNotSupportedDefault++;
2607 }
2608 }
2609
2610 if (pbs.SculptEntry && meshSculptedPrim)
2611 iPropertiesNotSupportedDefault++;
2612
2613 if (iPropertiesNotSupportedDefault == 0)
2614 {
2615#if SPAM
2616 m_log.Warn("NonMesh");
2617#endif
2618 return false;
2619 }
2620#if SPAM
2621 m_log.Debug("Mesh");
2622#endif
2623 return true;
2624 }
2625
2626 /// <summary>
2627 /// Called after our prim properties are set Scale, position etc.
2628 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
2629 /// This assures us that we have no race conditions
2630 /// </summary>
2631 /// <param name="prim"></param>
2632 public override void AddPhysicsActorTaint(PhysicsActor prim)
2633 {
2634
2635 if (prim is OdePrim)
2636 {
2637 OdePrim taintedprim = ((OdePrim) prim);
2638 lock (_taintedPrimLock)
2639 {
2640 if (!(_taintedPrimH.Contains(taintedprim)))
2641 {
2642//Console.WriteLine("AddPhysicsActorTaint to " + taintedprim.m_primName);
2643 _taintedPrimH.Add(taintedprim); // HashSet for searching
2644 _taintedPrimL.Add(taintedprim); // List for ordered readout
2645 }
2646 }
2647 return;
2648 }
2649 else if (prim is OdeCharacter)
2650 {
2651 OdeCharacter taintedchar = ((OdeCharacter)prim);
2652 lock (_taintedActors)
2653 {
2654 if (!(_taintedActors.Contains(taintedchar)))
2655 {
2656 _taintedActors.Add(taintedchar);
2657 if (taintedchar.bad)
2658 m_log.DebugFormat("[PHYSICS]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid);
2659 }
2660 }
2661 }
2662 }
2663
2664 /// <summary>
2665 /// This is our main simulate loop
2666 /// It's thread locked by a Mutex in the scene.
2667 /// It holds Collisions, it instructs ODE to step through the physical reactions
2668 /// It moves the objects around in memory
2669 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
2670 /// </summary>
2671 /// <param name="timeStep"></param>
2672 /// <returns></returns>
2673 public override float Simulate(float timeStep)
2674 {
2675 if (framecount >= int.MaxValue)
2676 framecount = 0;
2677 //if (m_worldOffset != Vector3.Zero)
2678 // return 0;
2679
2680 framecount++;
2681
2682 DateTime now = DateTime.UtcNow;
2683 TimeSpan SinceLastFrame = now - m_lastframe;
2684 m_lastframe = now;
2685 float realtime = (float)SinceLastFrame.TotalSeconds;
2686// Console.WriteLine("ts={0} rt={1}", timeStep, realtime);
2687 timeStep = realtime;
2688
2689 // float fps = 1.0f / realtime;
2690 float fps = 0.0f; // number of ODE steps in this Simulate step
2691 //m_log.Info(timeStep.ToString());
2692 step_time += timeStep;
2693
2694 // If We're loaded down by something else,
2695 // or debugging with the Visual Studio project on pause
2696 // skip a few frames to catch up gracefully.
2697 // without shooting the physicsactors all over the place
2698
2699 if (step_time >= m_SkipFramesAtms)
2700 {
2701 // Instead of trying to catch up, it'll do 5 physics frames only
2702 step_time = ODE_STEPSIZE;
2703 m_physicsiterations = 5;
2704 }
2705 else
2706 {
2707 m_physicsiterations = 10;
2708 }
2709
2710 if (SupportsNINJAJoints)
2711 {
2712 DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2713 CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
2714 }
2715
2716 lock (OdeLock)
2717 {
2718 // Process 10 frames if the sim is running normal..
2719 // process 5 frames if the sim is running slow
2720 //try
2721 //{
2722 //d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
2723 //}
2724 //catch (StackOverflowException)
2725 //{
2726 // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
2727 // ode.drelease(world);
2728 //base.TriggerPhysicsBasedRestart();
2729 //}
2730
2731 int i = 0;
2732
2733 // Figure out the Frames Per Second we're going at.
2734 //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size
2735
2736 // fps = (step_time / ODE_STEPSIZE) * 1000;
2737 // HACK: Using a time dilation of 1.0 to debug rubberbanding issues
2738 //m_timeDilation = Math.Min((step_time / ODE_STEPSIZE) / (0.09375f / ODE_STEPSIZE), 1.0f);
2739
2740 // step_time = 0.09375f;
2741
2742 while (step_time > 0.0f)
2743 {
2744 //lock (ode)
2745 //{
2746 //if (!ode.lockquery())
2747 //{
2748 // ode.dlock(world);
2749 try
2750 {
2751 // Insert, remove Characters
2752 bool processedtaints = false;
2753
2754 lock (_taintedActors)
2755 {
2756 if (_taintedActors.Count > 0)
2757 {
2758 foreach (OdeCharacter character in _taintedActors)
2759 {
2760
2761 character.ProcessTaints(ODE_STEPSIZE);
2762
2763 processedtaints = true;
2764 //character.m_collisionscore = 0;
2765 }
2766
2767 if (processedtaints)
2768 _taintedActors.Clear();
2769 }
2770 } // end lock _taintedActors
2771
2772 // Modify other objects in the scene.
2773 processedtaints = false;
2774
2775 lock (_taintedPrimLock)
2776 {
2777 foreach (OdePrim prim in _taintedPrimL)
2778 {
2779 if (prim.m_taintremove)
2780 {
2781 //Console.WriteLine("Simulate calls RemovePrimThreadLocked");
2782 RemovePrimThreadLocked(prim);
2783 }
2784 else
2785 {
2786 //Console.WriteLine("Simulate calls ProcessTaints");
2787 prim.ProcessTaints(ODE_STEPSIZE);
2788 }
2789 processedtaints = true;
2790 prim.m_collisionscore = 0;
2791
2792 // This loop can block up the Heartbeat for a very long time on large regions.
2793 // We need to let the Watchdog know that the Heartbeat is not dead
2794 // NOTE: This is currently commented out, but if things like OAR loading are
2795 // timing the heartbeat out we will need to uncomment it
2796 //Watchdog.UpdateThread();
2797 }
2798
2799 if (SupportsNINJAJoints)
2800 {
2801 // Create pending joints, if possible
2802
2803 // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
2804 // a joint requires specifying the body id of both involved bodies
2805 if (pendingJoints.Count > 0)
2806 {
2807 List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
2808 //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
2809 foreach (PhysicsJoint joint in pendingJoints)
2810 {
2811 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
2812 string[] jointParams = joint.RawParams.Split(" ".ToCharArray(),
2813 System.StringSplitOptions.RemoveEmptyEntries);
2814 List<IntPtr> jointBodies = new List<IntPtr>();
2815 bool allJointBodiesAreReady = true;
2816 foreach (string jointParam in jointParams)
2817 {
2818 if (jointParam == "NULL")
2819 {
2820 //DoJointErrorMessage(joint, "attaching NULL joint to world");
2821 jointBodies.Add(IntPtr.Zero);
2822 }
2823 else
2824 {
2825 //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
2826 bool foundPrim = false;
2827 lock (_prims)
2828 {
2829 foreach (OdePrim prim in _prims) // FIXME: inefficient
2830 {
2831 if (prim.SOPName == jointParam)
2832 {
2833 //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
2834 if (prim.IsPhysical && prim.Body != IntPtr.Zero)
2835 {
2836 jointBodies.Add(prim.Body);
2837 foundPrim = true;
2838 break;
2839 }
2840 else
2841 {
2842 DoJointErrorMessage(joint, "prim name " + jointParam +
2843 " exists but is not (yet) physical; deferring joint creation. " +
2844 "IsPhysical property is " + prim.IsPhysical +
2845 " and body is " + prim.Body);
2846 foundPrim = false;
2847 break;
2848 }
2849 }
2850 }
2851 }
2852 if (foundPrim)
2853 {
2854 // all is fine
2855 }
2856 else
2857 {
2858 allJointBodiesAreReady = false;
2859 break;
2860 }
2861 }
2862 }
2863 if (allJointBodiesAreReady)
2864 {
2865 //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
2866 if (jointBodies[0] == jointBodies[1])
2867 {
2868 DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
2869 }
2870 else
2871 {
2872 switch (joint.Type)
2873 {
2874 case PhysicsJointType.Ball:
2875 {
2876 IntPtr odeJoint;
2877 //DoJointErrorMessage(joint, "ODE creating ball joint ");
2878 odeJoint = d.JointCreateBall(world, IntPtr.Zero);
2879 //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2880 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2881 //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
2882 d.JointSetBallAnchor(odeJoint,
2883 joint.Position.X,
2884 joint.Position.Y,
2885 joint.Position.Z);
2886 //DoJointErrorMessage(joint, "ODE joint setting OK");
2887 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
2888 //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
2889 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
2890 //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
2891
2892 if (joint is OdePhysicsJoint)
2893 {
2894 ((OdePhysicsJoint)joint).jointID = odeJoint;
2895 }
2896 else
2897 {
2898 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2899 }
2900 }
2901 break;
2902 case PhysicsJointType.Hinge:
2903 {
2904 IntPtr odeJoint;
2905 //DoJointErrorMessage(joint, "ODE creating hinge joint ");
2906 odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
2907 //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
2908 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
2909 //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
2910 d.JointSetHingeAnchor(odeJoint,
2911 joint.Position.X,
2912 joint.Position.Y,
2913 joint.Position.Z);
2914 // We use the orientation of the x-axis of the joint's coordinate frame
2915 // as the axis for the hinge.
2916
2917 // Therefore, we must get the joint's coordinate frame based on the
2918 // joint.Rotation field, which originates from the orientation of the
2919 // joint's proxy object in the scene.
2920
2921 // The joint's coordinate frame is defined as the transformation matrix
2922 // that converts a vector from joint-local coordinates into world coordinates.
2923 // World coordinates are defined as the XYZ coordinate system of the sim,
2924 // as shown in the top status-bar of the viewer.
2925
2926 // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
2927 // and use that as the hinge axis.
2928
2929 //joint.Rotation.Normalize();
2930 Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
2931
2932 // Now extract the X axis of the joint's coordinate frame.
2933
2934 // Do not try to use proxyFrame.AtAxis or you will become mired in the
2935 // tar pit of transposed, inverted, and generally messed-up orientations.
2936 // (In other words, Matrix4.AtAxis() is borked.)
2937 // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
2938
2939 // Instead, compute the X axis of the coordinate frame by transforming
2940 // the (1,0,0) vector. At least that works.
2941
2942 //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
2943 Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
2944 //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
2945 //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
2946 d.JointSetHingeAxis(odeJoint,
2947 jointAxis.X,
2948 jointAxis.Y,
2949 jointAxis.Z);
2950 //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
2951 if (joint is OdePhysicsJoint)
2952 {
2953 ((OdePhysicsJoint)joint).jointID = odeJoint;
2954 }
2955 else
2956 {
2957 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
2958 }
2959 }
2960 break;
2961 }
2962 successfullyProcessedPendingJoints.Add(joint);
2963 }
2964 }
2965 else
2966 {
2967 DoJointErrorMessage(joint, "joint could not yet be created; still pending");
2968 }
2969 }
2970 foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
2971 {
2972 //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
2973 //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
2974 InternalRemovePendingJoint(successfullyProcessedJoint);
2975 //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
2976 InternalAddActiveJoint(successfullyProcessedJoint);
2977 //DoJointErrorMessage(successfullyProcessedJoint, "done");
2978 }
2979 }
2980 } // end SupportsNINJAJoints
2981
2982 if (processedtaints)
2983//Console.WriteLine("Simulate calls Clear of _taintedPrim list");
2984 _taintedPrimH.Clear(); // ??? if this only ???
2985 _taintedPrimL.Clear();
2986 } // end lock _taintedPrimLock
2987
2988 // Move characters
2989 lock (_characters)
2990 {
2991 List<OdeCharacter> defects = new List<OdeCharacter>();
2992 foreach (OdeCharacter actor in _characters)
2993 {
2994 if (actor != null)
2995 actor.Move(ODE_STEPSIZE, defects);
2996 }
2997 if (0 != defects.Count)
2998 {
2999 foreach (OdeCharacter defect in defects)
3000 {
3001 RemoveCharacter(defect);
3002 }
3003 }
3004 } // end lock _characters
3005
3006 // Move other active objects
3007 lock (_activeprims)
3008 {
3009 foreach (OdePrim prim in _activeprims)
3010 {
3011 prim.m_collisionscore = 0;
3012 prim.Move(ODE_STEPSIZE);
3013 }
3014 } // end lock _activeprims
3015
3016 //if ((framecount % m_randomizeWater) == 0)
3017 // randomizeWater(waterlevel);
3018
3019 //int RayCastTimeMS = m_rayCastManager.ProcessQueuedRequests();
3020 m_rayCastManager.ProcessQueuedRequests();
3021
3022 collision_optimized(ODE_STEPSIZE);
3023
3024 lock (_collisionEventPrim)
3025 {
3026 foreach (PhysicsActor obj in _collisionEventPrim)
3027 {
3028 if (obj == null)
3029 continue;
3030
3031 switch ((ActorTypes)obj.PhysicsActorType)
3032 {
3033 case ActorTypes.Agent:
3034 OdeCharacter cobj = (OdeCharacter)obj;
3035 cobj.AddCollisionFrameTime(100);
3036 cobj.SendCollisions();
3037 break;
3038 case ActorTypes.Prim:
3039 OdePrim pobj = (OdePrim)obj;
3040 pobj.SendCollisions();
3041 break;
3042 }
3043 }
3044 } // end lock _collisionEventPrim
3045
3046 //if (m_global_contactcount > 5)
3047 //{
3048 // m_log.DebugFormat("[PHYSICS]: Contacts:{0}", m_global_contactcount);
3049 //}
3050
3051 m_global_contactcount = 0;
3052
3053 d.WorldQuickStep(world, ODE_STEPSIZE);
3054 d.JointGroupEmpty(contactgroup);
3055 fps++;
3056 //ode.dunlock(world);
3057 } // end try
3058 catch (Exception e)
3059 {
3060 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
3061 ode.dunlock(world);
3062 }
3063
3064 step_time -= ODE_STEPSIZE;
3065 i++;
3066 //}
3067 //else
3068 //{
3069 //fps = 0;
3070 //}
3071 //}
3072 } // end while (step_time > 0.0f)
3073
3074 lock (_characters)
3075 {
3076 foreach (OdeCharacter actor in _characters)
3077 {
3078 if (actor != null)
3079 {
3080 if (actor.bad)
3081 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
3082 actor.UpdatePositionAndVelocity();
3083 }
3084 }
3085 }
3086
3087 lock (_badCharacter)
3088 {
3089 if (_badCharacter.Count > 0)
3090 {
3091 foreach (OdeCharacter chr in _badCharacter)
3092 {
3093 RemoveCharacter(chr);
3094 }
3095 _badCharacter.Clear();
3096 }
3097 }
3098
3099 lock (_activeprims)
3100 {
3101 //if (timeStep < 0.2f)
3102 {
3103 foreach (OdePrim actor in _activeprims)
3104 {
3105 if (actor.IsPhysical && (d.BodyIsEnabled(actor.Body) || !actor._zeroFlag))
3106 {
3107 actor.UpdatePositionAndVelocity();
3108
3109 if (SupportsNINJAJoints)
3110 {
3111 // If an actor moved, move its joint proxy objects as well.
3112 // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
3113 // for this purpose but it is never called! So we just do the joint
3114 // movement code here.
3115
3116 if (actor.SOPName != null &&
3117 joints_connecting_actor.ContainsKey(actor.SOPName) &&
3118 joints_connecting_actor[actor.SOPName] != null &&
3119 joints_connecting_actor[actor.SOPName].Count > 0)
3120 {
3121 foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
3122 {
3123 if (affectedJoint.IsInPhysicsEngine)
3124 {
3125 DoJointMoved(affectedJoint);
3126 }
3127 else
3128 {
3129 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);
3130 }
3131 }
3132 }
3133 }
3134 }
3135 }
3136 }
3137 } // end lock _activeprims
3138
3139 //DumpJointInfo();
3140
3141 // Finished with all sim stepping. If requested, dump world state to file for debugging.
3142 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
3143 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
3144 if (physics_logging && (physics_logging_interval>0) && (framecount % physics_logging_interval == 0))
3145 {
3146 string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
3147 string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
3148
3149 if (physics_logging_append_existing_logfile)
3150 {
3151 string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
3152 TextWriter fwriter = File.AppendText(fname);
3153 fwriter.WriteLine(header);
3154 fwriter.Close();
3155 }
3156 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
3157 }
3158 } // end lock OdeLock
3159
3160 return fps * 1000.0f; //NB This is a FRAME COUNT, not a time! AND is divide by 1000 in SimStatusReporter!
3161 } // end Simulate
3162
3163 public override void GetResults()
3164 {
3165 }
3166
3167 public override bool IsThreaded
3168 {
3169 // for now we won't be multithreaded
3170 get { return (false); }
3171 }
3172
3173 #region ODE Specific Terrain Fixes
3174 public float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
3175 {
3176 float[] returnarr = new float[262144];
3177 float[,] resultarr = new float[(int)WorldExtents.X, (int)WorldExtents.Y];
3178
3179 // Filling out the array into its multi-dimensional components
3180 for (int y = 0; y < WorldExtents.Y; y++)
3181 {
3182 for (int x = 0; x < WorldExtents.X; x++)
3183 {
3184 resultarr[y, x] = heightMap[y * (int)WorldExtents.Y + x];
3185 }
3186 }
3187
3188 // Resize using Nearest Neighbour
3189
3190 // This particular way is quick but it only works on a multiple of the original
3191
3192 // The idea behind this method can be described with the following diagrams
3193 // second pass and third pass happen in the same loop really.. just separated
3194 // them to show what this does.
3195
3196 // First Pass
3197 // ResultArr:
3198 // 1,1,1,1,1,1
3199 // 1,1,1,1,1,1
3200 // 1,1,1,1,1,1
3201 // 1,1,1,1,1,1
3202 // 1,1,1,1,1,1
3203 // 1,1,1,1,1,1
3204
3205 // Second Pass
3206 // ResultArr2:
3207 // 1,,1,,1,,1,,1,,1,
3208 // ,,,,,,,,,,
3209 // 1,,1,,1,,1,,1,,1,
3210 // ,,,,,,,,,,
3211 // 1,,1,,1,,1,,1,,1,
3212 // ,,,,,,,,,,
3213 // 1,,1,,1,,1,,1,,1,
3214 // ,,,,,,,,,,
3215 // 1,,1,,1,,1,,1,,1,
3216 // ,,,,,,,,,,
3217 // 1,,1,,1,,1,,1,,1,
3218
3219 // Third pass fills in the blanks
3220 // ResultArr2:
3221 // 1,1,1,1,1,1,1,1,1,1,1,1
3222 // 1,1,1,1,1,1,1,1,1,1,1,1
3223 // 1,1,1,1,1,1,1,1,1,1,1,1
3224 // 1,1,1,1,1,1,1,1,1,1,1,1
3225 // 1,1,1,1,1,1,1,1,1,1,1,1
3226 // 1,1,1,1,1,1,1,1,1,1,1,1
3227 // 1,1,1,1,1,1,1,1,1,1,1,1
3228 // 1,1,1,1,1,1,1,1,1,1,1,1
3229 // 1,1,1,1,1,1,1,1,1,1,1,1
3230 // 1,1,1,1,1,1,1,1,1,1,1,1
3231 // 1,1,1,1,1,1,1,1,1,1,1,1
3232
3233 // X,Y = .
3234 // X+1,y = ^
3235 // X,Y+1 = *
3236 // X+1,Y+1 = #
3237
3238 // Filling in like this;
3239 // .*
3240 // ^#
3241 // 1st .
3242 // 2nd *
3243 // 3rd ^
3244 // 4th #
3245 // on single loop.
3246
3247 float[,] resultarr2 = new float[512, 512];
3248 for (int y = 0; y < WorldExtents.Y; y++)
3249 {
3250 for (int x = 0; x < WorldExtents.X; x++)
3251 {
3252 resultarr2[y * 2, x * 2] = resultarr[y, x];
3253
3254 if (y < WorldExtents.Y)
3255 {
3256 resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
3257 }
3258 if (x < WorldExtents.X)
3259 {
3260 resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
3261 }
3262 if (x < WorldExtents.X && y < WorldExtents.Y)
3263 {
3264 resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
3265 }
3266 }
3267 }
3268
3269 //Flatten out the array
3270 int i = 0;
3271 for (int y = 0; y < 512; y++)
3272 {
3273 for (int x = 0; x < 512; x++)
3274 {
3275 if (resultarr2[y, x] <= 0)
3276 returnarr[i] = 0.0000001f;
3277 else
3278 returnarr[i] = resultarr2[y, x];
3279
3280 i++;
3281 }
3282 }
3283
3284 return returnarr;
3285 }
3286
3287 public float[] ResizeTerrain512Interpolation(float[] heightMap)
3288 {
3289 float[] returnarr = new float[262144];
3290 float[,] resultarr = new float[512,512];
3291
3292 // Filling out the array into its multi-dimensional components
3293 for (int y = 0; y < 256; y++)
3294 {
3295 for (int x = 0; x < 256; x++)
3296 {
3297 resultarr[y, x] = heightMap[y * 256 + x];
3298 }
3299 }
3300
3301 // Resize using interpolation
3302
3303 // This particular way is quick but it only works on a multiple of the original
3304
3305 // The idea behind this method can be described with the following diagrams
3306 // second pass and third pass happen in the same loop really.. just separated
3307 // them to show what this does.
3308
3309 // First Pass
3310 // ResultArr:
3311 // 1,1,1,1,1,1
3312 // 1,1,1,1,1,1
3313 // 1,1,1,1,1,1
3314 // 1,1,1,1,1,1
3315 // 1,1,1,1,1,1
3316 // 1,1,1,1,1,1
3317
3318 // Second Pass
3319 // ResultArr2:
3320 // 1,,1,,1,,1,,1,,1,
3321 // ,,,,,,,,,,
3322 // 1,,1,,1,,1,,1,,1,
3323 // ,,,,,,,,,,
3324 // 1,,1,,1,,1,,1,,1,
3325 // ,,,,,,,,,,
3326 // 1,,1,,1,,1,,1,,1,
3327 // ,,,,,,,,,,
3328 // 1,,1,,1,,1,,1,,1,
3329 // ,,,,,,,,,,
3330 // 1,,1,,1,,1,,1,,1,
3331
3332 // Third pass fills in the blanks
3333 // ResultArr2:
3334 // 1,1,1,1,1,1,1,1,1,1,1,1
3335 // 1,1,1,1,1,1,1,1,1,1,1,1
3336 // 1,1,1,1,1,1,1,1,1,1,1,1
3337 // 1,1,1,1,1,1,1,1,1,1,1,1
3338 // 1,1,1,1,1,1,1,1,1,1,1,1
3339 // 1,1,1,1,1,1,1,1,1,1,1,1
3340 // 1,1,1,1,1,1,1,1,1,1,1,1
3341 // 1,1,1,1,1,1,1,1,1,1,1,1
3342 // 1,1,1,1,1,1,1,1,1,1,1,1
3343 // 1,1,1,1,1,1,1,1,1,1,1,1
3344 // 1,1,1,1,1,1,1,1,1,1,1,1
3345
3346 // X,Y = .
3347 // X+1,y = ^
3348 // X,Y+1 = *
3349 // X+1,Y+1 = #
3350
3351 // Filling in like this;
3352 // .*
3353 // ^#
3354 // 1st .
3355 // 2nd *
3356 // 3rd ^
3357 // 4th #
3358 // on single loop.
3359
3360 float[,] resultarr2 = new float[512,512];
3361 for (int y = 0; y < (int)Constants.RegionSize; y++)
3362 {
3363 for (int x = 0; x < (int)Constants.RegionSize; x++)
3364 {
3365 resultarr2[y*2, x*2] = resultarr[y, x];
3366
3367 if (y < (int)Constants.RegionSize)
3368 {
3369 if (y + 1 < (int)Constants.RegionSize)
3370 {
3371 if (x + 1 < (int)Constants.RegionSize)
3372 {
3373 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
3374 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3375 }
3376 else
3377 {
3378 resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
3379 }
3380 }
3381 else
3382 {
3383 resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
3384 }
3385 }
3386 if (x < (int)Constants.RegionSize)
3387 {
3388 if (x + 1 < (int)Constants.RegionSize)
3389 {
3390 if (y + 1 < (int)Constants.RegionSize)
3391 {
3392 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3393 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3394 }
3395 else
3396 {
3397 resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
3398 }
3399 }
3400 else
3401 {
3402 resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
3403 }
3404 }
3405 if (x < (int)Constants.RegionSize && y < (int)Constants.RegionSize)
3406 {
3407 if ((x + 1 < (int)Constants.RegionSize) && (y + 1 < (int)Constants.RegionSize))
3408 {
3409 resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
3410 resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
3411 }
3412 else
3413 {
3414 resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
3415 }
3416 }
3417 }
3418 }
3419 //Flatten out the array
3420 int i = 0;
3421 for (int y = 0; y < 512; y++)
3422 {
3423 for (int x = 0; x < 512; x++)
3424 {
3425 if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
3426 {
3427 m_log.Warn("[PHYSICS]: Non finite heightfield element detected. Setting it to 0");
3428 resultarr2[y, x] = 0;
3429 }
3430 returnarr[i] = resultarr2[y, x];
3431 i++;
3432 }
3433 }
3434
3435 return returnarr;
3436 }
3437
3438 #endregion
3439
3440 public override void SetTerrain(float[] heightMap)
3441 {
3442 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
3443 {
3444 if (m_parentScene is OdeScene)
3445 {
3446 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
3447 }
3448 }
3449 else
3450 {
3451 SetTerrain(heightMap, m_worldOffset);
3452 }
3453 }
3454
3455 public void SetTerrain(float[] heightMap, Vector3 pOffset)
3456 {
3457
3458 int regionsize = (int) Constants.RegionSize; // visible region size eg. 256(M)
3459
3460 int heightmapWidth = regionsize + 2; // ODE map size 257 x 257 (Meters) (1 extra
3461 int heightmapHeight = regionsize + 2;
3462
3463 int heightmapWidthSamples = (int)regionsize + 2; // Sample file size, 258 x 258 samples
3464 int heightmapHeightSamples = (int)regionsize + 2;
3465
3466 // Array of height samples for ODE
3467 float[] _heightmap;
3468 _heightmap = new float[(heightmapWidthSamples * heightmapHeightSamples)]; // loaded samples 258 x 258
3469
3470 // Other ODE parameters
3471 const float scale = 1.0f;
3472 const float offset = 0.0f;
3473 const float thickness = 2.0f; // Was 0.2f, Larger appears to prevent Av fall-through
3474 const int wrap = 0;
3475
3476 float hfmin = 2000f;
3477 float hfmax = -2000f;
3478 float minele = 0.0f; // Dont allow -ve heights
3479
3480 int x = 0;
3481 int y = 0;
3482 int xx = 0;
3483 int yy = 0;
3484
3485 // load the height samples array from the heightMap
3486 for ( x = 0; x < heightmapWidthSamples; x++) // 0 to 257
3487 {
3488 for ( y = 0; y < heightmapHeightSamples; y++) // 0 to 257
3489 {
3490 xx = x - 1;
3491 if (xx < 0) xx = 0;
3492 if (xx > (regionsize - 1)) xx = regionsize - 1;
3493
3494 yy = y - 1;
3495 if (yy < 0) yy = 0;
3496 if (yy > (regionsize - 1)) yy = regionsize - 1;
3497 // Input xx = 0 0 1 2 ..... 254 255 255 256 total in
3498 // Output x = 0 1 2 3 ..... 255 256 257 258 total out
3499 float val= heightMap[(yy * regionsize) + xx]; // input from heightMap, <0-255 * 256> <0-255>
3500 if (val < minele) val = minele;
3501 _heightmap[x * (regionsize + 2) + y] = val; // samples output to _heightmap, <0-257 * 258> <0-257>
3502 hfmin = (val < hfmin) ? val : hfmin;
3503 hfmax = (val > hfmax) ? val : hfmax;
3504 }
3505 }
3506
3507 lock (OdeLock)
3508 {
3509 IntPtr GroundGeom = IntPtr.Zero;
3510 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
3511 {
3512 RegionTerrain.Remove(pOffset);
3513 if (GroundGeom != IntPtr.Zero)
3514 {
3515 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
3516 {
3517 TerrainHeightFieldHeights.Remove(GroundGeom);
3518 }
3519 d.SpaceRemove(space, GroundGeom);
3520 d.GeomDestroy(GroundGeom);
3521 }
3522 }
3523 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
3524 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0,
3525 heightmapWidth, heightmapHeight, (int)heightmapWidthSamples,
3526 (int)heightmapHeightSamples, scale, offset, thickness, wrap);
3527 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
3528 GroundGeom = d.CreateHeightfield(space, HeightmapData, 1);
3529 if (GroundGeom != IntPtr.Zero)
3530 {
3531 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
3532 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
3533 }
3534 geom_name_map[GroundGeom] = "Terrain";
3535
3536 d.Matrix3 R = new d.Matrix3();
3537
3538 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
3539 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
3540 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
3541
3542 q1 = q1 * q2;
3543 //q1 = q1 * q3;
3544 Vector3 v3;
3545 float angle;
3546 q1.GetAxisAngle(out v3, out angle);
3547
3548 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
3549 d.GeomSetRotation(GroundGeom, ref R);
3550 d.GeomSetPosition(GroundGeom, (pOffset.X + (regionsize * 0.5f)) - 0.5f, (pOffset.Y + (regionsize * 0.5f)) - 0.5f, 0);
3551 IntPtr testGround = IntPtr.Zero;
3552 if (RegionTerrain.TryGetValue(pOffset, out testGround))
3553 {
3554 RegionTerrain.Remove(pOffset);
3555 }
3556 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
3557 TerrainHeightFieldHeights.Add(GroundGeom,_heightmap);
3558 }
3559 }
3560
3561 public override void DeleteTerrain()
3562 {
3563 }
3564
3565 public float GetWaterLevel()
3566 {
3567 return waterlevel;
3568 }
3569
3570 public override bool SupportsCombining()
3571 {
3572 return true;
3573 }
3574
3575 public override void UnCombine(PhysicsScene pScene)
3576 {
3577 IntPtr localGround = IntPtr.Zero;
3578// float[] localHeightfield;
3579 bool proceed = false;
3580 List<IntPtr> geomDestroyList = new List<IntPtr>();
3581
3582 lock (OdeLock)
3583 {
3584 if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround))
3585 {
3586 foreach (IntPtr geom in TerrainHeightFieldHeights.Keys)
3587 {
3588 if (geom == localGround)
3589 {
3590// localHeightfield = TerrainHeightFieldHeights[geom];
3591 proceed = true;
3592 }
3593 else
3594 {
3595 geomDestroyList.Add(geom);
3596 }
3597 }
3598
3599 if (proceed)
3600 {
3601 m_worldOffset = Vector3.Zero;
3602 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
3603 m_parentScene = null;
3604
3605 foreach (IntPtr g in geomDestroyList)
3606 {
3607 // removingHeightField needs to be done or the garbage collector will
3608 // collect the terrain data before we tell ODE to destroy it causing
3609 // memory corruption
3610 if (TerrainHeightFieldHeights.ContainsKey(g))
3611 {
3612// float[] removingHeightField = TerrainHeightFieldHeights[g];
3613 TerrainHeightFieldHeights.Remove(g);
3614
3615 if (RegionTerrain.ContainsKey(g))
3616 {
3617 RegionTerrain.Remove(g);
3618 }
3619
3620 d.GeomDestroy(g);
3621 //removingHeightField = new float[0];
3622 }
3623 }
3624
3625 }
3626 else
3627 {
3628 m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data.");
3629
3630 }
3631 }
3632 }
3633 }
3634
3635 public override void SetWaterLevel(float baseheight)
3636 {
3637 waterlevel = baseheight;
3638 randomizeWater(waterlevel);
3639 }
3640
3641 public void randomizeWater(float baseheight)
3642 {
3643 const uint heightmapWidth = m_regionWidth + 2;
3644 const uint heightmapHeight = m_regionHeight + 2;
3645 const uint heightmapWidthSamples = m_regionWidth + 2;
3646 const uint heightmapHeightSamples = m_regionHeight + 2;
3647 const float scale = 1.0f;
3648 const float offset = 0.0f;
3649 const float thickness = 2.9f;
3650 const int wrap = 0;
3651
3652 for (int i = 0; i < (258 * 258); i++)
3653 {
3654 _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
3655 // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
3656 }
3657
3658 lock (OdeLock)
3659 {
3660 if (WaterGeom != IntPtr.Zero)
3661 {
3662 d.SpaceRemove(space, WaterGeom);
3663 }
3664 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
3665 d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
3666 (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
3667 offset, thickness, wrap);
3668 d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
3669 WaterGeom = d.CreateHeightfield(space, HeightmapData, 1);
3670 if (WaterGeom != IntPtr.Zero)
3671 {
3672 d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
3673 d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
3674
3675 }
3676 geom_name_map[WaterGeom] = "Water";
3677
3678 d.Matrix3 R = new d.Matrix3();
3679
3680 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
3681 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
3682 //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
3683
3684 q1 = q1 * q2;
3685 //q1 = q1 * q3;
3686 Vector3 v3;
3687 float angle;
3688 q1.GetAxisAngle(out v3, out angle);
3689
3690 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
3691 d.GeomSetRotation(WaterGeom, ref R);
3692 d.GeomSetPosition(WaterGeom, 128, 128, 0);
3693
3694 }
3695
3696 }
3697
3698 public override void Dispose()
3699 {
3700 m_rayCastManager.Dispose();
3701 m_rayCastManager = null;
3702
3703 lock (OdeLock)
3704 {
3705 lock (_prims)
3706 {
3707 foreach (OdePrim prm in _prims)
3708 {
3709 RemovePrim(prm);
3710 }
3711 }
3712
3713 //foreach (OdeCharacter act in _characters)
3714 //{
3715 //RemoveAvatar(act);
3716 //}
3717 d.WorldDestroy(world);
3718 //d.CloseODE();
3719 }
3720 }
3721 public override Dictionary<uint, float> GetTopColliders()
3722 {
3723 Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
3724 int cnt = 0;
3725 lock (_prims)
3726 {
3727 foreach (OdePrim prm in _prims)
3728 {
3729 if (prm.CollisionScore > 0)
3730 {
3731 returncolliders.Add(prm.m_localID, prm.CollisionScore);
3732 cnt++;
3733 prm.CollisionScore = 0f;
3734 if (cnt > 25)
3735 {
3736 break;
3737 }
3738 }
3739 }
3740 }
3741 return returncolliders;
3742 }
3743
3744 public override bool SupportsRayCast()
3745 {
3746 return true;
3747 }
3748
3749 public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
3750 {
3751 if (retMethod != null)
3752 {
3753 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
3754 }
3755 }
3756
3757#if USE_DRAWSTUFF
3758 // Keyboard callback
3759 public void command(int cmd)
3760 {
3761 IntPtr geom;
3762 d.Mass mass;
3763 d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
3764
3765
3766
3767 Char ch = Char.ToLower((Char)cmd);
3768 switch ((Char)ch)
3769 {
3770 case 'w':
3771 try
3772 {
3773 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));
3774
3775 xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
3776 ds.SetViewpoint(ref xyz, ref hpr);
3777 }
3778 catch (ArgumentException)
3779 { hpr.X = 0; }
3780 break;
3781
3782 case 'a':
3783 hpr.X++;
3784 ds.SetViewpoint(ref xyz, ref hpr);
3785 break;
3786
3787 case 's':
3788 try
3789 {
3790 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));
3791
3792 xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
3793 ds.SetViewpoint(ref xyz, ref hpr);
3794 }
3795 catch (ArgumentException)
3796 { hpr.X = 0; }
3797 break;
3798 case 'd':
3799 hpr.X--;
3800 ds.SetViewpoint(ref xyz, ref hpr);
3801 break;
3802 case 'r':
3803 xyz.Z++;
3804 ds.SetViewpoint(ref xyz, ref hpr);
3805 break;
3806 case 'f':
3807 xyz.Z--;
3808 ds.SetViewpoint(ref xyz, ref hpr);
3809 break;
3810 case 'e':
3811 xyz.Y++;
3812 ds.SetViewpoint(ref xyz, ref hpr);
3813 break;
3814 case 'q':
3815 xyz.Y--;
3816 ds.SetViewpoint(ref xyz, ref hpr);
3817 break;
3818 }
3819 }
3820
3821 public void step(int pause)
3822 {
3823
3824 ds.SetColor(1.0f, 1.0f, 0.0f);
3825 ds.SetTexture(ds.Texture.Wood);
3826 lock (_prims)
3827 {
3828 foreach (OdePrim prm in _prims)
3829 {
3830 //IntPtr body = d.GeomGetBody(prm.prim_geom);
3831 if (prm.prim_geom != IntPtr.Zero)
3832 {
3833 d.Vector3 pos;
3834 d.GeomCopyPosition(prm.prim_geom, out pos);
3835 //d.BodyCopyPosition(body, out pos);
3836
3837 d.Matrix3 R;
3838 d.GeomCopyRotation(prm.prim_geom, out R);
3839 //d.BodyCopyRotation(body, out R);
3840
3841
3842 d.Vector3 sides = new d.Vector3();
3843 sides.X = prm.Size.X;
3844 sides.Y = prm.Size.Y;
3845 sides.Z = prm.Size.Z;
3846
3847 ds.DrawBox(ref pos, ref R, ref sides);
3848 }
3849 }
3850 }
3851 ds.SetColor(1.0f, 0.0f, 0.0f);
3852 lock (_characters)
3853 {
3854 foreach (OdeCharacter chr in _characters)
3855 {
3856 if (chr.Shell != IntPtr.Zero)
3857 {
3858 IntPtr body = d.GeomGetBody(chr.Shell);
3859
3860 d.Vector3 pos;
3861 d.GeomCopyPosition(chr.Shell, out pos);
3862 //d.BodyCopyPosition(body, out pos);
3863
3864 d.Matrix3 R;
3865 d.GeomCopyRotation(chr.Shell, out R);
3866 //d.BodyCopyRotation(body, out R);
3867
3868 ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
3869 d.Vector3 sides = new d.Vector3();
3870 sides.X = 0.5f;
3871 sides.Y = 0.5f;
3872 sides.Z = 0.5f;
3873
3874 ds.DrawBox(ref pos, ref R, ref sides);
3875 }
3876 }
3877 }
3878 }
3879
3880 public void start(int unused)
3881 {
3882 ds.SetViewpoint(ref xyz, ref hpr);
3883 }
3884#endif
3885 }
3886}
generated by cgit v1.1 at 2024-12-18 23:40:04 +0000