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