aboutsummaryrefslogtreecommitdiffstatshomepage
path: root/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
diff options
context:
space:
mode:
Diffstat (limited to 'OpenSim/Region/PhysicsModules/Ode/OdeScene.cs')
-rw-r--r--OpenSim/Region/PhysicsModules/Ode/OdeScene.cs4455
1 files changed, 2602 insertions, 1853 deletions
diff --git a/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs b/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
index 410463c..ed2aad4 100644
--- a/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
+++ b/OpenSim/Region/PhysicsModules/Ode/OdeScene.cs
@@ -25,54 +25,52 @@
25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 25 * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
26 */ 26 */
27 27
28// Revision 2011/12/13 by Ubit Umarov 28// changes for varsize regions
29//#define SPAM 29// note that raycasts need to have limited range
30// (even in normal regions)
31// or application thread stack may just blowup
32// see RayCast(ODERayCastRequest req)
30 33
31using System; 34using System;
32using System.Collections.Generic; 35using System.Collections.Generic;
36using System.Diagnostics;
37using System.IO;
33using System.Linq; 38using System.Linq;
34using System.Reflection; 39using System.Reflection;
40using System.Runtime.ExceptionServices;
35using System.Runtime.InteropServices; 41using System.Runtime.InteropServices;
36using System.Threading; 42using System.Threading;
37using System.IO;
38using System.Diagnostics;
39using log4net; 43using log4net;
40using Nini.Config; 44using Nini.Config;
41using Mono.Addins; 45using Mono.Addins;
42using OdeAPI; 46using OpenMetaverse;
43using OpenSim.Framework; 47using OpenSim.Framework;
48using OpenSim.Region.PhysicsModules.SharedBase;
44using OpenSim.Region.Framework.Scenes; 49using OpenSim.Region.Framework.Scenes;
45using OpenSim.Region.Framework.Interfaces; 50using OpenSim.Region.Framework.Interfaces;
46using OpenSim.Region.PhysicsModules.SharedBase;
47using OpenMetaverse;
48 51
49namespace OpenSim.Region.PhysicsModule.ubOde 52namespace OpenSim.Region.PhysicsModule.ODE
50{ 53{
51 // colision flags of things others can colide with 54 public enum StatusIndicators : int
52 // rays, sensors, probes removed since can't be colided with 55 {
53 // The top space where things are placed provided further selection 56 Generic = 0,
54 // ie physical are in active space nonphysical in static 57 Start = 1,
55 // this should be exclusive as possible 58 End = 2
59 }
56 60
57 [Flags] 61 [Flags]
58 public enum CollisionCategories : uint 62 public enum CollisionCategories : int
59 { 63 {
60 Disabled = 0, 64 Disabled = 0,
61 //by 'things' types 65 Geom = 0x00000001,
62 Space = 0x01, 66 Body = 0x00000002,
63 Geom = 0x02, // aka prim/part 67 Space = 0x00000004,
64 Character = 0x04, 68 Character = 0x00000008,
65 Land = 0x08, 69 Land = 0x00000010,
66 Water = 0x010, 70 Water = 0x00000020,
67 71 Wind = 0x00000040,
68 // by state 72 Sensor = 0x00000080,
69 Phantom = 0x01000, 73 Selected = 0x00000100
70 VolumeDtc = 0x02000,
71 Selected = 0x04000,
72 NoShape = 0x08000,
73
74
75 All = 0xffffffff
76 } 74 }
77 75
78 /// <summary> 76 /// <summary>
@@ -93,579 +91,836 @@ namespace OpenSim.Region.PhysicsModule.ubOde
93 /// <summary></summary> 91 /// <summary></summary>
94 Plastic = 5, 92 Plastic = 5,
95 /// <summary></summary> 93 /// <summary></summary>
96 Rubber = 6, 94 Rubber = 6
97
98 light = 7 // compatibility with old viewers
99 }
100
101 public enum changes : int
102 {
103 Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
104 Remove,
105 Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
106 // or removes from a object if arg is null
107 DeLink,
108 Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
109 Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
110 PosOffset, // not in use
111 // arg Vector3 new position in local coords. Changes prim position in object
112 OriOffset, // not in use
113 // arg Vector3 new position in local coords. Changes prim position in object
114 Velocity,
115 TargetVelocity,
116 AngVelocity,
117 Acceleration,
118 Force,
119 Torque,
120 Momentum,
121
122 AddForce,
123 AddAngForce,
124 AngLock,
125
126 Buoyancy,
127
128 PIDTarget,
129 PIDTau,
130 PIDActive,
131
132 PIDHoverHeight,
133 PIDHoverType,
134 PIDHoverTau,
135 PIDHoverActive,
136
137 Size,
138 AvatarSize,
139 Shape,
140 PhysRepData,
141 AddPhysRep,
142
143 CollidesWater,
144 VolumeDtc,
145
146 Physical,
147 Phantom,
148 Selected,
149 disabled,
150 building,
151
152 VehicleType,
153 VehicleFloatParam,
154 VehicleVectorParam,
155 VehicleRotationParam,
156 VehicleFlags,
157 SetVehicle,
158
159 Null //keep this last used do dim the methods array. does nothing but pulsing the prim
160 } 95 }
161 96
162 public struct ODEchangeitem 97 public class OdeScene : PhysicsScene
163 { 98 {
164 public PhysicsActor actor; 99 private readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString());
165 public OdeCharacter character;
166 public changes what;
167 public Object arg;
168 }
169 100
170 public class ODEScene : PhysicsScene 101 // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
171 {
172 private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
173 102
174 public bool m_OSOdeLib = false; 103 /// <summary>
175 public Scene m_frameWorkScene = null; 104 /// Provide a sync object so that only one thread calls d.Collide() at a time across all OdeScene instances.
105 /// </summary>
106 /// <remarks>
107 /// With ODE as of r1755 (though also tested on r1860), only one thread can call d.Collide() at a
108 /// time, even where physics objects are in entirely different ODE worlds. This is because generating contacts
109 /// uses a static cache at the ODE level.
110 ///
111 /// Without locking, simulators running multiple regions will eventually crash with a native stack trace similar
112 /// to
113 ///
114 /// mono() [0x489171]
115 /// mono() [0x4d154f]
116 /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f6ded592c60]
117 /// .../opensim/bin/libode-x86_64.so(_ZN6Opcode11OBBCollider8_CollideEPKNS_14AABBNoLeafNodeE+0xd7a) [0x7f6dd822628a]
118 ///
119 /// ODE provides an experimental option to cache in thread local storage but compiling ODE with this option
120 /// causes OpenSimulator to immediately crash with a native stack trace similar to
121 ///
122 /// mono() [0x489171]
123 /// mono() [0x4d154f]
124 /// /lib/x86_64-linux-gnu/libpthread.so.0(+0xfc60) [0x7f03c9849c60]
125 /// .../opensim/bin/libode-x86_64.so(_Z12dCollideCCTLP6dxGeomS0_iP12dContactGeomi+0x92) [0x7f03b44bcf82]
126 /// </remarks>
127 internal static Object UniversalColliderSyncObject = new Object();
128 internal static Object SimulationLock = new Object();
129
130 /// <summary>
131 /// Is stats collecting enabled for this ODE scene?
132 /// </summary>
133 public bool CollectStats { get; set; }
176 134
177// private int threadid = 0; 135 /// <summary>
136 /// Statistics for this scene.
137 /// </summary>
138 private Dictionary<string, float> m_stats = new Dictionary<string, float>();
178 139
179// const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; 140 /// <summary>
141 /// Stat name for total number of avatars in this ODE scene.
142 /// </summary>
143 public const string ODETotalAvatarsStatName = "ODETotalAvatars";
180 144
181 const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1 | d.ContactFlags.Slip1 | d.ContactFlags.Slip2; 145 /// <summary>
182 const float comumContactERP = 0.75f; 146 /// Stat name for total number of prims in this ODE scene.
183 const float comumContactCFM = 0.0001f; 147 /// </summary>
184 const float comumContactSLIP = 0f; 148 public const string ODETotalPrimsStatName = "ODETotalPrims";
185 149
186 float frictionMovementMult = 0.8f; 150 /// <summary>
151 /// Stat name for total number of prims with active physics in this ODE scene.
152 /// </summary>
153 public const string ODEActivePrimsStatName = "ODEActivePrims";
154
155 /// <summary>
156 /// Stat name for the total time spent in ODE frame processing.
157 /// </summary>
158 /// <remarks>
159 /// A sanity check for the main scene loop physics time.
160 /// </remarks>
161 public const string ODETotalFrameMsStatName = "ODETotalFrameMS";
162
163 /// <summary>
164 /// Stat name for time spent processing avatar taints per frame
165 /// </summary>
166 public const string ODEAvatarTaintMsStatName = "ODEAvatarTaintFrameMS";
167
168 /// <summary>
169 /// Stat name for time spent processing prim taints per frame
170 /// </summary>
171 public const string ODEPrimTaintMsStatName = "ODEPrimTaintFrameMS";
172
173 /// <summary>
174 /// Stat name for time spent calculating avatar forces per frame.
175 /// </summary>
176 public const string ODEAvatarForcesFrameMsStatName = "ODEAvatarForcesFrameMS";
177
178 /// <summary>
179 /// Stat name for time spent calculating prim forces per frame
180 /// </summary>
181 public const string ODEPrimForcesFrameMsStatName = "ODEPrimForcesFrameMS";
182
183 /// <summary>
184 /// Stat name for time spent fulfilling raycasting requests per frame
185 /// </summary>
186 public const string ODERaycastingFrameMsStatName = "ODERaycastingFrameMS";
187
188 /// <summary>
189 /// Stat name for time spent in native code that actually steps through the simulation.
190 /// </summary>
191 public const string ODENativeStepFrameMsStatName = "ODENativeStepFrameMS";
192
193 /// <summary>
194 /// Stat name for the number of milliseconds that ODE spends in native space collision code.
195 /// </summary>
196 public const string ODENativeSpaceCollisionFrameMsStatName = "ODENativeSpaceCollisionFrameMS";
197
198 /// <summary>
199 /// Stat name for milliseconds that ODE spends in native geom collision code.
200 /// </summary>
201 public const string ODENativeGeomCollisionFrameMsStatName = "ODENativeGeomCollisionFrameMS";
202
203 /// <summary>
204 /// Time spent in collision processing that is not spent in native space or geom collision code.
205 /// </summary>
206 public const string ODEOtherCollisionFrameMsStatName = "ODEOtherCollisionFrameMS";
207
208 /// <summary>
209 /// Stat name for time spent notifying listeners of collisions
210 /// </summary>
211 public const string ODECollisionNotificationFrameMsStatName = "ODECollisionNotificationFrameMS";
212
213 /// <summary>
214 /// Stat name for milliseconds spent updating avatar position and velocity
215 /// </summary>
216 public const string ODEAvatarUpdateFrameMsStatName = "ODEAvatarUpdateFrameMS";
217
218 /// <summary>
219 /// Stat name for the milliseconds spent updating prim position and velocity
220 /// </summary>
221 public const string ODEPrimUpdateFrameMsStatName = "ODEPrimUpdateFrameMS";
222
223 /// <summary>
224 /// Stat name for avatar collisions with another entity.
225 /// </summary>
226 public const string ODEAvatarContactsStatsName = "ODEAvatarContacts";
227
228 /// <summary>
229 /// Stat name for prim collisions with another entity.
230 /// </summary>
231 public const string ODEPrimContactsStatName = "ODEPrimContacts";
232
233 /// <summary>
234 /// Used to hold tick numbers for stat collection purposes.
235 /// </summary>
236 private int m_nativeCollisionStartTick;
237
238 /// <summary>
239 /// A messy way to tell if we need to avoid adding a collision time because this was already done in the callback.
240 /// </summary>
241 private bool m_inCollisionTiming;
242
243 /// <summary>
244 /// A temporary holder for the number of avatar collisions in a frame, so we can work out how many object
245 /// collisions occured using the _perloopcontact if stats collection is enabled.
246 /// </summary>
247 private int m_tempAvatarCollisionsThisFrame;
248
249 /// <summary>
250 /// Used in calculating physics frame time dilation
251 /// </summary>
252 private int tickCountFrameRun;
187 253
188 float TerrainBounce = 0.1f; 254 /// <summary>
189 float TerrainFriction = 0.3f; 255 /// Used in calculating physics frame time dilation
256 /// </summary>
257 private int latertickcount;
190 258
191 public float AvatarFriction = 0;// 0.9f * 0.5f; 259 private Random fluidRandomizer = new Random(Environment.TickCount);
192 260
193 // this netx dimensions are only relevant for terrain partition (mega regions)
194 // WorldExtents below has the simulation dimensions
195 // they should be identical except on mega regions
196 private uint m_regionWidth = Constants.RegionSize; 261 private uint m_regionWidth = Constants.RegionSize;
197 private uint m_regionHeight = Constants.RegionSize; 262 private uint m_regionHeight = Constants.RegionSize;
198 263
199 public float ODE_STEPSIZE = 0.020f; 264 private float ODE_STEPSIZE = 0.0178f;
200 public float HalfOdeStep = 0.01f; 265 private float metersInSpace = 29.9f;
201 public int odetimestepMS = 20; // rounded
202 private float metersInSpace = 25.6f;
203 private float m_timeDilation = 1.0f; 266 private float m_timeDilation = 1.0f;
204 267
205 private DateTime m_lastframe;
206 private DateTime m_lastMeshExpire;
207
208 public float gravityx = 0f; 268 public float gravityx = 0f;
209 public float gravityy = 0f; 269 public float gravityy = 0f;
210 public float gravityz = -9.8f; 270 public float gravityz = -9.8f;
211 271
272 public float AvatarTerminalVelocity { get; set; }
273
274 private float contactsurfacelayer = 0.001f;
275
276 private int HashspaceLow = -5;
277 private int HashspaceHigh = 12;
278
212 private float waterlevel = 0f; 279 private float waterlevel = 0f;
213 private int framecount = 0; 280 private int framecount = 0;
281 //private int m_returncollisions = 10;
282
283 private IntPtr contactgroup;
284
285// internal IntPtr WaterGeom;
286
287 private float nmTerrainContactFriction = 255.0f;
288 private float nmTerrainContactBounce = 0.1f;
289 private float nmTerrainContactERP = 0.1025f;
290
291 private float mTerrainContactFriction = 75f;
292 private float mTerrainContactBounce = 0.1f;
293 private float mTerrainContactERP = 0.05025f;
294
295 private float nmAvatarObjectContactFriction = 250f;
296 private float nmAvatarObjectContactBounce = 0.1f;
297
298 private float mAvatarObjectContactFriction = 75f;
299 private float mAvatarObjectContactBounce = 0.1f;
300
301 private float avPIDD = 3200f;
302 private float avPIDP = 1400f;
303 private float avCapRadius = 0.37f;
304 private float avStandupTensor = 2000000f;
305
306 /// <summary>
307 /// true = old compatibility mode with leaning capsule; false = new corrected mode
308 /// </summary>
309 /// <remarks>
310 /// Even when set to false, the capsule still tilts but this is done in a different way.
311 /// </remarks>
312 public bool IsAvCapsuleTilted { get; private set; }
214 313
215 private float avDensity = 80f; 314 private float avDensity = 80f;
216 private float avMovementDivisorWalk = 1.3f; 315 private float avMovementDivisorWalk = 1.3f;
217 private float avMovementDivisorRun = 0.8f; 316 private float avMovementDivisorRun = 0.8f;
218 private float minimumGroundFlightOffset = 3f; 317 private float minimumGroundFlightOffset = 3f;
219 public float maximumMassObject = 10000.01f; 318 public float maximumMassObject = 10000.01f;
220 public float geomDefaultDensity = 10.0f;
221 319
222 public float maximumAngularVelocity = 12.0f; // default 12rad/s 320 public bool meshSculptedPrim = true;
223 public float maxAngVelocitySQ = 144f; // squared value 321 public bool forceSimplePrimMeshing = false;
322
323 public float meshSculptLOD = 32;
324 public float MeshSculptphysicalLOD = 16;
325
326 public float geomDefaultDensity = 10.000006836f;
327
328 public int geomContactPointsStartthrottle = 3;
329 public int geomUpdatesPerThrottledUpdate = 15;
330 private const int avatarExpectedContacts = 3;
224 331
225 public float bodyPIDD = 35f; 332 public float bodyPIDD = 35f;
226 public float bodyPIDG = 25; 333 public float bodyPIDG = 25;
227 334
228 public int bodyFramesAutoDisable = 5; 335 public int bodyFramesAutoDisable = 20;
336
337 private bool m_filterCollisions = true;
229 338
230 private d.NearCallback nearCallback; 339 private d.NearCallback nearCallback;
340 public d.TriCallback triCallback;
341 public d.TriArrayCallback triArrayCallback;
231 342
232 private Dictionary<uint,OdePrim> _prims = new Dictionary<uint,OdePrim>(); 343 /// <summary>
233 private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>(); 344 /// Avatars in the physics scene.
234 private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>(); 345 /// </summary>
235 private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>(); 346 private readonly HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
236 347
237 public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>(); 348 /// <summary>
349 /// Prims in the physics scene.
350 /// </summary>
351 private readonly HashSet<OdePrim> _prims = new HashSet<OdePrim>();
238 352
239 /// <summary> 353 /// <summary>
240 /// A list of actors that should receive collision events. 354 /// Prims in the physics scene that are subject to physics, not just collisions.
241 /// </summary> 355 /// </summary>
242 private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>(); 356 private readonly HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
243 private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>(); 357
244 358 /// <summary>
245 private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>(); 359 /// Prims that the simulator has created/deleted/updated and so need updating in ODE.
360 /// </summary>
361 private readonly HashSet<OdePrim> _taintedPrims = new HashSet<OdePrim>();
362
363 /// <summary>
364 /// Record a character that has taints to be processed.
365 /// </summary>
366 private readonly HashSet<OdeCharacter> _taintedActors = new HashSet<OdeCharacter>();
367
368 /// <summary>
369 /// Keep record of contacts in the physics loop so that we can remove duplicates.
370 /// </summary>
371 private readonly List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>();
372
373 /// <summary>
374 /// A dictionary of actors that should receive collision events.
375 /// </summary>
376 private readonly Dictionary<uint, PhysicsActor> m_collisionEventActors = new Dictionary<uint, PhysicsActor>();
377
378 /// <summary>
379 /// A dictionary of collision event changes that are waiting to be processed.
380 /// </summary>
381 private readonly Dictionary<uint, PhysicsActor> m_collisionEventActorsChanges = new Dictionary<uint, PhysicsActor>();
382
383 /// <summary>
384 /// Maps a unique geometry id (a memory location) to a physics actor name.
385 /// </summary>
386 /// <remarks>
387 /// Only actors participating in collisions have geometries. This has to be maintained separately from
388 /// actor_name_map because terrain and water currently don't conceptually have a physics actor of their own
389 /// apart from the singleton PANull
390 /// </remarks>
391 public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
392
393 /// <summary>
394 /// Maps a unique geometry id (a memory location) to a physics actor.
395 /// </summary>
396 /// <remarks>
397 /// Only actors participating in collisions have geometries.
398 /// </remarks>
246 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>(); 399 public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
247 400
248 private float contactsurfacelayer = 0.002f; 401 /// <summary>
402 /// Defects list to remove characters that no longer have finite positions due to some other bug.
403 /// </summary>
404 /// <remarks>
405 /// Used repeatedly in Simulate() but initialized once here.
406 /// </remarks>
407 private readonly List<OdeCharacter> defects = new List<OdeCharacter>();
249 408
250 private int contactsPerCollision = 80; 409 private bool m_NINJA_physics_joints_enabled = false;
251 internal IntPtr ContactgeomsArray = IntPtr.Zero; 410 //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>();
252 private IntPtr GlobalContactsArray = IntPtr.Zero; 411 private readonly Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>();
253 private d.Contact SharedTmpcontact = new d.Contact(); 412 private d.ContactGeom[] contacts;
254 413
255 const int maxContactsbeforedeath = 6000; 414 /// <summary>
256 private volatile int m_global_contactcount = 0; 415 /// Lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
416 /// </summary>
417 private readonly List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>();
257 418
258 private IntPtr contactgroup; 419 /// <summary>
420 /// can lock for longer. accessed only by OdeScene.
421 /// </summary>
422 private readonly List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>();
259 423
260 public ContactData[] m_materialContactsData = new ContactData[8]; 424 /// <summary>
425 /// can lock for longer. accessed only by OdeScene.
426 /// </summary>
427 private readonly List<PhysicsJoint> activeJoints = new List<PhysicsJoint>();
261 428
262 private IntPtr TerrainGeom; 429 /// <summary>
263 private float[] TerrainHeightFieldHeight; 430 /// lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active
264 private GCHandle TerrainHeightFieldHeightsHandler = new GCHandle(); 431 /// </summary>
265 432 private readonly List<string> requestedJointsToBeDeleted = new List<string>();
266 private int m_physicsiterations = 15; 433
434 private Object externalJointRequestsLock = new Object();
435 private readonly Dictionary<String, PhysicsJoint> SOPName_to_activeJoint = new Dictionary<String, PhysicsJoint>();
436 private readonly Dictionary<String, PhysicsJoint> SOPName_to_pendingJoint = new Dictionary<String, PhysicsJoint>();
437 private readonly DoubleDictionary<Vector3, IntPtr, IntPtr> RegionTerrain = new DoubleDictionary<Vector3, IntPtr, IntPtr>();
438 private readonly Dictionary<IntPtr,float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>();
439
440 private d.Contact contact;
441 private d.Contact TerrainContact;
442 private d.Contact AvatarMovementprimContact;
443 private d.Contact AvatarMovementTerrainContact;
444 private d.Contact WaterContact;
445 private d.Contact[,] m_materialContacts;
446
447 private int m_physicsiterations = 10;
267 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag 448 private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
268// private PhysicsActor PANull = new NullPhysicsActor(); 449 private readonly PhysicsActor PANull = new NullPhysicsActor();
269 private float step_time = 0.0f; 450 private float step_time = 0.0f;
270
271 public IntPtr world; 451 public IntPtr world;
452 private uint obj2LocalID = 0;
453 private OdeCharacter cc1;
454 private OdePrim cp1;
455 private OdeCharacter cc2;
456 private OdePrim cp2;
457 private int p1ExpectedPoints = 0;
458 private int p2ExpectedPoints = 0;
272 459
273 // split the spaces acording to contents type 460 public IntPtr space;
274 // ActiveSpace contains characters and active prims
275 // StaticSpace contains land and other that is mostly static in enviroment
276 // this can contain subspaces, like the grid in staticspace
277 // as now space only contains this 2 top spaces
278
279 public IntPtr TopSpace; // the global space
280 public IntPtr ActiveSpace; // space for active prims
281 public IntPtr CharsSpace; // space for active prims
282 public IntPtr StaticSpace; // space for the static things around
283 public IntPtr GroundSpace; // space for ground
284 461
285 // some speedup variables 462 // split static geometry collision handling into spaces of 30 meters
286 private int spaceGridMaxX; 463 public IntPtr[,] staticPrimspace;
287 private int spaceGridMaxY;
288 private float spacesPerMeterX;
289 private float spacesPerMeterY;
290 464
291 // split static geometry collision into a grid as before 465 /// <summary>
292 private IntPtr[,] staticPrimspace; 466 /// Used to lock the entire physics scene. Locked during the main part of Simulate()
293 private IntPtr[] staticPrimspaceOffRegion; 467 /// </summary>
468 internal Object OdeLock = new Object();
294 469
295 public Object OdeLock; 470 private bool _worldInitialized = false;
296 public static Object SimulationLock;
297 471
298 public IMesher mesher; 472 public IMesher mesher;
299 473
300 public IConfigSource m_config; 474 private IConfigSource m_config;
301 475
302 public bool physics_logging = false; 476 public bool physics_logging = false;
303 public int physics_logging_interval = 0; 477 public int physics_logging_interval = 0;
304 public bool physics_logging_append_existing_logfile = false; 478 public bool physics_logging_append_existing_logfile = false;
305 479
306 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); 480 private bool avplanted = false;
481 private bool av_av_collisions_off = false;
307 482
308 private ODERayCastRequestManager m_rayCastManager; 483 public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f);
309 public ODEMeshWorker m_meshWorker; 484 public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
310 485
311 /* maybe needed if ode uses tls 486 private volatile int m_global_contactcount = 0;
312 private void checkThread()
313 {
314 487
315 int th = Thread.CurrentThread.ManagedThreadId; 488 private Vector3 m_worldOffset = Vector3.Zero;
316 if(th != threadid) 489 public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
317 { 490 private PhysicsScene m_parentScene = null;
318 threadid = th;
319 d.AllocateODEDataForThread(~0U);
320 }
321 }
322 */
323 491
324 IConfig physicsconfig = null; 492 float spacesPerMeterX;
493 float spacesPerMeterY;
494 int spaceGridMaxX;
495 int spaceGridMaxY;
325 496
326 public ODEScene(Scene pscene, IConfigSource psourceconfig, string pname, string pversion, bool pOSOdeLib) 497 private ODERayCastRequestManager m_rayCastManager;
327 { 498
328 OdeLock = new Object(); 499 public Scene m_frameWorkScene = null;
329 500
501 public OdeScene(Scene pscene, IConfigSource psourceconfig, string pname, string pversion)
502 {
503 m_config = psourceconfig;
504 m_frameWorkScene = pscene;
505
330 EngineType = pname; 506 EngineType = pname;
331 PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName; 507 PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName;
332 EngineName = pname + " " + pversion; 508 EngineName = pname + " " + pversion;
333 m_config = psourceconfig;
334 m_OSOdeLib = pOSOdeLib;
335
336// m_OSOdeLib = false; //debug
337
338 m_frameWorkScene = pscene;
339 509
340 m_frameWorkScene.RegisterModuleInterface<PhysicsScene>(this); 510 pscene.RegisterModuleInterface<PhysicsScene>(this);
511 Vector3 extent = new Vector3(pscene.RegionInfo.RegionSizeX, pscene.RegionInfo.RegionSizeY, pscene.RegionInfo.RegionSizeZ);
512 Initialise(extent);
513 InitialiseFromConfig(m_config);
341 514
342 Initialization(); 515 // This may not be that good since terrain may not be avaiable at this point
516 base.Initialise(pscene.PhysicsRequestAsset,
517 (pscene.Heightmap != null ? pscene.Heightmap.GetFloatsSerialised() : new float[(int)(extent.X * extent.Y)]),
518 (float)pscene.RegionInfo.RegionSettings.WaterHeight);
343 519
344 base.Initialise(m_frameWorkScene.PhysicsRequestAsset,
345 (m_frameWorkScene.Heightmap != null ? m_frameWorkScene.Heightmap.GetFloatsSerialised() : new float[m_frameWorkScene.RegionInfo.RegionSizeX * m_frameWorkScene.RegionInfo.RegionSizeY]),
346 (float)m_frameWorkScene.RegionInfo.RegionSettings.WaterHeight);
347 } 520 }
348 521
349 public void RegionLoaded() 522 public void RegionLoaded()
350 { 523 {
351 mesher = m_frameWorkScene.RequestModuleInterface<IMesher>(); 524 mesher = m_frameWorkScene.RequestModuleInterface<IMesher>();
352 if (mesher == null) 525 if (mesher == null)
353 { 526 m_log.WarnFormat("[ODE SCENE]: No mesher in {0}. Things will not work well.", PhysicsSceneName);
354 m_log.ErrorFormat("[ubOde] No mesher. module disabled"); 527
355 return; 528 m_frameWorkScene.PhysicsEnabled = true;
356 }
357
358 m_meshWorker = new ODEMeshWorker(this, m_log, mesher, physicsconfig);
359 m_frameWorkScene.PhysicsEnabled = true;
360 } 529 }
530
361 /// <summary> 531 /// <summary>
362 /// Initiailizes the scene 532 /// Initiailizes the scene
363 /// Sets many properties that ODE requires to be stable 533 /// Sets many properties that ODE requires to be stable
364 /// These settings need to be tweaked 'exactly' right or weird stuff happens. 534 /// These settings need to be tweaked 'exactly' right or weird stuff happens.
365 /// </summary> 535 /// </summary>
366 private void Initialization() 536 private void Initialise(Vector3 regionExtent)
367 { 537 {
368 d.AllocateODEDataForThread(~0U); 538 WorldExtents.X = regionExtent.X;
369 539 m_regionWidth = (uint)regionExtent.X;
370 SimulationLock = new Object(); 540 WorldExtents.Y = regionExtent.Y;
371 541 m_regionHeight = (uint)regionExtent.Y;
542
372 nearCallback = near; 543 nearCallback = near;
373
374 m_rayCastManager = new ODERayCastRequestManager(this); 544 m_rayCastManager = new ODERayCastRequestManager(this);
375 545
376 WorldExtents.X = m_frameWorkScene.RegionInfo.RegionSizeX; 546 // Create the world and the first space
377 m_regionWidth = (uint)WorldExtents.X; 547 world = d.WorldCreate();
378 WorldExtents.Y = m_frameWorkScene.RegionInfo.RegionSizeY; 548 space = d.HashSpaceCreate(IntPtr.Zero);
379 m_regionHeight = (uint)WorldExtents.Y;
380
381 lock (OdeLock)
382 {
383 // Create the world and the first space
384 try
385 {
386 world = d.WorldCreate();
387 TopSpace = d.HashSpaceCreate(IntPtr.Zero);
388
389 // now the major subspaces
390 ActiveSpace = d.HashSpaceCreate(TopSpace);
391 CharsSpace = d.HashSpaceCreate(TopSpace);
392 StaticSpace = d.HashSpaceCreate(TopSpace);
393 GroundSpace = d.HashSpaceCreate(TopSpace);
394 }
395 catch
396 {
397 // i must RtC#FM
398 // i did!
399 }
400
401 d.HashSpaceSetLevels(TopSpace, -5, 12);
402 d.HashSpaceSetLevels(ActiveSpace, -5, 10);
403 d.HashSpaceSetLevels(CharsSpace, -4, 3);
404 d.HashSpaceSetLevels(StaticSpace, -5, 12);
405 d.HashSpaceSetLevels(GroundSpace, 0, 8);
406
407 // demote to second level
408 d.SpaceSetSublevel(ActiveSpace, 1);
409 d.SpaceSetSublevel(CharsSpace, 1);
410 d.SpaceSetSublevel(StaticSpace, 1);
411 d.SpaceSetSublevel(GroundSpace, 1);
412
413 d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
414 CollisionCategories.Geom |
415 CollisionCategories.Character |
416 CollisionCategories.Phantom |
417 CollisionCategories.VolumeDtc
418 ));
419 d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space |
420 CollisionCategories.Geom |
421 CollisionCategories.Character |
422 CollisionCategories.Phantom |
423 CollisionCategories.VolumeDtc
424 ));
425 d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space |
426 CollisionCategories.Geom |
427 CollisionCategories.Character |
428 CollisionCategories.Phantom |
429 CollisionCategories.VolumeDtc
430 ));
431 d.GeomSetCollideBits(CharsSpace, 0);
432
433 d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
434 CollisionCategories.Geom |
435 // CollisionCategories.Land |
436 // CollisionCategories.Water |
437 CollisionCategories.Phantom |
438 CollisionCategories.VolumeDtc
439 ));
440 d.GeomSetCollideBits(StaticSpace, 0);
441
442 d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land));
443 d.GeomSetCollideBits(GroundSpace, 0);
444 549
445 contactgroup = d.JointGroupCreate(maxContactsbeforedeath + 1); 550 contactgroup = d.JointGroupCreate(0);
446 //contactgroup
447 551
448 d.WorldSetAutoDisableFlag(world, false); 552 d.WorldSetAutoDisableFlag(world, false);
449 } 553 }
450
451
452 // checkThread();
453 554
555 // Initialize from configs
556 private void InitialiseFromConfig(IConfigSource config)
557 {
558 InitializeExtraStats();
454 559
560 m_config = config;
455 // Defaults 561 // Defaults
456 562
457 int contactsPerCollision = 80; 563 avPIDD = 2200.0f;
564 avPIDP = 900.0f;
565 avStandupTensor = 550000f;
458 566
459 physicsconfig = null; 567 int contactsPerCollision = 80;
460 568
461 if (m_config != null) 569 if (m_config != null)
462 { 570 {
463 physicsconfig = m_config.Configs["ODEPhysicsSettings"]; 571 IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
464 if (physicsconfig != null) 572 if (physicsconfig != null)
465 { 573 {
466 gravityx = physicsconfig.GetFloat("world_gravityx", gravityx); 574 CollectStats = physicsconfig.GetBoolean("collect_stats", false);
467 gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
468 gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
469
470 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace);
471
472 // contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
473
474 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
475 575
476 avDensity = physicsconfig.GetFloat("av_density", avDensity); 576 gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
477 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); 577 gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
478 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun); 578 gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
479 579
480 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); 580 float avatarTerminalVelocity = physicsconfig.GetFloat("avatar_terminal_velocity", 54f);
481 581 AvatarTerminalVelocity = Util.Clamp<float>(avatarTerminalVelocity, 0, 255f);
482 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); 582 if (AvatarTerminalVelocity != avatarTerminalVelocity)
483 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); 583 {
484 584 m_log.WarnFormat(
485 physics_logging = physicsconfig.GetBoolean("physics_logging", false); 585 "[ODE SCENE]: avatar_terminal_velocity of {0} is invalid. Clamping to {1}",
486 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); 586 avatarTerminalVelocity, AvatarTerminalVelocity);
487 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); 587 }
488
489 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
490 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
491
492 avDensity *= 3f / 80f; // scale other engines density option to this
493 }
494 }
495
496 float heartbeat = 1/m_frameWorkScene.FrameTime;
497 maximumAngularVelocity = 0.49f * heartbeat *(float)Math.PI;
498 maxAngVelocitySQ = maximumAngularVelocity * maximumAngularVelocity;
499 588
500 d.WorldSetCFM(world, comumContactCFM); 589 HashspaceLow = physicsconfig.GetInt("world_hashspace_level_low", -5);
501 d.WorldSetERP(world, comumContactERP); 590 HashspaceHigh = physicsconfig.GetInt("world_hashspace_level_high", 12);
502 591
503 d.WorldSetGravity(world, gravityx, gravityy, gravityz); 592 metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f);
504 593
505 d.WorldSetLinearDamping(world, 0.002f); 594 contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f);
506 d.WorldSetAngularDamping(world, 0.002f);
507 d.WorldSetAngularDampingThreshold(world, 0f);
508 d.WorldSetLinearDampingThreshold(world, 0f);
509 d.WorldSetMaxAngularSpeed(world, maximumAngularVelocity);
510 595
511 d.WorldSetQuickStepNumIterations(world, m_physicsiterations); 596 nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f);
597 nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f);
598 nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f);
512 599
513 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); 600 mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f);
514 d.WorldSetContactMaxCorrectingVel(world, 60.0f); 601 mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.05f);
602 mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f);
515 603
516 HalfOdeStep = ODE_STEPSIZE * 0.5f; 604 nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f);
517 odetimestepMS = (int)(1000.0f * ODE_STEPSIZE + 0.5f); 605 nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f);
518 606
519 ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); 607 mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f);
520 GlobalContactsArray = Marshal.AllocHGlobal((maxContactsbeforedeath + 100) * d.Contact.unmanagedSizeOf); 608 mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f);
521 609
522 SharedTmpcontact.geom.g1 = IntPtr.Zero; 610 ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
523 SharedTmpcontact.geom.g2 = IntPtr.Zero; 611 m_physicsiterations = physicsconfig.GetInt("world_solver_iterations", 10);
524 612
525 SharedTmpcontact.geom.side1 = -1; 613 avDensity = physicsconfig.GetFloat("av_density", 80f);
526 SharedTmpcontact.geom.side2 = -1; 614// avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f);
615 avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f);
616 avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f);
617 avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
618 avplanted = physicsconfig.GetBoolean("av_planted", false);
619 av_av_collisions_off = physicsconfig.GetBoolean("av_av_collisions_off", false);
527 620
528 SharedTmpcontact.surface.mode = comumContactFlags; 621 IsAvCapsuleTilted = physicsconfig.GetBoolean("av_capsule_tilted", false);
529 SharedTmpcontact.surface.mu = 0;
530 SharedTmpcontact.surface.bounce = 0;
531 SharedTmpcontact.surface.soft_cfm = comumContactCFM;
532 SharedTmpcontact.surface.soft_erp = comumContactERP;
533 SharedTmpcontact.surface.slip1 = comumContactSLIP;
534 SharedTmpcontact.surface.slip2 = comumContactSLIP;
535 622
536 m_materialContactsData[(int)Material.Stone].mu = 0.8f; 623 contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", 80);
537 m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
538 624
539 m_materialContactsData[(int)Material.Metal].mu = 0.3f; 625 geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 5);
540 m_materialContactsData[(int)Material.Metal].bounce = 0.4f; 626 geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
541 627
542 m_materialContactsData[(int)Material.Glass].mu = 0.2f; 628 geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
543 m_materialContactsData[(int)Material.Glass].bounce = 0.7f; 629 bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
544 630
545 m_materialContactsData[(int)Material.Wood].mu = 0.6f; 631 bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
546 m_materialContactsData[(int)Material.Wood].bounce = 0.5f; 632 bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
547 633
548 m_materialContactsData[(int)Material.Flesh].mu = 0.9f; 634 forceSimplePrimMeshing = physicsconfig.GetBoolean("force_simple_prim_meshing", forceSimplePrimMeshing);
549 m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; 635 meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
636 meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
637 MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
638 m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", false);
550 639
551 m_materialContactsData[(int)Material.Plastic].mu = 0.4f; 640 avPIDD = physicsconfig.GetFloat("av_pid_derivative", 2200.0f);
552 m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; 641 avPIDP = physicsconfig.GetFloat("av_pid_proportional", 900.0f);
642 avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor", 550000f);
553 643
554 m_materialContactsData[(int)Material.Rubber].mu = 0.9f; 644 physics_logging = physicsconfig.GetBoolean("physics_logging", false);
555 m_materialContactsData[(int)Material.Rubber].bounce = 0.95f; 645 physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
646 physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
556 647
557 m_materialContactsData[(int)Material.light].mu = 0.0f; 648// m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
558 m_materialContactsData[(int)Material.light].bounce = 0.0f; 649 minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", 3f);
650 maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", 10000.01f);
651 }
652 }
559 653
654 contacts = new d.ContactGeom[contactsPerCollision];
560 655
561 spacesPerMeterX = 1.0f / metersInSpace; 656 spacesPerMeterX = 1.0f / metersInSpace;
562 spacesPerMeterY = spacesPerMeterX; 657 spacesPerMeterY = 1.0f / metersInSpace;
658
563 spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX); 659 spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX);
564 spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY); 660 spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY);
565 661
662 // note: limit number of spaces
566 if (spaceGridMaxX > 24) 663 if (spaceGridMaxX > 24)
567 { 664 {
568 spaceGridMaxX = 24; 665 spaceGridMaxX = 24;
569 spacesPerMeterX = spaceGridMaxX / WorldExtents.X; 666 spacesPerMeterX = spaceGridMaxX / WorldExtents.X;
570 } 667 }
571
572 if (spaceGridMaxY > 24) 668 if (spaceGridMaxY > 24)
573 { 669 {
574 spaceGridMaxY = 24; 670 spaceGridMaxY = 24;
575 spacesPerMeterY = spaceGridMaxY / WorldExtents.Y; 671 spacesPerMeterY = spaceGridMaxY / WorldExtents.Y;
576 } 672 }
577 673
578 staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; 674 staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY];
579 675
580 // create all spaces now 676 // make this index limits
581 int i, j; 677 spaceGridMaxX--;
582 IntPtr newspace; 678 spaceGridMaxY--;
583 679
584 for (i = 0; i < spaceGridMaxX; i++) 680 // Centeral contact friction and bounce
585 for (j = 0; j < spaceGridMaxY; j++) 681 // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why
586 { 682 // an avatar falls through in Z but not in X or Y when walking on a prim.
587 newspace = d.HashSpaceCreate(StaticSpace); 683 contact.surface.mode |= d.ContactFlags.SoftERP;
588 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); 684 contact.surface.mu = nmAvatarObjectContactFriction;
589 waitForSpaceUnlock(newspace); 685 contact.surface.bounce = nmAvatarObjectContactBounce;
590 d.SpaceSetSublevel(newspace, 2); 686 contact.surface.soft_cfm = 0.010f;
591 d.HashSpaceSetLevels(newspace, -2, 8); 687 contact.surface.soft_erp = 0.010f;
592 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | 688
593 CollisionCategories.Geom | 689 // Terrain contact friction and Bounce
594 CollisionCategories.Land | 690 // This is the *non* moving version. Use this when an avatar
595 CollisionCategories.Water | 691 // isn't moving to keep it in place better
596 CollisionCategories.Phantom | 692 TerrainContact.surface.mode |= d.ContactFlags.SoftERP;
597 CollisionCategories.VolumeDtc 693 TerrainContact.surface.mu = nmTerrainContactFriction;
598 )); 694 TerrainContact.surface.bounce = nmTerrainContactBounce;
599 d.GeomSetCollideBits(newspace, 0); 695 TerrainContact.surface.soft_erp = nmTerrainContactERP;
696
697 WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM);
698 WaterContact.surface.mu = 0f; // No friction
699 WaterContact.surface.bounce = 0.0f; // No bounce
700 WaterContact.surface.soft_cfm = 0.010f;
701 WaterContact.surface.soft_erp = 0.010f;
702
703 // Prim contact friction and bounce
704 // THis is the *non* moving version of friction and bounce
705 // Use this when an avatar comes in contact with a prim
706 // and is moving
707 AvatarMovementprimContact.surface.mu = mAvatarObjectContactFriction;
708 AvatarMovementprimContact.surface.bounce = mAvatarObjectContactBounce;
709
710 // Terrain contact friction bounce and various error correcting calculations
711 // Use this when an avatar is in contact with the terrain and moving.
712 AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP;
713 AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction;
714 AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce;
715 AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP;
600 716
601 staticPrimspace[i, j] = newspace; 717 /*
602 } 718 <summary></summary>
719 Stone = 0,
720 /// <summary></summary>
721 Metal = 1,
722 /// <summary></summary>
723 Glass = 2,
724 /// <summary></summary>
725 Wood = 3,
726 /// <summary></summary>
727 Flesh = 4,
728 /// <summary></summary>
729 Plastic = 5,
730 /// <summary></summary>
731 Rubber = 6
732 */
733
734 m_materialContacts = new d.Contact[7,2];
735
736 m_materialContacts[(int)Material.Stone, 0] = new d.Contact();
737 m_materialContacts[(int)Material.Stone, 0].surface.mode |= d.ContactFlags.SoftERP;
738 m_materialContacts[(int)Material.Stone, 0].surface.mu = nmAvatarObjectContactFriction;
739 m_materialContacts[(int)Material.Stone, 0].surface.bounce = nmAvatarObjectContactBounce;
740 m_materialContacts[(int)Material.Stone, 0].surface.soft_cfm = 0.010f;
741 m_materialContacts[(int)Material.Stone, 0].surface.soft_erp = 0.010f;
742
743 m_materialContacts[(int)Material.Stone, 1] = new d.Contact();
744 m_materialContacts[(int)Material.Stone, 1].surface.mode |= d.ContactFlags.SoftERP;
745 m_materialContacts[(int)Material.Stone, 1].surface.mu = mAvatarObjectContactFriction;
746 m_materialContacts[(int)Material.Stone, 1].surface.bounce = mAvatarObjectContactBounce;
747 m_materialContacts[(int)Material.Stone, 1].surface.soft_cfm = 0.010f;
748 m_materialContacts[(int)Material.Stone, 1].surface.soft_erp = 0.010f;
749
750 m_materialContacts[(int)Material.Metal, 0] = new d.Contact();
751 m_materialContacts[(int)Material.Metal, 0].surface.mode |= d.ContactFlags.SoftERP;
752 m_materialContacts[(int)Material.Metal, 0].surface.mu = nmAvatarObjectContactFriction;
753 m_materialContacts[(int)Material.Metal, 0].surface.bounce = nmAvatarObjectContactBounce;
754 m_materialContacts[(int)Material.Metal, 0].surface.soft_cfm = 0.010f;
755 m_materialContacts[(int)Material.Metal, 0].surface.soft_erp = 0.010f;
756
757 m_materialContacts[(int)Material.Metal, 1] = new d.Contact();
758 m_materialContacts[(int)Material.Metal, 1].surface.mode |= d.ContactFlags.SoftERP;
759 m_materialContacts[(int)Material.Metal, 1].surface.mu = mAvatarObjectContactFriction;
760 m_materialContacts[(int)Material.Metal, 1].surface.bounce = mAvatarObjectContactBounce;
761 m_materialContacts[(int)Material.Metal, 1].surface.soft_cfm = 0.010f;
762 m_materialContacts[(int)Material.Metal, 1].surface.soft_erp = 0.010f;
763
764 m_materialContacts[(int)Material.Glass, 0] = new d.Contact();
765 m_materialContacts[(int)Material.Glass, 0].surface.mode |= d.ContactFlags.SoftERP;
766 m_materialContacts[(int)Material.Glass, 0].surface.mu = 1f;
767 m_materialContacts[(int)Material.Glass, 0].surface.bounce = 0.5f;
768 m_materialContacts[(int)Material.Glass, 0].surface.soft_cfm = 0.010f;
769 m_materialContacts[(int)Material.Glass, 0].surface.soft_erp = 0.010f;
603 770
604 // let this now be index limit 771 /*
605 spaceGridMaxX--; 772 private float nmAvatarObjectContactFriction = 250f;
606 spaceGridMaxY--; 773 private float nmAvatarObjectContactBounce = 0.1f;
607 774
608 // create 4 off world spaces (x<0,x>max,y<0,y>max) 775 private float mAvatarObjectContactFriction = 75f;
609 staticPrimspaceOffRegion = new IntPtr[4]; 776 private float mAvatarObjectContactBounce = 0.1f;
777 */
778 m_materialContacts[(int)Material.Glass, 1] = new d.Contact();
779 m_materialContacts[(int)Material.Glass, 1].surface.mode |= d.ContactFlags.SoftERP;
780 m_materialContacts[(int)Material.Glass, 1].surface.mu = 1f;
781 m_materialContacts[(int)Material.Glass, 1].surface.bounce = 0.5f;
782 m_materialContacts[(int)Material.Glass, 1].surface.soft_cfm = 0.010f;
783 m_materialContacts[(int)Material.Glass, 1].surface.soft_erp = 0.010f;
784
785 m_materialContacts[(int)Material.Wood, 0] = new d.Contact();
786 m_materialContacts[(int)Material.Wood, 0].surface.mode |= d.ContactFlags.SoftERP;
787 m_materialContacts[(int)Material.Wood, 0].surface.mu = nmAvatarObjectContactFriction;
788 m_materialContacts[(int)Material.Wood, 0].surface.bounce = nmAvatarObjectContactBounce;
789 m_materialContacts[(int)Material.Wood, 0].surface.soft_cfm = 0.010f;
790 m_materialContacts[(int)Material.Wood, 0].surface.soft_erp = 0.010f;
791
792 m_materialContacts[(int)Material.Wood, 1] = new d.Contact();
793 m_materialContacts[(int)Material.Wood, 1].surface.mode |= d.ContactFlags.SoftERP;
794 m_materialContacts[(int)Material.Wood, 1].surface.mu = mAvatarObjectContactFriction;
795 m_materialContacts[(int)Material.Wood, 1].surface.bounce = mAvatarObjectContactBounce;
796 m_materialContacts[(int)Material.Wood, 1].surface.soft_cfm = 0.010f;
797 m_materialContacts[(int)Material.Wood, 1].surface.soft_erp = 0.010f;
798
799 m_materialContacts[(int)Material.Flesh, 0] = new d.Contact();
800 m_materialContacts[(int)Material.Flesh, 0].surface.mode |= d.ContactFlags.SoftERP;
801 m_materialContacts[(int)Material.Flesh, 0].surface.mu = nmAvatarObjectContactFriction;
802 m_materialContacts[(int)Material.Flesh, 0].surface.bounce = nmAvatarObjectContactBounce;
803 m_materialContacts[(int)Material.Flesh, 0].surface.soft_cfm = 0.010f;
804 m_materialContacts[(int)Material.Flesh, 0].surface.soft_erp = 0.010f;
805
806 m_materialContacts[(int)Material.Flesh, 1] = new d.Contact();
807 m_materialContacts[(int)Material.Flesh, 1].surface.mode |= d.ContactFlags.SoftERP;
808 m_materialContacts[(int)Material.Flesh, 1].surface.mu = mAvatarObjectContactFriction;
809 m_materialContacts[(int)Material.Flesh, 1].surface.bounce = mAvatarObjectContactBounce;
810 m_materialContacts[(int)Material.Flesh, 1].surface.soft_cfm = 0.010f;
811 m_materialContacts[(int)Material.Flesh, 1].surface.soft_erp = 0.010f;
812
813 m_materialContacts[(int)Material.Plastic, 0] = new d.Contact();
814 m_materialContacts[(int)Material.Plastic, 0].surface.mode |= d.ContactFlags.SoftERP;
815 m_materialContacts[(int)Material.Plastic, 0].surface.mu = nmAvatarObjectContactFriction;
816 m_materialContacts[(int)Material.Plastic, 0].surface.bounce = nmAvatarObjectContactBounce;
817 m_materialContacts[(int)Material.Plastic, 0].surface.soft_cfm = 0.010f;
818 m_materialContacts[(int)Material.Plastic, 0].surface.soft_erp = 0.010f;
819
820 m_materialContacts[(int)Material.Plastic, 1] = new d.Contact();
821 m_materialContacts[(int)Material.Plastic, 1].surface.mode |= d.ContactFlags.SoftERP;
822 m_materialContacts[(int)Material.Plastic, 1].surface.mu = mAvatarObjectContactFriction;
823 m_materialContacts[(int)Material.Plastic, 1].surface.bounce = mAvatarObjectContactBounce;
824 m_materialContacts[(int)Material.Plastic, 1].surface.soft_cfm = 0.010f;
825 m_materialContacts[(int)Material.Plastic, 1].surface.soft_erp = 0.010f;
826
827 m_materialContacts[(int)Material.Rubber, 0] = new d.Contact();
828 m_materialContacts[(int)Material.Rubber, 0].surface.mode |= d.ContactFlags.SoftERP;
829 m_materialContacts[(int)Material.Rubber, 0].surface.mu = nmAvatarObjectContactFriction;
830 m_materialContacts[(int)Material.Rubber, 0].surface.bounce = nmAvatarObjectContactBounce;
831 m_materialContacts[(int)Material.Rubber, 0].surface.soft_cfm = 0.010f;
832 m_materialContacts[(int)Material.Rubber, 0].surface.soft_erp = 0.010f;
833
834 m_materialContacts[(int)Material.Rubber, 1] = new d.Contact();
835 m_materialContacts[(int)Material.Rubber, 1].surface.mode |= d.ContactFlags.SoftERP;
836 m_materialContacts[(int)Material.Rubber, 1].surface.mu = mAvatarObjectContactFriction;
837 m_materialContacts[(int)Material.Rubber, 1].surface.bounce = mAvatarObjectContactBounce;
838 m_materialContacts[(int)Material.Rubber, 1].surface.soft_cfm = 0.010f;
839 m_materialContacts[(int)Material.Rubber, 1].surface.soft_erp = 0.010f;
840
841 d.HashSpaceSetLevels(space, HashspaceLow, HashspaceHigh);
842
843 // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
610 844
611 for (i = 0; i < 4; i++) 845 d.WorldSetGravity(world, gravityx, gravityy, gravityz);
612 { 846 d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
613 newspace = d.HashSpaceCreate(StaticSpace);
614 d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space);
615 waitForSpaceUnlock(newspace);
616 d.SpaceSetSublevel(newspace, 2);
617 d.HashSpaceSetLevels(newspace, -2, 8);
618 d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space |
619 CollisionCategories.Geom |
620 CollisionCategories.Land |
621 CollisionCategories.Water |
622 CollisionCategories.Phantom |
623 CollisionCategories.VolumeDtc
624 ));
625 d.GeomSetCollideBits(newspace, 0);
626 847
627 staticPrimspaceOffRegion[i] = newspace; 848 d.WorldSetLinearDamping(world, 256f);
628 } 849 d.WorldSetAngularDamping(world, 256f);
850 d.WorldSetAngularDampingThreshold(world, 256f);
851 d.WorldSetLinearDampingThreshold(world, 256f);
852 d.WorldSetMaxAngularSpeed(world, 256f);
629 853
630 m_lastframe = DateTime.UtcNow; 854 d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
631 m_lastMeshExpire = m_lastframe; 855 //d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
632 }
633 856
634 internal void waitForSpaceUnlock(IntPtr space) 857 for (int i = 0; i < staticPrimspace.GetLength(0); i++)
635 { 858 {
636 //if (space != IntPtr.Zero) 859 for (int j = 0; j < staticPrimspace.GetLength(1); j++)
637 //while (d.SpaceLockQuery(space)) { } // Wait and do nothing 860 {
861 staticPrimspace[i, j] = IntPtr.Zero;
862 }
863 }
864
865 _worldInitialized = true;
638 } 866 }
639 867
640 #region Collision Detection 868 #region Collision Detection
641 869
642 // sets a global contact for a joint for contactgeom , and base contact description) 870 /// <summary>
643 private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom,bool smooth) 871 /// Collides two geometries.
872 /// </summary>
873 /// <returns></returns>
874 /// <param name='geom1'></param>
875 /// <param name='geom2'>/param>
876 /// <param name='maxContacts'></param>
877 /// <param name='contactsArray'></param>
878 /// <param name='contactGeomSize'></param>
879 private int CollideGeoms(
880 IntPtr geom1, IntPtr geom2, int maxContacts, d.ContactGeom[] contactsArray, int contactGeomSize)
644 { 881 {
645 if (m_global_contactcount >= maxContactsbeforedeath) 882 int count;
646 return IntPtr.Zero;
647 883
648 m_global_contactcount++; 884 lock (OdeScene.UniversalColliderSyncObject)
649 if(smooth) 885 {
650 SharedTmpcontact.geom.depth = contactGeom.depth * 0.05f; 886 // We do this inside the lock so that we don't count any delay in acquiring it
651 else 887 if (CollectStats)
652 SharedTmpcontact.geom.depth = contactGeom.depth; 888 m_nativeCollisionStartTick = Util.EnvironmentTickCount();
653 SharedTmpcontact.geom.pos = contactGeom.pos; 889
654 SharedTmpcontact.geom.normal = contactGeom.normal; 890 count = d.Collide(geom1, geom2, maxContacts, contactsArray, contactGeomSize);
891 }
655 892
656 IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); 893 // We do this outside the lock so that any waiting threads aren't held up, though the effect is probably
657 Marshal.StructureToPtr(SharedTmpcontact, contact, true); 894 // negligable
658 return d.JointCreateContactPtr(world, contactgroup, contact); 895 if (CollectStats)
896 m_stats[ODENativeGeomCollisionFrameMsStatName]
897 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
898
899 return count;
659 } 900 }
660 901
661 private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) 902 /// <summary>
903 /// Collide two spaces or a space and a geometry.
904 /// </summary>
905 /// <param name='space1'></param>
906 /// <param name='space2'>/param>
907 /// <param name='data'></param>
908 private void CollideSpaces(IntPtr space1, IntPtr space2, IntPtr data)
662 { 909 {
663 if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision) 910 if (CollectStats)
664 return false; 911 {
912 m_inCollisionTiming = true;
913 m_nativeCollisionStartTick = Util.EnvironmentTickCount();
914 }
665 915
666 IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); 916 d.SpaceCollide2(space1, space2, data, nearCallback);
667 newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); 917
668 return true; 918 if (CollectStats && m_inCollisionTiming)
919 {
920 m_stats[ODENativeSpaceCollisionFrameMsStatName]
921 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
922 m_inCollisionTiming = false;
923 }
669 } 924 }
670 925
671 /// <summary> 926 /// <summary>
@@ -674,50 +929,70 @@ namespace OpenSim.Region.PhysicsModule.ubOde
674 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param> 929 /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
675 /// <param name="g1">a geometry or space</param> 930 /// <param name="g1">a geometry or space</param>
676 /// <param name="g2">another geometry or space</param> 931 /// <param name="g2">another geometry or space</param>
677 ///
678
679 private void near(IntPtr space, IntPtr g1, IntPtr g2) 932 private void near(IntPtr space, IntPtr g1, IntPtr g2)
680 { 933 {
681 // no lock here! It's invoked from within Simulate(), which is thread-locked 934 if (CollectStats && m_inCollisionTiming)
935 {
936 m_stats[ODENativeSpaceCollisionFrameMsStatName]
937 += Util.EnvironmentTickCountSubtract(m_nativeCollisionStartTick);
938 m_inCollisionTiming = false;
939 }
682 940
683 if (m_global_contactcount >= maxContactsbeforedeath) 941// m_log.DebugFormat("[PHYSICS]: Colliding {0} and {1} in {2}", g1, g2, space);
684 return; 942 // no lock here! It's invoked from within Simulate(), which is thread-locked
685 943
686 // Test if we're colliding a geom with a space. 944 // Test if we're colliding a geom with a space.
687 // If so we have to drill down into the space recursively 945 // If so we have to drill down into the space recursively
688 946
689 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
690 return;
691
692 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) 947 if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
693 { 948 {
949 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
950 return;
951
952 // Separating static prim geometry spaces.
694 // We'll be calling near recursivly if one 953 // We'll be calling near recursivly if one
695 // of them is a space to find all of the 954 // of them is a space to find all of the
696 // contact points in the space 955 // contact points in the space
697 try 956 try
698 { 957 {
699 d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); 958 CollideSpaces(g1, g2, IntPtr.Zero);
700 } 959 }
701 catch (AccessViolationException) 960 catch (AccessViolationException)
702 { 961 {
703 m_log.Warn("[PHYSICS]: Unable to collide test a space"); 962 m_log.Error("[ODE SCENE]: Unable to collide test a space");
704 return; 963 return;
705 } 964 }
706 //here one should check collisions of geoms inside a space 965 //Colliding a space or a geom with a space or a geom. so drill down
707 // but on each space we only should have geoms that not colide amoung each other 966
708 // so we don't dig inside spaces 967 //Collide all geoms in each space..
968 //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
969 //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
709 return; 970 return;
710 } 971 }
711 972
712 // get geom bodies to check if we already a joint contact 973 if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
713 // guess this shouldn't happen now 974 return;
975
714 IntPtr b1 = d.GeomGetBody(g1); 976 IntPtr b1 = d.GeomGetBody(g1);
715 IntPtr b2 = d.GeomGetBody(g2); 977 IntPtr b2 = d.GeomGetBody(g2);
716 978
717 // d.GeomClassID id = d.GeomGetClass(g1); 979 // d.GeomClassID id = d.GeomGetClass(g1);
718 980
981 String name1 = null;
982 String name2 = null;
983
984 if (!geom_name_map.TryGetValue(g1, out name1))
985 {
986 name1 = "null";
987 }
988 if (!geom_name_map.TryGetValue(g2, out name2))
989 {
990 name2 = "null";
991 }
992
719 // Figure out how many contact points we have 993 // Figure out how many contact points we have
720 int count = 0; 994 int count = 0;
995
721 try 996 try
722 { 997 {
723 // Colliding Geom To Geom 998 // Colliding Geom To Geom
@@ -728,329 +1003,444 @@ namespace OpenSim.Region.PhysicsModule.ubOde
728 1003
729 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) 1004 if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
730 return; 1005 return;
731 /*
732 // debug
733 PhysicsActor dp2;
734 if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass)
735 {
736 d.AABB aabb;
737 d.GeomGetAABB(g2, out aabb);
738 float x = aabb.MaxX - aabb.MinX;
739 float y = aabb.MaxY - aabb.MinY;
740 float z = aabb.MaxZ - aabb.MinZ;
741 if (x > 60.0f || y > 60.0f || z > 60.0f)
742 {
743 if (!actor_name_map.TryGetValue(g2, out dp2))
744 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
745 else
746 m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})",
747 dp2.Name, dp2.Size, x, y, z,
748 dp2.Position.ToString(),
749 dp2.Orientation.ToString(),
750 dp2.Orientation.Length());
751 return;
752 }
753 }
754 //
755 */
756 1006
1007 count = CollideGeoms(g1, g2, contacts.Length, contacts, d.ContactGeom.unmanagedSizeOf);
757 1008
758 if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || 1009 // All code after this is only relevant if we have any collisions
759 d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc) 1010 if (count <= 0)
760 { 1011 return;
761 int cflags; 1012
762 unchecked 1013 if (count > contacts.Length)
763 { 1014 m_log.Error("[ODE SCENE]: Got " + count + " contacts when we asked for a maximum of " + contacts.Length);
764 cflags = (int)(1 | d.CONTACTS_UNIMPORTANT);
765 }
766 count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
767 }
768 else
769 count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf);
770 } 1015 }
771 catch (SEHException) 1016 catch (SEHException)
772 { 1017 {
773 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."); 1018 m_log.Error(
774 // ode.drelease(world); 1019 "[ODE SCENE]: The Operating system shut down ODE because of corrupt memory. This could be a result of really irregular terrain. If this repeats continuously, restart using Basic Physics and terrain fill your terrain. Restarting the sim.");
775 base.TriggerPhysicsBasedRestart(); 1020 base.TriggerPhysicsBasedRestart();
776 } 1021 }
777 catch (Exception e) 1022 catch (Exception e)
778 { 1023 {
779 m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); 1024 m_log.ErrorFormat("[ODE SCENE]: Unable to collide test an object: {0}", e.Message);
780 return; 1025 return;
781 } 1026 }
782 1027
783 // contacts done
784 if (count == 0)
785 return;
786
787 // try get physical actors
788 PhysicsActor p1; 1028 PhysicsActor p1;
789 PhysicsActor p2; 1029 PhysicsActor p2;
790 1030
1031 p1ExpectedPoints = 0;
1032 p2ExpectedPoints = 0;
1033
791 if (!actor_name_map.TryGetValue(g1, out p1)) 1034 if (!actor_name_map.TryGetValue(g1, out p1))
792 { 1035 {
793 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1"); 1036 p1 = PANull;
794 return;
795 } 1037 }
796 1038
797 if (!actor_name_map.TryGetValue(g2, out p2)) 1039 if (!actor_name_map.TryGetValue(g2, out p2))
798 { 1040 {
799 m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); 1041 p2 = PANull;
800 return;
801 } 1042 }
802 1043
803
804 // get first contact
805 d.ContactGeom curContact = new d.ContactGeom();
806
807 if (!GetCurContactGeom(0, ref curContact))
808 return;
809
810 ContactPoint maxDepthContact = new ContactPoint(); 1044 ContactPoint maxDepthContact = new ContactPoint();
1045 if (p1.CollisionScore + count >= float.MaxValue)
1046 p1.CollisionScore = 0;
1047 p1.CollisionScore += count;
1048
1049 if (p2.CollisionScore + count >= float.MaxValue)
1050 p2.CollisionScore = 0;
1051 p2.CollisionScore += count;
811 1052
812 // do volume detection case 1053 for (int i = 0; i < count; i++)
813 if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
814 { 1054 {
815 maxDepthContact = new ContactPoint( 1055 d.ContactGeom curContact = contacts[i];
816 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), 1056
817 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), 1057 if (curContact.depth > maxDepthContact.PenetrationDepth)
818 curContact.depth, false 1058 {
1059 maxDepthContact = new ContactPoint(
1060 new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
1061 new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
1062 curContact.depth
819 ); 1063 );
1064 }
820 1065
821 collision_accounting_events(p1, p2, maxDepthContact); 1066 //m_log.Warn("[CCOUNT]: " + count);
822 return; 1067 IntPtr joint;
823 } 1068 // If we're colliding with terrain, use 'TerrainContact' instead of contact.
1069 // allows us to have different settings
1070
1071 // We only need to test p2 for 'jump crouch purposes'
1072 if (p2 is OdeCharacter && p1.PhysicsActorType == (int)ActorTypes.Prim)
1073 {
1074 // Testing if the collision is at the feet of the avatar
1075
1076 //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));
1077 if ((p2.Position.Z - curContact.pos.Z) > (p2.Size.Z * 0.6f))
1078 p2.IsColliding = true;
1079 }
1080 else
1081 {
1082 p2.IsColliding = true;
1083 }
1084
1085 //if ((framecount % m_returncollisions) == 0)
824 1086
825 // big messy collision analises 1087 switch (p1.PhysicsActorType)
1088 {
1089 case (int)ActorTypes.Agent:
1090 p1ExpectedPoints = avatarExpectedContacts;
1091 p2.CollidingObj = true;
1092 break;
1093 case (int)ActorTypes.Prim:
1094 if (p1 != null && p1 is OdePrim)
1095 p1ExpectedPoints = ((OdePrim) p1).ExpectedCollisionContacts;
826 1096
827 float mu = 0; 1097 if (p2.Velocity.LengthSquared() > 0.0f)
828 float bounce = 0; 1098 p2.CollidingObj = true;
829// bool IgnoreNegSides = false; 1099 break;
1100 case (int)ActorTypes.Unknown:
1101 p2.CollidingGround = true;
1102 break;
1103 default:
1104 p2.CollidingGround = true;
1105 break;
1106 }
830 1107
831 ContactData contactdata1 = new ContactData(0, 0, false); 1108 // we don't want prim or avatar to explode
832 ContactData contactdata2 = new ContactData(0, 0, false);
833 1109
834 bool dop1ava = false; 1110 #region InterPenetration Handling - Unintended physics explosions
835 bool dop2ava = false;
836 bool ignore = false;
837 bool smoothMesh = false;
838 1111
839 switch (p1.PhysicsActorType) 1112 if (curContact.depth >= 0.08f)
840 { 1113 {
841 case (int)ActorTypes.Agent: 1114 if (curContact.depth >= 1.00f)
842 { 1115 {
843 dop1ava = true; 1116 //m_log.Info("[P]: " + contact.depth.ToString());
844 switch (p2.PhysicsActorType) 1117 if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
1118 p1.PhysicsActorType == (int) ActorTypes.Unknown) ||
1119 (p1.PhysicsActorType == (int) ActorTypes.Agent &&
1120 p2.PhysicsActorType == (int) ActorTypes.Unknown))
845 { 1121 {
846 case (int)ActorTypes.Agent: 1122 if (p2.PhysicsActorType == (int) ActorTypes.Agent)
847 case (int)ActorTypes.Prim: 1123 {
848 break; 1124 if (p2 is OdeCharacter)
1125 {
1126 OdeCharacter character = (OdeCharacter) p2;
1127
1128 //p2.CollidingObj = true;
1129 curContact.depth = 0.00000003f;
1130 p2.Velocity = p2.Velocity + new Vector3(0f, 0f, 0.5f);
1131 curContact.pos =
1132 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
1133 curContact.pos.Y + (p1.Size.Y/2),
1134 curContact.pos.Z + (p1.Size.Z/2));
1135 character.SetPidStatus(true);
1136 }
1137 }
849 1138
850 default: 1139 if (p1.PhysicsActorType == (int) ActorTypes.Agent)
851 ignore = true; // avatar to terrain and water ignored 1140 {
852 break; 1141 if (p1 is OdeCharacter)
1142 {
1143 OdeCharacter character = (OdeCharacter) p1;
1144
1145 //p2.CollidingObj = true;
1146 curContact.depth = 0.00000003f;
1147 p1.Velocity = p1.Velocity + new Vector3(0f, 0f, 0.5f);
1148 curContact.pos =
1149 new d.Vector3(curContact.pos.X + (p1.Size.X/2),
1150 curContact.pos.Y + (p1.Size.Y/2),
1151 curContact.pos.Z + (p1.Size.Z/2));
1152 character.SetPidStatus(true);
1153 }
1154 }
853 } 1155 }
854 break;
855 } 1156 }
1157 }
856 1158
857 case (int)ActorTypes.Prim: 1159 #endregion
858 {
859 switch (p2.PhysicsActorType)
860 {
861 case (int)ActorTypes.Agent:
862 dop2ava = true;
863 break;
864 1160
865 case (int)ActorTypes.Prim: 1161 // Logic for collision handling
866 Vector3 relV = p1.rootVelocity - p2.rootVelocity; 1162 // Note, that if *all* contacts are skipped (VolumeDetect)
867 float relVlenSQ = relV.LengthSquared(); 1163 // The prim still detects (and forwards) collision events but
868 if (relVlenSQ > 0.0001f) 1164 // appears to be phantom for the world
869 { 1165 Boolean skipThisContact = false;
870 p1.CollidingObj = true;
871 p2.CollidingObj = true;
872 }
873 p1.getContactData(ref contactdata1);
874 p2.getContactData(ref contactdata2);
875 bounce = contactdata1.bounce * contactdata2.bounce;
876 mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
877 1166
878 if (relVlenSQ > 0.01f) 1167 if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect))
879 mu *= frictionMovementMult; 1168 skipThisContact = true; // No collision on volume detect prims
880 1169
881 if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass && 1170 if (av_av_collisions_off)
882 d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) 1171 if ((p1 is OdeCharacter) && (p2 is OdeCharacter))
883 smoothMesh = true; 1172 skipThisContact = true;
884 break;
885 1173
886 case (int)ActorTypes.Ground: 1174 if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
887 p1.getContactData(ref contactdata1); 1175 skipThisContact = true; // No collision on volume detect prims
888 bounce = contactdata1.bounce * TerrainBounce;
889 mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
890 1176
891 Vector3 v1 = p1.rootVelocity; 1177 if (!skipThisContact && curContact.depth < 0f)
892 if (Math.Abs(v1.X) > 0.1f || Math.Abs(v1.Y) > 0.1f) 1178 skipThisContact = true;
893 mu *= frictionMovementMult;
894 p1.CollidingGround = true;
895 1179
896 if(d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) 1180 if (!skipThisContact && checkDupe(curContact, p2.PhysicsActorType))
897 smoothMesh = true; 1181 skipThisContact = true;
898 break;
899 1182
900 case (int)ActorTypes.Water: 1183 const int maxContactsbeforedeath = 4000;
901 default: 1184 joint = IntPtr.Zero;
902 ignore = true;
903 break;
904 }
905 }
906 break;
907 1185
908 case (int)ActorTypes.Ground: 1186 if (!skipThisContact)
909 if (p2.PhysicsActorType == (int)ActorTypes.Prim) 1187 {
910 { 1188 _perloopContact.Add(curContact);
911 p2.CollidingGround = true;
912 p2.getContactData(ref contactdata2);
913 bounce = contactdata2.bounce * TerrainBounce;
914 mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
915
916// if (curContact.side1 > 0) // should be 2 ?
917// IgnoreNegSides = true;
918 Vector3 v2 = p2.rootVelocity;
919 if (Math.Abs(v2.X) > 0.1f || Math.Abs(v2.Y) > 0.1f)
920 mu *= frictionMovementMult;
921
922 if(d.GeomGetClass(g2) == d.GeomClassID.TriMeshClass)
923 smoothMesh = true;
924 }
925 else
926 ignore = true;
927 break;
928 1189
929 case (int)ActorTypes.Water: 1190 if (name1 == "Terrain" || name2 == "Terrain")
930 default: 1191 {
931 break; 1192 if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
932 } 1193 (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
1194 {
1195 p2ExpectedPoints = avatarExpectedContacts;
1196 // Avatar is moving on terrain, use the movement terrain contact
1197 AvatarMovementTerrainContact.geom = curContact;
933 1198
934 if (ignore) 1199 if (m_global_contactcount < maxContactsbeforedeath)
935 return; 1200 {
1201 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact);
1202 m_global_contactcount++;
1203 }
1204 }
1205 else
1206 {
1207 if (p2.PhysicsActorType == (int)ActorTypes.Agent)
1208 {
1209 p2ExpectedPoints = avatarExpectedContacts;
1210 // Avatar is standing on terrain, use the non moving terrain contact
1211 TerrainContact.geom = curContact;
936 1212
937 IntPtr Joint; 1213 if (m_global_contactcount < maxContactsbeforedeath)
938 bool FeetCollision = false; 1214 {
939 int ncontacts = 0; 1215 joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
1216 m_global_contactcount++;
1217 }
1218 }
1219 else
1220 {
1221 if (p2.PhysicsActorType == (int)ActorTypes.Prim && p1.PhysicsActorType == (int)ActorTypes.Prim)
1222 {
1223 // prim prim contact
1224 // int pj294950 = 0;
1225 int movintYN = 0;
1226 int material = (int) Material.Wood;
1227 // prim terrain contact
1228 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
1229 {
1230 movintYN = 1;
1231 }
940 1232
941 int i = 0; 1233 if (p2 is OdePrim)
1234 {
1235 material = ((OdePrim) p2).m_material;
1236 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1237 }
1238
1239 // Unnessesary because p1 is defined above
1240 //if (p1 is OdePrim)
1241 // {
1242 // p1ExpectedPoints = ((OdePrim)p1).ExpectedCollisionContacts;
1243 // }
1244 //m_log.DebugFormat("Material: {0}", material);
942 1245
943 maxDepthContact = new ContactPoint(); 1246 m_materialContacts[material, movintYN].geom = curContact;
944 maxDepthContact.PenetrationDepth = float.MinValue;
945 ContactPoint minDepthContact = new ContactPoint();
946 minDepthContact.PenetrationDepth = float.MaxValue;
947 1247
948 SharedTmpcontact.geom.depth = 0; 1248 if (m_global_contactcount < maxContactsbeforedeath)
949 SharedTmpcontact.surface.mu = mu; 1249 {
950 SharedTmpcontact.surface.bounce = bounce; 1250 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1251 m_global_contactcount++;
1252 }
1253 }
1254 else
1255 {
1256 int movintYN = 0;
1257 // prim terrain contact
1258 if (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f)
1259 {
1260 movintYN = 1;
1261 }
951 1262
952 d.ContactGeom altContact = new d.ContactGeom(); 1263 int material = (int)Material.Wood;
953 bool useAltcontact = false;
954 bool noskip = true;
955 1264
956 if(dop1ava || dop2ava) 1265 if (p2 is OdePrim)
957 smoothMesh = false; 1266 {
1267 material = ((OdePrim)p2).m_material;
1268 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1269 }
958 1270
959 while (true) 1271 //m_log.DebugFormat("Material: {0}", material);
960 { 1272 m_materialContacts[material, movintYN].geom = curContact;
961 noskip = true;
962 useAltcontact = false;
963 1273
964 if (dop1ava) 1274 if (m_global_contactcount < maxContactsbeforedeath)
965 { 1275 {
966 if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision))) 1276 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, movintYN]);
1277 m_global_contactcount++;
1278 }
1279 }
1280 }
1281 }
1282 //if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1283 //{
1284 //m_log.Debug("[PHYSICS]: prim contacting with ground");
1285 //}
1286 }
1287 else if (name1 == "Water" || name2 == "Water")
967 { 1288 {
968 if (p2.PhysicsActorType == (int)ActorTypes.Agent) 1289 /*
1290 if ((p2.PhysicsActorType == (int) ActorTypes.Prim))
969 { 1291 {
970 p1.CollidingObj = true;
971 p2.CollidingObj = true;
972 } 1292 }
973 else if (p2.rootVelocity.LengthSquared() > 0.0f) 1293 else
974 p2.CollidingObj = true; 1294 {
1295 }
1296 */
1297 //WaterContact.surface.soft_cfm = 0.0000f;
1298 //WaterContact.surface.soft_erp = 0.00000f;
1299 if (curContact.depth > 0.1f)
1300 {
1301 curContact.depth *= 52;
1302 //contact.normal = new d.Vector3(0, 0, 1);
1303 //contact.pos = new d.Vector3(0, 0, contact.pos.Z - 5f);
1304 }
1305
1306 WaterContact.geom = curContact;
1307
1308 if (m_global_contactcount < maxContactsbeforedeath)
1309 {
1310 joint = d.JointCreateContact(world, contactgroup, ref WaterContact);
1311 m_global_contactcount++;
1312 }
1313 //m_log.Info("[PHYSICS]: Prim Water Contact" + contact.depth);
975 } 1314 }
976 else 1315 else
977 noskip = false;
978 }
979 else if (dop2ava)
980 {
981 if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
982 { 1316 {
983 if (p1.PhysicsActorType == (int)ActorTypes.Agent) 1317 if ((p2.PhysicsActorType == (int)ActorTypes.Agent))
984 { 1318 {
985 p1.CollidingObj = true; 1319 p2ExpectedPoints = avatarExpectedContacts;
986 p2.CollidingObj = true; 1320 if ((Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
1321 {
1322 // Avatar is moving on a prim, use the Movement prim contact
1323 AvatarMovementprimContact.geom = curContact;
1324
1325 if (m_global_contactcount < maxContactsbeforedeath)
1326 {
1327 joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact);
1328 m_global_contactcount++;
1329 }
1330 }
1331 else
1332 {
1333 // Avatar is standing still on a prim, use the non movement contact
1334 contact.geom = curContact;
1335
1336 if (m_global_contactcount < maxContactsbeforedeath)
1337 {
1338 joint = d.JointCreateContact(world, contactgroup, ref contact);
1339 m_global_contactcount++;
1340 }
1341 }
1342 }
1343 else if (p2.PhysicsActorType == (int)ActorTypes.Prim)
1344 {
1345 //p1.PhysicsActorType
1346 int material = (int)Material.Wood;
1347
1348 if (p2 is OdePrim)
1349 {
1350 material = ((OdePrim)p2).m_material;
1351 p2ExpectedPoints = ((OdePrim)p2).ExpectedCollisionContacts;
1352 }
1353
1354 //m_log.DebugFormat("Material: {0}", material);
1355 m_materialContacts[material, 0].geom = curContact;
1356
1357 if (m_global_contactcount < maxContactsbeforedeath)
1358 {
1359 joint = d.JointCreateContact(world, contactgroup, ref m_materialContacts[material, 0]);
1360 m_global_contactcount++;
1361 }
987 } 1362 }
988 else if (p1.rootVelocity.LengthSquared() > 0.0f)
989 p1.CollidingObj = true;
990 } 1363 }
991 else 1364
992 noskip = false; 1365 if (m_global_contactcount < maxContactsbeforedeath && joint != IntPtr.Zero) // stack collide!
1366 {
1367 d.JointAttach(joint, b1, b2);
1368 m_global_contactcount++;
1369 }
993 } 1370 }
994 1371
995 if (noskip) 1372 collision_accounting_events(p1, p2, maxDepthContact);
1373
1374 if (count > ((p1ExpectedPoints + p2ExpectedPoints) * 0.25) + (geomContactPointsStartthrottle))
996 { 1375 {
997 if(useAltcontact) 1376 // If there are more then 3 contact points, it's likely
998 Joint = CreateContacJoint(ref altContact,smoothMesh); 1377 // that we've got a pile of objects, so ...
999 else 1378 // We don't want to send out hundreds of terse updates over and over again
1000 Joint = CreateContacJoint(ref curContact,smoothMesh); 1379 // so lets throttle them and send them again after it's somewhat sorted out.
1380 p2.ThrottleUpdates = true;
1381 }
1382 //m_log.Debug(count.ToString());
1383 //m_log.Debug("near: A collision was detected between {1} and {2}", 0, name1, name2);
1384 }
1385 }
1001 1386
1002 if (Joint == IntPtr.Zero) 1387 private bool checkDupe(d.ContactGeom contactGeom, int atype)
1003 break; 1388 {
1389 if (!m_filterCollisions)
1390 return false;
1004 1391
1005 d.JointAttach(Joint, b1, b2); 1392 bool result = false;
1006 1393
1007 ncontacts++; 1394 ActorTypes at = (ActorTypes)atype;
1008 1395
1009 if (curContact.depth > maxDepthContact.PenetrationDepth) 1396 foreach (d.ContactGeom contact in _perloopContact)
1397 {
1398 //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2))
1399 //{
1400 // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2)
1401 if (at == ActorTypes.Agent)
1402 {
1403 if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f)
1404 && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f)
1405 && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)))
1010 { 1406 {
1011 maxDepthContact.Position.X = curContact.pos.X; 1407 if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f)
1012 maxDepthContact.Position.Y = curContact.pos.Y; 1408 {
1013 maxDepthContact.Position.Z = curContact.pos.Z; 1409 result = true;
1014 maxDepthContact.PenetrationDepth = curContact.depth; 1410 break;
1015 maxDepthContact.CharacterFeet = FeetCollision; 1411 }
1016 } 1412 }
1017 1413 }
1018 if (curContact.depth < minDepthContact.PenetrationDepth) 1414 else if (at == ActorTypes.Prim)
1415 {
1416 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)))
1019 { 1417 {
1020 minDepthContact.PenetrationDepth = curContact.depth; 1418 if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z)
1021 minDepthContact.SurfaceNormal.X = curContact.normal.X; 1419 {
1022 minDepthContact.SurfaceNormal.Y = curContact.normal.Y; 1420 if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f)
1023 minDepthContact.SurfaceNormal.Z = curContact.normal.Z; 1421 {
1422 result = true;
1423 break;
1424 }
1425 }
1426 //m_log.DebugFormat("[Collision]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
1427 //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));
1024 } 1428 }
1025 } 1429 }
1026
1027 if (++i >= count)
1028 break;
1029
1030 if (!GetCurContactGeom(i, ref curContact))
1031 break;
1032 } 1430 }
1033 1431
1034 if (ncontacts > 0) 1432 return result;
1035 {
1036 maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X;
1037 maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y;
1038 maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z;
1039
1040 collision_accounting_events(p1, p2, maxDepthContact);
1041 }
1042 } 1433 }
1043 1434
1044 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) 1435 private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
1045 { 1436 {
1046 uint obj2LocalID = 0; 1437 // obj1LocalID = 0;
1047 1438 //returncollisions = false;
1048 // update actors collision score 1439 obj2LocalID = 0;
1049 if (p1.CollisionScore < float.MaxValue) 1440 //ctype = 0;
1050 p1.CollisionScore += 1.0f; 1441 //cStartStop = 0;
1051 if (p2.CollisionScore < float.MaxValue) 1442// if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
1052 p2.CollisionScore += 1.0f; 1443// return;
1053
1054 bool p1events = p1.SubscribedEvents(); 1444 bool p1events = p1.SubscribedEvents();
1055 bool p2events = p2.SubscribedEvents(); 1445 bool p2events = p2.SubscribedEvents();
1056 1446
@@ -1064,379 +1454,799 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1064 1454
1065 Vector3 vel = Vector3.Zero; 1455 Vector3 vel = Vector3.Zero;
1066 if (p2 != null && p2.IsPhysical) 1456 if (p2 != null && p2.IsPhysical)
1067 vel = p2.rootVelocity; 1457 vel = p2.Velocity;
1068 1458
1069 if (p1 != null && p1.IsPhysical) 1459 if (p1 != null && p1.IsPhysical)
1070 vel -= p1.rootVelocity; 1460 vel -= p1.Velocity;
1071 1461
1072 contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal); 1462 contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
1073 1463
1074 switch ((ActorTypes)p1.PhysicsActorType) 1464 switch ((ActorTypes)p2.PhysicsActorType)
1075 { 1465 {
1076 case ActorTypes.Agent: 1466 case ActorTypes.Agent:
1077 case ActorTypes.Prim: 1467 cc2 = (OdeCharacter)p2;
1078 { 1468
1079 switch ((ActorTypes)p2.PhysicsActorType) 1469 // obj1LocalID = cc2.m_localID;
1470 switch ((ActorTypes)p1.PhysicsActorType)
1080 { 1471 {
1081 case ActorTypes.Agent: 1472 case ActorTypes.Agent:
1473 cc1 = (OdeCharacter)p1;
1474 obj2LocalID = cc1.LocalID;
1475 cc1.AddCollisionEvent(cc2.LocalID, contact);
1476 break;
1477
1082 case ActorTypes.Prim: 1478 case ActorTypes.Prim:
1083 if (p2events) 1479 if (p1 is OdePrim)
1084 { 1480 {
1085 AddCollisionEventReporting(p2); 1481 cp1 = (OdePrim) p1;
1086 p2.AddCollisionEvent(p1.ParentActor.LocalID, contact); 1482 obj2LocalID = cp1.LocalID;
1483 cp1.AddCollisionEvent(cc2.LocalID, contact);
1087 } 1484 }
1088 obj2LocalID = p2.ParentActor.LocalID;
1089 break; 1485 break;
1090 1486
1091 case ActorTypes.Ground: 1487 case ActorTypes.Ground:
1092 case ActorTypes.Unknown: 1488 case ActorTypes.Unknown:
1093 default:
1094 obj2LocalID = 0; 1489 obj2LocalID = 0;
1095 break; 1490 break;
1096 } 1491 }
1097 if (p1events) 1492
1098 { 1493 cc2.AddCollisionEvent(obj2LocalID, contact);
1099 contact.SurfaceNormal = -contact.SurfaceNormal;
1100 contact.RelativeSpeed = -contact.RelativeSpeed;
1101 AddCollisionEventReporting(p1);
1102 p1.AddCollisionEvent(obj2LocalID, contact);
1103 }
1104 break; 1494 break;
1105 } 1495
1106 case ActorTypes.Ground: 1496 case ActorTypes.Prim:
1107 case ActorTypes.Unknown: 1497
1108 default: 1498 if (p2 is OdePrim)
1109 {
1110 if (p2events && !p2.IsVolumeDtc)
1111 { 1499 {
1112 AddCollisionEventReporting(p2); 1500 cp2 = (OdePrim) p2;
1113 p2.AddCollisionEvent(0, contact); 1501
1502 // obj1LocalID = cp2.m_localID;
1503 switch ((ActorTypes) p1.PhysicsActorType)
1504 {
1505 case ActorTypes.Agent:
1506 if (p1 is OdeCharacter)
1507 {
1508 cc1 = (OdeCharacter) p1;
1509 obj2LocalID = cc1.LocalID;
1510 cc1.AddCollisionEvent(cp2.LocalID, contact);
1511 }
1512 break;
1513 case ActorTypes.Prim:
1514
1515 if (p1 is OdePrim)
1516 {
1517 cp1 = (OdePrim) p1;
1518 obj2LocalID = cp1.LocalID;
1519 cp1.AddCollisionEvent(cp2.LocalID, contact);
1520 }
1521 break;
1522
1523 case ActorTypes.Ground:
1524 case ActorTypes.Unknown:
1525 obj2LocalID = 0;
1526 break;
1527 }
1528
1529 cp2.AddCollisionEvent(obj2LocalID, contact);
1114 } 1530 }
1115 break; 1531 break;
1116 }
1117 } 1532 }
1118 } 1533 }
1119
1120 /// <summary> 1534 /// <summary>
1121 /// This is our collision testing routine in ODE 1535 /// This is our collision testing routine in ODE
1122 /// </summary> 1536 /// </summary>
1123 /// <param name="timeStep"></param>
1124 private void collision_optimized() 1537 private void collision_optimized()
1125 { 1538 {
1126 lock (_characters) 1539 _perloopContact.Clear();
1127 { 1540
1541 foreach (OdeCharacter chr in _characters)
1542 {
1543 // Reset the collision values to false
1544 // since we don't know if we're colliding yet
1545 if (chr.Shell == IntPtr.Zero || chr.Body == IntPtr.Zero)
1546 continue;
1547
1548 chr.IsColliding = false;
1549 chr.CollidingGround = false;
1550 chr.CollidingObj = false;
1551
1552 // Test the avatar's geometry for collision with the space
1553 // This will return near and the space that they are the closest to
1554 // And we'll run this again against the avatar and the space segment
1555 // This will return with a bunch of possible objects in the space segment
1556 // and we'll run it again on all of them.
1128 try 1557 try
1129 { 1558 {
1130 foreach (OdeCharacter chr in _characters) 1559 CollideSpaces(space, chr.Shell, IntPtr.Zero);
1131 {
1132 if (chr == null)
1133 continue;
1134
1135 chr.IsColliding = false;
1136 // chr.CollidingGround = false; not done here
1137 chr.CollidingObj = false;
1138
1139 if(chr.Body == IntPtr.Zero || chr.collider == IntPtr.Zero )
1140 continue;
1141
1142 // do colisions with static space
1143 d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
1144
1145 // no coll with gnd
1146 }
1147 // chars with chars
1148 d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
1149
1150 } 1560 }
1151 catch (AccessViolationException) 1561 catch (AccessViolationException)
1152 { 1562 {
1153 m_log.Warn("[PHYSICS]: Unable to collide Character to static space"); 1563 m_log.ErrorFormat("[ODE SCENE]: Unable to space collide {0}", PhysicsSceneName);
1154 } 1564 }
1155 1565
1566 //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
1567 //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
1568 //{
1569 //chr.Position.Z = terrainheight + 10.0f;
1570 //forcedZ = true;
1571 //}
1156 } 1572 }
1157 1573
1158 lock (_activeprims) 1574 if (CollectStats)
1159 { 1575 {
1160 foreach (OdePrim aprim in _activeprims) 1576 m_tempAvatarCollisionsThisFrame = _perloopContact.Count;
1161 { 1577 m_stats[ODEAvatarContactsStatsName] += m_tempAvatarCollisionsThisFrame;
1162 aprim.CollisionScore = 0;
1163 aprim.IsColliding = false;
1164 }
1165 } 1578 }
1166 lock (_activegroups) 1579
1580 List<OdePrim> removeprims = null;
1581 foreach (OdePrim chr in _activeprims)
1167 { 1582 {
1168 try 1583 if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled))
1169 { 1584 {
1170 foreach (OdePrim aprim in _activegroups) 1585 try
1171 { 1586 {
1172 if(!aprim.m_outbounds && d.BodyIsEnabled(aprim.Body) && 1587 lock (chr)
1173 aprim.collide_geom != IntPtr.Zero)
1174 { 1588 {
1175 d.SpaceCollide2(StaticSpace, aprim.collide_geom, IntPtr.Zero, nearCallback); 1589 if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false)
1176 d.SpaceCollide2(GroundSpace, aprim.collide_geom, IntPtr.Zero, nearCallback); 1590 {
1591 CollideSpaces(space, chr.prim_geom, IntPtr.Zero);
1592 }
1593 else
1594 {
1595 if (removeprims == null)
1596 {
1597 removeprims = new List<OdePrim>();
1598 }
1599 removeprims.Add(chr);
1600 m_log.Error(
1601 "[ODE SCENE]: 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!");
1602 }
1177 } 1603 }
1178 } 1604 }
1179 } 1605 catch (AccessViolationException)
1180 catch (Exception e) 1606 {
1181 { 1607 m_log.Error("[ODE SCENE]: Unable to space collide");
1182 m_log.Warn("[PHYSICS]: Unable to collide Active to Static: " + e.Message); 1608 }
1183 } 1609 }
1184 } 1610 }
1185 1611
1186 // colide active amoung them 1612 if (CollectStats)
1187 try 1613 m_stats[ODEPrimContactsStatName] += _perloopContact.Count - m_tempAvatarCollisionsThisFrame;
1188 { 1614
1189 d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); 1615 if (removeprims != null)
1190 }
1191 catch (Exception e)
1192 { 1616 {
1193 m_log.Warn("[PHYSICS]: Unable to collide in Active: " + e.Message); 1617 foreach (OdePrim chr in removeprims)
1618 {
1619 _activeprims.Remove(chr);
1620 }
1194 } 1621 }
1622 }
1195 1623
1196 // and with chars 1624 #endregion
1197 try 1625
1626 // Recovered for use by fly height. Kitto Flora
1627 internal float GetTerrainHeightAtXY(float x, float y)
1628 {
1629 IntPtr heightFieldGeom = IntPtr.Zero;
1630 int offsetX = 0;
1631 int offsetY = 0;
1632
1633 if(RegionTerrain.TryGetValue(new Vector3(offsetX,offsetY,0), out heightFieldGeom))
1198 { 1634 {
1199 d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback); 1635 if (heightFieldGeom != IntPtr.Zero)
1636 {
1637 if (TerrainHeightFieldHeights.ContainsKey(heightFieldGeom))
1638 {
1639
1640 int index;
1641
1642
1643 if ((int)x > WorldExtents.X || (int)y > WorldExtents.Y ||
1644 (int)x < 0.001f || (int)y < 0.001f)
1645 return 0;
1646
1647 x = x - offsetX + 1f;
1648 y = y - offsetY + 1f;
1649
1650 // map is rotated
1651 index = (int)x * ((int)m_regionHeight + 3) + (int)y;
1652
1653 if (index < TerrainHeightFieldHeights[heightFieldGeom].Length)
1654 {
1655 //m_log.DebugFormat("x{0} y{1} = {2}", x, y, (float)TerrainHeightFieldHeights[heightFieldGeom][index]);
1656 return (float)TerrainHeightFieldHeights[heightFieldGeom][index];
1657 }
1658
1659 else
1660 return 0f;
1661 }
1662 else
1663 {
1664 return 0f;
1665 }
1666
1667 }
1668 else
1669 {
1670 return 0f;
1671 }
1672
1200 } 1673 }
1201 catch (Exception e) 1674 else
1202 { 1675 {
1203 m_log.Warn("[PHYSICS]: Unable to collide Active to Character: " + e.Message); 1676 return 0f;
1204 } 1677 }
1205 } 1678 }
1679// End recovered. Kitto Flora
1206 1680
1207 #endregion
1208 /// <summary> 1681 /// <summary>
1209 /// Add actor to the list that should receive collision events in the simulate loop. 1682 /// Add actor to the list that should receive collision events in the simulate loop.
1210 /// </summary> 1683 /// </summary>
1211 /// <param name="obj"></param> 1684 /// <param name="obj"></param>
1212 public void AddCollisionEventReporting(PhysicsActor obj) 1685 internal void AddCollisionEventReporting(PhysicsActor obj)
1213 { 1686 {
1214 if (!_collisionEventPrim.Contains(obj)) 1687// m_log.DebugFormat("[PHYSICS]: Adding {0} {1} to collision event reporting", obj.SOPName, obj.LocalID);
1215 _collisionEventPrim.Add(obj); 1688
1689 lock (m_collisionEventActorsChanges)
1690 m_collisionEventActorsChanges[obj.LocalID] = obj;
1216 } 1691 }
1217 1692
1218 /// <summary> 1693 /// <summary>
1219 /// Remove actor from the list that should receive collision events in the simulate loop. 1694 /// Remove actor from the list that should receive collision events in the simulate loop.
1220 /// </summary> 1695 /// </summary>
1221 /// <param name="obj"></param> 1696 /// <param name="obj"></param>
1222 public void RemoveCollisionEventReporting(PhysicsActor obj) 1697 internal void RemoveCollisionEventReporting(PhysicsActor obj)
1223 {
1224 lock(_collisionEventPrimRemove)
1225 {
1226 if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
1227 _collisionEventPrimRemove.Add(obj);
1228 }
1229 }
1230
1231 public override float TimeDilation
1232 { 1698 {
1233 get { return m_timeDilation; } 1699// m_log.DebugFormat("[PHYSICS]: Removing {0} {1} from collision event reporting", obj.SOPName, obj.LocalID);
1234 }
1235 1700
1236 public override bool SupportsNINJAJoints 1701 lock (m_collisionEventActorsChanges)
1237 { 1702 m_collisionEventActorsChanges[obj.LocalID] = null;
1238 get { return false; }
1239 } 1703 }
1240 1704
1241 #region Add/Remove Entities 1705 #region Add/Remove Entities
1242 1706
1243 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying) 1707 public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
1244 { 1708 {
1245 return null; 1709 d.AllocateODEDataForThread(0);
1246 } 1710
1711 OdeCharacter newAv
1712 = new OdeCharacter(
1713 avName, this, position, velocity, size, avPIDD, avPIDP,
1714 avCapRadius, avStandupTensor, avDensity,
1715 avMovementDivisorWalk, avMovementDivisorRun);
1247 1716
1248 public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying)
1249 {
1250 OdeCharacter newAv = new OdeCharacter(localID, avName, this, position,
1251 size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
1252 newAv.Flying = isFlying; 1717 newAv.Flying = isFlying;
1253 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; 1718 newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
1254 1719 newAv.m_avatarplanted = avplanted;
1720
1255 return newAv; 1721 return newAv;
1256 } 1722 }
1257 1723
1258 public void AddCharacter(OdeCharacter chr) 1724 public override void RemoveAvatar(PhysicsActor actor)
1259 { 1725 {
1260 lock (_characters) 1726// m_log.DebugFormat(
1727// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}",
1728// actor.Name, actor.LocalID, Name);
1729
1730 lock (OdeLock)
1261 { 1731 {
1262 if (!_characters.Contains(chr)) 1732 d.AllocateODEDataForThread(0);
1263 { 1733
1264 _characters.Add(chr); 1734 ((OdeCharacter) actor).Destroy();
1265 if (chr.bad)
1266 m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
1267 }
1268 } 1735 }
1269 } 1736 }
1270 1737
1271 public void RemoveCharacter(OdeCharacter chr) 1738 internal void AddCharacter(OdeCharacter chr)
1272 { 1739 {
1273 lock (_characters) 1740 chr.m_avatarplanted = avplanted;
1741 if (!_characters.Contains(chr))
1274 { 1742 {
1275 if (_characters.Contains(chr)) 1743 _characters.Add(chr);
1276 { 1744
1277 _characters.Remove(chr); 1745// m_log.DebugFormat(
1278 } 1746// "[ODE SCENE]: Adding physics character {0} {1} to physics scene {2}. Count now {3}",
1747// chr.Name, chr.LocalID, Name, _characters.Count);
1748
1749 if (chr.bad)
1750 m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to characters list", chr.m_uuid);
1751 }
1752 else
1753 {
1754 m_log.ErrorFormat(
1755 "[ODE SCENE]: Tried to add character {0} {1} but they are already in the set!",
1756 chr.Name, chr.LocalID);
1279 } 1757 }
1280 } 1758 }
1281 1759
1282 public void BadCharacter(OdeCharacter chr) 1760 internal void RemoveCharacter(OdeCharacter chr)
1283 { 1761 {
1284 lock (_badCharacter) 1762 if (_characters.Contains(chr))
1285 { 1763 {
1286 if (!_badCharacter.Contains(chr)) 1764 _characters.Remove(chr);
1287 _badCharacter.Add(chr); 1765
1766// m_log.DebugFormat(
1767// "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2}. Count now {3}",
1768// chr.Name, chr.LocalID, Name, _characters.Count);
1769 }
1770 else
1771 {
1772 m_log.ErrorFormat(
1773 "[ODE SCENE]: Tried to remove character {0} {1} but they are not in the list!",
1774 chr.Name, chr.LocalID);
1288 } 1775 }
1289 } 1776 }
1290 1777
1291 public override void RemoveAvatar(PhysicsActor actor) 1778 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
1779 PrimitiveBaseShape pbs, bool isphysical, uint localID)
1292 { 1780 {
1293 //m_log.Debug("[PHYSICS]:ODELOCK"); 1781 Vector3 pos = position;
1782 Vector3 siz = size;
1783 Quaternion rot = rotation;
1784
1785
1786 OdePrim newPrim;
1294 lock (OdeLock) 1787 lock (OdeLock)
1295 { 1788 {
1296 d.AllocateODEDataForThread(0); 1789 d.AllocateODEDataForThread(0);
1297 ((OdeCharacter) actor).Destroy(); 1790 newPrim = new OdePrim(name, this, pos, siz, rot, pbs, isphysical);
1791
1792 lock (_prims)
1793 _prims.Add(newPrim);
1298 } 1794 }
1795 newPrim.LocalID = localID;
1796 return newPrim;
1299 } 1797 }
1300 1798
1799 /// <summary>
1800 /// Make this prim subject to physics.
1801 /// </summary>
1802 /// <param name="prim"></param>
1803 internal void ActivatePrim(OdePrim prim)
1804 {
1805 // adds active prim.. (ones that should be iterated over in collisions_optimized
1806 if (!_activeprims.Contains(prim))
1807 _activeprims.Add(prim);
1808 //else
1809 // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
1810 }
1301 1811
1302 public void addActivePrim(OdePrim activatePrim) 1812 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
1813 Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
1303 { 1814 {
1304 // adds active prim.. 1815// m_log.DebugFormat("[ODE SCENE]: Adding physics prim {0} {1} to physics scene {2}", primName, localid, Name);
1305 lock (_activeprims) 1816
1306 { 1817 return AddPrim(primName, position, size, rotation, pbs, isPhysical, localid);
1307 if (!_activeprims.Contains(activatePrim))
1308 _activeprims.Add(activatePrim);
1309 }
1310 } 1818 }
1311 1819
1312 public void addActiveGroups(OdePrim activatePrim) 1820 public override float TimeDilation
1313 { 1821 {
1314 lock (_activegroups) 1822 get { return m_timeDilation; }
1315 {
1316 if (!_activegroups.Contains(activatePrim))
1317 _activegroups.Add(activatePrim);
1318 }
1319 } 1823 }
1320 1824
1321 private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, 1825 public override bool SupportsNINJAJoints
1322 PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
1323 { 1826 {
1324 OdePrim newPrim; 1827 get { return m_NINJA_physics_joints_enabled; }
1325 lock (OdeLock)
1326 {
1327
1328 newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID);
1329 }
1330 return newPrim;
1331 } 1828 }
1332 1829
1333 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, 1830 // internal utility function: must be called within a lock (OdeLock)
1334 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) 1831 private void InternalAddActiveJoint(PhysicsJoint joint)
1335 { 1832 {
1336 return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid); 1833 activeJoints.Add(joint);
1834 SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
1337 } 1835 }
1338 1836
1837 // internal utility function: must be called within a lock (OdeLock)
1838 private void InternalAddPendingJoint(OdePhysicsJoint joint)
1839 {
1840 pendingJoints.Add(joint);
1841 SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
1842 }
1339 1843
1340 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, 1844 // internal utility function: must be called within a lock (OdeLock)
1341 Vector3 size, Quaternion rotation, bool isPhysical, uint localid) 1845 private void InternalRemovePendingJoint(PhysicsJoint joint)
1342 { 1846 {
1343 return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid); 1847 pendingJoints.Remove(joint);
1848 SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene);
1344 } 1849 }
1345 1850
1346 public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, 1851 // internal utility function: must be called within a lock (OdeLock)
1347 Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid) 1852 private void InternalRemoveActiveJoint(PhysicsJoint joint)
1348 { 1853 {
1349 return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid); 1854 activeJoints.Remove(joint);
1855 SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
1350 } 1856 }
1351 1857
1352 public void remActivePrim(OdePrim deactivatePrim) 1858 public override void DumpJointInfo()
1353 { 1859 {
1354 lock (_activeprims) 1860 string hdr = "[NINJA] JOINTINFO: ";
1861 foreach (PhysicsJoint j in pendingJoints)
1862 {
1863 m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1864 }
1865 m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
1866 foreach (string jointName in SOPName_to_pendingJoint.Keys)
1867 {
1868 m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
1869 }
1870 m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
1871 foreach (PhysicsJoint j in activeJoints)
1872 {
1873 m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1874 }
1875 m_log.Debug(hdr + activeJoints.Count + " total active joints");
1876 foreach (string jointName in SOPName_to_activeJoint.Keys)
1355 { 1877 {
1356 _activeprims.Remove(deactivatePrim); 1878 m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
1879 }
1880 m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
1881
1882 m_log.Debug(hdr + " Per-body joint connectivity information follows.");
1883 m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
1884 foreach (string actorName in joints_connecting_actor.Keys)
1885 {
1886 m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it");
1887 foreach (PhysicsJoint j in joints_connecting_actor[actorName])
1888 {
1889 m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
1890 }
1891 m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
1357 } 1892 }
1358 } 1893 }
1359 public void remActiveGroup(OdePrim deactivatePrim) 1894
1895 public override void RequestJointDeletion(string ObjectNameInScene)
1360 { 1896 {
1361 lock (_activegroups) 1897 lock (externalJointRequestsLock)
1362 { 1898 {
1363 _activegroups.Remove(deactivatePrim); 1899 if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously
1900 {
1901 requestedJointsToBeDeleted.Add(ObjectNameInScene);
1902 }
1364 } 1903 }
1365 } 1904 }
1366 1905
1367 public override void RemovePrim(PhysicsActor prim) 1906 private void DeleteRequestedJoints()
1368 { 1907 {
1369 // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be 1908 List<string> myRequestedJointsToBeDeleted;
1370 // removed in the next physics simulate pass. 1909 lock (externalJointRequestsLock)
1371 if (prim is OdePrim) 1910 {
1911 // make a local copy of the shared list for processing (threading issues)
1912 myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
1913 }
1914
1915 foreach (string jointName in myRequestedJointsToBeDeleted)
1372 { 1916 {
1373// lock (OdeLock) 1917 lock (OdeLock)
1374 { 1918 {
1375 1919 //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
1376 OdePrim p = (OdePrim)prim; 1920 if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
1377 p.setPrimForRemoval(); 1921 {
1922 OdePhysicsJoint joint = null;
1923 if (SOPName_to_activeJoint.ContainsKey(jointName))
1924 {
1925 joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
1926 InternalRemoveActiveJoint(joint);
1927 }
1928 else if (SOPName_to_pendingJoint.ContainsKey(jointName))
1929 {
1930 joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
1931 InternalRemovePendingJoint(joint);
1932 }
1933
1934 if (joint != null)
1935 {
1936 //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
1937 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
1938 {
1939 string bodyName = joint.BodyNames[iBodyName];
1940 if (bodyName != "NULL")
1941 {
1942 joints_connecting_actor[bodyName].Remove(joint);
1943 if (joints_connecting_actor[bodyName].Count == 0)
1944 {
1945 joints_connecting_actor.Remove(bodyName);
1946 }
1947 }
1948 }
1949
1950 DoJointDeactivated(joint);
1951 if (joint.jointID != IntPtr.Zero)
1952 {
1953 d.JointDestroy(joint.jointID);
1954 joint.jointID = IntPtr.Zero;
1955 //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
1956 }
1957 else
1958 {
1959 //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
1960 }
1961 }
1962 else
1963 {
1964 // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
1965 }
1966 }
1967 else
1968 {
1969 // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
1970 }
1971 }
1972 }
1973
1974 // remove processed joints from the shared list
1975 lock (externalJointRequestsLock)
1976 {
1977 foreach (string jointName in myRequestedJointsToBeDeleted)
1978 {
1979 requestedJointsToBeDeleted.Remove(jointName);
1378 } 1980 }
1379 } 1981 }
1380 } 1982 }
1381 1983
1382 public void RemovePrimThreadLocked(OdePrim prim) 1984 // for pending joints we don't know if their associated bodies exist yet or not.
1985 // the joint is actually created during processing of the taints
1986 private void CreateRequestedJoints()
1383 { 1987 {
1384 //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); 1988 List<PhysicsJoint> myRequestedJointsToBeCreated;
1385 lock (prim) 1989 lock (externalJointRequestsLock)
1386 { 1990 {
1387// RemoveCollisionEventReporting(prim); 1991 // make a local copy of the shared list for processing (threading issues)
1388 lock (_prims) 1992 myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
1389 _prims.Remove(prim.LocalID); 1993 }
1994
1995 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
1996 {
1997 lock (OdeLock)
1998 {
1999 if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
2000 {
2001 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);
2002 continue;
2003 }
2004 if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
2005 {
2006 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);
2007 continue;
2008 }
2009
2010 InternalAddPendingJoint(joint as OdePhysicsJoint);
2011
2012 if (joint.BodyNames.Count >= 2)
2013 {
2014 for (int iBodyName = 0; iBodyName < 2; iBodyName++)
2015 {
2016 string bodyName = joint.BodyNames[iBodyName];
2017 if (bodyName != "NULL")
2018 {
2019 if (!joints_connecting_actor.ContainsKey(bodyName))
2020 {
2021 joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
2022 }
2023 joints_connecting_actor[bodyName].Add(joint);
2024 }
2025 }
2026 }
2027 }
1390 } 2028 }
1391 2029
2030 // remove processed joints from shared list
2031 lock (externalJointRequestsLock)
2032 {
2033 foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
2034 {
2035 requestedJointsToBeCreated.Remove(joint);
2036 }
2037 }
1392 } 2038 }
1393 2039
1394 public void addToPrims(OdePrim prim) 2040 /// <summary>
2041 /// Add a request for joint creation.
2042 /// </summary>
2043 /// <remarks>
2044 /// this joint will just be added to a waiting list that is NOT processed during the main
2045 /// Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
2046 /// </remarks>
2047 /// <param name="objectNameInScene"></param>
2048 /// <param name="jointType"></param>
2049 /// <param name="position"></param>
2050 /// <param name="rotation"></param>
2051 /// <param name="parms"></param>
2052 /// <param name="bodyNames"></param>
2053 /// <param name="trackedBodyName"></param>
2054 /// <param name="localRotation"></param>
2055 /// <returns></returns>
2056 public override PhysicsJoint RequestJointCreation(
2057 string objectNameInScene, PhysicsJointType jointType, Vector3 position,
2058 Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
1395 { 2059 {
1396 lock (_prims) 2060 OdePhysicsJoint joint = new OdePhysicsJoint();
1397 _prims[prim.LocalID] = prim; 2061 joint.ObjectNameInScene = objectNameInScene;
2062 joint.Type = jointType;
2063 joint.Position = position;
2064 joint.Rotation = rotation;
2065 joint.RawParams = parms;
2066 joint.BodyNames = new List<string>(bodyNames);
2067 joint.TrackedBodyName = trackedBodyName;
2068 joint.LocalRotation = localRotation;
2069 joint.jointID = IntPtr.Zero;
2070 joint.ErrorMessageCount = 0;
2071
2072 lock (externalJointRequestsLock)
2073 {
2074 if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice
2075 {
2076 requestedJointsToBeCreated.Add(joint);
2077 }
2078 }
2079
2080 return joint;
1398 } 2081 }
1399 2082
1400 public OdePrim getPrim(uint id) 2083 private void RemoveAllJointsConnectedToActor(PhysicsActor actor)
1401 { 2084 {
1402 lock (_prims) 2085 //m_log.Debug("RemoveAllJointsConnectedToActor: start");
2086 if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
1403 { 2087 {
1404 if(_prims.ContainsKey(id)) 2088 List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
1405 return _prims[id]; 2089 //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)
1406 else 2090 foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
1407 return null; 2091 {
2092 jointsToRemove.Add(j);
2093 }
2094 foreach (PhysicsJoint j in jointsToRemove)
2095 {
2096 //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
2097 RequestJointDeletion(j.ObjectNameInScene);
2098 //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
2099 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)
2100 }
1408 } 2101 }
1409 } 2102 }
1410 2103
1411 public bool havePrim(OdePrim prm) 2104 public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
1412 { 2105 {
1413 lock (_prims) 2106 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start");
1414 return _prims.ContainsKey(prm.LocalID); 2107 lock (OdeLock)
2108 {
2109 //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
2110 RemoveAllJointsConnectedToActor(actor);
2111 }
1415 } 2112 }
1416 2113
1417 public void changePrimID(OdePrim prim,uint oldID) 2114 // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2115 public override Vector3 GetJointAnchor(PhysicsJoint joint)
1418 { 2116 {
1419 lock (_prims) 2117 Debug.Assert(joint.IsInPhysicsEngine);
2118 d.Vector3 pos = new d.Vector3();
2119
2120 if (!(joint is OdePhysicsJoint))
2121 {
2122 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
2123 }
2124 else
1420 { 2125 {
1421 if(_prims.ContainsKey(oldID)) 2126 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
1422 _prims.Remove(oldID); 2127 switch (odeJoint.Type)
1423 _prims[prim.LocalID] = prim; 2128 {
2129 case PhysicsJointType.Ball:
2130 d.JointGetBallAnchor(odeJoint.jointID, out pos);
2131 break;
2132 case PhysicsJointType.Hinge:
2133 d.JointGetHingeAnchor(odeJoint.jointID, out pos);
2134 break;
2135 }
1424 } 2136 }
2137 return new Vector3(pos.X, pos.Y, pos.Z);
1425 } 2138 }
1426 2139
1427 public bool haveActor(PhysicsActor actor) 2140 /// <summary>
2141 /// Get joint axis.
2142 /// </summary>
2143 /// <remarks>
2144 /// normally called from within OnJointMoved, which is called from within a lock (OdeLock)
2145 /// WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
2146 /// appears to be unreliable. Fortunately we can compute the joint axis ourselves by
2147 /// keeping track of the joint's original orientation relative to one of the involved bodies.
2148 /// </remarks>
2149 /// <param name="joint"></param>
2150 /// <returns></returns>
2151 public override Vector3 GetJointAxis(PhysicsJoint joint)
1428 { 2152 {
1429 if (actor is OdePrim) 2153 Debug.Assert(joint.IsInPhysicsEngine);
2154 d.Vector3 axis = new d.Vector3();
2155
2156 if (!(joint is OdePhysicsJoint))
1430 { 2157 {
1431 lock (_prims) 2158 DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
1432 return _prims.ContainsKey(((OdePrim)actor).LocalID);
1433 } 2159 }
1434 else if (actor is OdeCharacter) 2160 else
2161 {
2162 OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
2163 switch (odeJoint.Type)
2164 {
2165 case PhysicsJointType.Ball:
2166 DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
2167 break;
2168 case PhysicsJointType.Hinge:
2169 d.JointGetHingeAxis(odeJoint.jointID, out axis);
2170 break;
2171 }
2172 }
2173 return new Vector3(axis.X, axis.Y, axis.Z);
2174 }
2175
2176 /// <summary>
2177 /// Stop this prim being subject to physics
2178 /// </summary>
2179 /// <param name="prim"></param>
2180 internal void DeactivatePrim(OdePrim prim)
2181 {
2182 _activeprims.Remove(prim);
2183 }
2184
2185 public override void RemovePrim(PhysicsActor prim)
2186 {
2187 // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
2188 // removed in the next physics simulate pass.
2189 if (prim is OdePrim)
2190 {
2191 lock (OdeLock)
2192 {
2193 OdePrim p = (OdePrim) prim;
2194
2195 p.setPrimForRemoval();
2196 AddPhysicsActorTaint(prim);
2197 }
2198 }
2199 }
2200
2201 /// <summary>
2202 /// This is called from within simulate but outside the locked portion
2203 /// We need to do our own locking here
2204 /// (Note: As of 20110801 this no longer appears to be true - this is being called within lock (odeLock) in
2205 /// Simulate() -- justincc).
2206 ///
2207 /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
2208 ///
2209 /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
2210 /// that the space was using.
2211 /// </summary>
2212 /// <param name="prim"></param>
2213 internal void RemovePrimThreadLocked(OdePrim prim)
2214 {
2215// m_log.DebugFormat("[ODE SCENE]: Removing physical prim {0} {1}", prim.Name, prim.LocalID);
2216
2217 lock (prim)
1435 { 2218 {
1436 lock (_characters) 2219 RemoveCollisionEventReporting(prim);
1437 return _characters.Contains((OdeCharacter)actor); 2220
2221 if (prim.prim_geom != IntPtr.Zero)
2222 {
2223 prim.ResetTaints();
2224
2225 if (prim.IsPhysical)
2226 {
2227 prim.disableBody();
2228 if (prim.childPrim)
2229 {
2230 prim.childPrim = false;
2231 prim.Body = IntPtr.Zero;
2232 prim.m_disabled = true;
2233 prim.IsPhysical = false;
2234 }
2235
2236
2237 }
2238 prim.m_targetSpace = IntPtr.Zero;
2239 if (!prim.RemoveGeom())
2240 m_log.Warn("[ODE SCENE]: Unable to remove prim from physics scene");
2241
2242 lock (_prims)
2243 _prims.Remove(prim);
2244
2245
2246 if (SupportsNINJAJoints)
2247 RemoveAllJointsConnectedToActorThreadLocked(prim);
2248 }
1438 } 2249 }
1439 return false;
1440 } 2250 }
1441 2251
1442 #endregion 2252 #endregion
@@ -1444,126 +2254,368 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1444 #region Space Separation Calculation 2254 #region Space Separation Calculation
1445 2255
1446 /// <summary> 2256 /// <summary>
1447 /// Called when a static prim moves or becomes static 2257 /// Takes a space pointer and zeros out the array we're using to hold the spaces
1448 /// Places the prim in a space one the static sub-spaces grid 2258 /// </summary>
2259 /// <param name="pSpace"></param>
2260 private void resetSpaceArrayItemToZero(IntPtr pSpace)
2261 {
2262 for (int x = 0; x < staticPrimspace.GetLength(0); x++)
2263 {
2264 for (int y = 0; y < staticPrimspace.GetLength(1); y++)
2265 {
2266 if (staticPrimspace[x, y] == pSpace)
2267 staticPrimspace[x, y] = IntPtr.Zero;
2268 }
2269 }
2270 }
2271
2272// private void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY)
2273// {
2274// staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero;
2275// }
2276
2277 /// <summary>
2278 /// Called when a static prim moves. Allocates a space for the prim based on its position
1449 /// </summary> 2279 /// </summary>
1450 /// <param name="geom">the pointer to the geom that moved</param> 2280 /// <param name="geom">the pointer to the geom that moved</param>
1451 /// <param name="pos">the position that the geom moved to</param> 2281 /// <param name="pos">the position that the geom moved to</param>
1452 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param> 2282 /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
1453 /// <returns>a pointer to the new space it's in</returns> 2283 /// <returns>a pointer to the new space it's in</returns>
1454 public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace) 2284 internal IntPtr recalculateSpaceForGeom(IntPtr geom, Vector3 pos, IntPtr currentspace)
1455 { 2285 {
1456 // moves a prim into another static sub-space or from another space into a static sub-space 2286 // Called from setting the Position and Size of an ODEPrim so
1457
1458 // Called ODEPrim so
1459 // it's already in locked space. 2287 // it's already in locked space.
1460 2288
1461 if (geom == IntPtr.Zero) // shouldn't happen 2289 // we don't want to remove the main space
1462 return IntPtr.Zero; 2290 // we don't need to test physical here because this function should
1463 2291 // never be called if the prim is physical(active)
1464 // get the static sub-space for current position
1465 IntPtr newspace = calculateSpaceForGeom(pos);
1466
1467 if (newspace == currentspace) // if we are there all done
1468 return newspace;
1469 2292
1470 // else remove it from its current space 2293 // All physical prim end up in the root space
1471 if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom)) 2294 //Thread.Sleep(20);
2295 if (currentspace != space)
1472 { 2296 {
1473 if (d.GeomIsSpace(currentspace)) 2297 //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString());
2298 //if (currentspace == IntPtr.Zero)
2299 //{
2300 //int adfadf = 0;
2301 //}
2302 if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero)
1474 { 2303 {
1475 waitForSpaceUnlock(currentspace); 2304 if (d.GeomIsSpace(currentspace))
1476 d.SpaceRemove(currentspace, geom); 2305 {
1477 2306// waitForSpaceUnlock(currentspace);
1478 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) 2307 d.SpaceRemove(currentspace, geom);
2308 }
2309 else
1479 { 2310 {
1480 d.SpaceDestroy(currentspace); 2311 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" + currentspace +
2312 " Geom:" + geom);
1481 } 2313 }
1482 } 2314 }
1483 else 2315 else
1484 { 2316 {
1485 m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace + 2317 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
1486 " Geom:" + geom); 2318 if (sGeomIsIn != IntPtr.Zero)
2319 {
2320 if (d.GeomIsSpace(currentspace))
2321 {
2322// waitForSpaceUnlock(sGeomIsIn);
2323 d.SpaceRemove(sGeomIsIn, geom);
2324 }
2325 else
2326 {
2327 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2328 sGeomIsIn + " Geom:" + geom);
2329 }
2330 }
2331 }
2332
2333 //If there are no more geometries in the sub-space, we don't need it in the main space anymore
2334 if (d.SpaceGetNumGeoms(currentspace) == 0)
2335 {
2336 if (currentspace != IntPtr.Zero)
2337 {
2338 if (d.GeomIsSpace(currentspace))
2339 {
2340 d.SpaceRemove(space, currentspace);
2341 // free up memory used by the space.
2342
2343 resetSpaceArrayItemToZero(currentspace);
2344 }
2345 else
2346 {
2347 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2348 currentspace + " Geom:" + geom);
2349 }
2350 }
1487 } 2351 }
1488 } 2352 }
1489 else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space 2353 else
1490 { 2354 {
1491 currentspace = d.GeomGetSpace(geom); 2355 // this is a physical object that got disabled. ;.;
1492 if (currentspace != IntPtr.Zero) 2356 if (currentspace != IntPtr.Zero && geom != IntPtr.Zero)
1493 { 2357 {
1494 if (d.GeomIsSpace(currentspace)) 2358 if (d.SpaceQuery(currentspace, geom))
1495 { 2359 {
1496 waitForSpaceUnlock(currentspace); 2360 if (d.GeomIsSpace(currentspace))
1497 d.SpaceRemove(currentspace, geom);
1498
1499 if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0)
1500 { 2361 {
1501 d.SpaceDestroy(currentspace); 2362// waitForSpaceUnlock(currentspace);
2363 d.SpaceRemove(currentspace, geom);
2364 }
2365 else
2366 {
2367 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2368 currentspace + " Geom:" + geom);
2369 }
2370 }
2371 else
2372 {
2373 IntPtr sGeomIsIn = d.GeomGetSpace(geom);
2374 if (sGeomIsIn != IntPtr.Zero)
2375 {
2376 if (d.GeomIsSpace(sGeomIsIn))
2377 {
2378// waitForSpaceUnlock(sGeomIsIn);
2379 d.SpaceRemove(sGeomIsIn, geom);
2380 }
2381 else
2382 {
2383 m_log.Info("[ODE SCENE]: Invalid Scene passed to 'recalculatespace':" +
2384 sGeomIsIn + " Geom:" + geom);
2385 }
1502 } 2386 }
1503
1504 } 2387 }
1505 } 2388 }
1506 } 2389 }
1507 2390
1508 // put the geom in the newspace 2391 // The routines in the Position and Size sections do the 'inserting' into the space,
1509 waitForSpaceUnlock(newspace); 2392 // so all we have to do is make sure that the space that we're putting the prim into
1510 d.SpaceAdd(newspace, geom); 2393 // is in the 'main' space.
2394 int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos);
2395 IntPtr newspace = calculateSpaceForGeom(pos);
2396
2397 if (newspace == IntPtr.Zero)
2398 {
2399 newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
2400 d.HashSpaceSetLevels(newspace, HashspaceLow, HashspaceHigh);
2401 }
1511 2402
1512 // let caller know this newspace
1513 return newspace; 2403 return newspace;
1514 } 2404 }
1515 2405
1516 /// <summary> 2406 /// <summary>
2407 /// Creates a new space at X Y
2408 /// </summary>
2409 /// <param name="iprimspaceArrItemX"></param>
2410 /// <param name="iprimspaceArrItemY"></param>
2411 /// <returns>A pointer to the created space</returns>
2412 internal IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
2413 {
2414 // creating a new space for prim and inserting it into main space.
2415 staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero);
2416 d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space);
2417// waitForSpaceUnlock(space);
2418 d.SpaceSetSublevel(space, 1);
2419 d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
2420
2421 return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
2422 }
2423
2424 /// <summary>
1517 /// Calculates the space the prim should be in by its position 2425 /// Calculates the space the prim should be in by its position
1518 /// </summary> 2426 /// </summary>
1519 /// <param name="pos"></param> 2427 /// <param name="pos"></param>
1520 /// <returns>a pointer to the space. This could be a new space or reused space.</returns> 2428 /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
1521 public IntPtr calculateSpaceForGeom(Vector3 pos) 2429 internal IntPtr calculateSpaceForGeom(Vector3 pos)
1522 { 2430 {
1523 int x, y; 2431 int[] xyspace = calculateSpaceArrayItemFromPos(pos);
2432 //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
2433 return staticPrimspace[xyspace[0], xyspace[1]];
2434 }
1524 2435
1525 if (pos.X < 0) 2436 /// <summary>
1526 return staticPrimspaceOffRegion[0]; 2437 /// Holds the space allocation logic
2438 /// </summary>
2439 /// <param name="pos"></param>
2440 /// <returns>an array item based on the position</returns>
2441 internal int[] calculateSpaceArrayItemFromPos(Vector3 pos)
2442 {
2443 int[] returnint = new int[2];
1527 2444
1528 if (pos.Y < 0) 2445 returnint[0] = (int) (pos.X * spacesPerMeterX);
1529 return staticPrimspaceOffRegion[2];
1530 2446
1531 x = (int)(pos.X * spacesPerMeterX); 2447 if (returnint[0] > spaceGridMaxX)
1532 if (x > spaceGridMaxX) 2448 returnint[0] = spaceGridMaxX;
1533 return staticPrimspaceOffRegion[1]; 2449 if (returnint[0] < 0)
1534 2450 returnint[0] = 0;
1535 y = (int)(pos.Y * spacesPerMeterY); 2451
1536 if (y > spaceGridMaxY) 2452 returnint[1] = (int)(pos.Y * spacesPerMeterY);
1537 return staticPrimspaceOffRegion[3]; 2453 if (returnint[1] > spaceGridMaxY)
2454 returnint[1] = spaceGridMaxY;
2455 if (returnint[1] < 0)
2456 returnint[1] = 0;
1538 2457
1539 return staticPrimspace[x, y]; 2458 return returnint;
1540 } 2459 }
1541
1542 #endregion
1543 2460
2461 #endregion
1544 2462
1545 /// <summary> 2463 /// <summary>
1546 /// Called to queue a change to a actor 2464 /// Routine to figure out if we need to mesh this prim with our mesher
1547 /// to use in place of old taint mechanism so changes do have a time sequence
1548 /// </summary> 2465 /// </summary>
1549 2466 /// <param name="pbs"></param>
1550 public void AddChange(PhysicsActor actor, changes what, Object arg) 2467 /// <returns></returns>
2468 internal bool needsMeshing(PrimitiveBaseShape pbs)
1551 { 2469 {
1552 ODEchangeitem item = new ODEchangeitem(); 2470 // most of this is redundant now as the mesher will return null if it cant mesh a prim
1553 item.actor = actor; 2471 // but we still need to check for sculptie meshing being enabled so this is the most
1554 item.what = what; 2472 // convenient place to do it for now...
1555 item.arg = arg; 2473
1556 ChangesQueue.Enqueue(item); 2474 // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
2475 // //m_log.Debug("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
2476 int iPropertiesNotSupportedDefault = 0;
2477
2478 if (pbs.SculptEntry && !meshSculptedPrim)
2479 {
2480#if SPAM
2481 m_log.Warn("NonMesh");
2482#endif
2483 return false;
2484 }
2485
2486 // 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
2487 if (!forceSimplePrimMeshing && !pbs.SculptEntry)
2488 {
2489 if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
2490 || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
2491 && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
2492 {
2493
2494 if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
2495 && pbs.ProfileHollow == 0
2496 && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
2497 && pbs.PathBegin == 0 && pbs.PathEnd == 0
2498 && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
2499 && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
2500 && pbs.PathShearX == 0 && pbs.PathShearY == 0)
2501 {
2502#if SPAM
2503 m_log.Warn("NonMesh");
2504#endif
2505 return false;
2506 }
2507 }
2508 }
2509
2510 if (pbs.ProfileHollow != 0)
2511 iPropertiesNotSupportedDefault++;
2512
2513 if ((pbs.PathBegin != 0) || pbs.PathEnd != 0)
2514 iPropertiesNotSupportedDefault++;
2515
2516 if ((pbs.PathTwistBegin != 0) || (pbs.PathTwist != 0))
2517 iPropertiesNotSupportedDefault++;
2518
2519 if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
2520 iPropertiesNotSupportedDefault++;
2521
2522 if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
2523 iPropertiesNotSupportedDefault++;
2524
2525 if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
2526 iPropertiesNotSupportedDefault++;
2527
2528 if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
2529 iPropertiesNotSupportedDefault++;
2530
2531 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
2532 iPropertiesNotSupportedDefault++;
2533
2534 if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
2535 iPropertiesNotSupportedDefault++;
2536
2537 // test for torus
2538 if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
2539 {
2540 if (pbs.PathCurve == (byte)Extrusion.Curve1)
2541 {
2542 iPropertiesNotSupportedDefault++;
2543 }
2544 }
2545 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
2546 {
2547 if (pbs.PathCurve == (byte)Extrusion.Straight)
2548 {
2549 iPropertiesNotSupportedDefault++;
2550 }
2551
2552 // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
2553 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2554 {
2555 iPropertiesNotSupportedDefault++;
2556 }
2557 }
2558 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
2559 {
2560 if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
2561 {
2562 iPropertiesNotSupportedDefault++;
2563 }
2564 }
2565 else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
2566 {
2567 if (pbs.PathCurve == (byte)Extrusion.Straight)
2568 {
2569 iPropertiesNotSupportedDefault++;
2570 }
2571 else if (pbs.PathCurve == (byte)Extrusion.Curve1)
2572 {
2573 iPropertiesNotSupportedDefault++;
2574 }
2575 }
2576
2577 if (pbs.SculptEntry && meshSculptedPrim)
2578 iPropertiesNotSupportedDefault++;
2579
2580 if (iPropertiesNotSupportedDefault == 0)
2581 {
2582#if SPAM
2583 m_log.Warn("NonMesh");
2584#endif
2585 return false;
2586 }
2587#if SPAM
2588 m_log.Debug("Mesh");
2589#endif
2590 return true;
1557 } 2591 }
1558 2592
1559 /// <summary> 2593 /// <summary>
1560 /// Called after our prim properties are set Scale, position etc. 2594 /// Called after our prim properties are set Scale, position etc.
2595 /// </summary>
2596 /// <remarks>
1561 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex 2597 /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
1562 /// This assures us that we have no race conditions 2598 /// This assures us that we have no race conditions
1563 /// </summary> 2599 /// </remarks>
1564 /// <param name="prim"></param> 2600 /// <param name="actor"></param>
1565 public override void AddPhysicsActorTaint(PhysicsActor prim) 2601 public override void AddPhysicsActorTaint(PhysicsActor actor)
1566 { 2602 {
2603 if (actor is OdePrim)
2604 {
2605 OdePrim taintedprim = ((OdePrim)actor);
2606 lock (_taintedPrims)
2607 _taintedPrims.Add(taintedprim);
2608 }
2609 else if (actor is OdeCharacter)
2610 {
2611 OdeCharacter taintedchar = ((OdeCharacter)actor);
2612 lock (_taintedActors)
2613 {
2614 _taintedActors.Add(taintedchar);
2615 if (taintedchar.bad)
2616 m_log.ErrorFormat("[ODE SCENE]: Added BAD actor {0} to tainted actors", taintedchar.m_uuid);
2617 }
2618 }
1567 } 2619 }
1568 2620
1569 // does all pending changes generated during region load process 2621 // does all pending changes generated during region load process
@@ -1573,315 +2625,319 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1573 { 2625 {
1574 if (world == IntPtr.Zero) 2626 if (world == IntPtr.Zero)
1575 { 2627 {
1576 ChangesQueue.Clear(); 2628 _taintedPrims.Clear();;
1577 return; 2629 return;
1578 } 2630 }
1579 2631
1580 d.AllocateODEDataForThread(~0U);
1581
1582 ODEchangeitem item;
1583
1584 int donechanges = 0; 2632 int donechanges = 0;
1585 if (ChangesQueue.Count > 0) 2633 if (_taintedPrims.Count > 0)
1586 { 2634 {
1587 m_log.InfoFormat("[ubOde] start processing pending actor operations"); 2635
2636 m_log.InfoFormat("[Ode] start processing pending actor operations");
1588 int tstart = Util.EnvironmentTickCount(); 2637 int tstart = Util.EnvironmentTickCount();
1589 2638
1590 while (ChangesQueue.Dequeue(out item)) 2639 d.AllocateODEDataForThread(0);
2640
2641 lock (_taintedPrims)
1591 { 2642 {
1592 if (item.actor != null) 2643 foreach (OdePrim prim in _taintedPrims)
1593 { 2644 {
1594 try 2645 if (prim.m_taintremove)
1595 { 2646 RemovePrimThreadLocked(prim);
1596 if (item.actor is OdeCharacter) 2647 else
1597 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); 2648 prim.ProcessTaints();
1598 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) 2649
1599 RemovePrimThreadLocked((OdePrim)item.actor); 2650 prim.m_collisionscore = 0;
1600 } 2651 donechanges++;
1601 catch
1602 {
1603 m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}",
1604 item.actor.Name, item.what.ToString());
1605 }
1606 } 2652 }
1607 donechanges++; 2653 _taintedPrims.Clear();
1608 } 2654 }
2655
1609 int time = Util.EnvironmentTickCountSubtract(tstart); 2656 int time = Util.EnvironmentTickCountSubtract(tstart);
1610 m_log.InfoFormat("[ubOde] finished {0} operations in {1}ms", donechanges, time); 2657 m_log.InfoFormat("[Ode] finished {0} operations in {1}ms", donechanges, time);
1611 } 2658 }
1612 m_log.InfoFormat("[ubOde] {0} prim actors loaded",_prims.Count); 2659 m_log.InfoFormat("[Ode] {0} prim actors loaded",_prims.Count);
1613 } 2660 }
1614 } 2661 }
1615 2662
2663
1616 /// <summary> 2664 /// <summary>
1617 /// This is our main simulate loop 2665 /// This is our main simulate loop
2666 /// </summary>
2667 /// <remarks>
1618 /// It's thread locked by a Mutex in the scene. 2668 /// It's thread locked by a Mutex in the scene.
1619 /// It holds Collisions, it instructs ODE to step through the physical reactions 2669 /// It holds Collisions, it instructs ODE to step through the physical reactions
1620 /// It moves the objects around in memory 2670 /// It moves the objects around in memory
1621 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) 2671 /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
1622 /// </summary> 2672 /// </remarks>
1623 /// <param name="timeStep"></param> 2673 /// <param name="timeStep"></param>
1624 /// <returns></returns> 2674 /// <returns>The number of frames simulated over that period.</returns>
1625 public override float Simulate(float reqTimeStep) 2675 public override float Simulate(float timeStep)
1626 { 2676 {
1627 DateTime now = DateTime.UtcNow; 2677 if (!_worldInitialized)
1628 TimeSpan timedif = now - m_lastframe; 2678 return 1.0f;
1629 float timeStep = (float)timedif.TotalSeconds;
1630 m_lastframe = now;
1631
1632 // acumulate time so we can reduce error
1633 step_time += timeStep;
1634 2679
1635 if (step_time < HalfOdeStep) 2680 int startFrameTick = CollectStats ? Util.EnvironmentTickCount() : 0;
1636 return 0; 2681 int tempTick = 0, tempTick2 = 0;
1637 2682
1638 if (framecount < 0) 2683 if (framecount >= int.MaxValue)
1639 framecount = 0; 2684 framecount = 0;
1640 2685
1641 framecount++; 2686 framecount++;
1642 2687
1643// checkThread();
1644 int nodeframes = 0;
1645 float fps = 0; 2688 float fps = 0;
1646 2689
1647 lock (SimulationLock) 2690 step_time += timeStep;
1648 lock(OdeLock) 2691
2692 float HalfOdeStep = ODE_STEPSIZE * 0.5f;
2693 if (step_time < HalfOdeStep)
2694 return 0;
2695
2696
2697 // We change _collisionEventPrimChanges to avoid locking _collisionEventPrim itself and causing potential
2698 // deadlock if the collision event tries to lock something else later on which is already locked by a
2699 // caller that is adding or removing the collision event.
2700 lock (m_collisionEventActorsChanges)
1649 { 2701 {
1650 if (world == IntPtr.Zero) 2702 foreach (KeyValuePair<uint, PhysicsActor> kvp in m_collisionEventActorsChanges)
1651 { 2703 {
1652 ChangesQueue.Clear(); 2704 if (kvp.Value == null)
1653 return 0; 2705 m_collisionEventActors.Remove(kvp.Key);
2706 else
2707 m_collisionEventActors[kvp.Key] = kvp.Value;
1654 } 2708 }
1655 2709
1656 ODEchangeitem item; 2710 m_collisionEventActorsChanges.Clear();
1657 2711 }
1658// d.WorldSetQuickStepNumIterations(world, curphysiteractions); 2712
1659 2713 if (SupportsNINJAJoints)
1660 int loopstartMS = Util.EnvironmentTickCount(); 2714 {
1661 int looptimeMS = 0; 2715 DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
1662 int changestimeMS = 0; 2716 CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
1663 int maxChangestime = (int)(reqTimeStep * 500f); // half the time 2717 }
1664 int maxLoopTime = (int)(reqTimeStep * 1200f); // 1.2 the time 2718
1665 2719
2720 lock (OdeLock)
2721 {
1666 d.AllocateODEDataForThread(~0U); 2722 d.AllocateODEDataForThread(~0U);
1667
1668 if (ChangesQueue.Count > 0)
1669 {
1670 while (ChangesQueue.Dequeue(out item))
1671 {
1672 if (item.actor != null)
1673 {
1674 try
1675 {
1676 if (item.actor is OdeCharacter)
1677 ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
1678 else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
1679 RemovePrimThreadLocked((OdePrim)item.actor);
1680 }
1681 catch
1682 {
1683 m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}",
1684 item.actor.Name, item.what.ToString());
1685 }
1686 }
1687 changestimeMS = Util.EnvironmentTickCountSubtract(loopstartMS);
1688 if (changestimeMS > maxChangestime)
1689 break;
1690 }
1691 }
1692 2723
1693 // do simulation taking at most 150ms total time including changes 2724 while (step_time > HalfOdeStep)
1694 while (step_time > HalfOdeStep)
1695 { 2725 {
1696 try 2726 try
1697 { 2727 {
1698 // clear pointer/counter to contacts to pass into joints 2728 if (CollectStats)
1699 m_global_contactcount = 0; 2729 tempTick = Util.EnvironmentTickCount();
1700 2730
2731 lock (_taintedActors)
2732 {
2733 foreach (OdeCharacter character in _taintedActors)
2734 character.ProcessTaints();
1701 2735
1702 // Move characters 2736 _taintedActors.Clear();
1703 lock (_characters) 2737 }
2738
2739 if (CollectStats)
1704 { 2740 {
1705 List<OdeCharacter> defects = new List<OdeCharacter>(); 2741 tempTick2 = Util.EnvironmentTickCount();
1706 foreach (OdeCharacter actor in _characters) 2742 m_stats[ODEAvatarTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
1707 { 2743 tempTick = tempTick2;
1708 if (actor != null) 2744 }
1709 actor.Move(defects); 2745
1710 } 2746 lock (_taintedPrims)
1711 if (defects.Count != 0) 2747 {
2748 foreach (OdePrim prim in _taintedPrims)
1712 { 2749 {
1713 foreach (OdeCharacter defect in defects) 2750 if (prim.m_taintremove)
2751 {
2752// Console.WriteLine("Simulate calls RemovePrimThreadLocked for {0}", prim.Name);
2753 RemovePrimThreadLocked(prim);
2754 }
2755 else
1714 { 2756 {
1715 RemoveCharacter(defect); 2757// Console.WriteLine("Simulate calls ProcessTaints for {0}", prim.Name);
2758 prim.ProcessTaints();
1716 } 2759 }
1717 defects.Clear(); 2760
2761 prim.m_collisionscore = 0;
2762
2763 // This loop can block up the Heartbeat for a very long time on large regions.
2764 // We need to let the Watchdog know that the Heartbeat is not dead
2765 // NOTE: This is currently commented out, but if things like OAR loading are
2766 // timing the heartbeat out we will need to uncomment it
2767 //Watchdog.UpdateThread();
1718 } 2768 }
2769
2770 if (SupportsNINJAJoints)
2771 SimulatePendingNINJAJoints();
2772
2773 _taintedPrims.Clear();
1719 } 2774 }
1720 2775
1721 // Move other active objects 2776 if (CollectStats)
1722 lock (_activegroups)
1723 { 2777 {
1724 foreach (OdePrim aprim in _activegroups) 2778 tempTick2 = Util.EnvironmentTickCount();
2779 m_stats[ODEPrimTaintMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2780 tempTick = tempTick2;
2781 }
2782
2783 // Move characters
2784 foreach (OdeCharacter actor in _characters)
2785 actor.Move(defects);
2786
2787 if (defects.Count != 0)
2788 {
2789 foreach (OdeCharacter actor in defects)
1725 { 2790 {
1726 aprim.Move(); 2791 m_log.ErrorFormat(
2792 "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when moving",
2793 actor.Name, actor.LocalID, PhysicsSceneName);
2794
2795 RemoveCharacter(actor);
2796 actor.DestroyOdeStructures();
1727 } 2797 }
2798
2799 defects.Clear();
2800 }
2801
2802 if (CollectStats)
2803 {
2804 tempTick2 = Util.EnvironmentTickCount();
2805 m_stats[ODEAvatarForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2806 tempTick = tempTick2;
2807 }
2808
2809 // Move other active objects
2810 foreach (OdePrim prim in _activeprims)
2811 {
2812 prim.m_collisionscore = 0;
2813 prim.Move(timeStep);
2814 }
2815
2816 if (CollectStats)
2817 {
2818 tempTick2 = Util.EnvironmentTickCount();
2819 m_stats[ODEPrimForcesFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2820 tempTick = tempTick2;
1728 } 2821 }
1729 2822
1730 m_rayCastManager.ProcessQueuedRequests(); 2823 m_rayCastManager.ProcessQueuedRequests();
1731 2824
2825 if (CollectStats)
2826 {
2827 tempTick2 = Util.EnvironmentTickCount();
2828 m_stats[ODERaycastingFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2829 tempTick = tempTick2;
2830 }
2831
1732 collision_optimized(); 2832 collision_optimized();
1733 List<OdePrim> sleepers = new List<OdePrim>();
1734 2833
1735 foreach (PhysicsActor obj in _collisionEventPrim) 2834 if (CollectStats)
1736 { 2835 {
1737 if (obj == null) 2836 tempTick2 = Util.EnvironmentTickCount();
1738 continue; 2837 m_stats[ODEOtherCollisionFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2838 tempTick = tempTick2;
2839 }
2840
2841 foreach (PhysicsActor obj in m_collisionEventActors.Values)
2842 {
2843 // m_log.DebugFormat("[PHYSICS]: Assessing {0} {1} for collision events", obj.SOPName, obj.LocalID);
1739 2844
1740 switch ((ActorTypes)obj.PhysicsActorType) 2845 switch ((ActorTypes)obj.PhysicsActorType)
1741 { 2846 {
1742 case ActorTypes.Agent: 2847 case ActorTypes.Agent:
1743 OdeCharacter cobj = (OdeCharacter)obj; 2848 OdeCharacter cobj = (OdeCharacter)obj;
1744 cobj.SendCollisions((int)(odetimestepMS)); 2849 cobj.AddCollisionFrameTime(100);
2850 cobj.SendCollisions();
1745 break; 2851 break;
1746 2852
1747 case ActorTypes.Prim: 2853 case ActorTypes.Prim:
1748 OdePrim pobj = (OdePrim)obj; 2854 OdePrim pobj = (OdePrim)obj;
1749 if (!pobj.m_outbounds) 2855 pobj.SendCollisions();
1750 {
1751 pobj.SendCollisions((int)(odetimestepMS));
1752 if(pobj.Body != IntPtr.Zero && !pobj.m_isSelected &&
1753 !pobj.m_disabled && !pobj.m_building &&
1754 !d.BodyIsEnabled(pobj.Body))
1755 sleepers.Add(pobj);
1756 }
1757 break; 2856 break;
1758 } 2857 }
1759 } 2858 }
1760 2859
1761 foreach(OdePrim prm in sleepers) 2860// if (m_global_contactcount > 0)
1762 prm.SleeperAddCollisionEvents(); 2861// m_log.DebugFormat(
1763 sleepers.Clear(); 2862// "[PHYSICS]: Collision contacts to process this frame = {0}", m_global_contactcount);
1764 2863
1765 lock(_collisionEventPrimRemove) 2864 m_global_contactcount = 0;
1766 {
1767 foreach (PhysicsActor obj in _collisionEventPrimRemove)
1768 _collisionEventPrim.Remove(obj);
1769 2865
1770 _collisionEventPrimRemove.Clear(); 2866 if (CollectStats)
2867 {
2868 tempTick2 = Util.EnvironmentTickCount();
2869 m_stats[ODECollisionNotificationFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2870 tempTick = tempTick2;
1771 } 2871 }
1772 2872
1773 // do a ode simulation step 2873 lock(SimulationLock)
1774 d.WorldQuickStep(world, ODE_STEPSIZE); 2874 d.WorldQuickStep(world, ODE_STEPSIZE);
1775 d.JointGroupEmpty(contactgroup);
1776
1777 // update managed ideia of physical data and do updates to core
1778 /*
1779 lock (_characters)
1780 {
1781 foreach (OdeCharacter actor in _characters)
1782 {
1783 if (actor != null)
1784 {
1785 if (actor.bad)
1786 m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
1787 2875
1788 actor.UpdatePositionAndVelocity(); 2876 if (CollectStats)
1789 } 2877 m_stats[ODENativeStepFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick);
1790 }
1791 }
1792 */
1793 2878
1794 lock (_activegroups) 2879 d.JointGroupEmpty(contactgroup);
1795 {
1796 {
1797 foreach (OdePrim actor in _activegroups)
1798 {
1799 if (actor.IsPhysical)
1800 {
1801 actor.UpdatePositionAndVelocity(framecount);
1802 }
1803 }
1804 }
1805 }
1806 } 2880 }
1807 catch (Exception e) 2881 catch (Exception e)
1808 { 2882 {
1809 m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); 2883 m_log.ErrorFormat("[ODE SCENE]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
1810// ode.dunlock(world);
1811 } 2884 }
1812 2885
1813 step_time -= ODE_STEPSIZE; 2886 step_time -= ODE_STEPSIZE;
1814 nodeframes++; 2887 fps += ODE_STEPSIZE;
2888 }
1815 2889
1816 looptimeMS = Util.EnvironmentTickCountSubtract(loopstartMS); 2890 if (CollectStats)
1817 if (looptimeMS > maxLoopTime) 2891 tempTick = Util.EnvironmentTickCount();
1818 break; 2892
2893 foreach (OdeCharacter actor in _characters)
2894 {
2895 if (actor.bad)
2896 m_log.ErrorFormat("[ODE SCENE]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
2897
2898 actor.UpdatePositionAndVelocity(defects);
1819 } 2899 }
1820 2900
1821 lock (_badCharacter) 2901 if (defects.Count != 0)
1822 { 2902 {
1823 if (_badCharacter.Count > 0) 2903 foreach (OdeCharacter actor in defects)
1824 { 2904 {
1825 foreach (OdeCharacter chr in _badCharacter) 2905 m_log.ErrorFormat(
1826 { 2906 "[ODE SCENE]: Removing physics character {0} {1} from physics scene {2} due to defect found when updating position and velocity",
1827 RemoveCharacter(chr); 2907 actor.Name, actor.LocalID, PhysicsSceneName);
1828 }
1829 2908
1830 _badCharacter.Clear(); 2909 RemoveCharacter(actor);
2910 actor.DestroyOdeStructures();
1831 } 2911 }
1832 }
1833 2912
1834 timedif = now - m_lastMeshExpire; 2913 defects.Clear();
2914 }
1835 2915
1836 if (timedif.Seconds > 10) 2916 if (CollectStats)
1837 { 2917 {
1838 mesher.ExpireReleaseMeshs(); 2918 tempTick2 = Util.EnvironmentTickCount();
1839 m_lastMeshExpire = now; 2919 m_stats[ODEAvatarUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick2, tempTick);
2920 tempTick = tempTick2;
1840 } 2921 }
1841 2922
1842// information block for in debug breakpoint only 2923 //if (timeStep < 0.2f)
1843/*
1844 int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
1845 int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
1846 int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace);
1847
1848 int nactivegeoms = 0;
1849 int nactivespaces = 0;
1850 2924
1851 int nstaticgeoms = 0; 2925 foreach (OdePrim prim in _activeprims)
1852 int nstaticspaces = 0;
1853 IntPtr sp;
1854
1855 for (int i = 0; i < ntopactivegeoms; i++)
1856 { 2926 {
1857 sp = d.SpaceGetGeom(ActiveSpace, i); 2927 if (prim.IsPhysical && (d.BodyIsEnabled(prim.Body) || !prim._zeroFlag))
1858 if (d.GeomIsSpace(sp))
1859 { 2928 {
1860 nactivespaces++; 2929 prim.UpdatePositionAndVelocity();
1861 nactivegeoms += d.SpaceGetNumGeoms(sp);
1862 }
1863 else
1864 nactivegeoms++;
1865 }
1866 2930
1867 for (int i = 0; i < ntopstaticgeoms; i++) 2931 if (SupportsNINJAJoints)
1868 { 2932 SimulateActorPendingJoints(prim);
1869 sp = d.SpaceGetGeom(StaticSpace, i);
1870 if (d.GeomIsSpace(sp))
1871 {
1872 nstaticspaces++;
1873 nstaticgeoms += d.SpaceGetNumGeoms(sp);
1874 } 2933 }
1875 else
1876 nstaticgeoms++;
1877 } 2934 }
1878 2935
1879 int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); 2936 if (CollectStats)
2937 m_stats[ODEPrimUpdateFrameMsStatName] += Util.EnvironmentTickCountSubtract(tempTick);
2938
2939 //DumpJointInfo();
1880 2940
1881 int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray
1882 int nbodies = d.NTotalBodies;
1883 int ngeoms = d.NTotalGeoms;
1884*/
1885 // Finished with all sim stepping. If requested, dump world state to file for debugging. 2941 // Finished with all sim stepping. If requested, dump world state to file for debugging.
1886 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? 2942 // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
1887 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? 2943 // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
@@ -1900,303 +2956,291 @@ namespace OpenSim.Region.PhysicsModule.ubOde
1900 2956
1901 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); 2957 d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
1902 } 2958 }
1903
1904 fps = (float)nodeframes * ODE_STEPSIZE / reqTimeStep;
1905
1906 if(step_time < HalfOdeStep)
1907 m_timeDilation = 1.0f;
1908 else if (step_time > m_SkipFramesAtms)
1909 {
1910 // if we lag too much skip frames
1911 m_timeDilation = 0.0f;
1912 step_time = 0;
1913 m_lastframe = DateTime.UtcNow; // skip also the time lost
1914 }
1915 else
1916 {
1917 m_timeDilation = ODE_STEPSIZE / step_time;
1918 if (m_timeDilation > 1)
1919 m_timeDilation = 1;
1920 }
1921 }
1922
1923 return fps;
1924 }
1925
1926 /// <summary>
1927 public override void GetResults()
1928 {
1929 }
1930
1931 public override bool IsThreaded
1932 {
1933 // for now we won't be multithreaded
1934 get { return (false); }
1935 }
1936
1937 public float GetTerrainHeightAtXY(float x, float y)
1938 {
1939 if (TerrainGeom == IntPtr.Zero)
1940 return 0f;
1941
1942 if (TerrainHeightFieldHeight == null || TerrainHeightFieldHeight.Length == 0)
1943 return 0f;
1944
1945 // TerrainHeightField for ODE as offset 1m
1946 x += 1f;
1947 y += 1f;
1948
1949 // make position fit into array
1950 if (x < 0)
1951 x = 0;
1952 if (y < 0)
1953 y = 0;
1954 2959
1955 // integer indexs 2960 latertickcount = Util.EnvironmentTickCountSubtract(tickCountFrameRun);
1956 int ix;
1957 int iy;
1958 // interpolators offset
1959 float dx;
1960 float dy;
1961 2961
1962 int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples 2962 // OpenSimulator above does 10 fps. 10 fps = means that the main thread loop and physics
1963 int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples 2963 // has a max of 100 ms to run theoretically.
1964 int regsize = regsizeX; 2964 // If the main loop stalls, it calls Simulate later which makes the tick count ms larger.
2965 // If Physics stalls, it takes longer which makes the tick count ms larger.
1965 2966
1966 if (m_OSOdeLib) 2967 if (latertickcount < 100)
1967 {
1968 if (x < regsizeX - 1)
1969 {
1970 ix = (int)x;
1971 dx = x - (float)ix;
1972 }
1973 else // out world use external height
1974 { 2968 {
1975 ix = regsizeX - 2; 2969 m_timeDilation = 1.0f;
1976 dx = 0;
1977 }
1978 if (y < regsizeY - 1)
1979 {
1980 iy = (int)y;
1981 dy = y - (float)iy;
1982 } 2970 }
1983 else 2971 else
1984 { 2972 {
1985 iy = regsizeY - 2; 2973 m_timeDilation = 100f / latertickcount;
1986 dy = 0; 2974 //m_timeDilation = Math.Min((Math.Max(100 - (Util.EnvironmentTickCount() - tickCountFrameRun), 1) / 100f), 1.0f);
1987 } 2975 }
1988 }
1989 else
1990 {
1991 // we still have square fixed size regions
1992 // also flip x and y because of how map is done for ODE fliped axis
1993 // so ix,iy,dx and dy are inter exchanged
1994 2976
1995 regsize = regsizeY; 2977 tickCountFrameRun = Util.EnvironmentTickCount();
1996 2978
1997 if (x < regsizeX - 1) 2979 if (CollectStats)
1998 { 2980 m_stats[ODETotalFrameMsStatName] += Util.EnvironmentTickCountSubtract(startFrameTick);
1999 iy = (int)x;
2000 dy = x - (float)iy;
2001 }
2002 else // out world use external height
2003 {
2004 iy = regsizeX - 2;
2005 dy = 0;
2006 }
2007 if (y < regsizeY - 1)
2008 {
2009 ix = (int)y;
2010 dx = y - (float)ix;
2011 }
2012 else
2013 {
2014 ix = regsizeY - 2;
2015 dx = 0;
2016 }
2017 } 2981 }
2018 2982
2019 float h0; 2983 fps *= 1.0f/timeStep;
2020 float h1; 2984 return fps;
2021 float h2; 2985 }
2022 2986
2023 iy *= regsize; 2987 /// <summary>
2024 iy += ix; // all indexes have iy + ix 2988 /// Simulate pending NINJA joints.
2989 /// </summary>
2990 /// <remarks>
2991 /// Called by the main Simulate() loop if NINJA joints are active. Should not be called from anywhere else.
2992 /// </remarks>
2993 private void SimulatePendingNINJAJoints()
2994 {
2995 // Create pending joints, if possible
2996
2997 // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
2998 // a joint requires specifying the body id of both involved bodies
2999 if (pendingJoints.Count > 0)
3000 {
3001 List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
3002 //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
3003 foreach (PhysicsJoint joint in pendingJoints)
3004 {
3005 //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
3006 string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries);
3007 List<IntPtr> jointBodies = new List<IntPtr>();
3008 bool allJointBodiesAreReady = true;
3009 foreach (string jointParam in jointParams)
3010 {
3011 if (jointParam == "NULL")
3012 {
3013 //DoJointErrorMessage(joint, "attaching NULL joint to world");
3014 jointBodies.Add(IntPtr.Zero);
3015 }
3016 else
3017 {
3018 //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
3019 bool foundPrim = false;
3020 lock (_prims)
3021 {
3022 foreach (OdePrim prim in _prims) // FIXME: inefficient
3023 {
3024 if (prim.SOPName == jointParam)
3025 {
3026 //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
3027 if (prim.IsPhysical && prim.Body != IntPtr.Zero)
3028 {
3029 jointBodies.Add(prim.Body);
3030 foundPrim = true;
3031 break;
3032 }
3033 else
3034 {
3035 DoJointErrorMessage(joint, "prim name " + jointParam +
3036 " exists but is not (yet) physical; deferring joint creation. " +
3037 "IsPhysical property is " + prim.IsPhysical +
3038 " and body is " + prim.Body);
3039 foundPrim = false;
3040 break;
3041 }
3042 }
3043 }
3044 }
3045 if (foundPrim)
3046 {
3047 // all is fine
3048 }
3049 else
3050 {
3051 allJointBodiesAreReady = false;
3052 break;
3053 }
3054 }
3055 }
2025 3056
2026 float[] heights = TerrainHeightFieldHeight; 3057 if (allJointBodiesAreReady)
2027 /* 3058 {
2028 if ((dx + dy) <= 1.0f) 3059 //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
3060 if (jointBodies[0] == jointBodies[1])
2029 { 3061 {
2030 h0 = ((float)heights[iy]); // 0,0 vertice 3062 DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
2031 h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
2032 h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
2033 } 3063 }
2034 else 3064 else
2035 { 3065 {
2036 h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice 3066 switch (joint.Type)
2037 h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 3067 {
2038 h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 3068 case PhysicsJointType.Ball:
3069 {
3070 IntPtr odeJoint;
3071 //DoJointErrorMessage(joint, "ODE creating ball joint ");
3072 odeJoint = d.JointCreateBall(world, IntPtr.Zero);
3073 //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
3074 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
3075 //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
3076 d.JointSetBallAnchor(odeJoint,
3077 joint.Position.X,
3078 joint.Position.Y,
3079 joint.Position.Z);
3080 //DoJointErrorMessage(joint, "ODE joint setting OK");
3081 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
3082 //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
3083 //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
3084 //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
3085
3086 if (joint is OdePhysicsJoint)
3087 {
3088 ((OdePhysicsJoint)joint).jointID = odeJoint;
3089 }
3090 else
3091 {
3092 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
3093 }
3094 }
3095 break;
3096 case PhysicsJointType.Hinge:
3097 {
3098 IntPtr odeJoint;
3099 //DoJointErrorMessage(joint, "ODE creating hinge joint ");
3100 odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
3101 //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
3102 d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
3103 //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
3104 d.JointSetHingeAnchor(odeJoint,
3105 joint.Position.X,
3106 joint.Position.Y,
3107 joint.Position.Z);
3108 // We use the orientation of the x-axis of the joint's coordinate frame
3109 // as the axis for the hinge.
3110
3111 // Therefore, we must get the joint's coordinate frame based on the
3112 // joint.Rotation field, which originates from the orientation of the
3113 // joint's proxy object in the scene.
3114
3115 // The joint's coordinate frame is defined as the transformation matrix
3116 // that converts a vector from joint-local coordinates into world coordinates.
3117 // World coordinates are defined as the XYZ coordinate system of the sim,
3118 // as shown in the top status-bar of the viewer.
3119
3120 // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
3121 // and use that as the hinge axis.
3122
3123 //joint.Rotation.Normalize();
3124 Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
3125
3126 // Now extract the X axis of the joint's coordinate frame.
3127
3128 // Do not try to use proxyFrame.AtAxis or you will become mired in the
3129 // tar pit of transposed, inverted, and generally messed-up orientations.
3130 // (In other words, Matrix4.AtAxis() is borked.)
3131 // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
3132
3133 // Instead, compute the X axis of the coordinate frame by transforming
3134 // the (1,0,0) vector. At least that works.
3135
3136 //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
3137 Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
3138 //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
3139 //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
3140 d.JointSetHingeAxis(odeJoint,
3141 jointAxis.X,
3142 jointAxis.Y,
3143 jointAxis.Z);
3144 //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
3145 if (joint is OdePhysicsJoint)
3146 {
3147 ((OdePhysicsJoint)joint).jointID = odeJoint;
3148 }
3149 else
3150 {
3151 DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
3152 }
3153 }
3154 break;
3155 }
3156 successfullyProcessedPendingJoints.Add(joint);
2039 } 3157 }
2040 */ 3158 }
2041 h0 = ((float)heights[iy]); // 0,0 vertice 3159 else
3160 {
3161 DoJointErrorMessage(joint, "joint could not yet be created; still pending");
3162 }
3163 }
2042 3164
2043 if (dy>dx) 3165 foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
2044 { 3166 {
2045 iy += regsize; 3167 //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
2046 h2 = (float)heights[iy]; // 0,1 vertice 3168 //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
2047 h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0 3169 InternalRemovePendingJoint(successfullyProcessedJoint);
2048 h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1 3170 //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
2049 } 3171 InternalAddActiveJoint(successfullyProcessedJoint);
2050 else 3172 //DoJointErrorMessage(successfullyProcessedJoint, "done");
2051 { 3173 }
2052 iy++;
2053 h2 = (float)heights[iy]; // vertice 1,0
2054 h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
2055 h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
2056 } 3174 }
2057
2058 return h0 + h1 + h2;
2059 } 3175 }
2060 3176
2061 public Vector3 GetTerrainNormalAtXY(float x, float y) 3177 /// <summary>
3178 /// Simulate the joint proxies of a NINJA actor.
3179 /// </summary>
3180 /// <remarks>
3181 /// Called as part of the Simulate() loop if NINJA physics is active. Must only be called from there.
3182 /// </remarks>
3183 /// <param name="actor"></param>
3184 private void SimulateActorPendingJoints(OdePrim actor)
2062 { 3185 {
2063 Vector3 norm = new Vector3(0, 0, 1); 3186 // If an actor moved, move its joint proxy objects as well.
2064 3187 // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
2065 if (TerrainGeom == IntPtr.Zero) 3188 // for this purpose but it is never called! So we just do the joint
2066 return norm; 3189 // movement code here.
2067
2068 if (TerrainHeightFieldHeight == null || TerrainHeightFieldHeight.Length == 0)
2069 return norm;
2070
2071 // TerrainHeightField for ODE as offset 1m
2072 x += 1f;
2073 y += 1f;
2074
2075 // make position fit into array
2076 if (x < 0)
2077 x = 0;
2078 if (y < 0)
2079 y = 0;
2080 3190
2081 // integer indexs 3191 if (actor.SOPName != null &&
2082 int ix; 3192 joints_connecting_actor.ContainsKey(actor.SOPName) &&
2083 int iy; 3193 joints_connecting_actor[actor.SOPName] != null &&
2084 // interpolators offset 3194 joints_connecting_actor[actor.SOPName].Count > 0)
2085 float dx;
2086 float dy;
2087
2088 int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples
2089 int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples
2090 int regsize = regsizeX;
2091
2092 int xstep = 1;
2093 int ystep = regsizeX;
2094 bool firstTri = false;
2095
2096 if (m_OSOdeLib)
2097 {
2098 if (x < regsizeX - 1)
2099 {
2100 ix = (int)x;
2101 dx = x - (float)ix;
2102 }
2103 else // out world use external height
2104 {
2105 ix = regsizeX - 2;
2106 dx = 0;
2107 }
2108 if (y < regsizeY - 1)
2109 {
2110 iy = (int)y;
2111 dy = y - (float)iy;
2112 }
2113 else
2114 {
2115 iy = regsizeY - 2;
2116 dy = 0;
2117 }
2118 firstTri = dy > dx;
2119 }
2120
2121 else
2122 { 3195 {
2123 xstep = regsizeY; 3196 foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
2124 ystep = 1;
2125 regsize = regsizeY;
2126
2127 // we still have square fixed size regions
2128 // also flip x and y because of how map is done for ODE fliped axis
2129 // so ix,iy,dx and dy are inter exchanged
2130 if (x < regsizeX - 1)
2131 { 3197 {
2132 iy = (int)x; 3198 if (affectedJoint.IsInPhysicsEngine)
2133 dy = x - (float)iy; 3199 {
2134 } 3200 DoJointMoved(affectedJoint);
2135 else // out world use external height 3201 }
2136 { 3202 else
2137 iy = regsizeX - 2; 3203 {
2138 dy = 0; 3204 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);
2139 } 3205 }
2140 if (y < regsizeY - 1)
2141 {
2142 ix = (int)y;
2143 dx = y - (float)ix;
2144 }
2145 else
2146 {
2147 ix = regsizeY - 2;
2148 dx = 0;
2149 } 3206 }
2150 firstTri = dx > dy;
2151 } 3207 }
3208 }
2152 3209
2153 float h0; 3210 public override void GetResults()
2154 float h1; 3211 {
2155 float h2; 3212 }
2156
2157 iy *= regsize;
2158 iy += ix; // all indexes have iy + ix
2159 3213
2160 float[] heights = TerrainHeightFieldHeight; 3214 public override bool IsThreaded
3215 {
3216 // for now we won't be multithreaded
3217 get { return false; }
3218 }
2161 3219
2162 if (firstTri) 3220 public override void SetTerrain(float[] heightMap)
3221 {
3222 if (m_worldOffset != Vector3.Zero && m_parentScene != null)
2163 { 3223 {
2164 h1 = ((float)heights[iy]); // 0,0 vertice 3224 if (m_parentScene is OdeScene)
2165 iy += ystep; 3225 {
2166 h0 = (float)heights[iy]; // 0,1 3226 ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset);
2167 h2 = (float)heights[iy+xstep]; // 1,1 vertice 3227 }
2168 norm.X = h0 - h2;
2169 norm.Y = h1 - h0;
2170 } 3228 }
2171 else 3229 else
2172 { 3230 {
2173 h2 = ((float)heights[iy]); // 0,0 vertice 3231 SetTerrain(heightMap, m_worldOffset);
2174 iy += xstep;
2175 h0 = ((float)heights[iy]); // 1,0 vertice
2176 h1 = (float)heights[iy+ystep]; // vertice 1,1
2177 norm.X = h2 - h0;
2178 norm.Y = h0 - h1;
2179 } 3232 }
2180 norm.Z = 1;
2181 norm.Normalize();
2182 return norm;
2183 } 3233 }
2184 3234
2185 public override void SetTerrain(float[] heightMap) 3235 private void SetTerrain(float[] heightMap, Vector3 pOffset)
2186 {
2187 if (m_OSOdeLib)
2188 OSSetTerrain(heightMap);
2189 else
2190 OriSetTerrain(heightMap);
2191 }
2192
2193 public void OriSetTerrain(float[] heightMap)
2194 { 3236 {
2195 // assumes 1m size grid and constante size square regions 3237 int startTime = Util.EnvironmentTickCount();
2196 // needs to know about sims around in future 3238 m_log.DebugFormat("[ODE SCENE]: Setting terrain for {0} with offset {1}", PhysicsSceneName, pOffset);
2197 3239
3240
2198 float[] _heightmap; 3241 float[] _heightmap;
2199 3242
3243 // ok im lasy this are just a aliases
2200 uint regionsizeX = m_regionWidth; 3244 uint regionsizeX = m_regionWidth;
2201 uint regionsizeY = m_regionHeight; 3245 uint regionsizeY = m_regionHeight;
2202 3246
@@ -2214,7 +3258,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2214 const float thickness = 10f; 3258 const float thickness = 10f;
2215 const int wrap = 0; 3259 const int wrap = 0;
2216 3260
2217 3261
2218 float hfmin = float.MaxValue; 3262 float hfmin = float.MaxValue;
2219 float hfmax = float.MinValue; 3263 float hfmax = float.MinValue;
2220 float val; 3264 float val;
@@ -2223,11 +3267,13 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2223 3267
2224 uint maxXX = regionsizeX - 1; 3268 uint maxXX = regionsizeX - 1;
2225 uint maxYY = regionsizeY - 1; 3269 uint maxYY = regionsizeY - 1;
3270
2226 // flipping map adding one margin all around so things don't fall in edges 3271 // flipping map adding one margin all around so things don't fall in edges
2227 3272
2228 uint xt = 0; 3273 uint xt = 0;
2229 xx = 0; 3274 xx = 0;
2230 3275
3276
2231 for (uint x = 0; x < heightmapWidthSamples; x++) 3277 for (uint x = 0; x < heightmapWidthSamples; x++)
2232 { 3278 {
2233 if (x > 1 && xx < maxXX) 3279 if (x > 1 && xx < maxXX)
@@ -2240,7 +3286,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2240 3286
2241 val = heightMap[yy + xx]; 3287 val = heightMap[yy + xx];
2242 if (val < 0.0f) 3288 if (val < 0.0f)
2243 val = 0.0f; // no neg terrain as in chode 3289 val = 0.0f;
2244 _heightmap[xt + y] = val; 3290 _heightmap[xt + y] = val;
2245 3291
2246 if (hfmin > val) 3292 if (hfmin > val)
@@ -2255,164 +3301,69 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2255 { 3301 {
2256 d.AllocateODEDataForThread(~0U); 3302 d.AllocateODEDataForThread(~0U);
2257 3303
2258 if (TerrainGeom != IntPtr.Zero) 3304 IntPtr GroundGeom = IntPtr.Zero;
3305 if (RegionTerrain.TryGetValue(pOffset, out GroundGeom))
2259 { 3306 {
2260 actor_name_map.Remove(TerrainGeom); 3307 RegionTerrain.Remove(pOffset);
2261 d.GeomDestroy(TerrainGeom); 3308 if (GroundGeom != IntPtr.Zero)
3309 {
3310 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
3311 {
3312 TerrainHeightFieldHeights.Remove(GroundGeom);
3313 }
3314 d.SpaceRemove(space, GroundGeom);
3315 d.GeomDestroy(GroundGeom);
3316 }
2262 3317
2263 } 3318 }
2264
2265 if (TerrainHeightFieldHeightsHandler.IsAllocated)
2266 TerrainHeightFieldHeightsHandler.Free();
2267
2268 IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); 3319 IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
2269 3320 d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0,
2270 TerrainHeightFieldHeightsHandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); 3321 heightmapWidth, heightmapHeight,
2271 3322 (int)heightmapWidthSamples,
2272 d.GeomHeightfieldDataBuildSingle(HeightmapData, TerrainHeightFieldHeightsHandler.AddrOfPinnedObject(), 0, 3323 (int)heightmapHeightSamples,
2273 heightmapHeight, heightmapWidth , 3324 scale, offset, thickness, wrap);
2274 (int)heightmapHeightSamples, (int)heightmapWidthSamples, scale,
2275 offset, thickness, wrap);
2276 3325
2277 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); 3326 d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2278 3327 GroundGeom = d.CreateHeightfield(space, HeightmapData, 1);
2279 TerrainGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1); 3328 if (GroundGeom != IntPtr.Zero)
2280
2281 if (TerrainGeom != IntPtr.Zero)
2282 { 3329 {
2283 d.GeomSetCategoryBits(TerrainGeom, (uint)(CollisionCategories.Land)); 3330 d.GeomSetCategoryBits(GroundGeom, (int)(CollisionCategories.Land));
2284 d.GeomSetCollideBits(TerrainGeom, 0); 3331 d.GeomSetCollideBits(GroundGeom, (int)(CollisionCategories.Space));
2285
2286 PhysicsActor pa = new NullPhysicsActor();
2287 pa.Name = "Terrain";
2288 pa.PhysicsActorType = (int)ActorTypes.Ground;
2289 actor_name_map[TerrainGeom] = pa;
2290
2291// geom_name_map[GroundGeom] = "Terrain";
2292
2293 d.Quaternion q = new d.Quaternion();
2294 q.X = 0.5f;
2295 q.Y = 0.5f;
2296 q.Z = 0.5f;
2297 q.W = 0.5f;
2298 3332
2299 d.GeomSetQuaternion(TerrainGeom, ref q);
2300 d.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
2301 TerrainHeightFieldHeight = _heightmap;
2302 } 3333 }
2303 else 3334 geom_name_map[GroundGeom] = "Terrain";
2304 TerrainHeightFieldHeightsHandler.Free();
2305 }
2306 }
2307 3335
2308 public void OSSetTerrain(float[] heightMap) 3336 d.Matrix3 R = new d.Matrix3();
2309 {
2310 // assumes 1m size grid and constante size square regions
2311 // needs to know about sims around in future
2312
2313 float[] _heightmap;
2314 3337
2315 uint regionsizeX = m_regionWidth; 3338 Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
2316 uint regionsizeY = m_regionHeight; 3339 Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
2317
2318 uint heightmapWidth = regionsizeX + 2;
2319 uint heightmapHeight = regionsizeY + 2;
2320
2321 uint heightmapWidthSamples = heightmapWidth + 1;
2322 uint heightmapHeightSamples = heightmapHeight + 1;
2323
2324 _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples];
2325
2326
2327 float hfmin = float.MaxValue;
2328// float hfmax = float.MinValue;
2329 float val;
2330
2331
2332 uint maxXX = regionsizeX - 1;
2333 uint maxYY = regionsizeY - 1;
2334 // adding one margin all around so things don't fall in edges
2335 3340
2336 uint xx; 3341 q1 = q1 * q2;
2337 uint yy = 0; 3342 Vector3 v3;
2338 uint yt = 0; 3343 float angle;
3344 q1.GetAxisAngle(out v3, out angle);
2339 3345
2340 for (uint y = 0; y < heightmapHeightSamples; y++) 3346 d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
2341 { 3347 d.GeomSetRotation(GroundGeom, ref R);
2342 if (y > 1 && y < maxYY) 3348 d.GeomSetPosition(GroundGeom, pOffset.X + regionsizeX * 0.5f, pOffset.Y + regionsizeY * 0.5f, 0.0f);
2343 yy += regionsizeX; 3349 IntPtr testGround = IntPtr.Zero;
2344 xx = 0; 3350 if (RegionTerrain.TryGetValue(pOffset, out testGround))
2345 for (uint x = 0; x < heightmapWidthSamples; x++)
2346 { 3351 {
2347 if (x > 1 && x < maxXX) 3352 RegionTerrain.Remove(pOffset);
2348 xx++;
2349
2350 val = heightMap[yy + xx];
2351 if (val < 0.0f)
2352 val = 0.0f; // no neg terrain as in chode
2353 _heightmap[yt + x] = val;
2354
2355 if (hfmin > val)
2356 hfmin = val;
2357// if (hfmax < val)
2358// hfmax = val;
2359 } 3353 }
2360 yt += heightmapWidthSamples; 3354 RegionTerrain.Add(pOffset, GroundGeom, GroundGeom);
3355 TerrainHeightFieldHeights.Add(GroundGeom,_heightmap);
2361 } 3356 }
2362 3357
2363 lock (OdeLock) 3358 m_log.DebugFormat(
2364 { 3359 "[ODE SCENE]: Setting terrain for {0} took {1}ms", PhysicsSceneName, Util.EnvironmentTickCountSubtract(startTime));
2365 if (TerrainGeom != IntPtr.Zero)
2366 {
2367 actor_name_map.Remove(TerrainGeom);
2368 d.GeomDestroy(TerrainGeom);
2369 }
2370
2371 if (TerrainHeightFieldHeightsHandler.IsAllocated)
2372 TerrainHeightFieldHeightsHandler.Free();
2373
2374 TerrainHeightFieldHeight = null;
2375
2376 IntPtr HeightmapData = d.GeomOSTerrainDataCreate();
2377
2378 const int wrap = 0;
2379 float thickness = hfmin;
2380 if (thickness < 0)
2381 thickness = 1;
2382
2383 TerrainHeightFieldHeightsHandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned);
2384
2385 d.GeomOSTerrainDataBuild(HeightmapData, TerrainHeightFieldHeightsHandler.AddrOfPinnedObject(), 0, 1.0f,
2386 (int)heightmapWidthSamples, (int)heightmapHeightSamples,
2387 thickness, wrap);
2388
2389// d.GeomOSTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1);
2390 TerrainGeom = d.CreateOSTerrain(GroundSpace, HeightmapData, 1);
2391 if (TerrainGeom != IntPtr.Zero)
2392 {
2393 d.GeomSetCategoryBits(TerrainGeom, (uint)(CollisionCategories.Land));
2394 d.GeomSetCollideBits(TerrainGeom, 0);
2395
2396 PhysicsActor pa = new NullPhysicsActor();
2397 pa.Name = "Terrain";
2398 pa.PhysicsActorType = (int)ActorTypes.Ground;
2399 actor_name_map[TerrainGeom] = pa;
2400
2401// geom_name_map[GroundGeom] = "Terrain";
2402
2403 d.GeomSetPosition(TerrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
2404 TerrainHeightFieldHeight = _heightmap;
2405 }
2406 else
2407 TerrainHeightFieldHeightsHandler.Free();
2408 }
2409 } 3360 }
2410 3361
2411 public override void DeleteTerrain() 3362 public override void DeleteTerrain()
2412 { 3363 {
2413 } 3364 }
2414 3365
2415 public float GetWaterLevel() 3366 internal float GetWaterLevel()
2416 { 3367 {
2417 return waterlevel; 3368 return waterlevel;
2418 } 3369 }
@@ -2422,18 +3373,18 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2422 waterlevel = baseheight; 3373 waterlevel = baseheight;
2423 } 3374 }
2424 3375
3376 [HandleProcessCorruptedStateExceptions]
2425 public override void Dispose() 3377 public override void Dispose()
2426 { 3378 {
2427 lock (OdeLock) 3379 lock(SimulationLock)
3380 lock(OdeLock)
2428 { 3381 {
2429 3382 if(world == IntPtr.Zero)
2430 if (world == IntPtr.Zero)
2431 return; 3383 return;
2432 3384
2433 d.AllocateODEDataForThread(~0U); 3385 _worldInitialized = false;
2434 3386
2435 if (m_meshWorker != null) 3387 d.AllocateODEDataForThread(~0U);
2436 m_meshWorker.Stop();
2437 3388
2438 if (m_rayCastManager != null) 3389 if (m_rayCastManager != null)
2439 { 3390 {
@@ -2443,49 +3394,37 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2443 3394
2444 lock (_prims) 3395 lock (_prims)
2445 { 3396 {
2446 ChangesQueue.Clear(); 3397 foreach (OdePrim prm in _prims)
2447 foreach (OdePrim prm in _prims.Values)
2448 { 3398 {
2449 prm.DoAChange(changes.Remove, null); 3399 RemovePrim(prm);
2450 _collisionEventPrim.Remove(prm);
2451 } 3400 }
2452 _prims.Clear();
2453 } 3401 }
2454 3402
2455 OdeCharacter[] chtorem; 3403 //foreach (OdeCharacter act in _characters)
2456 lock (_characters) 3404 //{
3405 //RemoveAvatar(act);
3406 //}
3407 IntPtr GroundGeom = IntPtr.Zero;
3408 if (RegionTerrain.TryGetValue(m_worldOffset, out GroundGeom))
2457 { 3409 {
2458 chtorem = new OdeCharacter[_characters.Count]; 3410 RegionTerrain.Remove(m_worldOffset);
2459 _characters.CopyTo(chtorem); 3411 if (GroundGeom != IntPtr.Zero)
2460 } 3412 {
2461 3413 if (TerrainHeightFieldHeights.ContainsKey(GroundGeom))
2462 ChangesQueue.Clear(); 3414 TerrainHeightFieldHeights.Remove(GroundGeom);
2463 foreach (OdeCharacter ch in chtorem) 3415 d.GeomDestroy(GroundGeom);
2464 ch.DoAChange(changes.Remove, null); 3416 }
2465 3417 }
2466 if (TerrainGeom != IntPtr.Zero)
2467 d.GeomDestroy(TerrainGeom);
2468 TerrainGeom = IntPtr.Zero;
2469
2470 if (TerrainHeightFieldHeightsHandler.IsAllocated)
2471 TerrainHeightFieldHeightsHandler.Free();
2472
2473 TerrainHeightFieldHeight = null;
2474 3418
2475 if (ContactgeomsArray != IntPtr.Zero) 3419 try
2476 { 3420 {
2477 Marshal.FreeHGlobal(ContactgeomsArray); 3421 d.WorldDestroy(world);
2478 ContactgeomsArray = IntPtr.Zero; 3422 world = IntPtr.Zero;
2479 } 3423 }
2480 if (GlobalContactsArray != IntPtr.Zero) 3424 catch (AccessViolationException e)
2481 { 3425 {
2482 Marshal.FreeHGlobal(GlobalContactsArray); 3426 m_log.ErrorFormat("[ODE SCENE]: exception {0}", e.Message);
2483 GlobalContactsArray = IntPtr.Zero;
2484 } 3427 }
2485
2486 d.WorldDestroy(world);
2487 world = IntPtr.Zero;
2488 //d.CloseODE();
2489 } 3428 }
2490 } 3429 }
2491 3430
@@ -2497,13 +3436,17 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2497 public override Dictionary<uint, float> GetTopColliders() 3436 public override Dictionary<uint, float> GetTopColliders()
2498 { 3437 {
2499 Dictionary<uint, float> topColliders; 3438 Dictionary<uint, float> topColliders;
2500 List<OdePrim> orderedPrims;
2501 lock (_activeprims)
2502 orderedPrims = new List<OdePrim>(_activeprims);
2503 3439
2504 orderedPrims.Sort(compareByCollisionsDesc); 3440 lock (_prims)
2505 topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore); 3441 {
2506 3442 List<OdePrim> orderedPrims = new List<OdePrim>(_prims);
3443 orderedPrims.Sort(compareByCollisionsDesc);
3444 topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
3445
3446 foreach (OdePrim p in _prims)
3447 p.CollisionScore = 0;
3448 }
3449
2507 return topColliders; 3450 return topColliders;
2508 } 3451 }
2509 3452
@@ -2516,16 +3459,7 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2516 { 3459 {
2517 if (retMethod != null) 3460 if (retMethod != null)
2518 { 3461 {
2519 ODERayRequest req = new ODERayRequest(); 3462 m_rayCastManager.QueueRequest(position, direction, length, retMethod);
2520 req.actor = null;
2521 req.callbackMethod = retMethod;
2522 req.length = length;
2523 req.Normal = direction;
2524 req.Origin = position;
2525 req.Count = 0;
2526 req.filter = RayFilterFlags.AllPrims;
2527
2528 m_rayCastManager.QueueRequest(req);
2529 } 3463 }
2530 } 3464 }
2531 3465
@@ -2533,260 +3467,75 @@ namespace OpenSim.Region.PhysicsModule.ubOde
2533 { 3467 {
2534 if (retMethod != null) 3468 if (retMethod != null)
2535 { 3469 {
2536 ODERayRequest req = new ODERayRequest(); 3470 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2537 req.actor = null;
2538 req.callbackMethod = retMethod;
2539 req.length = length;
2540 req.Normal = direction;
2541 req.Origin = position;
2542 req.Count = Count;
2543 req.filter = RayFilterFlags.AllPrims;
2544
2545 m_rayCastManager.QueueRequest(req);
2546 } 3471 }
2547 } 3472 }
2548 3473
2549
2550 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) 3474 public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
2551 { 3475 {
2552 List<ContactResult> ourresults = new List<ContactResult>(); 3476 ContactResult[] ourResults = null;
2553 object SyncObject = new object();
2554
2555 RayCallback retMethod = delegate(List<ContactResult> results) 3477 RayCallback retMethod = delegate(List<ContactResult> results)
2556 { 3478 {
2557 lock (SyncObject) 3479 ourResults = new ContactResult[results.Count];
2558 { 3480 results.CopyTo(ourResults, 0);
2559 ourresults = results;
2560 Monitor.PulseAll(SyncObject);
2561 }
2562 }; 3481 };
2563 3482 int waitTime = 0;
2564 ODERayRequest req = new ODERayRequest(); 3483 m_rayCastManager.QueueRequest(position, direction, length, Count, retMethod);
2565 req.actor = null; 3484 while (ourResults == null && waitTime < 1000)
2566 req.callbackMethod = retMethod;
2567 req.length = length;
2568 req.Normal = direction;
2569 req.Origin = position;
2570 req.Count = Count;
2571 req.filter = RayFilterFlags.AllPrims;
2572
2573 lock (SyncObject)
2574 { 3485 {
2575 m_rayCastManager.QueueRequest(req); 3486 Thread.Sleep(1);
2576 if (!Monitor.Wait(SyncObject, 500)) 3487 waitTime++;
2577 return null;
2578 else
2579 return ourresults;
2580 } 3488 }
2581 }
2582
2583 public override bool SupportsRaycastWorldFiltered()
2584 {
2585 return true;
2586 }
2587
2588 public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
2589 {
2590 object SyncObject = new object();
2591 List<ContactResult> ourresults = new List<ContactResult>();
2592
2593 RayCallback retMethod = delegate(List<ContactResult> results)
2594 {
2595 lock (SyncObject)
2596 {
2597 ourresults = results;
2598 Monitor.PulseAll(SyncObject);
2599 }
2600 };
2601
2602 ODERayRequest req = new ODERayRequest();
2603 req.actor = null;
2604 req.callbackMethod = retMethod;
2605 req.length = length;
2606 req.Normal = direction;
2607 req.Origin = position;
2608 req.Count = Count;
2609 req.filter = filter;
2610
2611 lock (SyncObject)
2612 {
2613 m_rayCastManager.QueueRequest(req);
2614 if (!Monitor.Wait(SyncObject, 500))
2615 return null;
2616 else
2617 return ourresults;
2618 }
2619 }
2620
2621 public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags)
2622 {
2623 if (actor == null)
2624 return new List<ContactResult>();
2625
2626 IntPtr geom;
2627 if (actor is OdePrim)
2628 geom = ((OdePrim)actor).prim_geom;
2629 else if (actor is OdeCharacter)
2630 geom = ((OdePrim)actor).prim_geom;
2631 else
2632 return new List<ContactResult>();
2633
2634 if (geom == IntPtr.Zero)
2635 return new List<ContactResult>();
2636
2637 List<ContactResult> ourResults = null;
2638 object SyncObject = new object();
2639
2640 RayCallback retMethod = delegate(List<ContactResult> results)
2641 {
2642 lock (SyncObject)
2643 {
2644 ourResults = results;
2645 Monitor.PulseAll(SyncObject);
2646 }
2647 };
2648
2649 ODERayRequest req = new ODERayRequest();
2650 req.actor = actor;
2651 req.callbackMethod = retMethod;
2652 req.length = length;
2653 req.Normal = direction;
2654 req.Origin = position;
2655 req.Count = Count;
2656 req.filter = flags;
2657
2658 lock (SyncObject)
2659 {
2660 m_rayCastManager.QueueRequest(req);
2661 if (!Monitor.Wait(SyncObject, 500))
2662 return new List<ContactResult>();
2663 }
2664
2665 if (ourResults == null)
2666 return new List<ContactResult>();
2667 return ourResults;
2668 }
2669
2670 public override List<ContactResult> BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags)
2671 {
2672 List<ContactResult> ourResults = null;
2673 object SyncObject = new object();
2674
2675 ProbeBoxCallback retMethod = delegate(List<ContactResult> results)
2676 {
2677 lock (SyncObject)
2678 {
2679 ourResults = results;
2680 Monitor.PulseAll(SyncObject);
2681 }
2682 };
2683
2684 ODERayRequest req = new ODERayRequest();
2685 req.actor = null;
2686 req.callbackMethod = retMethod;
2687 req.Normal = size;
2688 req.Origin = position;
2689 req.orientation = orientation;
2690 req.Count = Count;
2691 req.filter = flags;
2692
2693 lock (SyncObject)
2694 {
2695 m_rayCastManager.QueueRequest(req);
2696 if (!Monitor.Wait(SyncObject, 500))
2697 return new List<ContactResult>();
2698 }
2699
2700 if (ourResults == null) 3489 if (ourResults == null)
2701 return new List<ContactResult>(); 3490 return new List<ContactResult> ();
2702 return ourResults; 3491 return new List<ContactResult>(ourResults);
2703 } 3492 }
2704 3493
2705 public override List<ContactResult> SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags) 3494 public override Dictionary<string, float> GetStats()
2706 { 3495 {
2707 List<ContactResult> ourResults = null; 3496 if (!CollectStats)
2708 object SyncObject = new object(); 3497 return null;
2709 3498
2710 ProbeSphereCallback retMethod = delegate(List<ContactResult> results) 3499 Dictionary<string, float> returnStats;
2711 {
2712 ourResults = results;
2713 Monitor.PulseAll(SyncObject);
2714 };
2715
2716 ODERayRequest req = new ODERayRequest();
2717 req.actor = null;
2718 req.callbackMethod = retMethod;
2719 req.length = radius;
2720 req.Origin = position;
2721 req.Count = Count;
2722 req.filter = flags;
2723 3500
2724 3501 lock (OdeLock)
2725 lock (SyncObject)
2726 { 3502 {
2727 m_rayCastManager.QueueRequest(req); 3503 returnStats = new Dictionary<string, float>(m_stats);
2728 if (!Monitor.Wait(SyncObject, 500))
2729 return new List<ContactResult>();
2730 }
2731 3504
2732 if (ourResults == null) 3505 // FIXME: This is a SUPER DUMB HACK until we can establish stats that aren't subject to a division by
2733 return new List<ContactResult>(); 3506 // 3 from the SimStatsReporter.
2734 return ourResults; 3507 returnStats[ODETotalAvatarsStatName] = _characters.Count * 3;
2735 } 3508 returnStats[ODETotalPrimsStatName] = _prims.Count * 3;
3509 returnStats[ODEActivePrimsStatName] = _activeprims.Count * 3;
2736 3510
2737 public override List<ContactResult> PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags) 3511 InitializeExtraStats();
2738 {
2739 IntPtr geom = IntPtr.Zero;;
2740
2741 if (actor != null)
2742 {
2743 if (actor is OdePrim)
2744 geom = ((OdePrim)actor).prim_geom;
2745 else if (actor is OdeCharacter)
2746 geom = ((OdePrim)actor).prim_geom;
2747 } 3512 }
2748 3513
2749 List<ContactResult> ourResults = null; 3514 returnStats[ODEOtherCollisionFrameMsStatName]
2750 object SyncObject = new object(); 3515 = returnStats[ODEOtherCollisionFrameMsStatName]
3516 - returnStats[ODENativeSpaceCollisionFrameMsStatName]
3517 - returnStats[ODENativeGeomCollisionFrameMsStatName];
2751 3518
2752 ProbePlaneCallback retMethod = delegate(List<ContactResult> results) 3519 return returnStats;
2753 {
2754 ourResults = results;
2755 Monitor.PulseAll(SyncObject);
2756 };
2757
2758 ODERayRequest req = new ODERayRequest();
2759 req.actor = null;
2760 req.callbackMethod = retMethod;
2761 req.length = plane.W;
2762 req.Normal.X = plane.X;
2763 req.Normal.Y = plane.Y;
2764 req.Normal.Z = plane.Z;
2765 req.Count = Count;
2766 req.filter = flags;
2767
2768 lock (SyncObject)
2769 {
2770 m_rayCastManager.QueueRequest(req);
2771 if (!Monitor.Wait(SyncObject, 500))
2772 return new List<ContactResult>();
2773 }
2774
2775 if (ourResults == null)
2776 return new List<ContactResult>();
2777 return ourResults;
2778 } 3520 }
2779 3521
2780 public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse) 3522 private void InitializeExtraStats()
2781 { 3523 {
2782 Util.FireAndForget( delegate 3524 m_stats[ODETotalFrameMsStatName] = 0;
2783 { 3525 m_stats[ODEAvatarTaintMsStatName] = 0;
2784 ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager); 3526 m_stats[ODEPrimTaintMsStatName] = 0;
2785 if(sitAvatar != null) 3527 m_stats[ODEAvatarForcesFrameMsStatName] = 0;
2786 sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse); 3528 m_stats[ODEPrimForcesFrameMsStatName] = 0;
2787 }); 3529 m_stats[ODERaycastingFrameMsStatName] = 0;
2788 return 1; 3530 m_stats[ODENativeStepFrameMsStatName] = 0;
3531 m_stats[ODENativeSpaceCollisionFrameMsStatName] = 0;
3532 m_stats[ODENativeGeomCollisionFrameMsStatName] = 0;
3533 m_stats[ODEOtherCollisionFrameMsStatName] = 0;
3534 m_stats[ODECollisionNotificationFrameMsStatName] = 0;
3535 m_stats[ODEAvatarContactsStatsName] = 0;
3536 m_stats[ODEPrimContactsStatName] = 0;
3537 m_stats[ODEAvatarUpdateFrameMsStatName] = 0;
3538 m_stats[ODEPrimUpdateFrameMsStatName] = 0;
2789 } 3539 }
2790
2791 } 3540 }
2792} 3541}