ODEScene.cs 91 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464
  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. // Revision 2011/12/13 by Ubit Umarov
  28. using System;
  29. using System.Collections.Concurrent;
  30. using System.Collections.Generic;
  31. using System.Linq;
  32. using System.Reflection;
  33. using System.Runtime.InteropServices;
  34. using System.Threading;
  35. using log4net;
  36. using Nini.Config;
  37. using OpenSim.Framework;
  38. using OpenSim.Region.Framework.Scenes;
  39. using OpenSim.Region.PhysicsModules.SharedBase;
  40. using OpenMetaverse;
  41. namespace OpenSim.Region.PhysicsModule.ubOde
  42. {
  43. // colision flags of things others can colide with
  44. // rays, sensors, probes removed since can't be colided with
  45. // The top space where things are placed provided further selection
  46. // ie physical are in active space nonphysical in static
  47. // this should be exclusive as possible
  48. [Flags]
  49. public enum CollisionCategories : uint
  50. {
  51. Disabled = 0,
  52. //by 'things' types
  53. Space = 0x01,
  54. Geom = 0x02, // aka prim/part
  55. Character = 0x04,
  56. Land = 0x08,
  57. Water = 0x010,
  58. // by state
  59. Phantom = 0x01000,
  60. VolumeDtc = 0x02000,
  61. Selected = 0x04000,
  62. NoShape = 0x08000,
  63. All = 0xffffffff
  64. }
  65. /// <summary>
  66. /// Material type for a primitive
  67. /// </summary>
  68. public enum Material : int
  69. {
  70. /// <summary></summary>
  71. Stone = 0,
  72. /// <summary></summary>
  73. Metal = 1,
  74. /// <summary></summary>
  75. Glass = 2,
  76. /// <summary></summary>
  77. Wood = 3,
  78. /// <summary></summary>
  79. Flesh = 4,
  80. /// <summary></summary>
  81. Plastic = 5,
  82. /// <summary></summary>
  83. Rubber = 6,
  84. light = 7 // compatibility with old viewers
  85. }
  86. public enum changes : int
  87. {
  88. Add = 0, // arg null. finishs the prim creation. should be used internally only ( to remove later ?)
  89. Remove,
  90. Link, // arg AuroraODEPrim new parent prim or null to delink. Makes the prim part of a object with prim parent as root
  91. // or removes from a object if arg is null
  92. DeLink,
  93. Position, // arg Vector3 new position in world coords. Changes prim position. Prim must know if it is root or child
  94. Orientation, // arg Quaternion new orientation in world coords. Changes prim position. Prim must know it it is root or child
  95. PosOffset, // not in use
  96. // arg Vector3 new position in local coords. Changes prim position in object
  97. OriOffset, // not in use
  98. // arg Vector3 new position in local coords. Changes prim position in object
  99. Velocity,
  100. TargetVelocity,
  101. AngVelocity,
  102. Acceleration,
  103. Force,
  104. Torque,
  105. Momentum,
  106. AddForce,
  107. AddAngForce,
  108. AngLock,
  109. Buoyancy,
  110. PIDTarget,
  111. PIDTau,
  112. PIDActive,
  113. PIDHoverHeight,
  114. PIDHoverType,
  115. PIDHoverTau,
  116. PIDHoverActive,
  117. Size,
  118. AvatarSize,
  119. Shape,
  120. PhysRepData,
  121. AddPhysRep,
  122. CollidesWater,
  123. VolumeDtc,
  124. Physical,
  125. Phantom,
  126. Selected,
  127. disabled,
  128. building,
  129. VehicleType,
  130. VehicleFloatParam,
  131. VehicleVectorParam,
  132. VehicleRotationParam,
  133. VehicleFlags,
  134. SetVehicle,
  135. SetInertia,
  136. Null //keep this last used do dim the methods array. does nothing but pulsing the prim
  137. }
  138. public struct ODEchangeitem
  139. {
  140. public PhysicsActor actor;
  141. public changes what;
  142. public Object arg;
  143. }
  144. public class ODEScene : PhysicsScene
  145. {
  146. private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
  147. public bool m_OSOdeLib = false;
  148. public Scene m_frameWorkScene = null;
  149. // private int threadid = 0;
  150. // const d.ContactFlags comumContactFlags = d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM |d.ContactFlags.Approx1 | d.ContactFlags.Bounce;
  151. // const d.ContactFlags comumContactFlags = d.ContactFlags.Bounce | d.ContactFlags.Approx1 | d.ContactFlags.Slip1 | d.ContactFlags.Slip2;
  152. const SafeNativeMethods.ContactFlags comumContactFlags = SafeNativeMethods.ContactFlags.Bounce | SafeNativeMethods.ContactFlags.Approx1;
  153. const float comumContactERP = 0.75f;
  154. const float comumContactCFM = 0.0001f;
  155. const float comumContactSLIP = 0f;
  156. // float frictionMovementMult = 0.2f;
  157. float TerrainBounce = 0.001f;
  158. float TerrainFriction = 0.3f;
  159. public float AvatarFriction = 0;// 0.9f * 0.5f;
  160. // this netx dimensions are only relevant for terrain partition (mega regions)
  161. // WorldExtents below has the simulation dimensions
  162. // they should be identical except on mega regions
  163. private int m_regionWidth = (int)Constants.RegionSize;
  164. private int m_regionHeight = (int)Constants.RegionSize;
  165. public float ODE_STEPSIZE = 0.020f;
  166. public float HalfOdeStep = 0.01f;
  167. public int odetimestepMS = 20; // rounded
  168. private float m_timeDilation = 1.0f;
  169. private double m_lastframe;
  170. private double m_lastMeshExpire;
  171. public float gravityx = 0f;
  172. public float gravityy = 0f;
  173. public float gravityz = -9.8f;
  174. private float waterlevel = 0f;
  175. private int framecount = 0;
  176. private float avDensity = 80f;
  177. private float avMovementDivisorWalk = 1.3f;
  178. private float avMovementDivisorRun = 0.8f;
  179. private float minimumGroundFlightOffset = 3f;
  180. public float maximumMassObject = 10000.01f;
  181. public float geomDefaultDensity = 10.0f;
  182. public float maximumAngularVelocity = 12.0f; // default 12rad/s
  183. public float maxAngVelocitySQ = 144f; // squared value
  184. public float bodyPIDD = 35f;
  185. public float bodyPIDG = 25;
  186. public int bodyFramesAutoDisable = 10;
  187. private SafeNativeMethods.NearCallback nearCallback;
  188. private Dictionary<uint,OdePrim> _prims = new Dictionary<uint,OdePrim>();
  189. private HashSet<OdeCharacter> _characters = new HashSet<OdeCharacter>();
  190. private HashSet<OdePrim> _activeprims = new HashSet<OdePrim>();
  191. private HashSet<OdePrim> _activegroups = new HashSet<OdePrim>();
  192. public ConcurrentQueue<ODEchangeitem> ChangesQueue = new ConcurrentQueue<ODEchangeitem>();
  193. /// <summary>
  194. /// A list of actors that should receive collision events.
  195. /// </summary>
  196. private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
  197. private List<PhysicsActor> _collisionEventPrimRemove = new List<PhysicsActor>();
  198. private HashSet<OdeCharacter> _badCharacter = new HashSet<OdeCharacter>();
  199. public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
  200. private float contactsurfacelayer = 0.002f;
  201. private int contactsPerCollision = 80;
  202. internal IntPtr ContactgeomsArray = IntPtr.Zero;
  203. private IntPtr GlobalContactsArray = IntPtr.Zero;
  204. private SafeNativeMethods.Contact SharedTmpcontact = new SafeNativeMethods.Contact();
  205. const int maxContactsbeforedeath = 6000;
  206. private volatile int m_global_contactcount = 0;
  207. private IntPtr contactgroup;
  208. public ContactData[] m_materialContactsData = new ContactData[8];
  209. private IntPtr m_terrainGeom;
  210. private float[] m_terrainHeights;
  211. private GCHandle m_terrainHeightsHandler = new GCHandle();
  212. private IntPtr HeightmapData;
  213. private int m_lastRegionWidth;
  214. private int m_lastRegionHeight;
  215. private int m_physicsiterations = 15;
  216. private const float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
  217. // private PhysicsActor PANull = new NullPhysicsActor();
  218. private float step_time = 0.0f;
  219. public IntPtr world;
  220. // split the spaces acording to contents type
  221. // ActiveSpace contains characters and active prims
  222. // StaticSpace contains land and other that is mostly static in enviroment
  223. // this can contain subspaces, like the grid in staticspace
  224. // as now space only contains this 2 top spaces
  225. public IntPtr TopSpace; // the global space
  226. public IntPtr ActiveSpace; // space for active prims
  227. public IntPtr CharsSpace; // space for active prims
  228. public IntPtr StaticSpace; // space for the static things around
  229. public IntPtr GroundSpace; // space for ground
  230. public object OdeLock = new object();
  231. public static object SimulationLock = new object();
  232. public IMesher mesher;
  233. public IConfigSource m_config;
  234. public Vector2 WorldExtents = new Vector2((int)Constants.RegionSize, (int)Constants.RegionSize);
  235. private ODERayCastRequestManager m_rayCastManager;
  236. public ODEMeshWorker m_meshWorker;
  237. /* maybe needed if ode uses tls
  238. private void checkThread()
  239. {
  240. int th = Thread.CurrentThread.ManagedThreadId;
  241. if(th != threadid)
  242. {
  243. threadid = th;
  244. d.AllocateODEDataForThread(~0U);
  245. }
  246. }
  247. */
  248. IConfig physicsconfig = null;
  249. public ODEScene(Scene pscene, IConfigSource psourceconfig, string pname, string pversion, bool pOSOdeLib)
  250. {
  251. EngineType = pname;
  252. PhysicsSceneName = EngineType + "/" + pscene.RegionInfo.RegionName;
  253. EngineName = pname + " " + pversion;
  254. m_config = psourceconfig;
  255. m_OSOdeLib = pOSOdeLib;
  256. // m_OSOdeLib = false; //debug
  257. m_frameWorkScene = pscene;
  258. m_frameWorkScene.RegisterModuleInterface<PhysicsScene>(this);
  259. Initialization();
  260. }
  261. public void RegionLoaded()
  262. {
  263. mesher = m_frameWorkScene.RequestModuleInterface<IMesher>();
  264. if (mesher == null)
  265. {
  266. m_log.ErrorFormat("[ubOde] No mesher. module disabled");
  267. return;
  268. }
  269. m_meshWorker = new ODEMeshWorker(this, m_log, mesher, physicsconfig);
  270. m_frameWorkScene.PhysicsEnabled = true;
  271. }
  272. /// <summary>
  273. /// Initiailizes the scene
  274. /// Sets many properties that ODE requires to be stable
  275. /// These settings need to be tweaked 'exactly' right or weird stuff happens.
  276. /// </summary>
  277. private void Initialization()
  278. {
  279. SafeNativeMethods.AllocateODEDataForThread(~0U);
  280. nearCallback = near;
  281. m_rayCastManager = new ODERayCastRequestManager(this);
  282. WorldExtents.X = m_frameWorkScene.RegionInfo.RegionSizeX;
  283. m_regionWidth = (int)WorldExtents.X;
  284. WorldExtents.Y = m_frameWorkScene.RegionInfo.RegionSizeY;
  285. m_regionHeight = (int)WorldExtents.Y;
  286. lock (OdeLock)
  287. {
  288. // Create the world and the first space
  289. try
  290. {
  291. world = SafeNativeMethods.WorldCreate();
  292. TopSpace = SafeNativeMethods.SimpleSpaceCreate(IntPtr.Zero);
  293. ActiveSpace = SafeNativeMethods.SimpleSpaceCreate(TopSpace);
  294. CharsSpace = SafeNativeMethods.SimpleSpaceCreate(TopSpace);
  295. GroundSpace = SafeNativeMethods.SimpleSpaceCreate(TopSpace);
  296. float sx = WorldExtents.X + 16;
  297. float sy = WorldExtents.Y + 16;
  298. SafeNativeMethods.Vector3 ex =new SafeNativeMethods.Vector3(sx, sy, 0);
  299. SafeNativeMethods.Vector3 px =new SafeNativeMethods.Vector3(sx * 0.5f, sx * 0.5f, 0);
  300. if(sx < sy)
  301. sx = sy;
  302. sx = (float)Math.Log(sx) * 1.442695f + 0.5f;
  303. int dp = (int)sx - 2;
  304. if(dp > 8)
  305. dp = 8;
  306. else if(dp < 4)
  307. dp = 4;
  308. StaticSpace = SafeNativeMethods.QuadTreeSpaceCreate(TopSpace, ref px, ref ex, dp);
  309. }
  310. catch
  311. {
  312. // i must RtC#FM
  313. // i did!
  314. }
  315. // demote to second level
  316. SafeNativeMethods.SpaceSetSublevel(ActiveSpace, 1);
  317. SafeNativeMethods.SpaceSetSublevel(CharsSpace, 1);
  318. SafeNativeMethods.SpaceSetSublevel(StaticSpace, 1);
  319. SafeNativeMethods.SpaceSetSublevel(GroundSpace, 1);
  320. SafeNativeMethods.GeomSetCategoryBits(ActiveSpace, (uint)(CollisionCategories.Space |
  321. CollisionCategories.Geom |
  322. CollisionCategories.Character |
  323. CollisionCategories.Phantom |
  324. CollisionCategories.VolumeDtc
  325. ));
  326. SafeNativeMethods.GeomSetCollideBits(ActiveSpace, (uint)(CollisionCategories.Space |
  327. CollisionCategories.Geom |
  328. CollisionCategories.Character |
  329. CollisionCategories.Phantom |
  330. CollisionCategories.VolumeDtc
  331. ));
  332. SafeNativeMethods.GeomSetCategoryBits(CharsSpace, (uint)(CollisionCategories.Space |
  333. CollisionCategories.Geom |
  334. CollisionCategories.Character |
  335. CollisionCategories.Phantom |
  336. CollisionCategories.VolumeDtc
  337. ));
  338. SafeNativeMethods.GeomSetCollideBits(CharsSpace, 0);
  339. SafeNativeMethods.GeomSetCategoryBits(StaticSpace, (uint)(CollisionCategories.Space |
  340. CollisionCategories.Geom |
  341. // CollisionCategories.Land |
  342. // CollisionCategories.Water |
  343. CollisionCategories.Phantom |
  344. CollisionCategories.VolumeDtc
  345. ));
  346. SafeNativeMethods.GeomSetCollideBits(StaticSpace, 0);
  347. SafeNativeMethods.GeomSetCategoryBits(GroundSpace, (uint)(CollisionCategories.Land));
  348. SafeNativeMethods.GeomSetCollideBits(GroundSpace, 0);
  349. contactgroup = SafeNativeMethods.JointGroupCreate(maxContactsbeforedeath + 1);
  350. //contactgroup
  351. SafeNativeMethods.WorldSetAutoDisableFlag(world, false);
  352. }
  353. // checkThread();
  354. // Defaults
  355. int contactsPerCollision = 80;
  356. physicsconfig = null;
  357. if (m_config != null)
  358. {
  359. physicsconfig = m_config.Configs["ODEPhysicsSettings"];
  360. if (physicsconfig != null)
  361. {
  362. gravityx = physicsconfig.GetFloat("world_gravityx", gravityx);
  363. gravityy = physicsconfig.GetFloat("world_gravityy", gravityy);
  364. gravityz = physicsconfig.GetFloat("world_gravityz", gravityz);
  365. // contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", contactsurfacelayer);
  366. ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", ODE_STEPSIZE);
  367. avDensity = physicsconfig.GetFloat("av_density", avDensity);
  368. avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", avMovementDivisorWalk);
  369. avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", avMovementDivisorRun);
  370. contactsPerCollision = physicsconfig.GetInt("contacts_per_collision", contactsPerCollision);
  371. geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", geomDefaultDensity);
  372. // bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", bodyFramesAutoDisable);
  373. minimumGroundFlightOffset = physicsconfig.GetFloat("minimum_ground_flight_offset", minimumGroundFlightOffset);
  374. maximumMassObject = physicsconfig.GetFloat("maximum_mass_object", maximumMassObject);
  375. avDensity *= 3f / 80f; // scale other engines density option to this
  376. }
  377. }
  378. float heartbeat = 1/m_frameWorkScene.FrameTime;
  379. maximumAngularVelocity = 0.49f * heartbeat *(float)Math.PI;
  380. maxAngVelocitySQ = maximumAngularVelocity * maximumAngularVelocity;
  381. SafeNativeMethods.WorldSetCFM(world, comumContactCFM);
  382. SafeNativeMethods.WorldSetERP(world, comumContactERP);
  383. SafeNativeMethods.WorldSetGravity(world, gravityx, gravityy, gravityz);
  384. SafeNativeMethods.WorldSetLinearDamping(world, 0.001f);
  385. SafeNativeMethods.WorldSetAngularDamping(world, 0.002f);
  386. SafeNativeMethods.WorldSetAngularDampingThreshold(world, 0f);
  387. SafeNativeMethods.WorldSetLinearDampingThreshold(world, 0f);
  388. SafeNativeMethods.WorldSetMaxAngularSpeed(world, maximumAngularVelocity);
  389. SafeNativeMethods.WorldSetQuickStepNumIterations(world, m_physicsiterations);
  390. SafeNativeMethods.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
  391. SafeNativeMethods.WorldSetContactMaxCorrectingVel(world, 60.0f);
  392. HalfOdeStep = ODE_STEPSIZE * 0.5f;
  393. odetimestepMS = (int)(1000.0f * ODE_STEPSIZE + 0.5f);
  394. ContactgeomsArray = Marshal.AllocHGlobal(contactsPerCollision * SafeNativeMethods.ContactGeom.unmanagedSizeOf);
  395. GlobalContactsArray = Marshal.AllocHGlobal((maxContactsbeforedeath + 100) * SafeNativeMethods.Contact.unmanagedSizeOf);
  396. SharedTmpcontact.geom.g1 = IntPtr.Zero;
  397. SharedTmpcontact.geom.g2 = IntPtr.Zero;
  398. SharedTmpcontact.geom.side1 = -1;
  399. SharedTmpcontact.geom.side2 = -1;
  400. SharedTmpcontact.surface.mode = comumContactFlags;
  401. SharedTmpcontact.surface.mu = 0;
  402. SharedTmpcontact.surface.bounce = 0;
  403. SharedTmpcontact.surface.bounce_vel = 1.5f;
  404. SharedTmpcontact.surface.soft_cfm = comumContactCFM;
  405. SharedTmpcontact.surface.soft_erp = comumContactERP;
  406. SharedTmpcontact.surface.slip1 = comumContactSLIP;
  407. SharedTmpcontact.surface.slip2 = comumContactSLIP;
  408. m_materialContactsData[(int)Material.Stone].mu = 0.8f;
  409. m_materialContactsData[(int)Material.Stone].bounce = 0.4f;
  410. m_materialContactsData[(int)Material.Metal].mu = 0.3f;
  411. m_materialContactsData[(int)Material.Metal].bounce = 0.4f;
  412. m_materialContactsData[(int)Material.Glass].mu = 0.2f;
  413. m_materialContactsData[(int)Material.Glass].bounce = 0.7f;
  414. m_materialContactsData[(int)Material.Wood].mu = 0.6f;
  415. m_materialContactsData[(int)Material.Wood].bounce = 0.5f;
  416. m_materialContactsData[(int)Material.Flesh].mu = 0.9f;
  417. m_materialContactsData[(int)Material.Flesh].bounce = 0.3f;
  418. m_materialContactsData[(int)Material.Plastic].mu = 0.4f;
  419. m_materialContactsData[(int)Material.Plastic].bounce = 0.7f;
  420. m_materialContactsData[(int)Material.Rubber].mu = 0.9f;
  421. m_materialContactsData[(int)Material.Rubber].bounce = 0.95f;
  422. m_materialContactsData[(int)Material.light].mu = 0.0f;
  423. m_materialContactsData[(int)Material.light].bounce = 0.0f;
  424. m_lastframe = Util.GetTimeStamp();
  425. m_lastMeshExpire = m_lastframe;
  426. step_time = -1;
  427. base.Initialise(m_frameWorkScene.PhysicsRequestAsset,
  428. (m_frameWorkScene.Heightmap != null ? m_frameWorkScene.Heightmap.GetFloatsSerialised() : new float[m_frameWorkScene.RegionInfo.RegionSizeX * m_frameWorkScene.RegionInfo.RegionSizeY]),
  429. (float)m_frameWorkScene.RegionInfo.RegionSettings.WaterHeight);
  430. }
  431. internal void waitForSpaceUnlock(IntPtr space)
  432. {
  433. //if (space != IntPtr.Zero)
  434. //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
  435. }
  436. #region Collision Detection
  437. // sets a global contact for a joint for contactgeom , and base contact description)
  438. private IntPtr CreateContacJoint(ref SafeNativeMethods.ContactGeom contactGeom,bool smooth)
  439. {
  440. if (m_global_contactcount >= maxContactsbeforedeath)
  441. return IntPtr.Zero;
  442. m_global_contactcount++;
  443. if(smooth)
  444. SharedTmpcontact.geom.depth = contactGeom.depth * 0.05f;
  445. else
  446. SharedTmpcontact.geom.depth = contactGeom.depth;
  447. SharedTmpcontact.geom.pos = contactGeom.pos;
  448. SharedTmpcontact.geom.normal = contactGeom.normal;
  449. IntPtr contact = new IntPtr(GlobalContactsArray.ToInt64() + (Int64)(m_global_contactcount * SafeNativeMethods.Contact.unmanagedSizeOf));
  450. Marshal.StructureToPtr(SharedTmpcontact, contact, true);
  451. return SafeNativeMethods.JointCreateContactPtr(world, contactgroup, contact);
  452. }
  453. private bool GetCurContactGeom(int index, ref SafeNativeMethods.ContactGeom newcontactgeom)
  454. {
  455. if (ContactgeomsArray == IntPtr.Zero || index >= contactsPerCollision)
  456. return false;
  457. IntPtr contactptr = new IntPtr(ContactgeomsArray.ToInt64() + (Int64)(index * SafeNativeMethods.ContactGeom.unmanagedSizeOf));
  458. newcontactgeom = (SafeNativeMethods.ContactGeom)Marshal.PtrToStructure(contactptr, typeof(SafeNativeMethods.ContactGeom));
  459. return true;
  460. }
  461. /// <summary>
  462. /// This is our near callback. A geometry is near a body
  463. /// </summary>
  464. /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
  465. /// <param name="g1">a geometry or space</param>
  466. /// <param name="g2">another geometry or space</param>
  467. ///
  468. private void near(IntPtr space, IntPtr g1, IntPtr g2)
  469. {
  470. // no lock here! It's invoked from within Simulate(), which is thread-locked
  471. if (m_global_contactcount >= maxContactsbeforedeath)
  472. return;
  473. // Test if we're colliding a geom with a space.
  474. // If so we have to drill down into the space recursively
  475. if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
  476. return;
  477. if (SafeNativeMethods.GeomIsSpace(g1) || SafeNativeMethods.GeomIsSpace(g2))
  478. {
  479. // We'll be calling near recursivly if one
  480. // of them is a space to find all of the
  481. // contact points in the space
  482. try
  483. {
  484. SafeNativeMethods.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
  485. }
  486. catch (AccessViolationException)
  487. {
  488. m_log.Warn("[PHYSICS]: Unable to collide test a space");
  489. return;
  490. }
  491. //here one should check collisions of geoms inside a space
  492. // but on each space we only should have geoms that not colide amoung each other
  493. // so we don't dig inside spaces
  494. return;
  495. }
  496. // Figure out how many contact points we have
  497. int count = 0;
  498. try
  499. {
  500. if (g1 == g2)
  501. return; // Can't collide with yourself
  502. if (SafeNativeMethods.GeomGetCategoryBits(g1) == (uint)CollisionCategories.VolumeDtc ||
  503. SafeNativeMethods.GeomGetCategoryBits(g2) == (uint)CollisionCategories.VolumeDtc)
  504. {
  505. int cflags;
  506. unchecked
  507. {
  508. cflags = (int)(1 | SafeNativeMethods.CONTACTS_UNIMPORTANT);
  509. }
  510. count = SafeNativeMethods.CollidePtr(g1, g2, cflags, ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf);
  511. }
  512. else
  513. count = SafeNativeMethods.CollidePtr(g1, g2, (contactsPerCollision & 0xffff), ContactgeomsArray, SafeNativeMethods.ContactGeom.unmanagedSizeOf);
  514. }
  515. catch (SEHException)
  516. {
  517. 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.");
  518. // ode.drelease(world);
  519. base.TriggerPhysicsBasedRestart();
  520. }
  521. catch (Exception e)
  522. {
  523. m_log.WarnFormat("[PHYSICS]: Unable to collide test an object: {0}", e.Message);
  524. return;
  525. }
  526. // contacts done
  527. if (count == 0)
  528. return;
  529. // get first contact
  530. SafeNativeMethods.ContactGeom curContact = new SafeNativeMethods.ContactGeom();
  531. if (!GetCurContactGeom(0, ref curContact))
  532. return;
  533. // try get physical actors
  534. PhysicsActor p1;
  535. if (!actor_name_map.TryGetValue(g1, out p1))
  536. {
  537. m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 1");
  538. return;
  539. }
  540. PhysicsActor p2;
  541. if (!actor_name_map.TryGetValue(g2, out p2))
  542. {
  543. m_log.WarnFormat("[PHYSICS]: failed actor mapping for geom 2");
  544. return;
  545. }
  546. ContactPoint maxDepthContact = new ContactPoint();
  547. // do volume detection case
  548. if ((p1.IsVolumeDtc || p2.IsVolumeDtc))
  549. {
  550. maxDepthContact = new ContactPoint(
  551. new Vector3(curContact.pos.X, curContact.pos.Y, curContact.pos.Z),
  552. new Vector3(curContact.normal.X, curContact.normal.Y, curContact.normal.Z),
  553. curContact.depth, false
  554. );
  555. collision_accounting_events(p1, p2, maxDepthContact);
  556. return;
  557. }
  558. // big messy collision analises
  559. float mu = 0;
  560. float bounce = 0;
  561. // bool IgnoreNegSides = false;
  562. ContactData contactdata1 = new ContactData(0, 0, false);
  563. ContactData contactdata2 = new ContactData(0, 0, false);
  564. bool dop1ava = false;
  565. bool dop2ava = false;
  566. bool ignore = false;
  567. bool smoothMesh = false;
  568. switch (p1.PhysicsActorType)
  569. {
  570. case (int)ActorTypes.Agent:
  571. {
  572. dop1ava = true;
  573. switch (p2.PhysicsActorType)
  574. {
  575. case (int)ActorTypes.Agent:
  576. case (int)ActorTypes.Prim:
  577. break;
  578. default:
  579. ignore = true; // avatar to terrain and water ignored
  580. break;
  581. }
  582. break;
  583. }
  584. case (int)ActorTypes.Prim:
  585. {
  586. switch (p2.PhysicsActorType)
  587. {
  588. case (int)ActorTypes.Agent:
  589. dop2ava = true;
  590. break;
  591. case (int)ActorTypes.Prim:
  592. // Vector3 relV = p1.rootVelocity - p2.rootVelocity;
  593. // float relVlenSQ = relV.LengthSquared();
  594. // if (relVlenSQ > 0.0001f)
  595. {
  596. p1.CollidingObj = true;
  597. p2.CollidingObj = true;
  598. }
  599. p1.getContactData(ref contactdata1);
  600. p2.getContactData(ref contactdata2);
  601. bounce = contactdata1.bounce * contactdata2.bounce;
  602. mu = (float)Math.Sqrt(contactdata1.mu * contactdata2.mu);
  603. // if (relVlenSQ > 0.01f)
  604. // mu *= frictionMovementMult;
  605. if(SafeNativeMethods.GeomGetClass(g2) == SafeNativeMethods.GeomClassID.TriMeshClass &&
  606. SafeNativeMethods.GeomGetClass(g1) == SafeNativeMethods.GeomClassID.TriMeshClass)
  607. smoothMesh = true;
  608. break;
  609. case (int)ActorTypes.Ground:
  610. p1.getContactData(ref contactdata1);
  611. bounce = contactdata1.bounce * TerrainBounce;
  612. mu = (float)Math.Sqrt(contactdata1.mu * TerrainFriction);
  613. // Vector3 v1 = p1.rootVelocity;
  614. // if (Math.Abs(v1.X) > 0.1f || Math.Abs(v1.Y) > 0.1f)
  615. // mu *= frictionMovementMult;
  616. p1.CollidingGround = true;
  617. if(SafeNativeMethods.GeomGetClass(g1) == SafeNativeMethods.GeomClassID.TriMeshClass)
  618. smoothMesh = true;
  619. break;
  620. case (int)ActorTypes.Water:
  621. default:
  622. ignore = true;
  623. break;
  624. }
  625. }
  626. break;
  627. case (int)ActorTypes.Ground:
  628. if (p2.PhysicsActorType == (int)ActorTypes.Prim)
  629. {
  630. p2.CollidingGround = true;
  631. p2.getContactData(ref contactdata2);
  632. bounce = contactdata2.bounce * TerrainBounce;
  633. mu = (float)Math.Sqrt(contactdata2.mu * TerrainFriction);
  634. // if (curContact.side1 > 0) // should be 2 ?
  635. // IgnoreNegSides = true;
  636. // Vector3 v2 = p2.rootVelocity;
  637. // if (Math.Abs(v2.X) > 0.1f || Math.Abs(v2.Y) > 0.1f)
  638. // mu *= frictionMovementMult;
  639. if(SafeNativeMethods.GeomGetClass(g2) == SafeNativeMethods.GeomClassID.TriMeshClass)
  640. smoothMesh = true;
  641. }
  642. else
  643. ignore = true;
  644. break;
  645. case (int)ActorTypes.Water:
  646. default:
  647. break;
  648. }
  649. if (ignore)
  650. return;
  651. IntPtr Joint;
  652. bool FeetCollision = false;
  653. int ncontacts = 0;
  654. int i = 0;
  655. maxDepthContact = new ContactPoint();
  656. maxDepthContact.PenetrationDepth = float.MinValue;
  657. ContactPoint minDepthContact = new ContactPoint();
  658. minDepthContact.PenetrationDepth = float.MaxValue;
  659. SharedTmpcontact.geom.depth = 0;
  660. SharedTmpcontact.surface.mu = mu;
  661. SharedTmpcontact.surface.bounce = bounce;
  662. SafeNativeMethods.ContactGeom altContact = new SafeNativeMethods.ContactGeom();
  663. bool useAltcontact;
  664. bool noskip;
  665. if(dop1ava || dop2ava)
  666. smoothMesh = false;
  667. IntPtr b1 = SafeNativeMethods.GeomGetBody(g1);
  668. IntPtr b2 = SafeNativeMethods.GeomGetBody(g2);
  669. while (true)
  670. {
  671. noskip = true;
  672. useAltcontact = false;
  673. if (dop1ava)
  674. {
  675. if ((((OdeCharacter)p1).Collide(g1, g2, false, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
  676. {
  677. if (p2.PhysicsActorType == (int)ActorTypes.Agent)
  678. {
  679. p1.CollidingObj = true;
  680. p2.CollidingObj = true;
  681. }
  682. else if (p2.rootVelocity.LengthSquared() > 0.0f)
  683. p2.CollidingObj = true;
  684. }
  685. else
  686. noskip = false;
  687. }
  688. else if (dop2ava)
  689. {
  690. if ((((OdeCharacter)p2).Collide(g2, g1, true, ref curContact, ref altContact , ref useAltcontact, ref FeetCollision)))
  691. {
  692. if (p1.PhysicsActorType == (int)ActorTypes.Agent)
  693. {
  694. p1.CollidingObj = true;
  695. p2.CollidingObj = true;
  696. }
  697. else if (p1.rootVelocity.LengthSquared() > 0.0f)
  698. p1.CollidingObj = true;
  699. }
  700. else
  701. noskip = false;
  702. }
  703. if (noskip)
  704. {
  705. if(useAltcontact)
  706. Joint = CreateContacJoint(ref altContact,smoothMesh);
  707. else
  708. Joint = CreateContacJoint(ref curContact,smoothMesh);
  709. if (Joint == IntPtr.Zero)
  710. break;
  711. SafeNativeMethods.JointAttach(Joint, b1, b2);
  712. ncontacts++;
  713. if (curContact.depth > maxDepthContact.PenetrationDepth)
  714. {
  715. maxDepthContact.Position.X = curContact.pos.X;
  716. maxDepthContact.Position.Y = curContact.pos.Y;
  717. maxDepthContact.Position.Z = curContact.pos.Z;
  718. maxDepthContact.PenetrationDepth = curContact.depth;
  719. maxDepthContact.CharacterFeet = FeetCollision;
  720. }
  721. if (curContact.depth < minDepthContact.PenetrationDepth)
  722. {
  723. minDepthContact.PenetrationDepth = curContact.depth;
  724. minDepthContact.SurfaceNormal.X = curContact.normal.X;
  725. minDepthContact.SurfaceNormal.Y = curContact.normal.Y;
  726. minDepthContact.SurfaceNormal.Z = curContact.normal.Z;
  727. }
  728. }
  729. if (++i >= count)
  730. break;
  731. if (!GetCurContactGeom(i, ref curContact))
  732. break;
  733. }
  734. if (ncontacts > 0)
  735. {
  736. maxDepthContact.SurfaceNormal.X = minDepthContact.SurfaceNormal.X;
  737. maxDepthContact.SurfaceNormal.Y = minDepthContact.SurfaceNormal.Y;
  738. maxDepthContact.SurfaceNormal.Z = minDepthContact.SurfaceNormal.Z;
  739. collision_accounting_events(p1, p2, maxDepthContact);
  740. }
  741. }
  742. private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, ContactPoint contact)
  743. {
  744. uint obj2LocalID = 0;
  745. // update actors collision score
  746. if (p1.CollisionScore < float.MaxValue)
  747. p1.CollisionScore += 1.0f;
  748. if (p2.CollisionScore < float.MaxValue)
  749. p2.CollisionScore += 1.0f;
  750. bool p1events = p1.SubscribedEvents();
  751. bool p2events = p2.SubscribedEvents();
  752. if (p1.IsVolumeDtc)
  753. p2events = false;
  754. if (p2.IsVolumeDtc)
  755. p1events = false;
  756. if (!p2events && !p1events)
  757. return;
  758. Vector3 vel = Vector3.Zero;
  759. if (p2 != null && p2.IsPhysical)
  760. vel = p2.rootVelocity;
  761. if (p1 != null && p1.IsPhysical)
  762. vel -= p1.rootVelocity;
  763. contact.RelativeSpeed = Vector3.Dot(vel, contact.SurfaceNormal);
  764. switch ((ActorTypes)p1.PhysicsActorType)
  765. {
  766. case ActorTypes.Agent:
  767. case ActorTypes.Prim:
  768. {
  769. switch ((ActorTypes)p2.PhysicsActorType)
  770. {
  771. case ActorTypes.Agent:
  772. case ActorTypes.Prim:
  773. if (p2events)
  774. {
  775. //AddCollisionEventReporting(p2);
  776. p2.AddCollisionEvent(p1.ParentActor.LocalID, contact);
  777. }
  778. else if(p1.IsVolumeDtc)
  779. p2.AddVDTCCollisionEvent(p1.ParentActor.LocalID, contact);
  780. obj2LocalID = p2.ParentActor.LocalID;
  781. break;
  782. case ActorTypes.Ground:
  783. case ActorTypes.Unknown:
  784. default:
  785. obj2LocalID = 0;
  786. break;
  787. }
  788. if (p1events)
  789. {
  790. contact.SurfaceNormal = -contact.SurfaceNormal;
  791. contact.RelativeSpeed = -contact.RelativeSpeed;
  792. //AddCollisionEventReporting(p1);
  793. p1.AddCollisionEvent(obj2LocalID, contact);
  794. }
  795. else if(p2.IsVolumeDtc)
  796. {
  797. contact.SurfaceNormal = -contact.SurfaceNormal;
  798. contact.RelativeSpeed = -contact.RelativeSpeed;
  799. //AddCollisionEventReporting(p1);
  800. p1.AddVDTCCollisionEvent(obj2LocalID, contact);
  801. }
  802. break;
  803. }
  804. case ActorTypes.Ground:
  805. case ActorTypes.Unknown:
  806. default:
  807. {
  808. if (p2events && !p2.IsVolumeDtc)
  809. {
  810. //AddCollisionEventReporting(p2);
  811. p2.AddCollisionEvent(0, contact);
  812. }
  813. break;
  814. }
  815. }
  816. }
  817. /// <summary>
  818. /// This is our collision testing routine in ODE
  819. /// </summary>
  820. /// <param name="timeStep"></param>
  821. private void collision_optimized()
  822. {
  823. lock (_characters)
  824. {
  825. try
  826. {
  827. foreach (OdeCharacter chr in _characters)
  828. {
  829. if (chr == null)
  830. continue;
  831. chr.IsColliding = false;
  832. // chr.CollidingGround = false; not done here
  833. chr.CollidingObj = false;
  834. if(chr.Body == IntPtr.Zero || chr.collider == IntPtr.Zero )
  835. continue;
  836. // do colisions with static space
  837. SafeNativeMethods.SpaceCollide2(chr.collider, StaticSpace, IntPtr.Zero, nearCallback);
  838. // no coll with gnd
  839. }
  840. // chars with chars
  841. SafeNativeMethods.SpaceCollide(CharsSpace, IntPtr.Zero, nearCallback);
  842. }
  843. catch (AccessViolationException)
  844. {
  845. m_log.Warn("[PHYSICS]: Unable to collide Character to static space");
  846. }
  847. }
  848. lock (_activeprims)
  849. {
  850. foreach (OdePrim aprim in _activeprims)
  851. {
  852. aprim.CollisionScore = 0;
  853. aprim.IsColliding = false;
  854. if(!aprim.m_outbounds && SafeNativeMethods.BodyIsEnabled(aprim.Body))
  855. aprim.clearSleeperCollisions();
  856. }
  857. }
  858. lock (_activegroups)
  859. {
  860. try
  861. {
  862. foreach (OdePrim aprim in _activegroups)
  863. {
  864. if(!aprim.m_outbounds && SafeNativeMethods.BodyIsEnabled(aprim.Body) &&
  865. aprim.collide_geom != IntPtr.Zero)
  866. {
  867. SafeNativeMethods.SpaceCollide2(StaticSpace, aprim.collide_geom, IntPtr.Zero, nearCallback);
  868. SafeNativeMethods.SpaceCollide2(GroundSpace, aprim.collide_geom, IntPtr.Zero, nearCallback);
  869. }
  870. }
  871. }
  872. catch (Exception e)
  873. {
  874. m_log.Warn("[PHYSICS]: Unable to collide Active to Static: " + e.Message);
  875. }
  876. }
  877. // colide active amoung them
  878. try
  879. {
  880. SafeNativeMethods.SpaceCollide(ActiveSpace, IntPtr.Zero, nearCallback);
  881. }
  882. catch (Exception e)
  883. {
  884. m_log.Warn("[PHYSICS]: Unable to collide in Active: " + e.Message);
  885. }
  886. // and with chars
  887. try
  888. {
  889. SafeNativeMethods.SpaceCollide2(CharsSpace,ActiveSpace, IntPtr.Zero, nearCallback);
  890. }
  891. catch (Exception e)
  892. {
  893. m_log.Warn("[PHYSICS]: Unable to collide Active to Character: " + e.Message);
  894. }
  895. }
  896. #endregion
  897. /// <summary>
  898. /// Add actor to the list that should receive collision events in the simulate loop.
  899. /// </summary>
  900. /// <param name="obj"></param>
  901. public void AddCollisionEventReporting(PhysicsActor obj)
  902. {
  903. if (!_collisionEventPrim.Contains(obj))
  904. _collisionEventPrim.Add(obj);
  905. }
  906. /// <summary>
  907. /// Remove actor from the list that should receive collision events in the simulate loop.
  908. /// </summary>
  909. /// <param name="obj"></param>
  910. public void RemoveCollisionEventReporting(PhysicsActor obj)
  911. {
  912. lock(_collisionEventPrimRemove)
  913. {
  914. if (_collisionEventPrim.Contains(obj) && !_collisionEventPrimRemove.Contains(obj))
  915. _collisionEventPrimRemove.Add(obj);
  916. }
  917. }
  918. public override float TimeDilation
  919. {
  920. get { return m_timeDilation; }
  921. }
  922. #region Add/Remove Entities
  923. public override PhysicsActor AddAvatar(string avName, Vector3 position, Vector3 velocity, Vector3 size, bool isFlying)
  924. {
  925. return null;
  926. }
  927. public override PhysicsActor AddAvatar(uint localID, string avName, Vector3 position, Vector3 size, float feetOffset, bool isFlying)
  928. {
  929. OdeCharacter newAv = new OdeCharacter(localID, avName, this, position,
  930. size, feetOffset, avDensity, avMovementDivisorWalk, avMovementDivisorRun);
  931. newAv.Flying = isFlying;
  932. newAv.MinimumGroundFlightOffset = minimumGroundFlightOffset;
  933. return newAv;
  934. }
  935. public void AddCharacter(OdeCharacter chr)
  936. {
  937. lock (_characters)
  938. {
  939. if (!_characters.Contains(chr))
  940. {
  941. _characters.Add(chr);
  942. if (chr.bad)
  943. m_log.DebugFormat("[PHYSICS] Added BAD actor {0} to characters list", chr.m_uuid);
  944. }
  945. }
  946. }
  947. public void RemoveCharacter(OdeCharacter chr)
  948. {
  949. lock (_characters)
  950. {
  951. if (_characters.Contains(chr))
  952. {
  953. _characters.Remove(chr);
  954. }
  955. }
  956. }
  957. public void BadCharacter(OdeCharacter chr)
  958. {
  959. lock (_badCharacter)
  960. {
  961. if (!_badCharacter.Contains(chr))
  962. _badCharacter.Add(chr);
  963. }
  964. }
  965. public override void RemoveAvatar(PhysicsActor actor)
  966. {
  967. //m_log.Debug("[PHYSICS]:ODELOCK");
  968. if (world == IntPtr.Zero)
  969. return;
  970. ((OdeCharacter) actor).Destroy();
  971. }
  972. public void addActivePrim(OdePrim activatePrim)
  973. {
  974. // adds active prim..
  975. lock (_activeprims)
  976. {
  977. if (!_activeprims.Contains(activatePrim))
  978. _activeprims.Add(activatePrim);
  979. }
  980. }
  981. public void addActiveGroups(OdePrim activatePrim)
  982. {
  983. lock (_activegroups)
  984. {
  985. if (!_activegroups.Contains(activatePrim))
  986. _activegroups.Add(activatePrim);
  987. }
  988. }
  989. private PhysicsActor AddPrim(String name, Vector3 position, Vector3 size, Quaternion rotation,
  990. PrimitiveBaseShape pbs, bool isphysical, bool isPhantom, byte shapeType, uint localID)
  991. {
  992. OdePrim newPrim;
  993. lock (OdeLock)
  994. {
  995. newPrim = new OdePrim(name, this, position, size, rotation, pbs, isphysical, isPhantom, shapeType, localID);
  996. }
  997. return newPrim;
  998. }
  999. public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
  1000. Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, uint localid)
  1001. {
  1002. return AddPrim(primName, position, size, rotation, pbs, isPhysical, isPhantom, 0 , localid);
  1003. }
  1004. public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
  1005. Vector3 size, Quaternion rotation, bool isPhysical, uint localid)
  1006. {
  1007. return AddPrim(primName, position, size, rotation, pbs, isPhysical,false, 0, localid);
  1008. }
  1009. public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, Vector3 position,
  1010. Vector3 size, Quaternion rotation, bool isPhysical, bool isPhantom, byte shapeType, uint localid)
  1011. {
  1012. return AddPrim(primName, position, size, rotation, pbs, isPhysical,isPhantom, shapeType, localid);
  1013. }
  1014. public void remActivePrim(OdePrim deactivatePrim)
  1015. {
  1016. lock (_activeprims)
  1017. {
  1018. _activeprims.Remove(deactivatePrim);
  1019. }
  1020. }
  1021. public void remActiveGroup(OdePrim deactivatePrim)
  1022. {
  1023. lock (_activegroups)
  1024. {
  1025. _activegroups.Remove(deactivatePrim);
  1026. }
  1027. }
  1028. public override void RemovePrim(PhysicsActor prim)
  1029. {
  1030. // As with all ODE physics operations, we don't remove the prim immediately but signal that it should be
  1031. // removed in the next physics simulate pass.
  1032. if (prim is OdePrim)
  1033. {
  1034. OdePrim p = (OdePrim)prim;
  1035. p.setPrimForRemoval();
  1036. }
  1037. }
  1038. public void RemovePrimThreadLocked(OdePrim prim)
  1039. {
  1040. //Console.WriteLine("RemovePrimThreadLocked " + prim.m_primName);
  1041. lock (_prims)
  1042. _prims.Remove(prim.LocalID);
  1043. }
  1044. public void addToPrims(OdePrim prim)
  1045. {
  1046. lock (_prims)
  1047. _prims[prim.LocalID] = prim;
  1048. }
  1049. public OdePrim getPrim(uint id)
  1050. {
  1051. lock (_prims)
  1052. {
  1053. if(_prims.ContainsKey(id))
  1054. return _prims[id];
  1055. else
  1056. return null;
  1057. }
  1058. }
  1059. public bool havePrim(OdePrim prm)
  1060. {
  1061. lock (_prims)
  1062. return _prims.ContainsKey(prm.LocalID);
  1063. }
  1064. public void changePrimID(OdePrim prim,uint oldID)
  1065. {
  1066. lock (_prims)
  1067. {
  1068. if(_prims.ContainsKey(oldID))
  1069. _prims.Remove(oldID);
  1070. _prims[prim.LocalID] = prim;
  1071. }
  1072. }
  1073. public bool haveActor(PhysicsActor actor)
  1074. {
  1075. if (actor is OdePrim)
  1076. {
  1077. lock (_prims)
  1078. return _prims.ContainsKey(((OdePrim)actor).LocalID);
  1079. }
  1080. else if (actor is OdeCharacter)
  1081. {
  1082. lock (_characters)
  1083. return _characters.Contains((OdeCharacter)actor);
  1084. }
  1085. return false;
  1086. }
  1087. #endregion
  1088. #region Space Separation Calculation
  1089. /// <summary>
  1090. /// Called when a static prim moves or becomes static
  1091. /// Places the prim in a space one the static space
  1092. /// </summary>
  1093. /// <param name="geom">the pointer to the geom that moved</param>
  1094. /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
  1095. /// <returns>a pointer to the new space it's in</returns>
  1096. public IntPtr MoveGeomToStaticSpace(IntPtr geom, IntPtr currentspace)
  1097. {
  1098. // moves a prim into static sub-space
  1099. // Called ODEPrim so
  1100. // it's already in locked space.
  1101. if (geom == IntPtr.Zero) // shouldn't happen
  1102. return IntPtr.Zero;
  1103. if (StaticSpace == currentspace) // if we are there all done
  1104. return StaticSpace;
  1105. // else remove it from its current space
  1106. if (currentspace != IntPtr.Zero && SafeNativeMethods.SpaceQuery(currentspace, geom))
  1107. {
  1108. if (SafeNativeMethods.GeomIsSpace(currentspace))
  1109. {
  1110. waitForSpaceUnlock(currentspace);
  1111. SafeNativeMethods.SpaceRemove(currentspace, geom);
  1112. if (SafeNativeMethods.SpaceGetSublevel(currentspace) > 2 && SafeNativeMethods.SpaceGetNumGeoms(currentspace) == 0)
  1113. {
  1114. SafeNativeMethods.SpaceDestroy(currentspace);
  1115. }
  1116. }
  1117. else
  1118. {
  1119. m_log.Info("[Physics]: Invalid or empty Space passed to 'MoveGeomToStaticSpace':" + currentspace +
  1120. " Geom:" + geom);
  1121. }
  1122. }
  1123. else
  1124. {
  1125. currentspace = SafeNativeMethods.GeomGetSpace(geom);
  1126. if (currentspace != IntPtr.Zero)
  1127. {
  1128. if (SafeNativeMethods.GeomIsSpace(currentspace))
  1129. {
  1130. waitForSpaceUnlock(currentspace);
  1131. SafeNativeMethods.SpaceRemove(currentspace, geom);
  1132. if (SafeNativeMethods.SpaceGetSublevel(currentspace) > 2 && SafeNativeMethods.SpaceGetNumGeoms(currentspace) == 0)
  1133. {
  1134. SafeNativeMethods.SpaceDestroy(currentspace);
  1135. }
  1136. }
  1137. }
  1138. }
  1139. // put the geom in the newspace
  1140. waitForSpaceUnlock(StaticSpace);
  1141. if(SafeNativeMethods.SpaceQuery(StaticSpace, geom))
  1142. m_log.Info("[Physics]: 'MoveGeomToStaticSpace' geom already in static space:" + geom);
  1143. else
  1144. SafeNativeMethods.SpaceAdd(StaticSpace, geom);
  1145. return StaticSpace;
  1146. }
  1147. #endregion
  1148. /// <summary>
  1149. /// Called to queue a change to a actor
  1150. /// to use in place of old taint mechanism so changes do have a time sequence
  1151. /// </summary>
  1152. public void AddChange(PhysicsActor _actor, changes _what, Object _arg)
  1153. {
  1154. if (world == IntPtr.Zero)
  1155. return;
  1156. ODEchangeitem item = new ODEchangeitem
  1157. {
  1158. actor = _actor,
  1159. what = _what,
  1160. arg = _arg
  1161. };
  1162. ChangesQueue.Enqueue(item);
  1163. }
  1164. /// <summary>
  1165. /// Called after our prim properties are set Scale, position etc.
  1166. /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
  1167. /// This assures us that we have no race conditions
  1168. /// </summary>
  1169. /// <param name="prim"></param>
  1170. public override void AddPhysicsActorTaint(PhysicsActor prim)
  1171. {
  1172. }
  1173. // does all pending changes generated during region load process
  1174. public override void ProcessPreSimulation()
  1175. {
  1176. lock (OdeLock)
  1177. {
  1178. if (world == IntPtr.Zero)
  1179. return;
  1180. ODEchangeitem item;
  1181. int donechanges = 0;
  1182. if (!ChangesQueue.IsEmpty)
  1183. {
  1184. m_log.InfoFormat("[ubOde] start processing pending actor operations");
  1185. int tstart = Util.EnvironmentTickCount();
  1186. SafeNativeMethods.AllocateODEDataForThread(~0U);
  1187. while (ChangesQueue.TryDequeue(out item))
  1188. {
  1189. if (item.actor != null)
  1190. {
  1191. try
  1192. {
  1193. lock (SimulationLock)
  1194. {
  1195. if (item.actor is OdeCharacter)
  1196. ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
  1197. else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
  1198. RemovePrimThreadLocked((OdePrim)item.actor);
  1199. }
  1200. }
  1201. catch
  1202. {
  1203. m_log.WarnFormat("[PHYSICS]: Operation failed for a actor {0} {1}",
  1204. item.actor.Name, item.what.ToString());
  1205. }
  1206. }
  1207. donechanges++;
  1208. }
  1209. int time = Util.EnvironmentTickCountSubtract(tstart);
  1210. m_log.InfoFormat("[ubOde] finished {0} operations in {1}ms", donechanges, time);
  1211. }
  1212. m_log.InfoFormat("[ubOde] {0} prim actors loaded",_prims.Count);
  1213. }
  1214. m_lastframe = Util.GetTimeStamp() + 0.5;
  1215. step_time = -0.5f;
  1216. }
  1217. /// <summary>
  1218. /// This is our main simulate loop
  1219. /// It's thread locked by a Mutex in the scene.
  1220. /// It holds Collisions, it instructs ODE to step through the physical reactions
  1221. /// It moves the objects around in memory
  1222. /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
  1223. /// </summary>
  1224. /// <param name="timeStep"></param>
  1225. /// <returns></returns>
  1226. public override float Simulate(float reqTimeStep)
  1227. {
  1228. if (world == IntPtr.Zero)
  1229. return 0;
  1230. double now = Util.GetTimeStamp();
  1231. double timeStep = now - m_lastframe;
  1232. m_lastframe = now;
  1233. // acumulate time so we can reduce error
  1234. step_time += (float)timeStep;
  1235. if (step_time < HalfOdeStep)
  1236. return 0;
  1237. if (framecount < 0)
  1238. framecount = 0;
  1239. framecount++;
  1240. // checkThread();
  1241. int nodeframes = 0;
  1242. float fps = 0;
  1243. lock (OdeLock)
  1244. {
  1245. // d.WorldSetQuickStepNumIterations(world, curphysiteractions);
  1246. double loopstartMS = Util.GetTimeStampMS();
  1247. double looptimeMS = 0;
  1248. double changestimeMS = 0;
  1249. double maxChangestime = (int)(reqTimeStep * 500f); // half the time
  1250. double maxLoopTime = (int)(reqTimeStep * 1200f); // 1.2 the time
  1251. /*
  1252. double collisionTime = 0;
  1253. double qstepTIme = 0;
  1254. double tmpTime = 0;
  1255. double changestot = 0;
  1256. double collisonRepo = 0;
  1257. double updatesTime = 0;
  1258. double moveTime = 0;
  1259. double rayTime = 0;
  1260. */
  1261. SafeNativeMethods.AllocateODEDataForThread(~0U);
  1262. if (!ChangesQueue.IsEmpty)
  1263. {
  1264. ODEchangeitem item;
  1265. while (ChangesQueue.TryDequeue(out item))
  1266. {
  1267. if (item.actor != null)
  1268. {
  1269. try
  1270. {
  1271. lock (SimulationLock)
  1272. {
  1273. if (item.actor is OdeCharacter)
  1274. ((OdeCharacter)item.actor).DoAChange(item.what, item.arg);
  1275. else if (((OdePrim)item.actor).DoAChange(item.what, item.arg))
  1276. RemovePrimThreadLocked((OdePrim)item.actor);
  1277. }
  1278. }
  1279. catch
  1280. {
  1281. m_log.WarnFormat("[PHYSICS]: doChange failed for a actor {0} {1}",
  1282. item.actor.Name, item.what.ToString());
  1283. }
  1284. }
  1285. changestimeMS = Util.GetTimeStampMS() - loopstartMS;
  1286. if (changestimeMS > maxChangestime)
  1287. break;
  1288. }
  1289. }
  1290. // do simulation taking at most 150ms total time including changes
  1291. while (step_time > HalfOdeStep)
  1292. {
  1293. try
  1294. {
  1295. // clear pointer/counter to contacts to pass into joints
  1296. m_global_contactcount = 0;
  1297. // tmpTime = Util.GetTimeStampMS();
  1298. // Move characters
  1299. lock (_characters)
  1300. {
  1301. foreach (OdeCharacter actor in _characters)
  1302. {
  1303. lock (SimulationLock)
  1304. actor.Move();
  1305. }
  1306. }
  1307. // Move other active objects
  1308. lock (_activegroups)
  1309. {
  1310. foreach (OdePrim aprim in _activegroups)
  1311. {
  1312. lock (SimulationLock)
  1313. aprim.Move();
  1314. }
  1315. }
  1316. // moveTime += Util.GetTimeStampMS() - tmpTime;
  1317. // tmpTime = Util.GetTimeStampMS();
  1318. lock (SimulationLock)
  1319. {
  1320. m_rayCastManager.ProcessQueuedRequests();
  1321. // rayTime += Util.GetTimeStampMS() - tmpTime;
  1322. // tmpTime = Util.GetTimeStampMS();
  1323. collision_optimized();
  1324. }
  1325. // collisionTime += Util.GetTimeStampMS() - tmpTime;
  1326. // tmpTime = Util.GetTimeStampMS();
  1327. lock(_collisionEventPrimRemove)
  1328. {
  1329. foreach (PhysicsActor obj in _collisionEventPrimRemove)
  1330. _collisionEventPrim.Remove(obj);
  1331. _collisionEventPrimRemove.Clear();
  1332. }
  1333. List<OdePrim> sleepers = new List<OdePrim>();
  1334. foreach (PhysicsActor obj in _collisionEventPrim)
  1335. {
  1336. switch ((ActorTypes)obj.PhysicsActorType)
  1337. {
  1338. case ActorTypes.Agent:
  1339. OdeCharacter cobj = (OdeCharacter)obj;
  1340. cobj.SendCollisions((int)(odetimestepMS));
  1341. break;
  1342. case ActorTypes.Prim:
  1343. OdePrim pobj = (OdePrim)obj;
  1344. if (!pobj.m_outbounds)
  1345. {
  1346. pobj.SendCollisions((int)(odetimestepMS));
  1347. lock(SimulationLock)
  1348. {
  1349. if(pobj.Body != IntPtr.Zero && !pobj.m_isSelected &&
  1350. !pobj.m_disabled && !pobj.m_building &&
  1351. !SafeNativeMethods.BodyIsEnabled(pobj.Body))
  1352. sleepers.Add(pobj);
  1353. }
  1354. }
  1355. break;
  1356. }
  1357. }
  1358. foreach(OdePrim prm in sleepers)
  1359. prm.SleeperAddCollisionEvents();
  1360. sleepers.Clear();
  1361. // collisonRepo += Util.GetTimeStampMS() - tmpTime;
  1362. // do a ode simulation step
  1363. // tmpTime = Util.GetTimeStampMS();
  1364. lock (SimulationLock)
  1365. {
  1366. SafeNativeMethods.WorldQuickStep(world, ODE_STEPSIZE);
  1367. SafeNativeMethods.JointGroupEmpty(contactgroup);
  1368. }
  1369. // qstepTIme += Util.GetTimeStampMS() - tmpTime;
  1370. // update managed ideia of physical data and do updates to core
  1371. /*
  1372. lock (_characters)
  1373. {
  1374. foreach (OdeCharacter actor in _characters)
  1375. {
  1376. if (actor != null)
  1377. {
  1378. if (actor.bad)
  1379. m_log.WarnFormat("[PHYSICS]: BAD Actor {0} in _characters list was not removed?", actor.m_uuid);
  1380. actor.UpdatePositionAndVelocity();
  1381. }
  1382. }
  1383. }
  1384. */
  1385. // tmpTime = Util.GetTimeStampMS();
  1386. //lock (SimulationLock)
  1387. {
  1388. lock (_activegroups)
  1389. {
  1390. {
  1391. foreach (OdePrim actor in _activegroups)
  1392. {
  1393. if (actor.IsPhysical)
  1394. {
  1395. lock (SimulationLock)
  1396. actor.UpdatePositionAndVelocity(framecount);
  1397. }
  1398. }
  1399. }
  1400. }
  1401. }
  1402. // updatesTime += Util.GetTimeStampMS() - tmpTime;
  1403. }
  1404. catch (Exception e)
  1405. {
  1406. m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
  1407. // ode.dunlock(world);
  1408. }
  1409. step_time -= ODE_STEPSIZE;
  1410. nodeframes++;
  1411. looptimeMS = Util.GetTimeStampMS() - loopstartMS;
  1412. if (looptimeMS > maxLoopTime)
  1413. break;
  1414. }
  1415. lock (_badCharacter)
  1416. {
  1417. if (_badCharacter.Count > 0)
  1418. {
  1419. foreach (OdeCharacter chr in _badCharacter)
  1420. {
  1421. RemoveCharacter(chr);
  1422. }
  1423. _badCharacter.Clear();
  1424. }
  1425. }
  1426. /*
  1427. // information block for in debug breakpoint only
  1428. int ntopactivegeoms = d.SpaceGetNumGeoms(ActiveSpace);
  1429. int ntopstaticgeoms = d.SpaceGetNumGeoms(StaticSpace);
  1430. int ngroundgeoms = d.SpaceGetNumGeoms(GroundSpace);
  1431. int nactivegeoms = 0;
  1432. int nactivespaces = 0;
  1433. int nstaticgeoms = 0;
  1434. int nstaticspaces = 0;
  1435. IntPtr sp;
  1436. for (int i = 0; i < ntopactivegeoms; i++)
  1437. {
  1438. sp = d.SpaceGetGeom(ActiveSpace, i);
  1439. if (d.GeomIsSpace(sp))
  1440. {
  1441. nactivespaces++;
  1442. nactivegeoms += d.SpaceGetNumGeoms(sp);
  1443. }
  1444. else
  1445. nactivegeoms++;
  1446. }
  1447. for (int i = 0; i < ntopstaticgeoms; i++)
  1448. {
  1449. sp = d.SpaceGetGeom(StaticSpace, i);
  1450. if (d.GeomIsSpace(sp))
  1451. {
  1452. nstaticspaces++;
  1453. nstaticgeoms += d.SpaceGetNumGeoms(sp);
  1454. }
  1455. else
  1456. nstaticgeoms++;
  1457. }
  1458. int ntopgeoms = d.SpaceGetNumGeoms(TopSpace);
  1459. int totgeoms = nstaticgeoms + nactivegeoms + ngroundgeoms + 1; // one ray
  1460. int nbodies = d.NTotalBodies;
  1461. int ngeoms = d.NTotalGeoms;
  1462. */
  1463. /*
  1464. looptimeMS /= nodeframes;
  1465. collisionTime /= nodeframes;
  1466. qstepTIme /= nodeframes;
  1467. changestot /= nodeframes;
  1468. collisonRepo /= nodeframes;
  1469. updatesTime /= nodeframes;
  1470. moveTime /= nodeframes;
  1471. rayTime /= nodeframes;
  1472. if(looptimeMS > .05)
  1473. {
  1474. }
  1475. */
  1476. fps = (float)nodeframes * ODE_STEPSIZE / reqTimeStep;
  1477. if(step_time < HalfOdeStep)
  1478. m_timeDilation = 1.0f;
  1479. else if (step_time > m_SkipFramesAtms)
  1480. {
  1481. // if we lag too much skip frames
  1482. m_timeDilation = 0.0f;
  1483. step_time = 0;
  1484. m_lastframe = Util.GetTimeStamp(); // skip also the time lost
  1485. }
  1486. else
  1487. {
  1488. m_timeDilation = ODE_STEPSIZE / step_time;
  1489. if (m_timeDilation > 1)
  1490. m_timeDilation = 1;
  1491. }
  1492. if (m_timeDilation == 1 && now - m_lastMeshExpire > 30)
  1493. {
  1494. mesher.ExpireReleaseMeshs();
  1495. m_lastMeshExpire = now;
  1496. }
  1497. }
  1498. return fps;
  1499. }
  1500. public float GetTerrainHeightAtXY(float x, float y)
  1501. {
  1502. if (m_terrainGeom == IntPtr.Zero)
  1503. return 0f;
  1504. if (m_terrainHeights == null || m_terrainHeights.Length == 0)
  1505. return 0f;
  1506. // TerrainHeightField for ODE as offset 1m
  1507. x += 1f;
  1508. y += 1f;
  1509. // make position fit into array
  1510. if (x < 0)
  1511. x = 0;
  1512. if (y < 0)
  1513. y = 0;
  1514. // integer indexs
  1515. int ix;
  1516. int iy;
  1517. // interpolators offset
  1518. float dx;
  1519. float dy;
  1520. int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples
  1521. int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples
  1522. int regsize = regsizeX;
  1523. if (x < regsizeX - 1)
  1524. {
  1525. ix = (int)x;
  1526. dx = x - (float)ix;
  1527. }
  1528. else // out world use external height
  1529. {
  1530. ix = regsizeX - 2;
  1531. dx = 0;
  1532. }
  1533. if (y < regsizeY - 1)
  1534. {
  1535. iy = (int)y;
  1536. dy = y - (float)iy;
  1537. }
  1538. else
  1539. {
  1540. iy = regsizeY - 2;
  1541. dy = 0;
  1542. }
  1543. float h0;
  1544. float h1;
  1545. float h2;
  1546. iy *= regsize;
  1547. iy += ix; // all indexes have iy + ix
  1548. float[] heights = m_terrainHeights;
  1549. /*
  1550. if ((dx + dy) <= 1.0f)
  1551. {
  1552. h0 = ((float)heights[iy]); // 0,0 vertice
  1553. h1 = (((float)heights[iy + 1]) - h0) * dx; // 1,0 vertice minus 0,0
  1554. h2 = (((float)heights[iy + regsize]) - h0) * dy; // 0,1 vertice minus 0,0
  1555. }
  1556. else
  1557. {
  1558. h0 = ((float)heights[iy + regsize + 1]); // 1,1 vertice
  1559. h1 = (((float)heights[iy + 1]) - h0) * (1 - dy); // 1,1 vertice minus 1,0
  1560. h2 = (((float)heights[iy + regsize]) - h0) * (1 - dx); // 1,1 vertice minus 0,1
  1561. }
  1562. */
  1563. h0 = ((float)heights[iy]); // 0,0 vertice
  1564. if (dy>dx)
  1565. {
  1566. iy += regsize;
  1567. h2 = (float)heights[iy]; // 0,1 vertice
  1568. h1 = (h2 - h0) * dy; // 0,1 vertice minus 0,0
  1569. h2 = ((float)heights[iy + 1] - h2) * dx; // 1,1 vertice minus 0,1
  1570. }
  1571. else
  1572. {
  1573. iy++;
  1574. h2 = (float)heights[iy]; // vertice 1,0
  1575. h1 = (h2 - h0) * dx; // 1,0 vertice minus 0,0
  1576. h2 = (((float)heights[iy + regsize]) - h2) * dy; // 1,1 vertice minus 1,0
  1577. }
  1578. return h0 + h1 + h2;
  1579. }
  1580. public Vector3 GetTerrainNormalAtXY(float x, float y)
  1581. {
  1582. Vector3 norm = new Vector3(0, 0, 1);
  1583. if (m_terrainGeom == IntPtr.Zero)
  1584. return norm;
  1585. if (m_terrainHeights == null || m_terrainHeights.Length == 0)
  1586. return norm;
  1587. // TerrainHeightField for ODE as offset 1m
  1588. x += 1f;
  1589. y += 1f;
  1590. // make position fit into array
  1591. if (x < 0)
  1592. x = 0;
  1593. if (y < 0)
  1594. y = 0;
  1595. // integer indexs
  1596. int ix;
  1597. int iy;
  1598. // interpolators offset
  1599. float dx;
  1600. float dy;
  1601. int regsizeX = (int)m_regionWidth + 3; // map size see setterrain number of samples
  1602. int regsizeY = (int)m_regionHeight + 3; // map size see setterrain number of samples
  1603. int regsize = regsizeX;
  1604. int xstep = 1;
  1605. int ystep = regsizeX;
  1606. bool firstTri = false;
  1607. if (x < regsizeX - 1)
  1608. {
  1609. ix = (int)x;
  1610. dx = x - (float)ix;
  1611. }
  1612. else // out world use external height
  1613. {
  1614. ix = regsizeX - 2;
  1615. dx = 0;
  1616. }
  1617. if (y < regsizeY - 1)
  1618. {
  1619. iy = (int)y;
  1620. dy = y - (float)iy;
  1621. }
  1622. else
  1623. {
  1624. iy = regsizeY - 2;
  1625. dy = 0;
  1626. }
  1627. firstTri = dy > dx;
  1628. float h0;
  1629. float h1;
  1630. float h2;
  1631. iy *= regsize;
  1632. iy += ix; // all indexes have iy + ix
  1633. float[] heights = m_terrainHeights;
  1634. if (firstTri)
  1635. {
  1636. h1 = ((float)heights[iy]); // 0,0 vertice
  1637. iy += ystep;
  1638. h0 = (float)heights[iy]; // 0,1
  1639. h2 = (float)heights[iy+xstep]; // 1,1 vertice
  1640. norm.X = h0 - h2;
  1641. norm.Y = h1 - h0;
  1642. }
  1643. else
  1644. {
  1645. h2 = ((float)heights[iy]); // 0,0 vertice
  1646. iy += xstep;
  1647. h0 = ((float)heights[iy]); // 1,0 vertice
  1648. h1 = (float)heights[iy+ystep]; // vertice 1,1
  1649. norm.X = h2 - h0;
  1650. norm.Y = h0 - h1;
  1651. }
  1652. norm.Z = 1;
  1653. norm.Normalize();
  1654. return norm;
  1655. }
  1656. private void InitTerrain()
  1657. {
  1658. lock(SimulationLock)
  1659. lock (OdeLock)
  1660. {
  1661. SafeNativeMethods.AllocateODEDataForThread(~0U);
  1662. if (m_terrainGeom != IntPtr.Zero)
  1663. {
  1664. actor_name_map.Remove(m_terrainGeom);
  1665. SafeNativeMethods.GeomDestroy(m_terrainGeom);
  1666. }
  1667. if (m_terrainHeightsHandler.IsAllocated)
  1668. m_terrainHeightsHandler.Free();
  1669. m_terrainHeights = null;
  1670. int heightmapWidthSamples = m_regionWidth + 3;
  1671. int heightmapHeightSamples = m_regionHeight + 3;
  1672. m_terrainHeights = new float[heightmapWidthSamples * heightmapHeightSamples];
  1673. m_terrainHeightsHandler = GCHandle.Alloc(m_terrainHeights, GCHandleType.Pinned);
  1674. m_lastRegionWidth = m_regionWidth;
  1675. HeightmapData = SafeNativeMethods.GeomOSTerrainDataCreate();
  1676. SafeNativeMethods.GeomOSTerrainDataBuild(HeightmapData, m_terrainHeightsHandler.AddrOfPinnedObject(), 0, 1.0f,
  1677. heightmapWidthSamples, heightmapHeightSamples,
  1678. 1, 0);
  1679. m_terrainGeom = SafeNativeMethods.CreateOSTerrain(GroundSpace, HeightmapData, 1);
  1680. if (m_terrainGeom != IntPtr.Zero)
  1681. {
  1682. SafeNativeMethods.GeomSetCategoryBits(m_terrainGeom, (uint)(CollisionCategories.Land));
  1683. SafeNativeMethods.GeomSetCollideBits(m_terrainGeom, 0);
  1684. PhysicsActor pa = new NullPhysicsActor();
  1685. pa.Name = "Terrain";
  1686. pa.PhysicsActorType = (int)ActorTypes.Ground;
  1687. actor_name_map[m_terrainGeom] = pa;
  1688. //geom_name_map[GroundGeom] = "Terrain";
  1689. SafeNativeMethods.GeomSetPosition(m_terrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
  1690. }
  1691. else
  1692. m_terrainHeightsHandler.Free();
  1693. }
  1694. }
  1695. public override void SetTerrain(float[] heightMap)
  1696. {
  1697. // assumes 1m size grid and constante size square regions
  1698. // needs to know about sims around in future
  1699. if(m_regionWidth != m_lastRegionWidth ||
  1700. m_regionHeight != m_lastRegionHeight ||
  1701. !m_terrainHeightsHandler.IsAllocated ||
  1702. m_terrainGeom == IntPtr.Zero)
  1703. InitTerrain();
  1704. int regionsizeX = m_regionWidth;
  1705. int regionsizeY = m_regionHeight;
  1706. int heightmapWidth = regionsizeX + 2;
  1707. int heightmapHeight = regionsizeY + 2;
  1708. int heightmapWidthSamples = heightmapWidth + 1;
  1709. int heightmapHeightSamples = heightmapHeight + 1;
  1710. float val;
  1711. int maxXX = regionsizeX + 1;
  1712. int maxYY = regionsizeY + 1;
  1713. // adding one margin all around so things don't fall in edges
  1714. int xx;
  1715. int yy = 0;
  1716. int yt = 0;
  1717. float minH = float.MaxValue;
  1718. float maxH = float.MinValue;
  1719. for (int y = 0; y < heightmapHeightSamples; y++)
  1720. {
  1721. if (y > 1 && y < maxYY)
  1722. yy += regionsizeX;
  1723. xx = 0;
  1724. lock(OdeLock)
  1725. {
  1726. for (int x = 0; x < heightmapWidthSamples; x++)
  1727. {
  1728. if (x > 1 && x < maxXX)
  1729. xx++;
  1730. val = Util.Clamp(heightMap[yy + xx], Constants.MinTerrainHeightmap, Constants.MaxTerrainHeightmap);
  1731. if(val > maxH)
  1732. maxH = val;
  1733. if(val < minH)
  1734. minH = val;
  1735. m_terrainHeights[yt + x] = val;
  1736. }
  1737. }
  1738. yt += heightmapWidthSamples;
  1739. }
  1740. lock(SimulationLock)
  1741. lock (OdeLock)
  1742. {
  1743. SafeNativeMethods.GeomOSTerrainDataSetBounds(HeightmapData, minH, maxH);
  1744. SafeNativeMethods.GeomSetPosition(m_terrainGeom, m_regionWidth * 0.5f, m_regionHeight * 0.5f, 0.0f);
  1745. }
  1746. }
  1747. public override void DeleteTerrain()
  1748. {
  1749. }
  1750. public float GetWaterLevel()
  1751. {
  1752. return waterlevel;
  1753. }
  1754. public override void SetWaterLevel(float baseheight)
  1755. {
  1756. waterlevel = baseheight;
  1757. }
  1758. public override void Dispose()
  1759. {
  1760. lock(SimulationLock)
  1761. lock (OdeLock)
  1762. {
  1763. if (world == IntPtr.Zero)
  1764. return;
  1765. SafeNativeMethods.AllocateODEDataForThread(~0U);
  1766. if (m_meshWorker != null)
  1767. m_meshWorker.Stop();
  1768. if (m_rayCastManager != null)
  1769. {
  1770. m_rayCastManager.Dispose();
  1771. m_rayCastManager = null;
  1772. }
  1773. lock (_prims)
  1774. {
  1775. foreach (OdePrim prm in _prims.Values)
  1776. {
  1777. prm.DoAChange(changes.Remove, null);
  1778. _collisionEventPrim.Remove(prm);
  1779. }
  1780. _prims.Clear();
  1781. }
  1782. OdeCharacter[] chtorem;
  1783. lock (_characters)
  1784. {
  1785. chtorem = new OdeCharacter[_characters.Count];
  1786. _characters.CopyTo(chtorem);
  1787. }
  1788. foreach (OdeCharacter ch in chtorem)
  1789. ch.DoAChange(changes.Remove, null);
  1790. if (m_terrainGeom != IntPtr.Zero)
  1791. SafeNativeMethods.GeomDestroy(m_terrainGeom);
  1792. m_terrainGeom = IntPtr.Zero;
  1793. if (m_terrainHeightsHandler.IsAllocated)
  1794. m_terrainHeightsHandler.Free();
  1795. m_terrainHeights = null;
  1796. m_lastRegionWidth = 0;
  1797. m_lastRegionHeight = 0;
  1798. if (ContactgeomsArray != IntPtr.Zero)
  1799. {
  1800. Marshal.FreeHGlobal(ContactgeomsArray);
  1801. ContactgeomsArray = IntPtr.Zero;
  1802. }
  1803. if (GlobalContactsArray != IntPtr.Zero)
  1804. {
  1805. Marshal.FreeHGlobal(GlobalContactsArray);
  1806. GlobalContactsArray = IntPtr.Zero;
  1807. }
  1808. SafeNativeMethods.WorldDestroy(world);
  1809. world = IntPtr.Zero;
  1810. //d.CloseODE();
  1811. }
  1812. }
  1813. private int compareByCollisionsDesc(OdePrim A, OdePrim B)
  1814. {
  1815. return -A.CollisionScore.CompareTo(B.CollisionScore);
  1816. }
  1817. public override Dictionary<uint, float> GetTopColliders()
  1818. {
  1819. Dictionary<uint, float> topColliders;
  1820. List<OdePrim> orderedPrims;
  1821. lock (_activeprims)
  1822. orderedPrims = new List<OdePrim>(_activeprims);
  1823. orderedPrims.Sort(compareByCollisionsDesc);
  1824. topColliders = orderedPrims.Take(25).ToDictionary(p => p.LocalID, p => p.CollisionScore);
  1825. return topColliders;
  1826. }
  1827. public override bool SupportsRayCast()
  1828. {
  1829. return true;
  1830. }
  1831. public override void RaycastWorld(Vector3 position, Vector3 direction, float length, RaycastCallback retMethod)
  1832. {
  1833. if (retMethod != null)
  1834. {
  1835. ODERayRequest req = new ODERayRequest();
  1836. req.actor = null;
  1837. req.callbackMethod = retMethod;
  1838. req.length = length;
  1839. req.Normal = direction;
  1840. req.Origin = position;
  1841. req.Count = 0;
  1842. req.filter = RayFilterFlags.AllPrims;
  1843. m_rayCastManager.QueueRequest(req);
  1844. }
  1845. }
  1846. public override void RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayCallback retMethod)
  1847. {
  1848. if (retMethod != null)
  1849. {
  1850. ODERayRequest req = new ODERayRequest();
  1851. req.actor = null;
  1852. req.callbackMethod = retMethod;
  1853. req.length = length;
  1854. req.Normal = direction;
  1855. req.Origin = position;
  1856. req.Count = Count;
  1857. req.filter = RayFilterFlags.AllPrims;
  1858. m_rayCastManager.QueueRequest(req);
  1859. }
  1860. }
  1861. public override List<ContactResult> RaycastWorld(Vector3 position, Vector3 direction, float length, int Count)
  1862. {
  1863. List<ContactResult> ourresults = new List<ContactResult>();
  1864. object SyncObject = new object();
  1865. RayCallback retMethod = delegate(List<ContactResult> results)
  1866. {
  1867. lock (SyncObject)
  1868. {
  1869. ourresults = results;
  1870. Monitor.PulseAll(SyncObject);
  1871. }
  1872. };
  1873. ODERayRequest req = new ODERayRequest();
  1874. req.actor = null;
  1875. req.callbackMethod = retMethod;
  1876. req.length = length;
  1877. req.Normal = direction;
  1878. req.Origin = position;
  1879. req.Count = Count;
  1880. req.filter = RayFilterFlags.AllPrims;
  1881. lock (SyncObject)
  1882. {
  1883. m_rayCastManager.QueueRequest(req);
  1884. if (!Monitor.Wait(SyncObject, 500))
  1885. return null;
  1886. else
  1887. return ourresults;
  1888. }
  1889. }
  1890. public override bool SupportsRaycastWorldFiltered()
  1891. {
  1892. return true;
  1893. }
  1894. public override object RaycastWorld(Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags filter)
  1895. {
  1896. object SyncObject = new object();
  1897. List<ContactResult> ourresults = new List<ContactResult>();
  1898. RayCallback retMethod = delegate(List<ContactResult> results)
  1899. {
  1900. lock (SyncObject)
  1901. {
  1902. ourresults = results;
  1903. Monitor.PulseAll(SyncObject);
  1904. }
  1905. };
  1906. ODERayRequest req = new ODERayRequest();
  1907. req.actor = null;
  1908. req.callbackMethod = retMethod;
  1909. req.length = length;
  1910. req.Normal = direction;
  1911. req.Origin = position;
  1912. req.Count = Count;
  1913. req.filter = filter;
  1914. lock (SyncObject)
  1915. {
  1916. m_rayCastManager.QueueRequest(req);
  1917. if (!Monitor.Wait(SyncObject, 500))
  1918. return null;
  1919. else
  1920. return ourresults;
  1921. }
  1922. }
  1923. public override List<ContactResult> RaycastActor(PhysicsActor actor, Vector3 position, Vector3 direction, float length, int Count, RayFilterFlags flags)
  1924. {
  1925. if (actor == null)
  1926. return new List<ContactResult>();
  1927. IntPtr geom;
  1928. if (actor is OdePrim)
  1929. geom = ((OdePrim)actor).prim_geom;
  1930. else if (actor is OdeCharacter)
  1931. geom = ((OdePrim)actor).prim_geom;
  1932. else
  1933. return new List<ContactResult>();
  1934. if (geom == IntPtr.Zero)
  1935. return new List<ContactResult>();
  1936. List<ContactResult> ourResults = null;
  1937. object SyncObject = new object();
  1938. RayCallback retMethod = delegate(List<ContactResult> results)
  1939. {
  1940. lock (SyncObject)
  1941. {
  1942. ourResults = results;
  1943. Monitor.PulseAll(SyncObject);
  1944. }
  1945. };
  1946. ODERayRequest req = new ODERayRequest();
  1947. req.actor = actor;
  1948. req.callbackMethod = retMethod;
  1949. req.length = length;
  1950. req.Normal = direction;
  1951. req.Origin = position;
  1952. req.Count = Count;
  1953. req.filter = flags;
  1954. lock (SyncObject)
  1955. {
  1956. m_rayCastManager.QueueRequest(req);
  1957. if (!Monitor.Wait(SyncObject, 500))
  1958. return new List<ContactResult>();
  1959. }
  1960. if (ourResults == null)
  1961. return new List<ContactResult>();
  1962. return ourResults;
  1963. }
  1964. public override List<ContactResult> BoxProbe(Vector3 position, Vector3 size, Quaternion orientation, int Count, RayFilterFlags flags)
  1965. {
  1966. List<ContactResult> ourResults = null;
  1967. object SyncObject = new object();
  1968. ProbeBoxCallback retMethod = delegate(List<ContactResult> results)
  1969. {
  1970. lock (SyncObject)
  1971. {
  1972. ourResults = results;
  1973. Monitor.PulseAll(SyncObject);
  1974. }
  1975. };
  1976. ODERayRequest req = new ODERayRequest();
  1977. req.actor = null;
  1978. req.callbackMethod = retMethod;
  1979. req.Normal = size;
  1980. req.Origin = position;
  1981. req.orientation = orientation;
  1982. req.Count = Count;
  1983. req.filter = flags;
  1984. lock (SyncObject)
  1985. {
  1986. m_rayCastManager.QueueRequest(req);
  1987. if (!Monitor.Wait(SyncObject, 500))
  1988. return new List<ContactResult>();
  1989. }
  1990. if (ourResults == null)
  1991. return new List<ContactResult>();
  1992. return ourResults;
  1993. }
  1994. public override List<ContactResult> SphereProbe(Vector3 position, float radius, int Count, RayFilterFlags flags)
  1995. {
  1996. List<ContactResult> ourResults = null;
  1997. object SyncObject = new object();
  1998. ProbeSphereCallback retMethod = delegate(List<ContactResult> results)
  1999. {
  2000. ourResults = results;
  2001. Monitor.PulseAll(SyncObject);
  2002. };
  2003. ODERayRequest req = new ODERayRequest();
  2004. req.actor = null;
  2005. req.callbackMethod = retMethod;
  2006. req.length = radius;
  2007. req.Origin = position;
  2008. req.Count = Count;
  2009. req.filter = flags;
  2010. lock (SyncObject)
  2011. {
  2012. m_rayCastManager.QueueRequest(req);
  2013. if (!Monitor.Wait(SyncObject, 500))
  2014. return new List<ContactResult>();
  2015. }
  2016. if (ourResults == null)
  2017. return new List<ContactResult>();
  2018. return ourResults;
  2019. }
  2020. public override List<ContactResult> PlaneProbe(PhysicsActor actor, Vector4 plane, int Count, RayFilterFlags flags)
  2021. {
  2022. IntPtr geom = IntPtr.Zero;;
  2023. if (actor != null)
  2024. {
  2025. if (actor is OdePrim)
  2026. geom = ((OdePrim)actor).prim_geom;
  2027. else if (actor is OdeCharacter)
  2028. geom = ((OdePrim)actor).prim_geom;
  2029. }
  2030. List<ContactResult> ourResults = null;
  2031. object SyncObject = new object();
  2032. ProbePlaneCallback retMethod = delegate(List<ContactResult> results)
  2033. {
  2034. ourResults = results;
  2035. Monitor.PulseAll(SyncObject);
  2036. };
  2037. ODERayRequest req = new ODERayRequest();
  2038. req.actor = null;
  2039. req.callbackMethod = retMethod;
  2040. req.length = plane.W;
  2041. req.Normal.X = plane.X;
  2042. req.Normal.Y = plane.Y;
  2043. req.Normal.Z = plane.Z;
  2044. req.Count = Count;
  2045. req.filter = flags;
  2046. lock (SyncObject)
  2047. {
  2048. m_rayCastManager.QueueRequest(req);
  2049. if (!Monitor.Wait(SyncObject, 500))
  2050. return new List<ContactResult>();
  2051. }
  2052. if (ourResults == null)
  2053. return new List<ContactResult>();
  2054. return ourResults;
  2055. }
  2056. public override int SitAvatar(PhysicsActor actor, Vector3 AbsolutePosition, Vector3 CameraPosition, Vector3 offset, Vector3 AvatarSize, SitAvatarCallback PhysicsSitResponse)
  2057. {
  2058. Util.FireAndForget( delegate
  2059. {
  2060. ODESitAvatar sitAvatar = new ODESitAvatar(this, m_rayCastManager);
  2061. if(sitAvatar != null)
  2062. sitAvatar.Sit(actor, AbsolutePosition, CameraPosition, offset, AvatarSize, PhysicsSitResponse);
  2063. });
  2064. return 1;
  2065. }
  2066. }
  2067. }