diff options
Diffstat (limited to 'OpenSim/Region/PhysicsModules/UbitOdePlugin/OdeScene.cs')
-rw-r--r-- | OpenSim/Region/PhysicsModules/UbitOdePlugin/OdeScene.cs | 2930 |
1 files changed, 2930 insertions, 0 deletions
diff --git a/OpenSim/Region/PhysicsModules/UbitOdePlugin/OdeScene.cs b/OpenSim/Region/PhysicsModules/UbitOdePlugin/OdeScene.cs new file mode 100644 index 0000000..9373d2b --- /dev/null +++ b/OpenSim/Region/PhysicsModules/UbitOdePlugin/OdeScene.cs | |||
@@ -0,0 +1,2930 @@ | |||
1 | /* | ||
2 | * Copyright (c) Contributors, http://opensimulator.org/ | ||
3 | * See CONTRIBUTORS.TXT for a full list of copyright holders. | ||
4 | * | ||
5 | * Redistribution and use in source and binary forms, with or without | ||
6 | * modification, are permitted provided that the following conditions are met: | ||
7 | * * Redistributions of source code must retain the above copyright | ||
8 | * notice, this list of conditions and the following disclaimer. | ||
9 | * * Redistributions in binary form must reproduce the above copyright | ||
10 | * notice, this list of conditions and the following disclaimer in the | ||
11 | * documentation and/or other materials provided with the distribution. | ||
12 | * * Neither the name of the OpenSimulator Project nor the | ||
13 | * names of its contributors may be used to endorse or promote products | ||
14 | * derived from this software without specific prior written permission. | ||
15 | * | ||
16 | * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY | ||
17 | * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED | ||
18 | * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE | ||
19 | * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY | ||
20 | * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES | ||
21 | * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; | ||
22 | * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND | ||
23 | * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT | ||
24 | * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS | ||
25 | * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. | ||
26 | */ | ||
27 | |||
28 | // Revision 2011/12/13 by Ubit Umarov | ||
29 | //#define SPAM | ||
30 | |||
31 | using System; | ||
32 | using System.Collections.Generic; | ||
33 | using System.Reflection; | ||
34 | using System.Runtime.InteropServices; | ||
35 | using System.Threading; | ||
36 | using System.IO; | ||
37 | using System.Diagnostics; | ||
38 | using log4net; | ||
39 | using Nini.Config; | ||
40 | using OdeAPI; | ||
41 | using OpenSim.Framework; | ||
42 | using OpenSim.Region.Framework.Interfaces; | ||
43 | using OpenSim.Region.PhysicsModules.SharedBase; | ||
44 | using OpenMetaverse; | ||
45 | |||
46 | namespace OpenSim.Region.PhysicsModules.OdePlugin | ||
47 | { | ||
48 | // colision flags of things others can colide with | ||
49 | // rays, sensors, probes removed since can't be colided with | ||
50 | // The top space where things are placed provided further selection | ||
51 | // ie physical are in active space nonphysical in static | ||
52 | // this should be exclusive as possible | ||
53 | |||
54 | [Flags] | ||
55 | public enum CollisionCategories : uint | ||
56 | { | ||
57 | Disabled = 0, | ||
58 | //by 'things' types | ||
59 | Space = 0x01, | ||
60 | Geom = 0x02, // aka prim/part | ||
61 | Character = 0x04, | ||
62 | Land = 0x08, | ||
63 | Water = 0x010, | ||
64 | |||
65 | // by state | ||
66 | Phantom = 0x01000, | ||
67 | VolumeDtc = 0x02000, | ||
68 | Selected = 0x04000, | ||
69 | NoShape = 0x08000, | ||
70 | |||
71 | |||
72 | All = 0xffffffff | ||
73 | } | ||
74 | |||
75 | /// <summary> | ||
76 | /// Material type for a primitive | ||
77 | /// </summary> | ||
78 | public enum Material : int | ||
79 | { | ||
80 | /// <summary></summary> | ||
81 | Stone = 0, | ||
82 | /// <summary></summary> | ||
83 | Metal = 1, | ||
84 | /// <summary></summary> | ||
85 | Glass = 2, | ||
86 | /// <summary></summary> | ||
87 | Wood = 3, | ||
88 | /// <summary></summary> | ||
89 | Flesh = 4, | ||
90 | /// <summary></summary> | ||
91 | Plastic = 5, | ||
92 | /// <summary></summary> | ||
93 | Rubber = 6, | ||
94 | |||
95 | light = 7 // compatibility with old viewers | ||
96 | } | ||
97 | |||
98 | public enum changes : int | ||
99 | { | ||
100 | Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?) | ||
101 | Remove, | ||
102 | Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root | ||
103 | // or removes from a object if arg is null | ||
104 | DeLink, | ||
105 | Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child | ||
106 | Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child | ||
107 | PosOffset, // not in use | ||
108 | // arg Vector3 new position in local coords. Changes prim position in object | ||
109 | OriOffset, // not in use | ||
110 | // arg Vector3 new position in local coords. Changes prim position in object | ||
111 | Velocity, | ||
112 | AngVelocity, | ||
113 | Acceleration, | ||
114 | Force, | ||
115 | Torque, | ||
116 | Momentum, | ||
117 | |||
118 | AddForce, | ||
119 | AddAngForce, | ||
120 | AngLock, | ||
121 | |||
122 | Buoyancy, | ||
123 | |||
124 | PIDTarget, | ||
125 | PIDTau, | ||
126 | PIDActive, | ||
127 | |||
128 | PIDHoverHeight, | ||
129 | PIDHoverType, | ||
130 | PIDHoverTau, | ||
131 | PIDHoverActive, | ||
132 | |||
133 | Size, | ||
134 | AvatarSize, | ||
135 | Shape, | ||
136 | PhysRepData, | ||
137 | AddPhysRep, | ||
138 | |||
139 | CollidesWater, | ||
140 | VolumeDtc, | ||
141 | |||
142 | Physical, | ||
143 | Phantom, | ||
144 | Selected, | ||
145 | disabled, | ||
146 | building, | ||
147 | |||
148 | VehicleType, | ||
149 | VehicleFloatParam, | ||
150 | VehicleVectorParam, | ||
151 | VehicleRotationParam, | ||
152 | VehicleFlags, | ||
153 | SetVehicle, | ||
154 | |||
155 | Null //keep this last used do dim the methods array. does nothing but pulsing the prim | ||
156 | } | ||
157 | |||
158 | public struct ODEchangeitem | ||
159 | { | ||
160 | public PhysicsActor actor; | ||
161 | public OdeCharacter character; | ||
162 | public changes what; | ||
163 | public Object arg; | ||
164 | } | ||
165 | |||
166 | |||
167 | |||
168 | public class OdeScene : PhysicsScene, INonSharedRegionModule | ||
169 | { | ||
170 | private readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier); | ||
171 | // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>(); | ||
172 | |||
173 | public bool OdeUbitLib = false; | ||
174 | public bool m_suportCombine = false; // mega suport not tested | ||
175 | |||
176 | // private int threadid = 0; | ||
177 | // private Random fluidRandomizer = new Random(Environment.TickCount); | ||
178 | |||
179 | // const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce; | ||
180 | |||
181 | const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1 | d.ContactFlags.Slip1 | d.ContactFlags.Slip2; | ||
182 | const float comumContactERP = 0.7f; | ||
183 | const float comumContactCFM = 0.0001f; | ||
184 | const float comumContactSLIP = 0f; | ||
185 | |||
186 | float frictionMovementMult = 0.8f; | ||
187 | |||
188 | float TerrainBounce = 0.1f; | ||
189 | float TerrainFriction = 0.3f; | ||
190 | |||
191 | public float AvatarFriction = 0;// 0.9f * 0.5f; | ||
192 | |||
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; | ||
197 | private uint m_regionHeight = Constants.RegionSize; | ||
198 | |||
199 | public float ODE_STEPSIZE = 0.020f; | ||
200 | public float HalfOdeStep = 0.01f; | ||
201 | public int odetimestepMS = 20; // rounded | ||
202 | private float metersInSpace = 25.6f; | ||
203 | private float m_timeDilation = 1.0f; | ||
204 | |||
205 | private DateTime m_lastframe; | ||
206 | private DateTime m_lastMeshExpire; | ||
207 | |||
208 | public float gravityx = 0f; | ||
209 | public float gravityy = 0f; | ||
210 | public float gravityz = -9.8f; | ||
211 | |||
212 | private float waterlevel = 0f; | ||
213 | private int framecount = 0; | ||
214 | |||
215 | // private int m_meshExpireCntr; | ||
216 | |||
217 | private float avDensity = 3f; | ||
218 | private float avMovementDivisorWalk = 1.3f; | ||
219 | private float avMovementDivisorRun = 0.8f; | ||
220 | private float minimumGroundFlightOffset = 3f; | ||
221 | public float maximumMassObject = 10000.01f; | ||
222 | |||
223 | |||
224 | public float geomDefaultDensity = 10.000006836f; | ||
225 | |||
226 | public float bodyPIDD = 35f; | ||
227 | public float bodyPIDG = 25; | ||
228 | |||
229 | public int bodyFramesAutoDisable = 5; | ||
230 | |||
231 | private d.NearCallback nearCallback; | ||
232 | |||
233 | private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>(); | ||
234 | private HashSet<OdePrim> _prims = new HashSet<OdePrim>(); | ||
235 | private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>(); | ||
236 | private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>(); | ||
237 | |||
238 | public OpenSim.Framework.LocklessQueue<ODEchangeitem> ChangesQueue = new OpenSim.Framework.LocklessQueue<ODEchangeitem>(); | ||
239 | |||
240 | /// <summary> | ||
241 | /// A list of actors that should receive collision events. | ||
242 | /// </summary> | ||
243 | private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>(); | ||
244 | private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>(); | ||
245 | |||
246 | private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>(); | ||
247 | // public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>(); | ||
248 | public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>(); | ||
249 | |||
250 | private float contactsurfacelayer = 0.001f; | ||
251 | |||
252 | private int contactsPerCollision = 80; | ||
253 | internal IntPtr ContactgeomsArray = IntPtr.Zero; | ||
254 | private IntPtr GlobalContactsArray = IntPtr.Zero; | ||
255 | private d.Contact SharedTmpcontact = new d.Contact(); | ||
256 | |||
257 | const int maxContactsbeforedeath = 4000; | ||
258 | private volatile int m_global_contactcount = 0; | ||
259 | |||
260 | private IntPtr contactgroup; | ||
261 | |||
262 | public ContactData[] m_materialContactsData = new ContactData[8]; | ||
263 | |||
264 | private Dictionary<Vector3, IntPtr> RegionTerrain = new Dictionary<Vector3, IntPtr>(); | ||
265 | private Dictionary<IntPtr, float[]> TerrainHeightFieldHeights = new Dictionary<IntPtr, float[]>(); | ||
266 | private Dictionary<IntPtr, GCHandle> TerrainHeightFieldHeightsHandlers = new Dictionary<IntPtr, GCHandle>(); | ||
267 | |||
268 | private int m_physicsiterations = 15; | ||
269 | private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag | ||
270 | // private PhysicsActor PANull = new NullPhysicsActor(); | ||
271 | private float step_time = 0.0f; | ||
272 | |||
273 | public IntPtr world; | ||
274 | |||
275 | |||
276 | // split the spaces acording to contents type | ||
277 | // ActiveSpace contains characters and active prims | ||
278 | // StaticSpace contains land and other that is mostly static in enviroment | ||
279 | // this can contain subspaces, like the grid in staticspace | ||
280 | // as now space only contains this 2 top spaces | ||
281 | |||
282 | public IntPtr TopSpace; // the global space | ||
283 | public IntPtr ActiveSpace; // space for active prims | ||
284 | public IntPtr CharsSpace; // space for active prims | ||
285 | public IntPtr StaticSpace; // space for the static things around | ||
286 | public IntPtr GroundSpace; // space for ground | ||
287 | |||
288 | // some speedup variables | ||
289 | private int spaceGridMaxX; | ||
290 | private int spaceGridMaxY; | ||
291 | private float spacesPerMeterX; | ||
292 | private float spacesPerMeterY; | ||
293 | |||
294 | // split static geometry collision into a grid as before | ||
295 | private IntPtr[,] staticPrimspace; | ||
296 | private IntPtr[] staticPrimspaceOffRegion; | ||
297 | |||
298 | public Object OdeLock; | ||
299 | public static Object SimulationLock; | ||
300 | |||
301 | public IMesher mesher; | ||
302 | |||
303 | private IConfigSource m_config; | ||
304 | |||
305 | public bool physics_logging = false; | ||
306 | public int physics_logging_interval = 0; | ||
307 | public bool physics_logging_append_existing_logfile = false; | ||
308 | |||
309 | private Vector3 m_worldOffset = Vector3.Zero; | ||
310 | public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); | ||
311 | private PhysicsScene m_parentScene = null; | ||
312 | |||
313 | private ODERayCastRequestManager m_rayCastManager; | ||
314 | public ODEMeshWorker m_meshWorker; | ||
315 | |||
316 | /* maybe needed if ode uses tls | ||
317 | private void checkThread() | ||
318 | { | ||
319 | |||
320 | int th = Thread.CurrentThread.ManagedThreadId; | ||
321 | if(th != threadid) | ||
322 | { | ||
323 | threadid = th; | ||
324 | d.AllocateODEDataForThread(~0U); | ||
325 | } | ||
326 | } | ||
327 | */ | ||
328 | #region INonSharedRegionModule | ||
329 | public string Name | ||
330 | { | ||
331 | get { return "UbitODE"; } | ||
332 | } | ||
333 | |||
334 | public Type ReplaceableInterface | ||
335 | { | ||
336 | get { return null; } | ||
337 | } | ||
338 | |||
339 | public void Initialise(IConfigSource source) | ||
340 | { | ||
341 | // TODO: Move this out of Startup | ||
342 | IConfig config = source.Configs["Startup"]; | ||
343 | if (config != null) | ||
344 | { | ||
345 | string physics = config.GetString("physics", string.Empty); | ||
346 | if (physics == Name) | ||
347 | { | ||
348 | m_Enabled = true; | ||
349 | m_Config = source; | ||
350 | } | ||
351 | } | ||
352 | |||
353 | } | ||
354 | |||
355 | public void Close() | ||
356 | { | ||
357 | } | ||
358 | |||
359 | public void AddRegion(Scene scene) | ||
360 | { | ||
361 | if (!m_Enabled) | ||
362 | return; | ||
363 | |||
364 | EngineType = Name; | ||
365 | RegionName = scene.RegionInfo.RegionName; | ||
366 | PhysicsSceneName = EngineType + "/" + RegionName; | ||
367 | |||
368 | scene.RegisterModuleInterface<PhysicsScene>(this); | ||
369 | Vector3 extent = new Vector3(scene.RegionInfo.RegionSizeX, scene.RegionInfo.RegionSizeY, scene.RegionInfo.RegionSizeZ); | ||
370 | RawInitialization(); | ||
371 | Initialise(m_Config, extent); | ||
372 | |||
373 | base.Initialise(scene.PhysicsRequestAsset, | ||
374 | (scene.Heightmap != null ? scene.Heightmap.GetFloatsSerialised() : new float[scene.RegionInfo.RegionSizeX * scene.RegionInfo.RegionSizeY]), | ||
375 | (float)scene.RegionInfo.RegionSettings.WaterHeight); | ||
376 | |||
377 | } | ||
378 | |||
379 | public void RemoveRegion(Scene scene) | ||
380 | { | ||
381 | if (!m_Enabled) | ||
382 | return; | ||
383 | } | ||
384 | |||
385 | public void RegionLoaded(Scene scene) | ||
386 | { | ||
387 | if (!m_Enabled) | ||
388 | return; | ||
389 | |||
390 | mesher = scene.RequestModuleInterface<IMesher>(); | ||
391 | if (mesher == null) | ||
392 | m_log.WarnFormat("{0} No mesher. Things will not work well.", LogHeader); | ||
393 | |||
394 | scene.PhysicsEnabled = true; | ||
395 | } | ||
396 | #endregion | ||
397 | |||
398 | /// <summary> | ||
399 | /// Initiailizes the scene | ||
400 | /// Sets many properties that ODE requires to be stable | ||
401 | /// These settings need to be tweaked 'exactly' right or weird stuff happens. | ||
402 | /// </summary> | ||
403 | private void RawInitialization() | ||
404 | { | ||
405 | |||
406 | // checkThread(); | ||
407 | |||
408 | OdeLock = new Object(); | ||
409 | SimulationLock = new Object(); | ||
410 | |||
411 | nearCallback = near; | ||
412 | |||
413 | m_rayCastManager = new ODERayCastRequestManager(this); | ||
414 | |||
415 | lock (OdeLock) | ||
416 | { | ||
417 | // Create the world and the first space | ||
418 | try | ||
419 | { | ||
420 | world = d.WorldCreate(); | ||
421 | TopSpace = d.HashSpaceCreate(IntPtr.Zero); | ||
422 | |||
423 | // now the major subspaces | ||
424 | ActiveSpace = d.HashSpaceCreate(TopSpace); | ||
425 | CharsSpace = d.HashSpaceCreate(TopSpace); | ||
426 | StaticSpace = d.HashSpaceCreate(TopSpace); | ||
427 | GroundSpace = d.HashSpaceCreate(TopSpace); | ||
428 | } | ||
429 | catch | ||
430 | { | ||
431 | // i must RtC#FM | ||
432 | // i did! | ||
433 | } | ||
434 | |||
435 | d.HashSpaceSetLevels(TopSpace, -2, 8); | ||
436 | d.HashSpaceSetLevels(ActiveSpace, -2, 8); | ||
437 | d.HashSpaceSetLevels(CharsSpace, -4, 3); | ||
438 | d.HashSpaceSetLevels(StaticSpace, -2, 8); | ||
439 | d.HashSpaceSetLevels(GroundSpace, 0, 8); | ||
440 | |||
441 | // demote to second level | ||
442 | d.SpaceSetSublevel(ActiveSpace, 1); | ||
443 | d.SpaceSetSublevel(CharsSpace, 1); | ||
444 | d.SpaceSetSublevel(StaticSpace, 1); | ||
445 | d.SpaceSetSublevel(GroundSpace, 1); | ||
446 | |||
447 | d.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space | | ||
448 | CollisionCategories.Geom | | ||
449 | CollisionCategories.Character | | ||
450 | CollisionCategories.Phantom | | ||
451 | CollisionCategories.VolumeDtc | ||
452 | )); | ||
453 | d.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space | | ||
454 | CollisionCategories.Geom | | ||
455 | CollisionCategories.Character | | ||
456 | CollisionCategories.Phantom | | ||
457 | CollisionCategories.VolumeDtc | ||
458 | )); | ||
459 | d.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space | | ||
460 | CollisionCategories.Geom | | ||
461 | CollisionCategories.Character | | ||
462 | CollisionCategories.Phantom | | ||
463 | CollisionCategories.VolumeDtc | ||
464 | )); | ||
465 | d.GeomSetCollideBits(CharsSpace, 0); | ||
466 | |||
467 | d.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space | | ||
468 | CollisionCategories.Geom | | ||
469 | // CollisionCategories.Land | | ||
470 | // CollisionCategories.Water | | ||
471 | CollisionCategories.Phantom | | ||
472 | CollisionCategories.VolumeDtc | ||
473 | )); | ||
474 | d.GeomSetCollideBits(StaticSpace, 0); | ||
475 | |||
476 | d.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land)); | ||
477 | d.GeomSetCollideBits(GroundSpace, 0); | ||
478 | |||
479 | contactgroup = d.JointGroupCreate(maxContactsbeforedeath + 1); | ||
480 | //contactgroup | ||
481 | |||
482 | d.WorldSetAutoDisableFlag(world, false); | ||
483 | } | ||
484 | } | ||
485 | |||
486 | public void Initialise(IConfigSource config, Vector3 regionExtent) | ||
487 | { | ||
488 | WorldExtents.X = regionExtent.X; | ||
489 | m_regionWidth = (uint)regionExtent.X; | ||
490 | WorldExtents.Y = regionExtent.Y; | ||
491 | m_regionHeight = (uint)regionExtent.Y; | ||
492 | |||
493 | m_suportCombine = false; | ||
494 | // checkThread(); | ||
495 | m_config = config; | ||
496 | |||
497 | string ode_config = d.GetConfiguration(); | ||
498 | if (ode_config != null && ode_config != "") | ||
499 | { | ||
500 | m_log.WarnFormat("ODE configuration: {0}", ode_config); | ||
501 | |||
502 | if (ode_config.Contains("ODE_Ubit")) | ||
503 | { | ||
504 | OdeUbitLib = true; | ||
505 | } | ||
506 | } | ||
507 | |||
508 | // Defaults | ||
509 | |||
510 | int contactsPerCollision = 80; | ||
511 | |||
512 | IConfig physicsconfig = null; | ||
513 | |||
514 | if (m_config != null) | ||
515 | { | ||
516 | physicsconfig = m_config.Configs["ODEPhysicsSettings"]; | ||
517 | if (physicsconfig != null) | ||
518 | { | ||
519 | gravityx = physicsconfig.GetFloat("world_gravityx", gravityx); | ||
520 | gravityy = physicsconfig.GetFloat("world_gravityy", gravityy); | ||
521 | gravityz = physicsconfig.GetFloat("world_gravityz", gravityz); | ||
522 | |||
523 | metersInSpace = physicsconfig.GetFloat("meters_in_small_space", metersInSpace); | ||
524 | |||
525 | // contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer); | ||
526 | |||
527 | ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE); | ||
528 | |||
529 | avDensity = physicsconfig.GetFloat("av_density", avDensity); | ||
530 | avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk); | ||
531 | avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun); | ||
532 | |||
533 | contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision); | ||
534 | |||
535 | geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity); | ||
536 | bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable); | ||
537 | |||
538 | physics_logging = physicsconfig.GetBoolean("physics_logging", false); | ||
539 | physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0); | ||
540 | physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false); | ||
541 | |||
542 | minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset); | ||
543 | maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject); | ||
544 | } | ||
545 | } | ||
546 | |||
547 | |||
548 | d.WorldSetCFM(world, comumContactCFM); | ||
549 | d.WorldSetERP(world, comumContactERP); | ||
550 | |||
551 | d.WorldSetGravity(world, gravityx, gravityy, gravityz); | ||
552 | |||
553 | d.WorldSetLinearDamping(world, 0.002f); | ||
554 | d.WorldSetAngularDamping(world, 0.002f); | ||
555 | d.WorldSetAngularDampingThreshold(world, 0f); | ||
556 | d.WorldSetLinearDampingThreshold(world, 0f); | ||
557 | d.WorldSetMaxAngularSpeed(world, 100f); | ||
558 | |||
559 | d.WorldSetQuickStepNumIterations(world, m_physicsiterations); | ||
560 | |||
561 | d.WorldSetContactSurfaceLayer(world, contactsurfacelayer); | ||
562 | d.WorldSetContactMaxCorrectingVel(world, 60.0f); | ||
563 | |||
564 | m_meshWorker = new ODEMeshWorker(this, m_log, meshmerizer, physicsconfig); | ||
565 | |||
566 | HalfOdeStep = ODE_STEPSIZE * 0.5f; | ||
567 | odetimestepMS = (int)(1000.0f * ODE_STEPSIZE +0.5f); | ||
568 | |||
569 | ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * d.ContactGeom.unmanagedSizeOf); | ||
570 | GlobalContactsArray = Marshal.AllocHGlobal(maxContactsbeforedeath * d.Contact.unmanagedSizeOf); | ||
571 | |||
572 | SharedTmpcontact.geom.g1 = IntPtr.Zero; | ||
573 | SharedTmpcontact.geom.g2 = IntPtr.Zero; | ||
574 | |||
575 | SharedTmpcontact.geom.side1 = -1; | ||
576 | SharedTmpcontact.geom.side2 = -1; | ||
577 | |||
578 | SharedTmpcontact.surface.mode = comumContactFlags; | ||
579 | SharedTmpcontact.surface.mu = 0; | ||
580 | SharedTmpcontact.surface.bounce = 0; | ||
581 | SharedTmpcontact.surface.soft_cfm = comumContactCFM; | ||
582 | SharedTmpcontact.surface.soft_erp = comumContactERP; | ||
583 | SharedTmpcontact.surface.slip1 = comumContactSLIP; | ||
584 | SharedTmpcontact.surface.slip2 = comumContactSLIP; | ||
585 | |||
586 | m_materialContactsData[(int)Material.Stone].mu = 0.8f; | ||
587 | m_materialContactsData[(int)Material.Stone].bounce = 0.4f; | ||
588 | |||
589 | m_materialContactsData[(int)Material.Metal].mu = 0.3f; | ||
590 | m_materialContactsData[(int)Material.Metal].bounce = 0.4f; | ||
591 | |||
592 | m_materialContactsData[(int)Material.Glass].mu = 0.2f; | ||
593 | m_materialContactsData[(int)Material.Glass].bounce = 0.7f; | ||
594 | |||
595 | m_materialContactsData[(int)Material.Wood].mu = 0.6f; | ||
596 | m_materialContactsData[(int)Material.Wood].bounce = 0.5f; | ||
597 | |||
598 | m_materialContactsData[(int)Material.Flesh].mu = 0.9f; | ||
599 | m_materialContactsData[(int)Material.Flesh].bounce = 0.3f; | ||
600 | |||
601 | m_materialContactsData[(int)Material.Plastic].mu = 0.4f; | ||
602 | m_materialContactsData[(int)Material.Plastic].bounce = 0.7f; | ||
603 | |||
604 | m_materialContactsData[(int)Material.Rubber].mu = 0.9f; | ||
605 | m_materialContactsData[(int)Material.Rubber].bounce = 0.95f; | ||
606 | |||
607 | m_materialContactsData[(int)Material.light].mu = 0.0f; | ||
608 | m_materialContactsData[(int)Material.light].bounce = 0.0f; | ||
609 | |||
610 | |||
611 | spacesPerMeterX = 1.0f / metersInSpace; | ||
612 | spacesPerMeterY = spacesPerMeterX; | ||
613 | spaceGridMaxX = (int)(WorldExtents.X * spacesPerMeterX); | ||
614 | spaceGridMaxY = (int)(WorldExtents.Y * spacesPerMeterY); | ||
615 | |||
616 | if (spaceGridMaxX > 24) | ||
617 | { | ||
618 | spaceGridMaxX = 24; | ||
619 | spacesPerMeterX = spaceGridMaxX / WorldExtents.X ; | ||
620 | } | ||
621 | |||
622 | if (spaceGridMaxY > 24) | ||
623 | { | ||
624 | spaceGridMaxY = 24; | ||
625 | spacesPerMeterY = spaceGridMaxY / WorldExtents.Y ; | ||
626 | } | ||
627 | |||
628 | staticPrimspace = new IntPtr[spaceGridMaxX, spaceGridMaxY]; | ||
629 | |||
630 | // create all spaces now | ||
631 | int i, j; | ||
632 | IntPtr newspace; | ||
633 | |||
634 | for (i = 0; i < spaceGridMaxX; i++) | ||
635 | for (j = 0; j < spaceGridMaxY; j++) | ||
636 | { | ||
637 | newspace = d.HashSpaceCreate(StaticSpace); | ||
638 | d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); | ||
639 | waitForSpaceUnlock(newspace); | ||
640 | d.SpaceSetSublevel(newspace, 2); | ||
641 | d.HashSpaceSetLevels(newspace, -2, 8); | ||
642 | d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | | ||
643 | CollisionCategories.Geom | | ||
644 | CollisionCategories.Land | | ||
645 | CollisionCategories.Water | | ||
646 | CollisionCategories.Phantom | | ||
647 | CollisionCategories.VolumeDtc | ||
648 | )); | ||
649 | d.GeomSetCollideBits(newspace, 0); | ||
650 | |||
651 | staticPrimspace[i, j] = newspace; | ||
652 | } | ||
653 | |||
654 | // let this now be index limit | ||
655 | spaceGridMaxX--; | ||
656 | spaceGridMaxY--; | ||
657 | |||
658 | // create 4 off world spaces (x<0,x>max,y<0,y>max) | ||
659 | staticPrimspaceOffRegion = new IntPtr[4]; | ||
660 | |||
661 | for (i = 0; i < 4; i++) | ||
662 | { | ||
663 | newspace = d.HashSpaceCreate(StaticSpace); | ||
664 | d.GeomSetCategoryBits(newspace, (int)CollisionCategories.Space); | ||
665 | waitForSpaceUnlock(newspace); | ||
666 | d.SpaceSetSublevel(newspace, 2); | ||
667 | d.HashSpaceSetLevels(newspace, -2, 8); | ||
668 | d.GeomSetCategoryBits(newspace, (uint)(CollisionCategories.Space | | ||
669 | CollisionCategories.Geom | | ||
670 | CollisionCategories.Land | | ||
671 | CollisionCategories.Water | | ||
672 | CollisionCategories.Phantom | | ||
673 | CollisionCategories.VolumeDtc | ||
674 | )); | ||
675 | d.GeomSetCollideBits(newspace, 0); | ||
676 | |||
677 | staticPrimspaceOffRegion[i] = newspace; | ||
678 | } | ||
679 | |||
680 | m_lastframe = DateTime.UtcNow; | ||
681 | m_lastMeshExpire = m_lastframe; | ||
682 | } | ||
683 | |||
684 | internal void waitForSpaceUnlock(IntPtr space) | ||
685 | { | ||
686 | //if (space != IntPtr.Zero) | ||
687 | //while (d.SpaceLockQuery(space)) { } // Wait and do nothing | ||
688 | } | ||
689 | |||
690 | #region Collision Detection | ||
691 | |||
692 | // sets a global contact for a joint for contactgeom , and base contact description) | ||
693 | |||
694 | |||
695 | |||
696 | private IntPtr CreateContacJoint(ref d.ContactGeom contactGeom) | ||
697 | { | ||
698 | if (m_global_contactcount >= maxContactsbeforedeath) | ||
699 | return IntPtr.Zero; | ||
700 | |||
701 | m_global_contactcount++; | ||
702 | |||
703 | SharedTmpcontact.geom.depth = contactGeom.depth; | ||
704 | SharedTmpcontact.geom.pos = contactGeom.pos; | ||
705 | SharedTmpcontact.geom.normal = contactGeom.normal; | ||
706 | |||
707 | IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * d.Contact.unmanagedSizeOf)); | ||
708 | Marshal.StructureToPtr(SharedTmpcontact, contact, true); | ||
709 | return d.JointCreateContactPtr(world, contactgroup, contact); | ||
710 | } | ||
711 | |||
712 | private bool GetCurContactGeom(int index, ref d.ContactGeom newcontactgeom) | ||
713 | { | ||
714 | if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision) | ||
715 | return false; | ||
716 | |||
717 | IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * d.ContactGeom.unmanagedSizeOf)); | ||
718 | newcontactgeom = (d.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(d.ContactGeom)); | ||
719 | return true; | ||
720 | } | ||
721 | |||
722 | /// <summary> | ||
723 | /// This is our near callback. A geometry is near a body | ||
724 | /// </summary> | ||
725 | /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param> | ||
726 | /// <param name="g1">a geometry or space</param> | ||
727 | /// <param name="g2">another geometry or space</param> | ||
728 | /// | ||
729 | |||
730 | private void near(IntPtr space, IntPtr g1, IntPtr g2) | ||
731 | { | ||
732 | // no lock here! It's invoked from within Simulate(), which is thread-locked | ||
733 | |||
734 | if (m_global_contactcount >= maxContactsbeforedeath) | ||
735 | return; | ||
736 | |||
737 | // Test if we're colliding a geom with a space. | ||
738 | // If so we have to drill down into the space recursively | ||
739 | |||
740 | if (g1 == IntPtr.Zero || g2 == IntPtr.Zero) | ||
741 | return; | ||
742 | |||
743 | if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2)) | ||
744 | { | ||
745 | // We'll be calling near recursivly if one | ||
746 | // of them is a space to find all of the | ||
747 | // contact points in the space | ||
748 | try | ||
749 | { | ||
750 | d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback); | ||
751 | } | ||
752 | catch (AccessViolationException) | ||
753 | { | ||
754 | m_log.Warn("[PHYSICS]: Unable to collide test a space"); | ||
755 | return; | ||
756 | } | ||
757 | //here one should check collisions of geoms inside a space | ||
758 | // but on each space we only should have geoms that not colide amoung each other | ||
759 | // so we don't dig inside spaces | ||
760 | return; | ||
761 | } | ||
762 | |||
763 | // get geom bodies to check if we already a joint contact | ||
764 | // guess this shouldn't happen now | ||
765 | IntPtr b1 = d.GeomGetBody(g1); | ||
766 | IntPtr b2 = d.GeomGetBody(g2); | ||
767 | |||
768 | // d.GeomClassID id = d.GeomGetClass(g1); | ||
769 | |||
770 | // Figure out how many contact points we have | ||
771 | int count = 0; | ||
772 | try | ||
773 | { | ||
774 | // Colliding Geom To Geom | ||
775 | // This portion of the function 'was' blatantly ripped off from BoxStack.cs | ||
776 | |||
777 | if (g1 == g2) | ||
778 | return; // Can't collide with yourself | ||
779 | |||
780 | if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact)) | ||
781 | return; | ||
782 | /* | ||
783 | // debug | ||
784 | PhysicsActor dp2; | ||
785 | if (d.GeomGetClass(g1) == d.GeomClassID.HeightfieldClass) | ||
786 | { | ||
787 | d.AABB aabb; | ||
788 | d.GeomGetAABB(g2, out aabb); | ||
789 | float x = aabb.MaxX - aabb.MinX; | ||
790 | float y = aabb.MaxY - aabb.MinY; | ||
791 | float z = aabb.MaxZ - aabb.MinZ; | ||
792 | if (x > 60.0f || y > 60.0f || z > 60.0f) | ||
793 | { | ||
794 | if (!actor_name_map.TryGetValue(g2, out dp2)) | ||
795 | m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); | ||
796 | else | ||
797 | m_log.WarnFormat("[PHYSICS]: land versus large prim geo {0},size {1}, AABBsize <{2},{3},{4}>, at {5} ori {6},({7})", | ||
798 | dp2.Name, dp2.Size, x, y, z, | ||
799 | dp2.Position.ToString(), | ||
800 | dp2.Orientation.ToString(), | ||
801 | dp2.Orientation.Length()); | ||
802 | return; | ||
803 | } | ||
804 | } | ||
805 | // | ||
806 | */ | ||
807 | |||
808 | |||
809 | if (d.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc || | ||
810 | d.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc) | ||
811 | { | ||
812 | int cflags; | ||
813 | unchecked | ||
814 | { | ||
815 | cflags = (int)(1 | d.CONTACTS_UNIMPORTANT); | ||
816 | } | ||
817 | count = d.CollidePtr(g1, g2, cflags, ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); | ||
818 | } | ||
819 | else | ||
820 | count = d.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, d.ContactGeom.unmanagedSizeOf); | ||
821 | } | ||
822 | catch (SEHException) | ||
823 | { | ||
824 | 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."); | ||
825 | // ode.drelease(world); | ||
826 | base.TriggerPhysicsBasedRestart(); | ||
827 | } | ||
828 | catch (Exception e) | ||
829 | { | ||
830 | m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message); | ||
831 | return; | ||
832 | } | ||
833 | |||
834 | // contacts done | ||
835 | if (count == 0) | ||
836 | return; | ||
837 | |||
838 | // try get physical actors | ||
839 | PhysicsActor p1; | ||
840 | PhysicsActor p2; | ||
841 | |||
842 | if (!actor_name_map.TryGetValue(g1, out p1)) | ||
843 | { | ||
844 | m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1"); | ||
845 | return; | ||
846 | } | ||
847 | |||
848 | if (!actor_name_map.TryGetValue(g2, out p2)) | ||
849 | { | ||
850 | m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2"); | ||
851 | return; | ||
852 | } | ||
853 | |||
854 | // update actors collision score | ||
855 | if (p1.CollisionScore >= float.MaxValue - count) | ||
856 | p1.CollisionScore = 0; | ||
857 | p1.CollisionScore += count; | ||
858 | |||
859 | if (p2.CollisionScore >= float.MaxValue - count) | ||
860 | p2.CollisionScore = 0; | ||
861 | p2.CollisionScore += count; | ||
862 | |||
863 | // get first contact | ||
864 | d.ContactGeom curContact = new d.ContactGeom(); | ||
865 | |||
866 | if (!GetCurContactGeom(0, ref curContact)) | ||
867 | return; | ||
868 | |||
869 | ContactPoint maxDepthContact = new ContactPoint(); | ||
870 | |||
871 | // do volume detection case | ||
872 | if ((p1.IsVolumeDtc || p2.IsVolumeDtc)) | ||
873 | { | ||
874 | maxDepthContact = new ContactPoint( | ||
875 | new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z), | ||
876 | new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z), | ||
877 | curContact.depth, false | ||
878 | ); | ||
879 | |||
880 | collision_accounting_events(p1, p2, maxDepthContact); | ||
881 | return; | ||
882 | } | ||
883 | |||
884 | // big messy collision analises | ||
885 | |||
886 | float mu = 0; | ||
887 | float bounce = 0; | ||
888 | // bool IgnoreNegSides = false; | ||
889 | |||
890 | ContactData contactdata1 = new ContactData(0, 0, false); | ||
891 | ContactData contactdata2 = new ContactData(0, 0, false); | ||
892 | |||
893 | bool dop1ava = false; | ||
894 | bool dop2ava = false; | ||
895 | bool ignore = false; | ||
896 | |||
897 | switch (p1.PhysicsActorType) | ||
898 | { | ||
899 | case (int)ActorTypes.Agent: | ||
900 | { | ||
901 | dop1ava = true; | ||
902 | switch (p2.PhysicsActorType) | ||
903 | { | ||
904 | case (int)ActorTypes.Agent: | ||
905 | case (int)ActorTypes.Prim: | ||
906 | break; | ||
907 | |||
908 | default: | ||
909 | ignore = true; // avatar to terrain and water ignored | ||
910 | break; | ||
911 | } | ||
912 | break; | ||
913 | } | ||
914 | |||
915 | case (int)ActorTypes.Prim: | ||
916 | { | ||
917 | switch (p2.PhysicsActorType) | ||
918 | { | ||
919 | case (int)ActorTypes.Agent: | ||
920 | dop2ava = true; | ||
921 | break; | ||
922 | |||
923 | case (int)ActorTypes.Prim: | ||
924 | Vector3 relV = p1.Velocity - p2.Velocity; | ||
925 | float relVlenSQ = relV.LengthSquared(); | ||
926 | if (relVlenSQ > 0.0001f) | ||
927 | { | ||
928 | p1.CollidingObj = true; | ||
929 | p2.CollidingObj = true; | ||
930 | } | ||
931 | p1.getContactData(ref contactdata1); | ||
932 | p2.getContactData(ref contactdata2); | ||
933 | bounce = contactdata1.bounce * contactdata2.bounce; | ||
934 | mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu); | ||
935 | |||
936 | if (relVlenSQ > 0.01f) | ||
937 | mu *= frictionMovementMult; | ||
938 | |||
939 | break; | ||
940 | |||
941 | case (int)ActorTypes.Ground: | ||
942 | p1.getContactData(ref contactdata1); | ||
943 | bounce = contactdata1.bounce * TerrainBounce; | ||
944 | mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction); | ||
945 | |||
946 | if (Math.Abs(p1.Velocity.X) > 0.1f || Math.Abs(p1.Velocity.Y) > 0.1f) | ||
947 | mu *= frictionMovementMult; | ||
948 | p1.CollidingGround = true; | ||
949 | /* | ||
950 | if (d.GeomGetClass(g1) == d.GeomClassID.TriMeshClass) | ||
951 | { | ||
952 | if (curContact.side1 > 0) | ||
953 | IgnoreNegSides = true; | ||
954 | } | ||
955 | */ | ||
956 | break; | ||
957 | |||
958 | case (int)ActorTypes.Water: | ||
959 | default: | ||
960 | ignore = true; | ||
961 | break; | ||
962 | } | ||
963 | } | ||
964 | break; | ||
965 | |||
966 | case (int)ActorTypes.Ground: | ||
967 | if (p2.PhysicsActorType == (int)ActorTypes.Prim) | ||
968 | { | ||
969 | p2.CollidingGround = true; | ||
970 | p2.getContactData(ref contactdata2); | ||
971 | bounce = contactdata2.bounce * TerrainBounce; | ||
972 | mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction); | ||
973 | |||
974 | // if (curContact.side1 > 0) // should be 2 ? | ||
975 | // IgnoreNegSides = true; | ||
976 | |||
977 | if (Math.Abs(p2.Velocity.X) > 0.1f || Math.Abs(p2.Velocity.Y) > 0.1f) | ||
978 | mu *= frictionMovementMult; | ||
979 | } | ||
980 | else | ||
981 | ignore = true; | ||
982 | break; | ||
983 | |||
984 | case (int)ActorTypes.Water: | ||
985 | default: | ||
986 | break; | ||
987 | } | ||
988 | |||
989 | if (ignore) | ||
990 | return; | ||
991 | |||
992 | IntPtr Joint; | ||
993 | bool FeetCollision = false; | ||
994 | int ncontacts = 0; | ||
995 | |||
996 | int i = 0; | ||
997 | |||
998 | maxDepthContact = new ContactPoint(); | ||
999 | maxDepthContact.PenetrationDepth = float.MinValue; | ||
1000 | ContactPoint minDepthContact = new ContactPoint(); | ||
1001 | minDepthContact.PenetrationDepth = float.MaxValue; | ||
1002 | |||
1003 | SharedTmpcontact.geom.depth = 0; | ||
1004 | SharedTmpcontact.surface.mu = mu; | ||
1005 | SharedTmpcontact.surface.bounce = bounce; | ||
1006 | |||
1007 | d.ContactGeom altContact = new d.ContactGeom(); | ||
1008 | bool useAltcontact = false; | ||
1009 | bool noskip = true; | ||
1010 | |||
1011 | while (true) | ||
1012 | { | ||
1013 | // if (!(IgnoreNegSides && curContact.side1 < 0)) | ||
1014 | { | ||
1015 | noskip = true; | ||
1016 | useAltcontact = false; | ||
1017 | |||
1018 | if (dop1ava) | ||
1019 | { | ||
1020 | if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision))) | ||
1021 | { | ||
1022 | if (p2.PhysicsActorType == (int)ActorTypes.Agent) | ||
1023 | { | ||
1024 | p1.CollidingObj = true; | ||
1025 | p2.CollidingObj = true; | ||
1026 | } | ||
1027 | else if (p2.Velocity.LengthSquared() > 0.0f) | ||
1028 | p2.CollidingObj = true; | ||
1029 | } | ||
1030 | else | ||
1031 | noskip = false; | ||
1032 | } | ||
1033 | else if (dop2ava) | ||
1034 | { | ||
1035 | if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision))) | ||
1036 | { | ||
1037 | if (p1.PhysicsActorType == (int)ActorTypes.Agent) | ||
1038 | { | ||
1039 | p1.CollidingObj = true; | ||
1040 | p2.CollidingObj = true; | ||
1041 | } | ||
1042 | else if (p2.Velocity.LengthSquared() > 0.0f) | ||
1043 | p1.CollidingObj = true; | ||
1044 | } | ||
1045 | else | ||
1046 | noskip = false; | ||
1047 | } | ||
1048 | |||
1049 | if (noskip) | ||
1050 | { | ||
1051 | if(useAltcontact) | ||
1052 | Joint = CreateContacJoint(ref altContact); | ||
1053 | else | ||
1054 | Joint = CreateContacJoint(ref curContact); | ||
1055 | |||
1056 | if (Joint == IntPtr.Zero) | ||
1057 | break; | ||
1058 | |||
1059 | d.JointAttach(Joint, b1, b2); | ||
1060 | |||
1061 | ncontacts++; | ||
1062 | |||
1063 | if (curContact.depth > maxDepthContact.PenetrationDepth) | ||
1064 | { | ||
1065 | maxDepthContact.Position.X = curContact.pos.X; | ||
1066 | maxDepthContact.Position.Y = curContact.pos.Y; | ||
1067 | maxDepthContact.Position.Z = curContact.pos.Z; | ||
1068 | maxDepthContact.PenetrationDepth = curContact.depth; | ||
1069 | maxDepthContact.CharacterFeet = FeetCollision; | ||
1070 | } | ||
1071 | |||
1072 | if (curContact.depth < minDepthContact.PenetrationDepth) | ||
1073 | { | ||
1074 | minDepthContact.PenetrationDepth = curContact.depth; | ||
1075 | minDepthContact.SurfaceNormal.X = curContact.normal.X; | ||
1076 | minDepthContact.SurfaceNormal.Y = curContact.normal.Y; | ||
1077 | minDepthContact.SurfaceNormal.Z = curContact.normal.Z; | ||
1078 | } | ||
1079 | } | ||
1080 | } | ||
1081 | if (++i >= count) | ||
1082 | break; | ||
1083 | |||
1084 | if (!GetCurContactGeom(i, ref curContact)) | ||
1085 | break; | ||
1086 | } | ||
1087 | |||
1088 | if (ncontacts > 0) | ||
1089 | { | ||
1090 | maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X; | ||
1091 | maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y; | ||
1092 | maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z; | ||
1093 | |||
1094 | collision_accounting_events(p1, p2, maxDepthContact); | ||
1095 | } | ||
1096 | } | ||
1097 | |||
1098 | private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact) | ||
1099 | { | ||
1100 | uint obj2LocalID = 0; | ||
1101 | |||
1102 | bool p1events = p1.SubscribedEvents(); | ||
1103 | bool p2events = p2.SubscribedEvents(); | ||
1104 | |||
1105 | if (p1.IsVolumeDtc) | ||
1106 | p2events = false; | ||
1107 | if (p2.IsVolumeDtc) | ||
1108 | p1events = false; | ||
1109 | |||
1110 | if (!p2events && !p1events) | ||
1111 | return; | ||
1112 | |||
1113 | Vector3 vel = Vector3.Zero; | ||
1114 | if (p2 != null && p2.IsPhysical) | ||
1115 | vel = p2.Velocity; | ||
1116 | |||
1117 | if (p1 != null && p1.IsPhysical) | ||
1118 | vel -= p1.Velocity; | ||
1119 | |||
1120 | contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal); | ||
1121 | |||
1122 | switch ((ActorTypes)p1.PhysicsActorType) | ||
1123 | { | ||
1124 | case ActorTypes.Agent: | ||
1125 | case ActorTypes.Prim: | ||
1126 | { | ||
1127 | switch ((ActorTypes)p2.PhysicsActorType) | ||
1128 | { | ||
1129 | case ActorTypes.Agent: | ||
1130 | case ActorTypes.Prim: | ||
1131 | if (p2events) | ||
1132 | { | ||
1133 | AddCollisionEventReporting(p2); | ||
1134 | p2.AddCollisionEvent(p1.ParentActor.LocalID, contact); | ||
1135 | } | ||
1136 | obj2LocalID = p2.ParentActor.LocalID; | ||
1137 | break; | ||
1138 | |||
1139 | case ActorTypes.Ground: | ||
1140 | case ActorTypes.Unknown: | ||
1141 | default: | ||
1142 | obj2LocalID = 0; | ||
1143 | break; | ||
1144 | } | ||
1145 | if (p1events) | ||
1146 | { | ||
1147 | contact.SurfaceNormal = -contact.SurfaceNormal; | ||
1148 | AddCollisionEventReporting(p1); | ||
1149 | p1.AddCollisionEvent(obj2LocalID, contact); | ||
1150 | } | ||
1151 | break; | ||
1152 | } | ||
1153 | case ActorTypes.Ground: | ||
1154 | case ActorTypes.Unknown: | ||
1155 | default: | ||
1156 | { | ||
1157 | if (p2events && !p2.IsVolumeDtc) | ||
1158 | { | ||
1159 | AddCollisionEventReporting(p2); | ||
1160 | p2.AddCollisionEvent(0, contact); | ||
1161 | } | ||
1162 | break; | ||
1163 | } | ||
1164 | } | ||
1165 | } | ||
1166 | |||
1167 | /// <summary> | ||
1168 | /// This is our collision testing routine in ODE | ||
1169 | /// </summary> | ||
1170 | /// <param name="timeStep"></param> | ||
1171 | private void collision_optimized() | ||
1172 | { | ||
1173 | lock (_characters) | ||
1174 | { | ||
1175 | try | ||
1176 | { | ||
1177 | foreach (OdeCharacter chr in _characters) | ||
1178 | { | ||
1179 | if (chr == null || chr.Body == IntPtr.Zero) | ||
1180 | continue; | ||
1181 | |||
1182 | chr.IsColliding = false; | ||
1183 | // chr.CollidingGround = false; not done here | ||
1184 | chr.CollidingObj = false; | ||
1185 | // do colisions with static space | ||
1186 | d.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback); | ||
1187 | |||
1188 | // no coll with gnd | ||
1189 | } | ||
1190 | // chars with chars | ||
1191 | d.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback); | ||
1192 | |||
1193 | } | ||
1194 | catch (AccessViolationException) | ||
1195 | { | ||
1196 | m_log.Warn("[PHYSICS]: Unable to collide Character to static space"); | ||
1197 | } | ||
1198 | |||
1199 | } | ||
1200 | |||
1201 | lock (_activeprims) | ||
1202 | { | ||
1203 | foreach (OdePrim aprim in _activeprims) | ||
1204 | { | ||
1205 | aprim.CollisionScore = 0; | ||
1206 | aprim.IsColliding = false; | ||
1207 | } | ||
1208 | } | ||
1209 | |||
1210 | // collide active prims with static enviroment | ||
1211 | lock (_activegroups) | ||
1212 | { | ||
1213 | try | ||
1214 | { | ||
1215 | foreach (OdePrim prm in _activegroups) | ||
1216 | { | ||
1217 | if (!prm.m_outbounds) | ||
1218 | { | ||
1219 | if (d.BodyIsEnabled(prm.Body)) | ||
1220 | { | ||
1221 | d.SpaceCollide2(StaticSpace, prm.collide_geom, IntPtr.Zero, nearCallback); | ||
1222 | d.SpaceCollide2(GroundSpace, prm.collide_geom, IntPtr.Zero, nearCallback); | ||
1223 | } | ||
1224 | } | ||
1225 | } | ||
1226 | } | ||
1227 | catch (AccessViolationException) | ||
1228 | { | ||
1229 | m_log.Warn("[PHYSICS]: Unable to collide Active prim to static space"); | ||
1230 | } | ||
1231 | } | ||
1232 | // colide active amoung them | ||
1233 | try | ||
1234 | { | ||
1235 | d.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback); | ||
1236 | } | ||
1237 | catch (AccessViolationException) | ||
1238 | { | ||
1239 | m_log.Warn("[PHYSICS]: Unable to collide Active with Characters space"); | ||
1240 | } | ||
1241 | // and with chars | ||
1242 | try | ||
1243 | { | ||
1244 | d.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback); | ||
1245 | } | ||
1246 | catch (AccessViolationException) | ||
1247 | { | ||
1248 | m_log.Warn("[PHYSICS]: Unable to collide in Active space"); | ||
1249 | } | ||
1250 | // _perloopContact.Clear(); | ||
1251 | } | ||
1252 | |||
1253 | #endregion | ||
1254 | /// <summary> | ||
1255 | /// Add actor to the list that should receive collision events in the simulate loop. | ||
1256 | /// </summary> | ||
1257 | /// <param name="obj"></param> | ||
1258 | public void AddCollisionEventReporting(PhysicsActor obj) | ||
1259 | { | ||
1260 | if (!_collisionEventPrim.Contains(obj)) | ||
1261 | _collisionEventPrim.Add(obj); | ||
1262 | } | ||
1263 | |||
1264 | /// <summary> | ||
1265 | /// Remove actor from the list that should receive collision events in the simulate loop. | ||
1266 | /// </summary> | ||
1267 | /// <param name="obj"></param> | ||
1268 | public void RemoveCollisionEventReporting(PhysicsActor obj) | ||
1269 | { | ||
1270 | if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj)) | ||
1271 | _collisionEventPrimRemove.Add(obj); | ||
1272 | } | ||
1273 | |||
1274 | public override float TimeDilation | ||
1275 | { | ||
1276 | get { return m_timeDilation; } | ||
1277 | } | ||
1278 | |||
1279 | public override bool SupportsNINJAJoints | ||
1280 | { | ||
1281 | get { return false; } | ||
1282 | } | ||
1283 | |||
1284 | #region Add/Remove Entities | ||
1285 | |||
1286 | public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying) | ||
1287 | { | ||
1288 | return null; | ||
1289 | } | ||
1290 | |||
1291 | public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying) | ||
1292 | { | ||
1293 | OdeCharacter newAv = new OdeCharacter(localID, avName, this, position, | ||
1294 | size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun); | ||
1295 | newAv.Flying = isFlying; | ||
1296 | newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset; | ||
1297 | |||
1298 | return newAv; | ||
1299 | } | ||
1300 | |||
1301 | public void AddCharacter(OdeCharacter chr) | ||
1302 | { | ||
1303 | lock (_characters) | ||
1304 | { | ||
1305 | if (!_characters.Contains(chr)) | ||
1306 | { | ||
1307 | _characters.Add(chr); | ||
1308 | if (chr.bad) | ||
1309 | m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid); | ||
1310 | } | ||
1311 | } | ||
1312 | } | ||
1313 | |||
1314 | public void RemoveCharacter(OdeCharacter chr) | ||
1315 | { | ||
1316 | lock (_characters) | ||
1317 | { | ||
1318 | if (_characters.Contains(chr)) | ||
1319 | { | ||
1320 | _characters.Remove(chr); | ||
1321 | } | ||
1322 | } | ||
1323 | } | ||
1324 | |||
1325 | public void BadCharacter(OdeCharacter chr) | ||
1326 | { | ||
1327 | lock (_badCharacter) | ||
1328 | { | ||
1329 | if (!_badCharacter.Contains(chr)) | ||
1330 | _badCharacter.Add(chr); | ||
1331 | } | ||
1332 | } | ||
1333 | |||
1334 | public override void RemoveAvatar(PhysicsActor actor) | ||
1335 | { | ||
1336 | //m_log.Debug("[PHYSICS]:ODELOCK"); | ||
1337 | ((OdeCharacter) actor).Destroy(); | ||
1338 | } | ||
1339 | |||
1340 | |||
1341 | public void addActivePrim(OdePrim activatePrim) | ||
1342 | { | ||
1343 | // adds active prim.. | ||
1344 | lock (_activeprims) | ||
1345 | { | ||
1346 | if (!_activeprims.Contains(activatePrim)) | ||
1347 | _activeprims.Add(activatePrim); | ||
1348 | } | ||
1349 | } | ||
1350 | |||
1351 | public void addActiveGroups(OdePrim activatePrim) | ||
1352 | { | ||
1353 | lock (_activegroups) | ||
1354 | { | ||
1355 | if (!_activegroups.Contains(activatePrim)) | ||
1356 | _activegroups.Add(activatePrim); | ||
1357 | } | ||
1358 | } | ||
1359 | |||
1360 | private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation, | ||
1361 | PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID) | ||
1362 | { | ||
1363 | OdePrim newPrim; | ||
1364 | lock (OdeLock) | ||
1365 | { | ||
1366 | newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID); | ||
1367 | lock (_prims) | ||
1368 | _prims.Add(newPrim); | ||
1369 | } | ||
1370 | return newPrim; | ||
1371 | } | ||
1372 | |||
1373 | public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, | ||
1374 | Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid) | ||
1375 | { | ||
1376 | return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid); | ||
1377 | } | ||
1378 | |||
1379 | |||
1380 | public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, | ||
1381 | Vector3 size, Quaternion rotation, bool isPhysical, uint localid) | ||
1382 | { | ||
1383 | return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid); | ||
1384 | } | ||
1385 | |||
1386 | public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position, | ||
1387 | Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid) | ||
1388 | { | ||
1389 | |||
1390 | return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid); | ||
1391 | } | ||
1392 | |||
1393 | public void remActivePrim(OdePrim deactivatePrim) | ||
1394 | { | ||
1395 | lock (_activeprims) | ||
1396 | { | ||
1397 | _activeprims.Remove(deactivatePrim); | ||
1398 | } | ||
1399 | } | ||
1400 | public void remActiveGroup(OdePrim deactivatePrim) | ||
1401 | { | ||
1402 | lock (_activegroups) | ||
1403 | { | ||
1404 | _activegroups.Remove(deactivatePrim); | ||
1405 | } | ||
1406 | } | ||
1407 | |||
1408 | public override void RemovePrim(PhysicsActor prim) | ||
1409 | { | ||
1410 | // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be | ||
1411 | // removed in the next physics simulate pass. | ||
1412 | if (prim is OdePrim) | ||
1413 | { | ||
1414 | // lock (OdeLock) | ||
1415 | { | ||
1416 | |||
1417 | OdePrim p = (OdePrim)prim; | ||
1418 | p.setPrimForRemoval(); | ||
1419 | } | ||
1420 | } | ||
1421 | } | ||
1422 | |||
1423 | public void RemovePrimThreadLocked(OdePrim prim) | ||
1424 | { | ||
1425 | //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName); | ||
1426 | lock (prim) | ||
1427 | { | ||
1428 | // RemoveCollisionEventReporting(prim); | ||
1429 | lock (_prims) | ||
1430 | _prims.Remove(prim); | ||
1431 | } | ||
1432 | |||
1433 | } | ||
1434 | |||
1435 | public bool havePrim(OdePrim prm) | ||
1436 | { | ||
1437 | lock (_prims) | ||
1438 | return _prims.Contains(prm); | ||
1439 | } | ||
1440 | |||
1441 | public bool haveActor(PhysicsActor actor) | ||
1442 | { | ||
1443 | if (actor is OdePrim) | ||
1444 | { | ||
1445 | lock (_prims) | ||
1446 | return _prims.Contains((OdePrim)actor); | ||
1447 | } | ||
1448 | else if (actor is OdeCharacter) | ||
1449 | { | ||
1450 | lock (_characters) | ||
1451 | return _characters.Contains((OdeCharacter)actor); | ||
1452 | } | ||
1453 | return false; | ||
1454 | } | ||
1455 | |||
1456 | #endregion | ||
1457 | |||
1458 | #region Space Separation Calculation | ||
1459 | |||
1460 | /// <summary> | ||
1461 | /// Called when a static prim moves or becomes static | ||
1462 | /// Places the prim in a space one the static sub-spaces grid | ||
1463 | /// </summary> | ||
1464 | /// <param name="geom">the pointer to the geom that moved</param> | ||
1465 | /// <param name="pos">the position that the geom moved to</param> | ||
1466 | /// <param name="currentspace">a pointer to the space it was in before it was moved.</param> | ||
1467 | /// <returns>a pointer to the new space it's in</returns> | ||
1468 | public IntPtr MoveGeomToStaticSpace(IntPtr geom, Vector3 pos, IntPtr currentspace) | ||
1469 | { | ||
1470 | // moves a prim into another static sub-space or from another space into a static sub-space | ||
1471 | |||
1472 | // Called ODEPrim so | ||
1473 | // it's already in locked space. | ||
1474 | |||
1475 | if (geom == IntPtr.Zero) // shouldn't happen | ||
1476 | return IntPtr.Zero; | ||
1477 | |||
1478 | // get the static sub-space for current position | ||
1479 | IntPtr newspace = calculateSpaceForGeom(pos); | ||
1480 | |||
1481 | if (newspace == currentspace) // if we are there all done | ||
1482 | return newspace; | ||
1483 | |||
1484 | // else remove it from its current space | ||
1485 | if (currentspace != IntPtr.Zero && d.SpaceQuery(currentspace, geom)) | ||
1486 | { | ||
1487 | if (d.GeomIsSpace(currentspace)) | ||
1488 | { | ||
1489 | waitForSpaceUnlock(currentspace); | ||
1490 | d.SpaceRemove(currentspace, geom); | ||
1491 | |||
1492 | if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) | ||
1493 | { | ||
1494 | d.SpaceDestroy(currentspace); | ||
1495 | } | ||
1496 | } | ||
1497 | else | ||
1498 | { | ||
1499 | m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace + | ||
1500 | " Geom:" + geom); | ||
1501 | } | ||
1502 | } | ||
1503 | else // odd currentspace is null or doesn't contain the geom? lets try the geom ideia of current space | ||
1504 | { | ||
1505 | currentspace = d.GeomGetSpace(geom); | ||
1506 | if (currentspace != IntPtr.Zero) | ||
1507 | { | ||
1508 | if (d.GeomIsSpace(currentspace)) | ||
1509 | { | ||
1510 | waitForSpaceUnlock(currentspace); | ||
1511 | d.SpaceRemove(currentspace, geom); | ||
1512 | |||
1513 | if (d.SpaceGetSublevel(currentspace) > 2 && d.SpaceGetNumGeoms(currentspace) == 0) | ||
1514 | { | ||
1515 | d.SpaceDestroy(currentspace); | ||
1516 | } | ||
1517 | |||
1518 | } | ||
1519 | } | ||
1520 | } | ||
1521 | |||
1522 | // put the geom in the newspace | ||
1523 | waitForSpaceUnlock(newspace); | ||
1524 | d.SpaceAdd(newspace, geom); | ||
1525 | |||
1526 | // let caller know this newspace | ||
1527 | return newspace; | ||
1528 | } | ||
1529 | |||
1530 | /// <summary> | ||
1531 | /// Calculates the space the prim should be in by its position | ||
1532 | /// </summary> | ||
1533 | /// <param name="pos"></param> | ||
1534 | /// <returns>a pointer to the space. This could be a new space or reused space.</returns> | ||
1535 | public IntPtr calculateSpaceForGeom(Vector3 pos) | ||
1536 | { | ||
1537 | int x, y; | ||
1538 | |||
1539 | if (pos.X < 0) | ||
1540 | return staticPrimspaceOffRegion[0]; | ||
1541 | |||
1542 | if (pos.Y < 0) | ||
1543 | return staticPrimspaceOffRegion[2]; | ||
1544 | |||
1545 | x = (int)(pos.X * spacesPerMeterX); | ||
1546 | if (x > spaceGridMaxX) | ||
1547 | return staticPrimspaceOffRegion[1]; | ||
1548 | |||
1549 | y = (int)(pos.Y * spacesPerMeterY); | ||
1550 | if (y > spaceGridMaxY) | ||
1551 | return staticPrimspaceOffRegion[3]; | ||
1552 | |||
1553 | return staticPrimspace[x, y]; | ||
1554 | } | ||
1555 | |||
1556 | #endregion | ||
1557 | |||
1558 | |||
1559 | /// <summary> | ||
1560 | /// Called to queue a change to a actor | ||
1561 | /// to use in place of old taint mechanism so changes do have a time sequence | ||
1562 | /// </summary> | ||
1563 | |||
1564 | public void AddChange(PhysicsActor actor, changes what, Object arg) | ||
1565 | { | ||
1566 | ODEchangeitem item = new ODEchangeitem(); | ||
1567 | item.actor = actor; | ||
1568 | item.what = what; | ||
1569 | item.arg = arg; | ||
1570 | ChangesQueue.Enqueue(item); | ||
1571 | } | ||
1572 | |||
1573 | /// <summary> | ||
1574 | /// Called after our prim properties are set Scale, position etc. | ||
1575 | /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex | ||
1576 | /// This assures us that we have no race conditions | ||
1577 | /// </summary> | ||
1578 | /// <param name="prim"></param> | ||
1579 | public override void AddPhysicsActorTaint(PhysicsActor prim) | ||
1580 | { | ||
1581 | } | ||
1582 | |||
1583 | // does all pending changes generated during region load process | ||
1584 | public override void PrepareSimulation() | ||
1585 | { | ||
1586 | lock (OdeLock) | ||
1587 | { | ||
1588 | if (world == IntPtr.Zero) | ||
1589 | { | ||
1590 | ChangesQueue.Clear(); | ||
1591 | return; | ||
1592 | } | ||
1593 | |||
1594 | ODEchangeitem item; | ||
1595 | |||
1596 | int donechanges = 0; | ||
1597 | if (ChangesQueue.Count > 0) | ||
1598 | { | ||
1599 | m_log.InfoFormat("[ODE] start processing pending actor operations"); | ||
1600 | int tstart = Util.EnvironmentTickCount(); | ||
1601 | |||
1602 | while (ChangesQueue.Dequeue(out item)) | ||
1603 | { | ||
1604 | if (item.actor != null) | ||
1605 | { | ||
1606 | try | ||
1607 | { | ||
1608 | if (item.actor is OdeCharacter) | ||
1609 | ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); | ||
1610 | else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) | ||
1611 | RemovePrimThreadLocked((OdePrim)item.actor); | ||
1612 | } | ||
1613 | catch | ||
1614 | { | ||
1615 | m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}", | ||
1616 | item.actor.Name, item.what.ToString()); | ||
1617 | } | ||
1618 | } | ||
1619 | donechanges++; | ||
1620 | } | ||
1621 | int time = Util.EnvironmentTickCountSubtract(tstart); | ||
1622 | m_log.InfoFormat("[ODE] finished {0} operations in {1}ms", donechanges, time); | ||
1623 | } | ||
1624 | } | ||
1625 | } | ||
1626 | |||
1627 | /// <summary> | ||
1628 | /// This is our main simulate loop | ||
1629 | /// It's thread locked by a Mutex in the scene. | ||
1630 | /// It holds Collisions, it instructs ODE to step through the physical reactions | ||
1631 | /// It moves the objects around in memory | ||
1632 | /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup) | ||
1633 | /// </summary> | ||
1634 | /// <param name="timeStep"></param> | ||
1635 | /// <returns></returns> | ||
1636 | public override float Simulate(float timeStep) | ||
1637 | { | ||
1638 | DateTime now = DateTime.UtcNow; | ||
1639 | TimeSpan timedif = now - m_lastframe; | ||
1640 | timeStep = (float)timedif.TotalSeconds; | ||
1641 | m_lastframe = now; | ||
1642 | |||
1643 | // acumulate time so we can reduce error | ||
1644 | step_time += timeStep; | ||
1645 | |||
1646 | if (step_time < HalfOdeStep) | ||
1647 | return 0; | ||
1648 | |||
1649 | if (framecount < 0) | ||
1650 | framecount = 0; | ||
1651 | |||
1652 | framecount++; | ||
1653 | |||
1654 | // int curphysiteractions; | ||
1655 | |||
1656 | // if in trouble reduce step resolution | ||
1657 | // if (step_time >= m_SkipFramesAtms) | ||
1658 | // curphysiteractions = m_physicsiterations / 2; | ||
1659 | // else | ||
1660 | // curphysiteractions = m_physicsiterations; | ||
1661 | |||
1662 | // checkThread(); | ||
1663 | int nodeframes = 0; | ||
1664 | |||
1665 | lock (SimulationLock) | ||
1666 | lock(OdeLock) | ||
1667 | { | ||
1668 | if (world == IntPtr.Zero) | ||
1669 | { | ||
1670 | ChangesQueue.Clear(); | ||
1671 | return 0; | ||
1672 | } | ||
1673 | |||
1674 | ODEchangeitem item; | ||
1675 | |||
1676 | // d.WorldSetQuickStepNumIterations(world, curphysiteractions); | ||
1677 | |||
1678 | int loopstartMS = Util.EnvironmentTickCount(); | ||
1679 | int looptimeMS = 0; | ||
1680 | |||
1681 | |||
1682 | while (step_time > HalfOdeStep) | ||
1683 | { | ||
1684 | try | ||
1685 | { | ||
1686 | // clear pointer/counter to contacts to pass into joints | ||
1687 | m_global_contactcount = 0; | ||
1688 | |||
1689 | if (ChangesQueue.Count > 0) | ||
1690 | { | ||
1691 | int changestartMS = Util.EnvironmentTickCount(); | ||
1692 | int ttmp; | ||
1693 | while (ChangesQueue.Dequeue(out item)) | ||
1694 | { | ||
1695 | if (item.actor != null) | ||
1696 | { | ||
1697 | try | ||
1698 | { | ||
1699 | if (item.actor is OdeCharacter) | ||
1700 | ((OdeCharacter)item.actor).DoAChange(item.what, item.arg); | ||
1701 | else if (((OdePrim)item.actor).DoAChange(item.what, item.arg)) | ||
1702 | RemovePrimThreadLocked((OdePrim)item.actor); | ||
1703 | } | ||
1704 | catch | ||
1705 | { | ||
1706 | m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}", | ||
1707 | item.actor.Name, item.what.ToString()); | ||
1708 | } | ||
1709 | } | ||
1710 | ttmp = Util.EnvironmentTickCountSubtract(changestartMS); | ||
1711 | if (ttmp > 20) | ||
1712 | break; | ||
1713 | } | ||
1714 | } | ||
1715 | |||
1716 | // Move characters | ||
1717 | lock (_characters) | ||
1718 | { | ||
1719 | List<OdeCharacter> defects = new List<OdeCharacter>(); | ||
1720 | foreach (OdeCharacter actor in _characters) | ||
1721 | { | ||
1722 | if (actor != null) | ||
1723 | actor.Move(defects); | ||
1724 | } | ||
1725 | if (defects.Count != 0) | ||
1726 | { | ||
1727 | foreach (OdeCharacter defect in defects) | ||
1728 | { | ||
1729 | RemoveCharacter(defect); | ||
1730 | } | ||
1731 | defects.Clear(); | ||
1732 | } | ||
1733 | } | ||
1734 | |||
1735 | // Move other active objects | ||
1736 | lock (_activegroups) | ||
1737 | { | ||
1738 | foreach (OdePrim aprim in _activegroups) | ||
1739 | { | ||
1740 | aprim.Move(); | ||
1741 | } | ||
1742 | } | ||
1743 | |||
1744 | m_rayCastManager.ProcessQueuedRequests(); | ||
1745 | |||
1746 | collision_optimized(); | ||
1747 | |||
1748 | foreach (PhysicsActor obj in _collisionEventPrim) | ||
1749 | { | ||
1750 | if (obj == null) | ||
1751 | continue; | ||
1752 | |||
1753 | switch ((ActorTypes)obj.PhysicsActorType) | ||
1754 | { | ||
1755 | case ActorTypes.Agent: | ||
1756 | OdeCharacter cobj = (OdeCharacter)obj; | ||
1757 | cobj.AddCollisionFrameTime((int)(odetimestepMS)); | ||
1758 | cobj.SendCollisions(); | ||
1759 | break; | ||
1760 | |||
1761 | case ActorTypes.Prim: | ||
1762 | OdePrim pobj = (OdePrim)obj; | ||
1763 | if (pobj.Body == IntPtr.Zero || (d.BodyIsEnabled(pobj.Body) && !pobj.m_outbounds)) | ||
1764 | if (!pobj.m_outbounds) | ||
1765 | { | ||
1766 | pobj.AddCollisionFrameTime((int)(odetimestepMS)); | ||
1767 | pobj.SendCollisions(); | ||
1768 | } | ||
1769 | break; | ||
1770 | } | ||
1771 | } | ||
1772 | |||
1773 | foreach (PhysicsActor obj in _collisionEventPrimRemove) | ||
1774 | _collisionEventPrim.Remove(obj); | ||
1775 | |||
1776 | _collisionEventPrimRemove.Clear(); | ||
1777 | |||
1778 | // do a ode simulation step | ||
1779 | d.WorldQuickStep(world, ODE_STEPSIZE); | ||
1780 | // d.WorldStep(world, ODE_STEPSIZE); | ||
1781 | d.JointGroupEmpty(contactgroup); | ||
1782 | |||
1783 | // update managed ideia of physical data and do updates to core | ||
1784 | /* | ||
1785 | lock (_characters) | ||
1786 | { | ||
1787 | foreach (OdeCharacter actor in _characters) | ||
1788 | { | ||
1789 | if (actor != null) | ||
1790 | { | ||
1791 | if (actor.bad) | ||
1792 | m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid); | ||
1793 | |||
1794 | actor.UpdatePositionAndVelocity(); | ||
1795 | } | ||
1796 | } | ||
1797 | } | ||
1798 | */ | ||
1799 | |||
1800 | lock (_activegroups) | ||
1801 | { | ||
1802 | { | ||
1803 | foreach (OdePrim actor in _activegroups) | ||
1804 | { | ||
1805 | if (actor.IsPhysical) | ||
1806 | { | ||
1807 | actor.UpdatePositionAndVelocity(framecount); | ||
1808 | } | ||
1809 | } | ||
1810 | } | ||
1811 | } | ||
1812 | } | ||
1813 | catch (Exception e) | ||
1814 | { | ||
1815 | m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e); | ||
1816 | // ode.dunlock(world); | ||
1817 | } | ||
1818 | |||
1819 | step_time -= ODE_STEPSIZE; | ||
1820 | nodeframes++; | ||
1821 | |||
1822 | looptimeMS = Util.EnvironmentTickCountSubtract(loopstartMS); | ||
1823 | if (looptimeMS > 100) | ||
1824 | break; | ||
1825 | } | ||
1826 | |||
1827 | lock (_badCharacter) | ||
1828 | { | ||
1829 | if (_badCharacter.Count > 0) | ||
1830 | { | ||
1831 | foreach (OdeCharacter chr in _badCharacter) | ||
1832 | { | ||
1833 | RemoveCharacter(chr); | ||
1834 | } | ||
1835 | |||
1836 | _badCharacter.Clear(); | ||
1837 | } | ||
1838 | } | ||
1839 | |||
1840 | timedif = now - m_lastMeshExpire; | ||
1841 | |||
1842 | if (timedif.Seconds > 10) | ||
1843 | { | ||
1844 | mesher.ExpireReleaseMeshs(); | ||
1845 | m_lastMeshExpire = now; | ||
1846 | } | ||
1847 | |||
1848 | // information block running in debug only | ||
1849 | /* | ||
1850 | int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace); | ||
1851 | int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace); | ||
1852 | int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace); | ||
1853 | |||
1854 | int nactivegeoms = 0; | ||
1855 | int nactivespaces = 0; | ||
1856 | |||
1857 | int nstaticgeoms = 0; | ||
1858 | int nstaticspaces = 0; | ||
1859 | IntPtr sp; | ||
1860 | |||
1861 | for (int i = 0; i < ntopactivegeoms; i++) | ||
1862 | { | ||
1863 | sp = d.SpaceGetGeom(ActiveSpace, i); | ||
1864 | if (d.GeomIsSpace(sp)) | ||
1865 | { | ||
1866 | nactivespaces++; | ||
1867 | nactivegeoms += d.SpaceGetNumGeoms(sp); | ||
1868 | } | ||
1869 | else | ||
1870 | nactivegeoms++; | ||
1871 | } | ||
1872 | |||
1873 | for (int i = 0; i < ntopstaticgeoms; i++) | ||
1874 | { | ||
1875 | sp = d.SpaceGetGeom(StaticSpace, i); | ||
1876 | if (d.GeomIsSpace(sp)) | ||
1877 | { | ||
1878 | nstaticspaces++; | ||
1879 | nstaticgeoms += d.SpaceGetNumGeoms(sp); | ||
1880 | } | ||
1881 | else | ||
1882 | nstaticgeoms++; | ||
1883 | } | ||
1884 | |||
1885 | int ntopgeoms = d.SpaceGetNumGeoms(TopSpace); | ||
1886 | |||
1887 | int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray | ||
1888 | int nbodies = d.NTotalBodies; | ||
1889 | int ngeoms = d.NTotalGeoms; | ||
1890 | */ | ||
1891 | // Finished with all sim stepping. If requested, dump world state to file for debugging. | ||
1892 | // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed? | ||
1893 | // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots? | ||
1894 | if (physics_logging && (physics_logging_interval > 0) && (framecount % physics_logging_interval == 0)) | ||
1895 | { | ||
1896 | string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename | ||
1897 | string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file | ||
1898 | |||
1899 | if (physics_logging_append_existing_logfile) | ||
1900 | { | ||
1901 | string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------"; | ||
1902 | TextWriter fwriter = File.AppendText(fname); | ||
1903 | fwriter.WriteLine(header); | ||
1904 | fwriter.Close(); | ||
1905 | } | ||
1906 | |||
1907 | d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix); | ||
1908 | } | ||
1909 | |||
1910 | // think time dilation as to do with dinamic step size that we dont' have | ||
1911 | // even so tell something to world | ||
1912 | if (looptimeMS < 100) // we did the requested loops | ||
1913 | m_timeDilation = 1.0f; | ||
1914 | else if (step_time > 0) | ||
1915 | { | ||
1916 | m_timeDilation = timeStep / step_time; | ||
1917 | if (m_timeDilation > 1) | ||
1918 | m_timeDilation = 1; | ||
1919 | if (step_time > m_SkipFramesAtms) | ||
1920 | step_time = 0; | ||
1921 | m_lastframe = DateTime.UtcNow; // skip also the time lost | ||
1922 | } | ||
1923 | } | ||
1924 | return (float)nodeframes * ODE_STEPSIZE / timeStep; | ||
1925 | } | ||
1926 | |||
1927 | /// <summary> | ||
1928 | public override void GetResults() | ||
1929 | { | ||
1930 | } | ||
1931 | |||
1932 | public override bool IsThreaded | ||
1933 | { | ||
1934 | // for now we won't be multithreaded | ||
1935 | get { return (false); } | ||
1936 | } | ||
1937 | |||
1938 | public float GetTerrainHeightAtXY(float x, float y) | ||
1939 | { | ||
1940 | |||
1941 | int offsetX = 0; | ||
1942 | int offsetY = 0; | ||
1943 | |||
1944 | if (m_suportCombine) | ||
1945 | { | ||
1946 | offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; | ||
1947 | offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; | ||
1948 | } | ||
1949 | |||
1950 | // get region map | ||
1951 | IntPtr heightFieldGeom = IntPtr.Zero; | ||
1952 | if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) | ||
1953 | return 0f; | ||
1954 | |||
1955 | if (heightFieldGeom == IntPtr.Zero) | ||
1956 | return 0f; | ||
1957 | |||
1958 | if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) | ||
1959 | return 0f; | ||
1960 | |||
1961 | // TerrainHeightField for ODE as offset 1m | ||
1962 | x += 1f - offsetX; | ||
1963 | y += 1f - offsetY; | ||
1964 | |||
1965 | // make position fit into array | ||
1966 | if (x < 0) | ||
1967 | x = 0; | ||
1968 | if (y < 0) | ||
1969 | y = 0; | ||
1970 | |||
1971 | // integer indexs | ||
1972 | int ix; | ||
1973 | int iy; | ||
1974 | // interpolators offset | ||
1975 | float dx; | ||
1976 | float dy; | ||
1977 | |||
1978 | int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples | ||
1979 | int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples | ||
1980 | int regsize = regsizeX; | ||
1981 | |||
1982 | if (OdeUbitLib) | ||
1983 | { | ||
1984 | if (x < regsizeX - 1) | ||
1985 | { | ||
1986 | ix = (int)x; | ||
1987 | dx = x - (float)ix; | ||
1988 | } | ||
1989 | else // out world use external height | ||
1990 | { | ||
1991 | ix = regsizeX - 2; | ||
1992 | dx = 0; | ||
1993 | } | ||
1994 | if (y < regsizeY - 1) | ||
1995 | { | ||
1996 | iy = (int)y; | ||
1997 | dy = y - (float)iy; | ||
1998 | } | ||
1999 | else | ||
2000 | { | ||
2001 | iy = regsizeY - 2; | ||
2002 | dy = 0; | ||
2003 | } | ||
2004 | } | ||
2005 | else | ||
2006 | { | ||
2007 | // we still have square fixed size regions | ||
2008 | // also flip x and y because of how map is done for ODE fliped axis | ||
2009 | // so ix,iy,dx and dy are inter exchanged | ||
2010 | |||
2011 | regsize = regsizeY; | ||
2012 | |||
2013 | if (x < regsizeX - 1) | ||
2014 | { | ||
2015 | iy = (int)x; | ||
2016 | dy = x - (float)iy; | ||
2017 | } | ||
2018 | else // out world use external height | ||
2019 | { | ||
2020 | iy = regsizeX - 2; | ||
2021 | dy = 0; | ||
2022 | } | ||
2023 | if (y < regsizeY - 1) | ||
2024 | { | ||
2025 | ix = (int)y; | ||
2026 | dx = y - (float)ix; | ||
2027 | } | ||
2028 | else | ||
2029 | { | ||
2030 | ix = regsizeY - 2; | ||
2031 | dx = 0; | ||
2032 | } | ||
2033 | } | ||
2034 | |||
2035 | float h0; | ||
2036 | float h1; | ||
2037 | float h2; | ||
2038 | |||
2039 | iy *= regsize; | ||
2040 | iy += ix; // all indexes have iy + ix | ||
2041 | |||
2042 | float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; | ||
2043 | /* | ||
2044 | if ((dx + dy) <= 1.0f) | ||
2045 | { | ||
2046 | h0 = ((float)heights[iy]); // 0,0 vertice | ||
2047 | h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0 | ||
2048 | h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0 | ||
2049 | } | ||
2050 | else | ||
2051 | { | ||
2052 | h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice | ||
2053 | h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0 | ||
2054 | h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1 | ||
2055 | } | ||
2056 | */ | ||
2057 | h0 = ((float)heights[iy]); // 0,0 vertice | ||
2058 | |||
2059 | if (dy>dx) | ||
2060 | { | ||
2061 | iy += regsize; | ||
2062 | h2 = (float)heights[iy]; // 0,1 vertice | ||
2063 | h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0 | ||
2064 | h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1 | ||
2065 | } | ||
2066 | else | ||
2067 | { | ||
2068 | iy++; | ||
2069 | h2 = (float)heights[iy]; // vertice 1,0 | ||
2070 | h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0 | ||
2071 | h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0 | ||
2072 | } | ||
2073 | |||
2074 | return h0 + h1 + h2; | ||
2075 | } | ||
2076 | |||
2077 | public Vector3 GetTerrainNormalAtXY(float x, float y) | ||
2078 | { | ||
2079 | int offsetX = 0; | ||
2080 | int offsetY = 0; | ||
2081 | |||
2082 | if (m_suportCombine) | ||
2083 | { | ||
2084 | offsetX = ((int)(x / (int)Constants.RegionSize)) * (int)Constants.RegionSize; | ||
2085 | offsetY = ((int)(y / (int)Constants.RegionSize)) * (int)Constants.RegionSize; | ||
2086 | } | ||
2087 | |||
2088 | // get region map | ||
2089 | IntPtr heightFieldGeom = IntPtr.Zero; | ||
2090 | Vector3 norm = new Vector3(0, 0, 1); | ||
2091 | |||
2092 | if (!RegionTerrain.TryGetValue(new Vector3(offsetX, offsetY, 0), out heightFieldGeom)) | ||
2093 | return norm; ; | ||
2094 | |||
2095 | if (heightFieldGeom == IntPtr.Zero) | ||
2096 | return norm; | ||
2097 | |||
2098 | if (!TerrainHeightFieldHeights.ContainsKey(heightFieldGeom)) | ||
2099 | return norm; | ||
2100 | |||
2101 | // TerrainHeightField for ODE as offset 1m | ||
2102 | x += 1f - offsetX; | ||
2103 | y += 1f - offsetY; | ||
2104 | |||
2105 | // make position fit into array | ||
2106 | if (x < 0) | ||
2107 | x = 0; | ||
2108 | if (y < 0) | ||
2109 | y = 0; | ||
2110 | |||
2111 | // integer indexs | ||
2112 | int ix; | ||
2113 | int iy; | ||
2114 | // interpolators offset | ||
2115 | float dx; | ||
2116 | float dy; | ||
2117 | |||
2118 | int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples | ||
2119 | int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples | ||
2120 | int regsize = regsizeX; | ||
2121 | |||
2122 | int xstep = 1; | ||
2123 | int ystep = regsizeX; | ||
2124 | bool firstTri = false; | ||
2125 | |||
2126 | if (OdeUbitLib) | ||
2127 | { | ||
2128 | if (x < regsizeX - 1) | ||
2129 | { | ||
2130 | ix = (int)x; | ||
2131 | dx = x - (float)ix; | ||
2132 | } | ||
2133 | else // out world use external height | ||
2134 | { | ||
2135 | ix = regsizeX - 2; | ||
2136 | dx = 0; | ||
2137 | } | ||
2138 | if (y < regsizeY - 1) | ||
2139 | { | ||
2140 | iy = (int)y; | ||
2141 | dy = y - (float)iy; | ||
2142 | } | ||
2143 | else | ||
2144 | { | ||
2145 | iy = regsizeY - 2; | ||
2146 | dy = 0; | ||
2147 | } | ||
2148 | firstTri = dy > dx; | ||
2149 | } | ||
2150 | |||
2151 | else | ||
2152 | { | ||
2153 | xstep = regsizeY; | ||
2154 | ystep = 1; | ||
2155 | regsize = regsizeY; | ||
2156 | |||
2157 | // we still have square fixed size regions | ||
2158 | // also flip x and y because of how map is done for ODE fliped axis | ||
2159 | // so ix,iy,dx and dy are inter exchanged | ||
2160 | if (x < regsizeX - 1) | ||
2161 | { | ||
2162 | iy = (int)x; | ||
2163 | dy = x - (float)iy; | ||
2164 | } | ||
2165 | else // out world use external height | ||
2166 | { | ||
2167 | iy = regsizeX - 2; | ||
2168 | dy = 0; | ||
2169 | } | ||
2170 | if (y < regsizeY - 1) | ||
2171 | { | ||
2172 | ix = (int)y; | ||
2173 | dx = y - (float)ix; | ||
2174 | } | ||
2175 | else | ||
2176 | { | ||
2177 | ix = regsizeY - 2; | ||
2178 | dx = 0; | ||
2179 | } | ||
2180 | firstTri = dx > dy; | ||
2181 | } | ||
2182 | |||
2183 | float h0; | ||
2184 | float h1; | ||
2185 | float h2; | ||
2186 | |||
2187 | iy *= regsize; | ||
2188 | iy += ix; // all indexes have iy + ix | ||
2189 | |||
2190 | float[] heights = TerrainHeightFieldHeights[heightFieldGeom]; | ||
2191 | |||
2192 | if (firstTri) | ||
2193 | { | ||
2194 | h1 = ((float)heights[iy]); // 0,0 vertice | ||
2195 | iy += ystep; | ||
2196 | h0 = (float)heights[iy]; // 0,1 | ||
2197 | h2 = (float)heights[iy+xstep]; // 1,1 vertice | ||
2198 | norm.X = h0 - h2; | ||
2199 | norm.Y = h1 - h0; | ||
2200 | } | ||
2201 | else | ||
2202 | { | ||
2203 | h2 = ((float)heights[iy]); // 0,0 vertice | ||
2204 | iy += xstep; | ||
2205 | h0 = ((float)heights[iy]); // 1,0 vertice | ||
2206 | h1 = (float)heights[iy+ystep]; // vertice 1,1 | ||
2207 | norm.X = h2 - h0; | ||
2208 | norm.Y = h0 - h1; | ||
2209 | } | ||
2210 | norm.Z = 1; | ||
2211 | norm.Normalize(); | ||
2212 | return norm; | ||
2213 | } | ||
2214 | |||
2215 | public override void SetTerrain(float[] heightMap) | ||
2216 | { | ||
2217 | if (m_worldOffset != Vector3.Zero && m_parentScene != null) | ||
2218 | { | ||
2219 | if (m_parentScene is OdeScene) | ||
2220 | { | ||
2221 | ((OdeScene)m_parentScene).SetTerrain(heightMap, m_worldOffset); | ||
2222 | } | ||
2223 | } | ||
2224 | else | ||
2225 | { | ||
2226 | SetTerrain(heightMap, m_worldOffset); | ||
2227 | } | ||
2228 | } | ||
2229 | |||
2230 | public override void CombineTerrain(float[] heightMap, Vector3 pOffset) | ||
2231 | { | ||
2232 | if(m_suportCombine) | ||
2233 | SetTerrain(heightMap, pOffset); | ||
2234 | } | ||
2235 | |||
2236 | public void SetTerrain(float[] heightMap, Vector3 pOffset) | ||
2237 | { | ||
2238 | if (OdeUbitLib) | ||
2239 | UbitSetTerrain(heightMap, pOffset); | ||
2240 | else | ||
2241 | OriSetTerrain(heightMap, pOffset); | ||
2242 | } | ||
2243 | |||
2244 | public void OriSetTerrain(float[] heightMap, Vector3 pOffset) | ||
2245 | { | ||
2246 | // assumes 1m size grid and constante size square regions | ||
2247 | // needs to know about sims around in future | ||
2248 | |||
2249 | float[] _heightmap; | ||
2250 | |||
2251 | uint regionsizeX = m_regionWidth; | ||
2252 | uint regionsizeY = m_regionHeight; | ||
2253 | |||
2254 | // map is rotated | ||
2255 | uint heightmapWidth = regionsizeY + 2; | ||
2256 | uint heightmapHeight = regionsizeX + 2; | ||
2257 | |||
2258 | uint heightmapWidthSamples = heightmapWidth + 1; | ||
2259 | uint heightmapHeightSamples = heightmapHeight + 1; | ||
2260 | |||
2261 | _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; | ||
2262 | |||
2263 | const float scale = 1.0f; | ||
2264 | const float offset = 0.0f; | ||
2265 | const float thickness = 10f; | ||
2266 | const int wrap = 0; | ||
2267 | |||
2268 | |||
2269 | float hfmin = float.MaxValue; | ||
2270 | float hfmax = float.MinValue; | ||
2271 | float val; | ||
2272 | uint xx; | ||
2273 | uint yy; | ||
2274 | |||
2275 | uint maxXX = regionsizeX - 1; | ||
2276 | uint maxYY = regionsizeY - 1; | ||
2277 | // flipping map adding one margin all around so things don't fall in edges | ||
2278 | |||
2279 | uint xt = 0; | ||
2280 | xx = 0; | ||
2281 | |||
2282 | for (uint x = 0; x < heightmapWidthSamples; x++) | ||
2283 | { | ||
2284 | if (x > 1 && xx < maxXX) | ||
2285 | xx++; | ||
2286 | yy = 0; | ||
2287 | for (uint y = 0; y < heightmapHeightSamples; y++) | ||
2288 | { | ||
2289 | if (y > 1 && y < maxYY) | ||
2290 | yy += regionsizeX; | ||
2291 | |||
2292 | val = heightMap[yy + xx]; | ||
2293 | if (val < 0.0f) | ||
2294 | val = 0.0f; // no neg terrain as in chode | ||
2295 | _heightmap[xt + y] = val; | ||
2296 | |||
2297 | if (hfmin > val) | ||
2298 | hfmin = val; | ||
2299 | if (hfmax < val) | ||
2300 | hfmax = val; | ||
2301 | } | ||
2302 | xt += heightmapHeightSamples; | ||
2303 | } | ||
2304 | |||
2305 | lock (OdeLock) | ||
2306 | { | ||
2307 | IntPtr GroundGeom = IntPtr.Zero; | ||
2308 | if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) | ||
2309 | { | ||
2310 | RegionTerrain.Remove(pOffset); | ||
2311 | if (GroundGeom != IntPtr.Zero) | ||
2312 | { | ||
2313 | actor_name_map.Remove(GroundGeom); | ||
2314 | d.GeomDestroy(GroundGeom); | ||
2315 | |||
2316 | if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) | ||
2317 | { | ||
2318 | TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); | ||
2319 | TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); | ||
2320 | TerrainHeightFieldHeights.Remove(GroundGeom); | ||
2321 | } | ||
2322 | } | ||
2323 | } | ||
2324 | IntPtr HeightmapData = d.GeomHeightfieldDataCreate(); | ||
2325 | |||
2326 | GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); | ||
2327 | |||
2328 | d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, heightmapWidth , heightmapHeight, | ||
2329 | (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale, | ||
2330 | offset, thickness, wrap); | ||
2331 | |||
2332 | d.GeomHeightfieldDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); | ||
2333 | |||
2334 | GroundGeom = d.CreateHeightfield(GroundSpace, HeightmapData, 1); | ||
2335 | |||
2336 | if (GroundGeom != IntPtr.Zero) | ||
2337 | { | ||
2338 | d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); | ||
2339 | d.GeomSetCollideBits(GroundGeom, 0); | ||
2340 | |||
2341 | PhysicsActor pa = new NullPhysicsActor(); | ||
2342 | pa.Name = "Terrain"; | ||
2343 | pa.PhysicsActorType = (int)ActorTypes.Ground; | ||
2344 | actor_name_map[GroundGeom] = pa; | ||
2345 | |||
2346 | // geom_name_map[GroundGeom] = "Terrain"; | ||
2347 | |||
2348 | d.Matrix3 R = new d.Matrix3(); | ||
2349 | |||
2350 | Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f); | ||
2351 | Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f); | ||
2352 | |||
2353 | |||
2354 | q1 = q1 * q2; | ||
2355 | |||
2356 | Vector3 v3; | ||
2357 | float angle; | ||
2358 | q1.GetAxisAngle(out v3, out angle); | ||
2359 | |||
2360 | d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle); | ||
2361 | d.GeomSetRotation(GroundGeom, ref R); | ||
2362 | d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0); | ||
2363 | RegionTerrain.Add(pOffset, GroundGeom); | ||
2364 | TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); | ||
2365 | TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); | ||
2366 | } | ||
2367 | } | ||
2368 | } | ||
2369 | |||
2370 | public void UbitSetTerrain(float[] heightMap, Vector3 pOffset) | ||
2371 | { | ||
2372 | // assumes 1m size grid and constante size square regions | ||
2373 | // needs to know about sims around in future | ||
2374 | |||
2375 | float[] _heightmap; | ||
2376 | |||
2377 | uint regionsizeX = m_regionWidth; | ||
2378 | uint regionsizeY = m_regionHeight; | ||
2379 | |||
2380 | uint heightmapWidth = regionsizeX + 2; | ||
2381 | uint heightmapHeight = regionsizeY + 2; | ||
2382 | |||
2383 | uint heightmapWidthSamples = heightmapWidth + 1; | ||
2384 | uint heightmapHeightSamples = heightmapHeight + 1; | ||
2385 | |||
2386 | _heightmap = new float[heightmapWidthSamples * heightmapHeightSamples]; | ||
2387 | |||
2388 | |||
2389 | float hfmin = float.MaxValue; | ||
2390 | // float hfmax = float.MinValue; | ||
2391 | float val; | ||
2392 | |||
2393 | |||
2394 | uint maxXX = regionsizeX - 1; | ||
2395 | uint maxYY = regionsizeY - 1; | ||
2396 | // adding one margin all around so things don't fall in edges | ||
2397 | |||
2398 | uint xx; | ||
2399 | uint yy = 0; | ||
2400 | uint yt = 0; | ||
2401 | |||
2402 | for (uint y = 0; y < heightmapHeightSamples; y++) | ||
2403 | { | ||
2404 | if (y > 1 && y < maxYY) | ||
2405 | yy += regionsizeX; | ||
2406 | xx = 0; | ||
2407 | for (uint x = 0; x < heightmapWidthSamples; x++) | ||
2408 | { | ||
2409 | if (x > 1 && x < maxXX) | ||
2410 | xx++; | ||
2411 | |||
2412 | val = heightMap[yy + xx]; | ||
2413 | if (val < 0.0f) | ||
2414 | val = 0.0f; // no neg terrain as in chode | ||
2415 | _heightmap[yt + x] = val; | ||
2416 | |||
2417 | if (hfmin > val) | ||
2418 | hfmin = val; | ||
2419 | // if (hfmax < val) | ||
2420 | // hfmax = val; | ||
2421 | } | ||
2422 | yt += heightmapWidthSamples; | ||
2423 | } | ||
2424 | lock (OdeLock) | ||
2425 | { | ||
2426 | IntPtr GroundGeom = IntPtr.Zero; | ||
2427 | if (RegionTerrain.TryGetValue(pOffset, out GroundGeom)) | ||
2428 | { | ||
2429 | RegionTerrain.Remove(pOffset); | ||
2430 | if (GroundGeom != IntPtr.Zero) | ||
2431 | { | ||
2432 | actor_name_map.Remove(GroundGeom); | ||
2433 | d.GeomDestroy(GroundGeom); | ||
2434 | |||
2435 | if (TerrainHeightFieldHeights.ContainsKey(GroundGeom)) | ||
2436 | { | ||
2437 | if (TerrainHeightFieldHeightsHandlers[GroundGeom].IsAllocated) | ||
2438 | TerrainHeightFieldHeightsHandlers[GroundGeom].Free(); | ||
2439 | TerrainHeightFieldHeightsHandlers.Remove(GroundGeom); | ||
2440 | TerrainHeightFieldHeights.Remove(GroundGeom); | ||
2441 | } | ||
2442 | } | ||
2443 | } | ||
2444 | IntPtr HeightmapData = d.GeomUbitTerrainDataCreate(); | ||
2445 | |||
2446 | const int wrap = 0; | ||
2447 | float thickness = hfmin; | ||
2448 | if (thickness < 0) | ||
2449 | thickness = 1; | ||
2450 | |||
2451 | GCHandle _heightmaphandler = GCHandle.Alloc(_heightmap, GCHandleType.Pinned); | ||
2452 | |||
2453 | d.GeomUbitTerrainDataBuild(HeightmapData, _heightmaphandler.AddrOfPinnedObject(), 0, 1.0f, | ||
2454 | (int)heightmapWidthSamples, (int)heightmapHeightSamples, | ||
2455 | thickness, wrap); | ||
2456 | |||
2457 | // d.GeomUbitTerrainDataSetBounds(HeightmapData, hfmin - 1, hfmax + 1); | ||
2458 | GroundGeom = d.CreateUbitTerrain(GroundSpace, HeightmapData, 1); | ||
2459 | if (GroundGeom != IntPtr.Zero) | ||
2460 | { | ||
2461 | d.GeomSetCategoryBits(GroundGeom, (uint)(CollisionCategories.Land)); | ||
2462 | d.GeomSetCollideBits(GroundGeom, 0); | ||
2463 | |||
2464 | |||
2465 | PhysicsActor pa = new NullPhysicsActor(); | ||
2466 | pa.Name = "Terrain"; | ||
2467 | pa.PhysicsActorType = (int)ActorTypes.Ground; | ||
2468 | actor_name_map[GroundGeom] = pa; | ||
2469 | |||
2470 | // geom_name_map[GroundGeom] = "Terrain"; | ||
2471 | |||
2472 | d.GeomSetPosition(GroundGeom, pOffset.X + m_regionWidth * 0.5f, pOffset.Y + m_regionHeight * 0.5f, 0); | ||
2473 | RegionTerrain.Add(pOffset, GroundGeom); | ||
2474 | TerrainHeightFieldHeights.Add(GroundGeom, _heightmap); | ||
2475 | TerrainHeightFieldHeightsHandlers.Add(GroundGeom, _heightmaphandler); | ||
2476 | } | ||
2477 | } | ||
2478 | } | ||
2479 | |||
2480 | |||
2481 | public override void DeleteTerrain() | ||
2482 | { | ||
2483 | } | ||
2484 | |||
2485 | public float GetWaterLevel() | ||
2486 | { | ||
2487 | return waterlevel; | ||
2488 | } | ||
2489 | |||
2490 | public override bool SupportsCombining() | ||
2491 | { | ||
2492 | return m_suportCombine; | ||
2493 | } | ||
2494 | /* | ||
2495 | public override void UnCombine(PhysicsScene pScene) | ||
2496 | { | ||
2497 | IntPtr localGround = IntPtr.Zero; | ||
2498 | // float[] localHeightfield; | ||
2499 | bool proceed = false; | ||
2500 | List<IntPtr> geomDestroyList = new List<IntPtr>(); | ||
2501 | |||
2502 | lock (OdeLock) | ||
2503 | { | ||
2504 | if (RegionTerrain.TryGetValue(Vector3.Zero, out localGround)) | ||
2505 | { | ||
2506 | foreach (IntPtr geom in TerrainHeightFieldHeights.Keys) | ||
2507 | { | ||
2508 | if (geom == localGround) | ||
2509 | { | ||
2510 | // localHeightfield = TerrainHeightFieldHeights[geom]; | ||
2511 | proceed = true; | ||
2512 | } | ||
2513 | else | ||
2514 | { | ||
2515 | geomDestroyList.Add(geom); | ||
2516 | } | ||
2517 | } | ||
2518 | |||
2519 | if (proceed) | ||
2520 | { | ||
2521 | m_worldOffset = Vector3.Zero; | ||
2522 | WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize); | ||
2523 | m_parentScene = null; | ||
2524 | |||
2525 | foreach (IntPtr g in geomDestroyList) | ||
2526 | { | ||
2527 | // removingHeightField needs to be done or the garbage collector will | ||
2528 | // collect the terrain data before we tell ODE to destroy it causing | ||
2529 | // memory corruption | ||
2530 | if (TerrainHeightFieldHeights.ContainsKey(g)) | ||
2531 | { | ||
2532 | // float[] removingHeightField = TerrainHeightFieldHeights[g]; | ||
2533 | TerrainHeightFieldHeights.Remove(g); | ||
2534 | |||
2535 | if (RegionTerrain.ContainsKey(g)) | ||
2536 | { | ||
2537 | RegionTerrain.Remove(g); | ||
2538 | } | ||
2539 | |||
2540 | d.GeomDestroy(g); | ||
2541 | //removingHeightField = new float[0]; | ||
2542 | } | ||
2543 | } | ||
2544 | |||
2545 | } | ||
2546 | else | ||
2547 | { | ||
2548 | m_log.Warn("[PHYSICS]: Couldn't proceed with UnCombine. Region has inconsistant data."); | ||
2549 | } | ||
2550 | } | ||
2551 | } | ||
2552 | } | ||
2553 | */ | ||
2554 | public override void SetWaterLevel(float baseheight) | ||
2555 | { | ||
2556 | waterlevel = baseheight; | ||
2557 | } | ||
2558 | |||
2559 | public override void Dispose() | ||
2560 | { | ||
2561 | if (m_meshWorker != null) | ||
2562 | m_meshWorker.Stop(); | ||
2563 | |||
2564 | lock (OdeLock) | ||
2565 | { | ||
2566 | m_rayCastManager.Dispose(); | ||
2567 | m_rayCastManager = null; | ||
2568 | |||
2569 | lock (_prims) | ||
2570 | { | ||
2571 | ChangesQueue.Clear(); | ||
2572 | foreach (OdePrim prm in _prims) | ||
2573 | { | ||
2574 | prm.DoAChange(changes.Remove, null); | ||
2575 | _collisionEventPrim.Remove(prm); | ||
2576 | } | ||
2577 | _prims.Clear(); | ||
2578 | } | ||
2579 | |||
2580 | OdeCharacter[] chtorem; | ||
2581 | lock (_characters) | ||
2582 | { | ||
2583 | chtorem = new OdeCharacter[_characters.Count]; | ||
2584 | _characters.CopyTo(chtorem); | ||
2585 | } | ||
2586 | |||
2587 | ChangesQueue.Clear(); | ||
2588 | foreach (OdeCharacter ch in chtorem) | ||
2589 | ch.DoAChange(changes.Remove, null); | ||
2590 | |||
2591 | |||
2592 | foreach (IntPtr GroundGeom in RegionTerrain.Values) | ||
2593 | { | ||
2594 | if (GroundGeom != IntPtr.Zero) | ||
2595 | d.GeomDestroy(GroundGeom); | ||
2596 | } | ||
2597 | |||
2598 | |||
2599 | RegionTerrain.Clear(); | ||
2600 | |||
2601 | if (TerrainHeightFieldHeightsHandlers.Count > 0) | ||
2602 | { | ||
2603 | foreach (GCHandle gch in TerrainHeightFieldHeightsHandlers.Values) | ||
2604 | { | ||
2605 | if (gch.IsAllocated) | ||
2606 | gch.Free(); | ||
2607 | } | ||
2608 | } | ||
2609 | |||
2610 | TerrainHeightFieldHeightsHandlers.Clear(); | ||
2611 | TerrainHeightFieldHeights.Clear(); | ||
2612 | |||
2613 | if (ContactgeomsArray != IntPtr.Zero) | ||
2614 | Marshal.FreeHGlobal(ContactgeomsArray); | ||
2615 | if (GlobalContactsArray != IntPtr.Zero) | ||
2616 | Marshal.FreeHGlobal(GlobalContactsArray); | ||
2617 | |||
2618 | |||
2619 | d.WorldDestroy(world); | ||
2620 | world = IntPtr.Zero; | ||
2621 | //d.CloseODE(); | ||
2622 | } | ||
2623 | } | ||
2624 | |||
2625 | public override Dictionary<uint, float> GetTopColliders() | ||
2626 | { | ||
2627 | Dictionary<uint, float> returncolliders = new Dictionary<uint, float>(); | ||
2628 | int cnt = 0; | ||
2629 | lock (_prims) | ||
2630 | { | ||
2631 | foreach (OdePrim prm in _prims) | ||
2632 | { | ||
2633 | if (prm.CollisionScore > 0) | ||
2634 | { | ||
2635 | returncolliders.Add(prm.LocalID, prm.CollisionScore); | ||
2636 | cnt++; | ||
2637 | prm.CollisionScore = 0f; | ||
2638 | if (cnt > 25) | ||
2639 | { | ||
2640 | break; | ||
2641 | } | ||
2642 | } | ||
2643 | } | ||
2644 | } | ||
2645 | return returncolliders; | ||
2646 | } | ||
2647 | |||
2648 | public override bool SupportsRayCast() | ||
2649 | { | ||
2650 | return true; | ||
2651 | } | ||
2652 | |||
2653 | public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod) | ||
2654 | { | ||
2655 | if (retMethod != null) | ||
2656 | { | ||
2657 | ODERayRequest req = new ODERayRequest(); | ||
2658 | req.actor = null; | ||
2659 | req.callbackMethod = retMethod; | ||
2660 | req.length = length; | ||
2661 | req.Normal = direction; | ||
2662 | req.Origin = position; | ||
2663 | req.Count = 0; | ||
2664 | req.filter = RayFilterFlags.AllPrims; | ||
2665 | |||
2666 | m_rayCastManager.QueueRequest(req); | ||
2667 | } | ||
2668 | } | ||
2669 | |||
2670 | public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod) | ||
2671 | { | ||
2672 | if (retMethod != null) | ||
2673 | { | ||
2674 | ODERayRequest req = new ODERayRequest(); | ||
2675 | req.actor = null; | ||
2676 | req.callbackMethod = retMethod; | ||
2677 | req.length = length; | ||
2678 | req.Normal = direction; | ||
2679 | req.Origin = position; | ||
2680 | req.Count = Count; | ||
2681 | req.filter = RayFilterFlags.AllPrims; | ||
2682 | |||
2683 | m_rayCastManager.QueueRequest(req); | ||
2684 | } | ||
2685 | } | ||
2686 | |||
2687 | |||
2688 | public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count) | ||
2689 | { | ||
2690 | List<ContactResult> ourresults = new List<ContactResult>(); | ||
2691 | object SyncObject = new object(); | ||
2692 | |||
2693 | RayCallback retMethod = delegate(List<ContactResult> results) | ||
2694 | { | ||
2695 | lock (SyncObject) | ||
2696 | { | ||
2697 | ourresults = results; | ||
2698 | Monitor.PulseAll(SyncObject); | ||
2699 | } | ||
2700 | }; | ||
2701 | |||
2702 | ODERayRequest req = new ODERayRequest(); | ||
2703 | req.actor = null; | ||
2704 | req.callbackMethod = retMethod; | ||
2705 | req.length = length; | ||
2706 | req.Normal = direction; | ||
2707 | req.Origin = position; | ||
2708 | req.Count = Count; | ||
2709 | req.filter = RayFilterFlags.AllPrims; | ||
2710 | |||
2711 | lock (SyncObject) | ||
2712 | { | ||
2713 | m_rayCastManager.QueueRequest(req); | ||
2714 | if (!Monitor.Wait(SyncObject, 500)) | ||
2715 | return null; | ||
2716 | else | ||
2717 | return ourresults; | ||
2718 | } | ||
2719 | } | ||
2720 | |||
2721 | public override bool SupportsRaycastWorldFiltered() | ||
2722 | { | ||
2723 | return true; | ||
2724 | } | ||
2725 | |||
2726 | public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter) | ||
2727 | { | ||
2728 | object SyncObject = new object(); | ||
2729 | List<ContactResult> ourresults = new List<ContactResult>(); | ||
2730 | |||
2731 | RayCallback retMethod = delegate(List<ContactResult> results) | ||
2732 | { | ||
2733 | lock (SyncObject) | ||
2734 | { | ||
2735 | ourresults = results; | ||
2736 | Monitor.PulseAll(SyncObject); | ||
2737 | } | ||
2738 | }; | ||
2739 | |||
2740 | ODERayRequest req = new ODERayRequest(); | ||
2741 | req.actor = null; | ||
2742 | req.callbackMethod = retMethod; | ||
2743 | req.length = length; | ||
2744 | req.Normal = direction; | ||
2745 | req.Origin = position; | ||
2746 | req.Count = Count; | ||
2747 | req.filter = filter; | ||
2748 | |||
2749 | lock (SyncObject) | ||
2750 | { | ||
2751 | m_rayCastManager.QueueRequest(req); | ||
2752 | if (!Monitor.Wait(SyncObject, 500)) | ||
2753 | return null; | ||
2754 | else | ||
2755 | return ourresults; | ||
2756 | } | ||
2757 | } | ||
2758 | |||
2759 | public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags) | ||
2760 | { | ||
2761 | if (actor == null) | ||
2762 | return new List<ContactResult>(); | ||
2763 | |||
2764 | IntPtr geom; | ||
2765 | if (actor is OdePrim) | ||
2766 | geom = ((OdePrim)actor).prim_geom; | ||
2767 | else if (actor is OdeCharacter) | ||
2768 | geom = ((OdePrim)actor).prim_geom; | ||
2769 | else | ||
2770 | return new List<ContactResult>(); | ||
2771 | |||
2772 | if (geom == IntPtr.Zero) | ||
2773 | return new List<ContactResult>(); | ||
2774 | |||
2775 | List<ContactResult> ourResults = null; | ||
2776 | object SyncObject = new object(); | ||
2777 | |||
2778 | RayCallback retMethod = delegate(List<ContactResult> results) | ||
2779 | { | ||
2780 | lock (SyncObject) | ||
2781 | { | ||
2782 | ourResults = results; | ||
2783 | Monitor.PulseAll(SyncObject); | ||
2784 | } | ||
2785 | }; | ||
2786 | |||
2787 | ODERayRequest req = new ODERayRequest(); | ||
2788 | req.actor = actor; | ||
2789 | req.callbackMethod = retMethod; | ||
2790 | req.length = length; | ||
2791 | req.Normal = direction; | ||
2792 | req.Origin = position; | ||
2793 | req.Count = Count; | ||
2794 | req.filter = flags; | ||
2795 | |||
2796 | lock (SyncObject) | ||
2797 | { | ||
2798 | m_rayCastManager.QueueRequest(req); | ||
2799 | if (!Monitor.Wait(SyncObject, 500)) | ||
2800 | return new List<ContactResult>(); | ||
2801 | } | ||
2802 | |||
2803 | if (ourResults == null) | ||
2804 | return new List<ContactResult>(); | ||
2805 | return ourResults; | ||
2806 | } | ||
2807 | |||
2808 | public override List<ContactResult> BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags) | ||
2809 | { | ||
2810 | List<ContactResult> ourResults = null; | ||
2811 | object SyncObject = new object(); | ||
2812 | |||
2813 | ProbeBoxCallback retMethod = delegate(List<ContactResult> results) | ||
2814 | { | ||
2815 | lock (SyncObject) | ||
2816 | { | ||
2817 | ourResults = results; | ||
2818 | Monitor.PulseAll(SyncObject); | ||
2819 | } | ||
2820 | }; | ||
2821 | |||
2822 | ODERayRequest req = new ODERayRequest(); | ||
2823 | req.actor = null; | ||
2824 | req.callbackMethod = retMethod; | ||
2825 | req.Normal = size; | ||
2826 | req.Origin = position; | ||
2827 | req.orientation = orientation; | ||
2828 | req.Count = Count; | ||
2829 | req.filter = flags; | ||
2830 | |||
2831 | lock (SyncObject) | ||
2832 | { | ||
2833 | m_rayCastManager.QueueRequest(req); | ||
2834 | if (!Monitor.Wait(SyncObject, 500)) | ||
2835 | return new List<ContactResult>(); | ||
2836 | } | ||
2837 | |||
2838 | if (ourResults == null) | ||
2839 | return new List<ContactResult>(); | ||
2840 | return ourResults; | ||
2841 | } | ||
2842 | |||
2843 | public override List<ContactResult> SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags) | ||
2844 | { | ||
2845 | List<ContactResult> ourResults = null; | ||
2846 | object SyncObject = new object(); | ||
2847 | |||
2848 | ProbeSphereCallback retMethod = delegate(List<ContactResult> results) | ||
2849 | { | ||
2850 | ourResults = results; | ||
2851 | Monitor.PulseAll(SyncObject); | ||
2852 | }; | ||
2853 | |||
2854 | ODERayRequest req = new ODERayRequest(); | ||
2855 | req.actor = null; | ||
2856 | req.callbackMethod = retMethod; | ||
2857 | req.length = radius; | ||
2858 | req.Origin = position; | ||
2859 | req.Count = Count; | ||
2860 | req.filter = flags; | ||
2861 | |||
2862 | |||
2863 | lock (SyncObject) | ||
2864 | { | ||
2865 | m_rayCastManager.QueueRequest(req); | ||
2866 | if (!Monitor.Wait(SyncObject, 500)) | ||
2867 | return new List<ContactResult>(); | ||
2868 | } | ||
2869 | |||
2870 | if (ourResults == null) | ||
2871 | return new List<ContactResult>(); | ||
2872 | return ourResults; | ||
2873 | } | ||
2874 | |||
2875 | public override List<ContactResult> PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags) | ||
2876 | { | ||
2877 | IntPtr geom = IntPtr.Zero;; | ||
2878 | |||
2879 | if (actor != null) | ||
2880 | { | ||
2881 | if (actor is OdePrim) | ||
2882 | geom = ((OdePrim)actor).prim_geom; | ||
2883 | else if (actor is OdeCharacter) | ||
2884 | geom = ((OdePrim)actor).prim_geom; | ||
2885 | } | ||
2886 | |||
2887 | List<ContactResult> ourResults = null; | ||
2888 | object SyncObject = new object(); | ||
2889 | |||
2890 | ProbePlaneCallback retMethod = delegate(List<ContactResult> results) | ||
2891 | { | ||
2892 | ourResults = results; | ||
2893 | Monitor.PulseAll(SyncObject); | ||
2894 | }; | ||
2895 | |||
2896 | ODERayRequest req = new ODERayRequest(); | ||
2897 | req.actor = null; | ||
2898 | req.callbackMethod = retMethod; | ||
2899 | req.length = plane.W; | ||
2900 | req.Normal.X = plane.X; | ||
2901 | req.Normal.Y = plane.Y; | ||
2902 | req.Normal.Z = plane.Z; | ||
2903 | req.Count = Count; | ||
2904 | req.filter = flags; | ||
2905 | |||
2906 | lock (SyncObject) | ||
2907 | { | ||
2908 | m_rayCastManager.QueueRequest(req); | ||
2909 | if (!Monitor.Wait(SyncObject, 500)) | ||
2910 | return new List<ContactResult>(); | ||
2911 | } | ||
2912 | |||
2913 | if (ourResults == null) | ||
2914 | return new List<ContactResult>(); | ||
2915 | return ourResults; | ||
2916 | } | ||
2917 | |||
2918 | public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse) | ||
2919 | { | ||
2920 | Util.FireAndForget( delegate | ||
2921 | { | ||
2922 | ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager); | ||
2923 | if(sitAvatar != null) | ||
2924 | sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse); | ||
2925 | }); | ||
2926 | return 1; | ||
2927 | } | ||
2928 | |||
2929 | } | ||
2930 | } | ||