OdePlugin.cs 142 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959196019611962196319641965196619671968196919701971197219731974197519761977197819791980198119821983198419851986198719881989199019911992199319941995199619971998199920002001200220032004200520062007200820092010201120122013201420152016201720182019202020212022202320242025202620272028202920302031203220332034203520362037203820392040204120422043204420452046204720482049205020512052205320542055205620572058205920602061206220632064206520662067206820692070207120722073207420752076207720782079208020812082208320842085208620872088208920902091209220932094209520962097209820992100210121022103210421052106210721082109211021112112211321142115211621172118211921202121212221232124212521262127212821292130213121322133213421352136213721382139214021412142214321442145214621472148214921502151215221532154215521562157215821592160216121622163216421652166216721682169217021712172217321742175217621772178217921802181218221832184218521862187218821892190219121922193219421952196219721982199220022012202220322042205220622072208220922102211221222132214221522162217221822192220222122222223222422252226222722282229223022312232223322342235223622372238223922402241224222432244224522462247224822492250225122522253225422552256225722582259226022612262226322642265226622672268226922702271227222732274227522762277227822792280228122822283228422852286228722882289229022912292229322942295229622972298229923002301230223032304230523062307230823092310231123122313231423152316231723182319232023212322232323242325232623272328232923302331233223332334233523362337233823392340234123422343234423452346234723482349235023512352235323542355235623572358235923602361236223632364236523662367236823692370237123722373237423752376237723782379238023812382238323842385238623872388238923902391239223932394239523962397239823992400240124022403240424052406240724082409241024112412241324142415241624172418241924202421242224232424242524262427242824292430243124322433243424352436243724382439244024412442244324442445244624472448244924502451245224532454245524562457245824592460246124622463246424652466246724682469247024712472247324742475247624772478247924802481248224832484248524862487248824892490249124922493249424952496249724982499250025012502250325042505250625072508250925102511251225132514251525162517251825192520252125222523252425252526252725282529253025312532253325342535253625372538253925402541254225432544254525462547254825492550255125522553255425552556255725582559256025612562256325642565256625672568256925702571257225732574257525762577257825792580258125822583258425852586258725882589259025912592259325942595259625972598259926002601260226032604260526062607260826092610261126122613261426152616261726182619262026212622262326242625262626272628262926302631263226332634263526362637263826392640264126422643264426452646264726482649265026512652265326542655265626572658265926602661266226632664266526662667266826692670267126722673267426752676267726782679268026812682268326842685268626872688268926902691269226932694269526962697269826992700270127022703270427052706270727082709271027112712271327142715271627172718271927202721272227232724272527262727272827292730273127322733273427352736273727382739274027412742274327442745274627472748274927502751275227532754275527562757275827592760276127622763276427652766276727682769277027712772277327742775277627772778277927802781278227832784278527862787278827892790279127922793279427952796279727982799280028012802280328042805280628072808280928102811281228132814281528162817281828192820282128222823282428252826282728282829283028312832283328342835283628372838283928402841284228432844284528462847284828492850285128522853285428552856285728582859286028612862286328642865286628672868286928702871287228732874287528762877287828792880288128822883288428852886288728882889289028912892289328942895289628972898289929002901290229032904290529062907290829092910291129122913291429152916291729182919292029212922292329242925292629272928292929302931293229332934293529362937293829392940294129422943294429452946294729482949295029512952295329542955295629572958295929602961296229632964296529662967296829692970297129722973297429752976297729782979298029812982298329842985298629872988298929902991299229932994299529962997299829993000300130023003300430053006300730083009301030113012301330143015301630173018301930203021302230233024302530263027302830293030303130323033303430353036303730383039304030413042304330443045304630473048304930503051305230533054305530563057305830593060306130623063306430653066306730683069307030713072307330743075307630773078307930803081308230833084308530863087308830893090309130923093309430953096309730983099310031013102310331043105310631073108310931103111311231133114311531163117311831193120312131223123312431253126312731283129313031313132313331343135313631373138313931403141314231433144314531463147314831493150315131523153315431553156315731583159316031613162316331643165316631673168316931703171317231733174317531763177317831793180318131823183318431853186318731883189319031913192319331943195319631973198319932003201320232033204320532063207320832093210321132123213321432153216321732183219322032213222322332243225
  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 OpenSim 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. //#define USE_DRAWSTUFF
  28. using System;
  29. using System.Collections.Generic;
  30. using System.Reflection;
  31. using System.Runtime.InteropServices;
  32. using System.Threading;
  33. using System.IO;
  34. using System.Diagnostics;
  35. using log4net;
  36. using Nini.Config;
  37. using Ode.NET;
  38. #if USE_DRAWSTUFF
  39. using Drawstuff.NET;
  40. #endif
  41. using OpenSim.Framework;
  42. using OpenSim.Region.Physics.Manager;
  43. using OpenMetaverse;
  44. //using OpenSim.Region.Physics.OdePlugin.Meshing;
  45. namespace OpenSim.Region.Physics.OdePlugin
  46. {
  47. /// <summary>
  48. /// ODE plugin
  49. /// </summary>
  50. public class OdePlugin : IPhysicsPlugin
  51. {
  52. //private static readonly log4net.ILog m_log = log4net.LogManager.GetLogger(System.Reflection.MethodBase.GetCurrentMethod().DeclaringType);
  53. private CollisionLocker ode;
  54. private OdeScene _mScene;
  55. public OdePlugin()
  56. {
  57. ode = new CollisionLocker();
  58. }
  59. public bool Init()
  60. {
  61. return true;
  62. }
  63. public PhysicsScene GetScene(String sceneIdentifier)
  64. {
  65. if (_mScene == null)
  66. {
  67. // Initializing ODE only when a scene is created allows alternative ODE plugins to co-habit (according to
  68. // http://opensimulator.org/mantis/view.php?id=2750).
  69. d.InitODE();
  70. _mScene = new OdeScene(ode, sceneIdentifier);
  71. }
  72. return (_mScene);
  73. }
  74. public string GetName()
  75. {
  76. return ("OpenDynamicsEngine");
  77. }
  78. public void Dispose()
  79. {
  80. }
  81. }
  82. public enum StatusIndicators : int
  83. {
  84. Generic = 0,
  85. Start = 1,
  86. End = 2
  87. }
  88. public struct sCollisionData
  89. {
  90. public uint ColliderLocalId;
  91. public uint CollidedWithLocalId;
  92. public int NumberOfCollisions;
  93. public int CollisionType;
  94. public int StatusIndicator;
  95. public int lastframe;
  96. }
  97. [Flags]
  98. public enum CollisionCategories : int
  99. {
  100. Disabled = 0,
  101. Geom = 0x00000001,
  102. Body = 0x00000002,
  103. Space = 0x00000004,
  104. Character = 0x00000008,
  105. Land = 0x00000010,
  106. Water = 0x00000020,
  107. Wind = 0x00000040,
  108. Sensor = 0x00000080,
  109. Selected = 0x00000100
  110. }
  111. public class OdeScene : PhysicsScene
  112. {
  113. private ILog m_log;
  114. // private Dictionary<string, sCollisionData> m_storedCollisions = new Dictionary<string, sCollisionData>();
  115. CollisionLocker ode;
  116. protected Random fluidRandomizer = new Random(Environment.TickCount);
  117. private const uint m_regionWidth = Constants.RegionSize;
  118. private const uint m_regionHeight = Constants.RegionSize;
  119. private float ODE_STEPSIZE = 0.020f;
  120. private float metersInSpace = 29.9f;
  121. public float gravityx = 0f;
  122. public float gravityy = 0f;
  123. public float gravityz = -9.8f;
  124. private float contactsurfacelayer = 0.001f;
  125. private int worldHashspaceLow = -4;
  126. private int worldHashspaceHigh = 128;
  127. private int smallHashspaceLow = -4;
  128. private int smallHashspaceHigh = 66;
  129. private float waterlevel = 0f;
  130. private int framecount = 0;
  131. //private int m_returncollisions = 10;
  132. private IntPtr contactgroup;
  133. private IntPtr LandGeom;
  134. private IntPtr WaterGeom;
  135. private float nmTerrainContactFriction = 255.0f;
  136. private float nmTerrainContactBounce = 0.1f;
  137. private float nmTerrainContactERP = 0.1025f;
  138. private float mTerrainContactFriction = 75f;
  139. private float mTerrainContactBounce = 0.1f;
  140. private float mTerrainContactERP = 0.05025f;
  141. private float nmAvatarObjectContactFriction = 250f;
  142. private float nmAvatarObjectContactBounce = 0.1f;
  143. private float mAvatarObjectContactFriction = 75f;
  144. private float mAvatarObjectContactBounce = 0.1f;
  145. private float avPIDD = 3200f;
  146. private float avPIDP = 1400f;
  147. private float avCapRadius = 0.37f;
  148. private float avStandupTensor = 2000000f;
  149. private float avDensity = 80f;
  150. private float avHeightFudgeFactor = 0.52f;
  151. private float avMovementDivisorWalk = 1.3f;
  152. private float avMovementDivisorRun = 0.8f;
  153. public bool meshSculptedPrim = true;
  154. public float meshSculptLOD = 32;
  155. public float MeshSculptphysicalLOD = 16;
  156. public float geomDefaultDensity = 10.000006836f;
  157. public int geomContactPointsStartthrottle = 3;
  158. public int geomUpdatesPerThrottledUpdate = 15;
  159. public float bodyPIDD = 35f;
  160. public float bodyPIDG = 25;
  161. public int geomCrossingFailuresBeforeOutofbounds = 5;
  162. public float bodyMotorJointMaxforceTensor = 2;
  163. public int bodyFramesAutoDisable = 20;
  164. private float[] _heightmap;
  165. private float[] _watermap;
  166. private bool m_filterCollisions = true;
  167. // private float[] _origheightmap;
  168. private d.NearCallback nearCallback;
  169. public d.TriCallback triCallback;
  170. public d.TriArrayCallback triArrayCallback;
  171. private List<OdeCharacter> _characters = new List<OdeCharacter>();
  172. private List<OdePrim> _prims = new List<OdePrim>();
  173. private List<OdePrim> _activeprims = new List<OdePrim>();
  174. private List<OdePrim> _taintedPrim = new List<OdePrim>();
  175. private List<OdeCharacter> _taintedActors = new List<OdeCharacter>();
  176. private List<d.ContactGeom> _perloopContact = new List<d.ContactGeom>();
  177. private List<PhysicsActor> _collisionEventPrim = new List<PhysicsActor>();
  178. public Dictionary<IntPtr, String> geom_name_map = new Dictionary<IntPtr, String>();
  179. public Dictionary<IntPtr, PhysicsActor> actor_name_map = new Dictionary<IntPtr, PhysicsActor>();
  180. private bool m_NINJA_physics_joints_enabled = false;
  181. //private Dictionary<String, IntPtr> jointpart_name_map = new Dictionary<String,IntPtr>();
  182. private Dictionary<String, List<PhysicsJoint>> joints_connecting_actor = new Dictionary<String, List<PhysicsJoint>>();
  183. private d.ContactGeom[] contacts = new d.ContactGeom[80];
  184. private List<PhysicsJoint> requestedJointsToBeCreated = new List<PhysicsJoint>(); // lock only briefly. accessed by external code (to request new joints) and by OdeScene.Simulate() to move those joints into pending/active
  185. private List<PhysicsJoint> pendingJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
  186. private List<PhysicsJoint> activeJoints = new List<PhysicsJoint>(); // can lock for longer. accessed only by OdeScene.
  187. private List<string> requestedJointsToBeDeleted = new List<string>(); // lock only briefly. accessed by external code (to request deletion of joints) and by OdeScene.Simulate() to move those joints out of pending/active
  188. private Object externalJointRequestsLock = new Object();
  189. private Dictionary<String, PhysicsJoint> SOPName_to_activeJoint = new Dictionary<String, PhysicsJoint>();
  190. private Dictionary<String, PhysicsJoint> SOPName_to_pendingJoint = new Dictionary<String, PhysicsJoint>();
  191. private d.Contact contact;
  192. private d.Contact TerrainContact;
  193. private d.Contact AvatarMovementprimContact;
  194. private d.Contact AvatarMovementTerrainContact;
  195. private d.Contact WaterContact;
  196. //Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
  197. //Ckrinke private int m_randomizeWater = 200;
  198. private int m_physicsiterations = 10;
  199. private float m_SkipFramesAtms = 0.40f; // Drop frames gracefully at a 400 ms lag
  200. private PhysicsActor PANull = new NullPhysicsActor();
  201. private float step_time = 0.0f;
  202. //Ckrinke: Comment out until used. We declare it, initialize it, but do not use it
  203. //Ckrinke private int ms = 0;
  204. public IntPtr world;
  205. //private bool returncollisions = false;
  206. // private uint obj1LocalID = 0;
  207. private uint obj2LocalID = 0;
  208. //private int ctype = 0;
  209. private OdeCharacter cc1;
  210. private OdePrim cp1;
  211. private OdeCharacter cc2;
  212. private OdePrim cp2;
  213. //private int cStartStop = 0;
  214. //private string cDictKey = "";
  215. public IntPtr space;
  216. //private IntPtr tmpSpace;
  217. // split static geometry collision handling into spaces of 30 meters
  218. public IntPtr[,] staticPrimspace;
  219. public Object OdeLock;
  220. public IMesher mesher;
  221. private IConfigSource m_config;
  222. public bool physics_logging = false;
  223. public int physics_logging_interval = 0;
  224. public bool physics_logging_append_existing_logfile = false;
  225. public d.Vector3 xyz = new d.Vector3(128.1640f, 128.3079f, 25.7600f);
  226. public d.Vector3 hpr = new d.Vector3(125.5000f, -17.0000f, 0.0000f);
  227. /// <summary>
  228. /// Initiailizes the scene
  229. /// Sets many properties that ODE requires to be stable
  230. /// These settings need to be tweaked 'exactly' right or weird stuff happens.
  231. /// </summary>
  232. public OdeScene(CollisionLocker dode, string sceneIdentifier)
  233. {
  234. m_log
  235. = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType.ToString() + "." + sceneIdentifier);
  236. OdeLock = new Object();
  237. ode = dode;
  238. nearCallback = near;
  239. triCallback = TriCallback;
  240. triArrayCallback = TriArrayCallback;
  241. lock (OdeLock)
  242. {
  243. // Create the world and the first space
  244. world = d.WorldCreate();
  245. space = d.HashSpaceCreate(IntPtr.Zero);
  246. contactgroup = d.JointGroupCreate(0);
  247. //contactgroup
  248. d.WorldSetAutoDisableFlag(world, false);
  249. #if USE_DRAWSTUFF
  250. Thread viewthread = new Thread(new ParameterizedThreadStart(startvisualization));
  251. viewthread.Start();
  252. #endif
  253. }
  254. // zero out a heightmap array float array (single dimention [flattened]))
  255. _heightmap = new float[514*514];
  256. _watermap = new float[258 * 258];
  257. // Zero out the prim spaces array (we split our space into smaller spaces so
  258. // we can hit test less.
  259. }
  260. #if USE_DRAWSTUFF
  261. public void startvisualization(object o)
  262. {
  263. ds.Functions fn;
  264. fn.version = ds.VERSION;
  265. fn.start = new ds.CallbackFunction(start);
  266. fn.step = new ds.CallbackFunction(step);
  267. fn.command = new ds.CallbackFunction(command);
  268. fn.stop = null;
  269. fn.path_to_textures = "./textures";
  270. string[] args = new string[0];
  271. ds.SimulationLoop(args.Length, args, 352, 288, ref fn);
  272. }
  273. #endif
  274. // Initialize the mesh plugin
  275. public override void Initialise(IMesher meshmerizer, IConfigSource config)
  276. {
  277. mesher = meshmerizer;
  278. m_config = config;
  279. // Defaults
  280. if (Environment.OSVersion.Platform == PlatformID.Unix)
  281. {
  282. avPIDD = 3200.0f;
  283. avPIDP = 1400.0f;
  284. avStandupTensor = 2000000f;
  285. }
  286. else
  287. {
  288. avPIDD = 2200.0f;
  289. avPIDP = 900.0f;
  290. avStandupTensor = 550000f;
  291. }
  292. if (m_config != null)
  293. {
  294. IConfig physicsconfig = m_config.Configs["ODEPhysicsSettings"];
  295. if (physicsconfig != null)
  296. {
  297. gravityx = physicsconfig.GetFloat("world_gravityx", 0f);
  298. gravityy = physicsconfig.GetFloat("world_gravityy", 0f);
  299. gravityz = physicsconfig.GetFloat("world_gravityz", -9.8f);
  300. worldHashspaceLow = physicsconfig.GetInt("world_hashspace_low", -4);
  301. worldHashspaceHigh = physicsconfig.GetInt("world_hashspace_high", 128);
  302. metersInSpace = physicsconfig.GetFloat("meters_in_small_space", 29.9f);
  303. smallHashspaceLow = physicsconfig.GetInt("small_hashspace_size_low", -4);
  304. smallHashspaceHigh = physicsconfig.GetInt("small_hashspace_size_high", 66);
  305. contactsurfacelayer = physicsconfig.GetFloat("world_contact_surface_layer", 0.001f);
  306. nmTerrainContactFriction = physicsconfig.GetFloat("nm_terraincontact_friction", 255.0f);
  307. nmTerrainContactBounce = physicsconfig.GetFloat("nm_terraincontact_bounce", 0.1f);
  308. nmTerrainContactERP = physicsconfig.GetFloat("nm_terraincontact_erp", 0.1025f);
  309. mTerrainContactFriction = physicsconfig.GetFloat("m_terraincontact_friction", 75f);
  310. mTerrainContactBounce = physicsconfig.GetFloat("m_terraincontact_bounce", 0.1f);
  311. mTerrainContactERP = physicsconfig.GetFloat("m_terraincontact_erp", 0.05025f);
  312. nmAvatarObjectContactFriction = physicsconfig.GetFloat("objectcontact_friction", 250f);
  313. nmAvatarObjectContactBounce = physicsconfig.GetFloat("objectcontact_bounce", 0.2f);
  314. mAvatarObjectContactFriction = physicsconfig.GetFloat("m_avatarobjectcontact_friction", 75f);
  315. mAvatarObjectContactBounce = physicsconfig.GetFloat("m_avatarobjectcontact_bounce", 0.1f);
  316. ODE_STEPSIZE = physicsconfig.GetFloat("world_stepsize", 0.020f);
  317. m_physicsiterations = physicsconfig.GetInt("world_internal_steps_without_collisions", 10);
  318. avDensity = physicsconfig.GetFloat("av_density", 80f);
  319. avHeightFudgeFactor = physicsconfig.GetFloat("av_height_fudge_factor", 0.52f);
  320. avMovementDivisorWalk = physicsconfig.GetFloat("av_movement_divisor_walk", 1.3f);
  321. avMovementDivisorRun = physicsconfig.GetFloat("av_movement_divisor_run", 0.8f);
  322. avCapRadius = physicsconfig.GetFloat("av_capsule_radius", 0.37f);
  323. geomContactPointsStartthrottle = physicsconfig.GetInt("geom_contactpoints_start_throttling", 3);
  324. geomUpdatesPerThrottledUpdate = physicsconfig.GetInt("geom_updates_before_throttled_update", 15);
  325. geomCrossingFailuresBeforeOutofbounds = physicsconfig.GetInt("geom_crossing_faiures_before_outofbounds", 5);
  326. geomDefaultDensity = physicsconfig.GetFloat("geometry_default_density", 10.000006836f);
  327. bodyFramesAutoDisable = physicsconfig.GetInt("body_frames_auto_disable", 20);
  328. bodyPIDD = physicsconfig.GetFloat("body_pid_derivative", 35f);
  329. bodyPIDG = physicsconfig.GetFloat("body_pid_gain", 25f);
  330. meshSculptedPrim = physicsconfig.GetBoolean("mesh_sculpted_prim", true);
  331. meshSculptLOD = physicsconfig.GetFloat("mesh_lod", 32f);
  332. MeshSculptphysicalLOD = physicsconfig.GetFloat("mesh_physical_lod", 16f);
  333. m_filterCollisions = physicsconfig.GetBoolean("filter_collisions", true);
  334. if (Environment.OSVersion.Platform == PlatformID.Unix)
  335. {
  336. avPIDD = physicsconfig.GetFloat("av_pid_derivative_linux", 2200.0f);
  337. avPIDP = physicsconfig.GetFloat("av_pid_proportional_linux", 900.0f);
  338. avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_linux", 550000f);
  339. bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_linux", 5f);
  340. }
  341. else
  342. {
  343. avPIDD = physicsconfig.GetFloat("av_pid_derivative_win", 2200.0f);
  344. avPIDP = physicsconfig.GetFloat("av_pid_proportional_win", 900.0f);
  345. avStandupTensor = physicsconfig.GetFloat("av_capsule_standup_tensor_win", 550000f);
  346. bodyMotorJointMaxforceTensor = physicsconfig.GetFloat("body_motor_joint_maxforce_tensor_win", 5f);
  347. }
  348. physics_logging = physicsconfig.GetBoolean("physics_logging", false);
  349. physics_logging_interval = physicsconfig.GetInt("physics_logging_interval", 0);
  350. physics_logging_append_existing_logfile = physicsconfig.GetBoolean("physics_logging_append_existing_logfile", false);
  351. m_NINJA_physics_joints_enabled = physicsconfig.GetBoolean("use_NINJA_physics_joints", false);
  352. }
  353. }
  354. staticPrimspace = new IntPtr[(int)(300 / metersInSpace), (int)(300 / metersInSpace)];
  355. // Centeral contact friction and bounce
  356. // ckrinke 11/10/08 Enabling soft_erp but not soft_cfm until I figure out why
  357. // an avatar falls through in Z but not in X or Y when walking on a prim.
  358. contact.surface.mode |= d.ContactFlags.SoftERP;
  359. contact.surface.mu = nmAvatarObjectContactFriction;
  360. contact.surface.bounce = nmAvatarObjectContactBounce;
  361. contact.surface.soft_cfm = 0.010f;
  362. contact.surface.soft_erp = 0.010f;
  363. // Terrain contact friction and Bounce
  364. // This is the *non* moving version. Use this when an avatar
  365. // isn't moving to keep it in place better
  366. TerrainContact.surface.mode |= d.ContactFlags.SoftERP;
  367. TerrainContact.surface.mu = nmTerrainContactFriction;
  368. TerrainContact.surface.bounce = nmTerrainContactBounce;
  369. TerrainContact.surface.soft_erp = nmTerrainContactERP;
  370. WaterContact.surface.mode |= (d.ContactFlags.SoftERP | d.ContactFlags.SoftCFM);
  371. WaterContact.surface.mu = 0f; // No friction
  372. WaterContact.surface.bounce = 0.0f; // No bounce
  373. WaterContact.surface.soft_cfm = 0.010f;
  374. WaterContact.surface.soft_erp = 0.010f;
  375. // Prim contact friction and bounce
  376. // THis is the *non* moving version of friction and bounce
  377. // Use this when an avatar comes in contact with a prim
  378. // and is moving
  379. AvatarMovementprimContact.surface.mu = mAvatarObjectContactFriction;
  380. AvatarMovementprimContact.surface.bounce = mAvatarObjectContactBounce;
  381. // Terrain contact friction bounce and various error correcting calculations
  382. // Use this when an avatar is in contact with the terrain and moving.
  383. AvatarMovementTerrainContact.surface.mode |= d.ContactFlags.SoftERP;
  384. AvatarMovementTerrainContact.surface.mu = mTerrainContactFriction;
  385. AvatarMovementTerrainContact.surface.bounce = mTerrainContactBounce;
  386. AvatarMovementTerrainContact.surface.soft_erp = mTerrainContactERP;
  387. d.HashSpaceSetLevels(space, worldHashspaceLow, worldHashspaceHigh);
  388. // Set the gravity,, don't disable things automatically (we set it explicitly on some things)
  389. d.WorldSetGravity(world, gravityx, gravityy, gravityz);
  390. d.WorldSetContactSurfaceLayer(world, contactsurfacelayer);
  391. // Set how many steps we go without running collision testing
  392. // This is in addition to the step size.
  393. // Essentially Steps * m_physicsiterations
  394. d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
  395. //d.WorldSetContactMaxCorrectingVel(world, 1000.0f);
  396. for (int i = 0; i < staticPrimspace.GetLength(0); i++)
  397. {
  398. for (int j = 0; j < staticPrimspace.GetLength(1); j++)
  399. {
  400. staticPrimspace[i, j] = IntPtr.Zero;
  401. }
  402. }
  403. }
  404. internal void waitForSpaceUnlock(IntPtr space)
  405. {
  406. //if (space != IntPtr.Zero)
  407. //while (d.SpaceLockQuery(space)) { } // Wait and do nothing
  408. }
  409. /// <summary>
  410. /// Debug space message for printing the space that a prim/avatar is in.
  411. /// </summary>
  412. /// <param name="pos"></param>
  413. /// <returns>Returns which split up space the given position is in.</returns>
  414. public string whichspaceamIin(PhysicsVector pos)
  415. {
  416. return calculateSpaceForGeom(pos).ToString();
  417. }
  418. #region Collision Detection
  419. /// <summary>
  420. /// This is our near callback. A geometry is near a body
  421. /// </summary>
  422. /// <param name="space">The space that contains the geoms. Remember, spaces are also geoms</param>
  423. /// <param name="g1">a geometry or space</param>
  424. /// <param name="g2">another geometry or space</param>
  425. private void near(IntPtr space, IntPtr g1, IntPtr g2)
  426. {
  427. // no lock here! It's invoked from within Simulate(), which is thread-locked
  428. // Test if we're colliding a geom with a space.
  429. // If so we have to drill down into the space recursively
  430. if (d.GeomIsSpace(g1) || d.GeomIsSpace(g2))
  431. {
  432. if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
  433. return;
  434. // Separating static prim geometry spaces.
  435. // We'll be calling near recursivly if one
  436. // of them is a space to find all of the
  437. // contact points in the space
  438. try
  439. {
  440. d.SpaceCollide2(g1, g2, IntPtr.Zero, nearCallback);
  441. }
  442. catch (AccessViolationException)
  443. {
  444. m_log.Warn("[PHYSICS]: Unable to collide test a space");
  445. return;
  446. }
  447. //Colliding a space or a geom with a space or a geom. so drill down
  448. //Collide all geoms in each space..
  449. //if (d.GeomIsSpace(g1)) d.SpaceCollide(g1, IntPtr.Zero, nearCallback);
  450. //if (d.GeomIsSpace(g2)) d.SpaceCollide(g2, IntPtr.Zero, nearCallback);
  451. return;
  452. }
  453. if (g1 == IntPtr.Zero || g2 == IntPtr.Zero)
  454. return;
  455. IntPtr b1 = d.GeomGetBody(g1);
  456. IntPtr b2 = d.GeomGetBody(g2);
  457. // d.GeomClassID id = d.GeomGetClass(g1);
  458. String name1 = null;
  459. String name2 = null;
  460. if (!geom_name_map.TryGetValue(g1, out name1))
  461. {
  462. name1 = "null";
  463. }
  464. if (!geom_name_map.TryGetValue(g2, out name2))
  465. {
  466. name2 = "null";
  467. }
  468. //if (id == d.GeomClassId.TriMeshClass)
  469. //{
  470. // m_log.InfoFormat("near: A collision was detected between {1} and {2}", 0, name1, name2);
  471. //System.Console.WriteLine("near: A collision was detected between {1} and {2}", 0, name1, name2);
  472. //}
  473. // Figure out how many contact points we have
  474. int count = 0;
  475. try
  476. {
  477. // Colliding Geom To Geom
  478. // This portion of the function 'was' blatantly ripped off from BoxStack.cs
  479. if (g1 == g2)
  480. return; // Can't collide with yourself
  481. if (b1 != IntPtr.Zero && b2 != IntPtr.Zero && d.AreConnectedExcluding(b1, b2, d.JointType.Contact))
  482. return;
  483. lock (contacts)
  484. {
  485. count = d.Collide(g1, g2, contacts.GetLength(0), contacts, d.ContactGeom.SizeOf);
  486. }
  487. }
  488. catch (SEHException)
  489. {
  490. 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.");
  491. ode.drelease(world);
  492. base.TriggerPhysicsBasedRestart();
  493. }
  494. catch (AccessViolationException)
  495. {
  496. m_log.Warn("[PHYSICS]: Unable to collide test an object");
  497. return;
  498. }
  499. PhysicsActor p1;
  500. PhysicsActor p2;
  501. if (!actor_name_map.TryGetValue(g1, out p1))
  502. {
  503. p1 = PANull;
  504. }
  505. if (!actor_name_map.TryGetValue(g2, out p2))
  506. {
  507. p2 = PANull;
  508. }
  509. float max_collision_depth = 0f;
  510. if (p1.CollisionScore + count >= float.MaxValue)
  511. p1.CollisionScore = 0;
  512. p1.CollisionScore += count;
  513. if (p2.CollisionScore + count >= float.MaxValue)
  514. p2.CollisionScore = 0;
  515. p2.CollisionScore += count;
  516. for (int i = 0; i < count; i++)
  517. {
  518. max_collision_depth = (contacts[i].depth > max_collision_depth) ? contacts[i].depth : max_collision_depth;
  519. //m_log.Warn("[CCOUNT]: " + count);
  520. IntPtr joint;
  521. // If we're colliding with terrain, use 'TerrainContact' instead of contact.
  522. // allows us to have different settings
  523. // We only need to test p2 for 'jump crouch purposes'
  524. p2.IsColliding = true;
  525. //if ((framecount % m_returncollisions) == 0)
  526. switch (p1.PhysicsActorType)
  527. {
  528. case (int)ActorTypes.Agent:
  529. p2.CollidingObj = true;
  530. break;
  531. case (int)ActorTypes.Prim:
  532. if (p2.Velocity.X > 0 || p2.Velocity.Y > 0 || p2.Velocity.Z > 0)
  533. p2.CollidingObj = true;
  534. break;
  535. case (int)ActorTypes.Unknown:
  536. p2.CollidingGround = true;
  537. break;
  538. default:
  539. p2.CollidingGround = true;
  540. break;
  541. }
  542. // we don't want prim or avatar to explode
  543. #region InterPenetration Handling - Unintended physics explosions
  544. if (contacts[i].depth >= 0.08f)
  545. {
  546. //This is disabled at the moment only because it needs more tweaking
  547. //It will eventually be uncommented
  548. if (contacts[i].depth >= 1.00f)
  549. {
  550. //m_log.Debug("[PHYSICS]: " + contacts[i].depth.ToString());
  551. }
  552. //If you interpenetrate a prim with an agent
  553. if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
  554. p1.PhysicsActorType == (int) ActorTypes.Prim) ||
  555. (p1.PhysicsActorType == (int) ActorTypes.Agent &&
  556. p2.PhysicsActorType == (int) ActorTypes.Prim))
  557. {
  558. # region disabled code1
  559. //contacts[i].depth = contacts[i].depth * 4.15f;
  560. /*
  561. if (p2.PhysicsActorType == (int) ActorTypes.Agent)
  562. {
  563. p2.CollidingObj = true;
  564. contacts[i].depth = 0.003f;
  565. p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 2.5f);
  566. OdeCharacter character = (OdeCharacter) p2;
  567. character.SetPidStatus(true);
  568. contacts[i].pos = new d.Vector3(contacts[i].pos.X + (p1.Size.X / 2), contacts[i].pos.Y + (p1.Size.Y / 2), contacts[i].pos.Z + (p1.Size.Z / 2));
  569. }
  570. else
  571. {
  572. //contacts[i].depth = 0.0000000f;
  573. }
  574. if (p1.PhysicsActorType == (int) ActorTypes.Agent)
  575. {
  576. p1.CollidingObj = true;
  577. contacts[i].depth = 0.003f;
  578. p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 2.5f);
  579. contacts[i].pos = new d.Vector3(contacts[i].pos.X + (p2.Size.X / 2), contacts[i].pos.Y + (p2.Size.Y / 2), contacts[i].pos.Z + (p2.Size.Z / 2));
  580. OdeCharacter character = (OdeCharacter)p1;
  581. character.SetPidStatus(true);
  582. }
  583. else
  584. {
  585. //contacts[i].depth = 0.0000000f;
  586. }
  587. */
  588. #endregion
  589. }
  590. // If you interpenetrate a prim with another prim
  591. if (p1.PhysicsActorType == (int) ActorTypes.Prim && p2.PhysicsActorType == (int) ActorTypes.Prim)
  592. {
  593. #region disabledcode2
  594. //OdePrim op1 = (OdePrim)p1;
  595. //OdePrim op2 = (OdePrim)p2;
  596. //op1.m_collisionscore++;
  597. //op2.m_collisionscore++;
  598. //if (op1.m_collisionscore > 8000 || op2.m_collisionscore > 8000)
  599. //{
  600. //op1.m_taintdisable = true;
  601. //AddPhysicsActorTaint(p1);
  602. //op2.m_taintdisable = true;
  603. //AddPhysicsActorTaint(p2);
  604. //}
  605. //if (contacts[i].depth >= 0.25f)
  606. //{
  607. // Don't collide, one or both prim will expld.
  608. //op1.m_interpenetrationcount++;
  609. //op2.m_interpenetrationcount++;
  610. //interpenetrations_before_disable = 200;
  611. //if (op1.m_interpenetrationcount >= interpenetrations_before_disable)
  612. //{
  613. //op1.m_taintdisable = true;
  614. //AddPhysicsActorTaint(p1);
  615. //}
  616. //if (op2.m_interpenetrationcount >= interpenetrations_before_disable)
  617. //{
  618. // op2.m_taintdisable = true;
  619. //AddPhysicsActorTaint(p2);
  620. //}
  621. //contacts[i].depth = contacts[i].depth / 8f;
  622. //contacts[i].normal = new d.Vector3(0, 0, 1);
  623. //}
  624. //if (op1.m_disabled || op2.m_disabled)
  625. //{
  626. //Manually disabled objects stay disabled
  627. //contacts[i].depth = 0f;
  628. //}
  629. #endregion
  630. }
  631. if (contacts[i].depth >= 1.00f)
  632. {
  633. //m_log.Info("[P]: " + contacts[i].depth.ToString());
  634. if ((p2.PhysicsActorType == (int) ActorTypes.Agent &&
  635. p1.PhysicsActorType == (int) ActorTypes.Unknown) ||
  636. (p1.PhysicsActorType == (int) ActorTypes.Agent &&
  637. p2.PhysicsActorType == (int) ActorTypes.Unknown))
  638. {
  639. if (p2.PhysicsActorType == (int) ActorTypes.Agent)
  640. {
  641. OdeCharacter character = (OdeCharacter) p2;
  642. //p2.CollidingObj = true;
  643. contacts[i].depth = 0.00000003f;
  644. p2.Velocity = p2.Velocity + new PhysicsVector(0, 0, 0.5f);
  645. contacts[i].pos =
  646. new d.Vector3(contacts[i].pos.X + (p1.Size.X/2),
  647. contacts[i].pos.Y + (p1.Size.Y/2),
  648. contacts[i].pos.Z + (p1.Size.Z/2));
  649. character.SetPidStatus(true);
  650. }
  651. else
  652. {
  653. }
  654. if (p1.PhysicsActorType == (int) ActorTypes.Agent)
  655. {
  656. OdeCharacter character = (OdeCharacter)p1;
  657. //p2.CollidingObj = true;
  658. contacts[i].depth = 0.00000003f;
  659. p1.Velocity = p1.Velocity + new PhysicsVector(0, 0, 0.5f);
  660. contacts[i].pos =
  661. new d.Vector3(contacts[i].pos.X + (p1.Size.X/2),
  662. contacts[i].pos.Y + (p1.Size.Y/2),
  663. contacts[i].pos.Z + (p1.Size.Z/2));
  664. character.SetPidStatus(true);
  665. }
  666. else
  667. {
  668. //contacts[i].depth = 0.0000000f;
  669. }
  670. }
  671. }
  672. }
  673. #endregion
  674. // Logic for collision handling
  675. // Note, that if *all* contacts are skipped (VolumeDetect)
  676. // The prim still detects (and forwards) collision events but
  677. // appears to be phantom for the world
  678. Boolean skipThisContact = false;
  679. if ((p1 is OdePrim) && (((OdePrim)p1).m_isVolumeDetect))
  680. skipThisContact = true; // No collision on volume detect prims
  681. if (!skipThisContact && (p2 is OdePrim) && (((OdePrim)p2).m_isVolumeDetect))
  682. skipThisContact = true; // No collision on volume detect prims
  683. if (!skipThisContact && contacts[i].depth < 0f)
  684. skipThisContact = true;
  685. if (!skipThisContact && checkDupe(contacts[i], p2.PhysicsActorType))
  686. skipThisContact = true;
  687. if (!skipThisContact)
  688. {
  689. // If we're colliding against terrain
  690. if (name1 == "Terrain" || name2 == "Terrain")
  691. {
  692. // If we're moving
  693. if ((p2.PhysicsActorType == (int) ActorTypes.Agent) &&
  694. (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
  695. {
  696. // Use the movement terrain contact
  697. AvatarMovementTerrainContact.geom = contacts[i];
  698. _perloopContact.Add(contacts[i]);
  699. joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementTerrainContact);
  700. }
  701. else
  702. {
  703. // Use the non moving terrain contact
  704. TerrainContact.geom = contacts[i];
  705. _perloopContact.Add(contacts[i]);
  706. joint = d.JointCreateContact(world, contactgroup, ref TerrainContact);
  707. }
  708. //if (p2.PhysicsActorType == (int)ActorTypes.Prim)
  709. //{
  710. //m_log.Debug("[PHYSICS]: prim contacting with ground");
  711. //}
  712. }
  713. else if (name1 == "Water" || name2 == "Water")
  714. {
  715. if ((p2.PhysicsActorType == (int)ActorTypes.Prim))
  716. {
  717. }
  718. else
  719. {
  720. }
  721. //WaterContact.surface.soft_cfm = 0.0000f;
  722. //WaterContact.surface.soft_erp = 0.00000f;
  723. if (contacts[i].depth > 0.1f)
  724. {
  725. contacts[i].depth *= 52;
  726. //contacts[i].normal = new d.Vector3(0, 0, 1);
  727. //contacts[i].pos = new d.Vector3(0, 0, contacts[i].pos.Z - 5f);
  728. }
  729. WaterContact.geom = contacts[i];
  730. _perloopContact.Add(contacts[i]);
  731. joint = d.JointCreateContact(world, contactgroup, ref WaterContact);
  732. //m_log.Info("[PHYSICS]: Prim Water Contact" + contacts[i].depth);
  733. }
  734. else
  735. { // we're colliding with prim or avatar
  736. // check if we're moving
  737. if ((p2.PhysicsActorType == (int)ActorTypes.Agent) &&
  738. (Math.Abs(p2.Velocity.X) > 0.01f || Math.Abs(p2.Velocity.Y) > 0.01f))
  739. {
  740. // Use the Movement prim contact
  741. AvatarMovementprimContact.geom = contacts[i];
  742. _perloopContact.Add(contacts[i]);
  743. joint = d.JointCreateContact(world, contactgroup, ref AvatarMovementprimContact);
  744. }
  745. else
  746. { // Use the non movement contact
  747. contact.geom = contacts[i];
  748. _perloopContact.Add(contacts[i]);
  749. joint = d.JointCreateContact(world, contactgroup, ref contact);
  750. }
  751. }
  752. d.JointAttach(joint, b1, b2);
  753. }
  754. collision_accounting_events(p1, p2, max_collision_depth);
  755. if (count > geomContactPointsStartthrottle)
  756. {
  757. // If there are more then 3 contact points, it's likely
  758. // that we've got a pile of objects, so ...
  759. // We don't want to send out hundreds of terse updates over and over again
  760. // so lets throttle them and send them again after it's somewhat sorted out.
  761. p2.ThrottleUpdates = true;
  762. }
  763. //System.Console.WriteLine(count.ToString());
  764. //System.Console.WriteLine("near: A collision was detected between {1} and {2}", 0, name1, name2);
  765. }
  766. }
  767. private bool checkDupe(d.ContactGeom contactGeom, int atype)
  768. {
  769. bool result = false;
  770. //return result;
  771. if (!m_filterCollisions)
  772. return false;
  773. ActorTypes at = (ActorTypes)atype;
  774. lock (_perloopContact)
  775. {
  776. foreach (d.ContactGeom contact in _perloopContact)
  777. {
  778. //if ((contact.g1 == contactGeom.g1 && contact.g2 == contactGeom.g2))
  779. //{
  780. // || (contact.g2 == contactGeom.g1 && contact.g1 == contactGeom.g2)
  781. if (at == ActorTypes.Agent)
  782. {
  783. if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)) && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom)
  784. {
  785. if (Math.Abs(contact.depth - contactGeom.depth) < 0.052f)
  786. {
  787. //contactGeom.depth *= .00005f;
  788. //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
  789. // m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
  790. result = true;
  791. break;
  792. }
  793. else
  794. {
  795. //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
  796. }
  797. }
  798. else
  799. {
  800. //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
  801. //int i = 0;
  802. }
  803. }
  804. else if (at == ActorTypes.Prim)
  805. {
  806. //d.AABB aabb1 = new d.AABB();
  807. //d.AABB aabb2 = new d.AABB();
  808. //d.GeomGetAABB(contactGeom.g2, out aabb2);
  809. //d.GeomGetAABB(contactGeom.g1, out aabb1);
  810. //aabb1.
  811. if (((Math.Abs(contactGeom.normal.X - contact.normal.X) < 1.026f) && (Math.Abs(contactGeom.normal.Y - contact.normal.Y) < 0.303f) && (Math.Abs(contactGeom.normal.Z - contact.normal.Z) < 0.065f)) && contactGeom.g1 != LandGeom && contactGeom.g2 != LandGeom)
  812. {
  813. if (contactGeom.normal.X == contact.normal.X && contactGeom.normal.Y == contact.normal.Y && contactGeom.normal.Z == contact.normal.Z)
  814. {
  815. if (Math.Abs(contact.depth - contactGeom.depth) < 0.272f)
  816. {
  817. result = true;
  818. break;
  819. }
  820. }
  821. //m_log.DebugFormat("[Collsion]: Depth {0}", Math.Abs(contact.depth - contactGeom.depth));
  822. //m_log.DebugFormat("[Collision]: <{0},{1},{2}>", Math.Abs(contactGeom.normal.X - contact.normal.X), Math.Abs(contactGeom.normal.Y - contact.normal.Y), Math.Abs(contactGeom.normal.Z - contact.normal.Z));
  823. }
  824. }
  825. //}
  826. }
  827. }
  828. return result;
  829. }
  830. private void collision_accounting_events(PhysicsActor p1, PhysicsActor p2, float collisiondepth)
  831. {
  832. // obj1LocalID = 0;
  833. //returncollisions = false;
  834. obj2LocalID = 0;
  835. //ctype = 0;
  836. //cStartStop = 0;
  837. if (!p2.SubscribedEvents() && !p1.SubscribedEvents())
  838. return;
  839. switch ((ActorTypes)p2.PhysicsActorType)
  840. {
  841. case ActorTypes.Agent:
  842. cc2 = (OdeCharacter)p2;
  843. // obj1LocalID = cc2.m_localID;
  844. switch ((ActorTypes)p1.PhysicsActorType)
  845. {
  846. case ActorTypes.Agent:
  847. cc1 = (OdeCharacter)p1;
  848. obj2LocalID = cc1.m_localID;
  849. cc1.AddCollisionEvent(cc2.m_localID, collisiondepth);
  850. //ctype = (int)CollisionCategories.Character;
  851. //if (cc1.CollidingObj)
  852. //cStartStop = (int)StatusIndicators.Generic;
  853. //else
  854. //cStartStop = (int)StatusIndicators.Start;
  855. //returncollisions = true;
  856. break;
  857. case ActorTypes.Prim:
  858. cp1 = (OdePrim)p1;
  859. obj2LocalID = cp1.m_localID;
  860. cp1.AddCollisionEvent(cc2.m_localID, collisiondepth);
  861. //ctype = (int)CollisionCategories.Geom;
  862. //if (cp1.CollidingObj)
  863. //cStartStop = (int)StatusIndicators.Generic;
  864. //else
  865. //cStartStop = (int)StatusIndicators.Start;
  866. //returncollisions = true;
  867. break;
  868. case ActorTypes.Ground:
  869. case ActorTypes.Unknown:
  870. obj2LocalID = 0;
  871. //ctype = (int)CollisionCategories.Land;
  872. //returncollisions = true;
  873. break;
  874. }
  875. cc2.AddCollisionEvent(obj2LocalID, collisiondepth);
  876. break;
  877. case ActorTypes.Prim:
  878. cp2 = (OdePrim)p2;
  879. // obj1LocalID = cp2.m_localID;
  880. switch ((ActorTypes)p1.PhysicsActorType)
  881. {
  882. case ActorTypes.Agent:
  883. cc1 = (OdeCharacter)p1;
  884. obj2LocalID = cc1.m_localID;
  885. cc1.AddCollisionEvent(cp2.m_localID, collisiondepth);
  886. //ctype = (int)CollisionCategories.Character;
  887. //if (cc1.CollidingObj)
  888. //cStartStop = (int)StatusIndicators.Generic;
  889. //else
  890. //cStartStop = (int)StatusIndicators.Start;
  891. //returncollisions = true;
  892. break;
  893. case ActorTypes.Prim:
  894. cp1 = (OdePrim)p1;
  895. obj2LocalID = cp1.m_localID;
  896. cp1.AddCollisionEvent(cp2.m_localID, collisiondepth);
  897. //ctype = (int)CollisionCategories.Geom;
  898. //if (cp1.CollidingObj)
  899. //cStartStop = (int)StatusIndicators.Generic;
  900. //else
  901. //cStartStop = (int)StatusIndicators.Start;
  902. //returncollisions = true;
  903. break;
  904. case ActorTypes.Ground:
  905. case ActorTypes.Unknown:
  906. obj2LocalID = 0;
  907. //ctype = (int)CollisionCategories.Land;
  908. //returncollisions = true;
  909. break;
  910. }
  911. cp2.AddCollisionEvent(obj2LocalID, collisiondepth);
  912. break;
  913. }
  914. //if (returncollisions)
  915. //{
  916. //lock (m_storedCollisions)
  917. //{
  918. //cDictKey = obj1LocalID.ToString() + obj2LocalID.ToString() + cStartStop.ToString() + ctype.ToString();
  919. //if (m_storedCollisions.ContainsKey(cDictKey))
  920. //{
  921. //sCollisionData objd = m_storedCollisions[cDictKey];
  922. //objd.NumberOfCollisions += 1;
  923. //objd.lastframe = framecount;
  924. //m_storedCollisions[cDictKey] = objd;
  925. //}
  926. //else
  927. //{
  928. //sCollisionData objd = new sCollisionData();
  929. //objd.ColliderLocalId = obj1LocalID;
  930. //objd.CollidedWithLocalId = obj2LocalID;
  931. //objd.CollisionType = ctype;
  932. //objd.NumberOfCollisions = 1;
  933. //objd.lastframe = framecount;
  934. //objd.StatusIndicator = cStartStop;
  935. //m_storedCollisions.Add(cDictKey, objd);
  936. //}
  937. //}
  938. // }
  939. }
  940. public int TriArrayCallback(IntPtr trimesh, IntPtr refObject, int[] triangleIndex, int triCount)
  941. {
  942. /* String name1 = null;
  943. String name2 = null;
  944. if (!geom_name_map.TryGetValue(trimesh, out name1))
  945. {
  946. name1 = "null";
  947. }
  948. if (!geom_name_map.TryGetValue(refObject, out name2))
  949. {
  950. name2 = "null";
  951. }
  952. m_log.InfoFormat("TriArrayCallback: A collision was detected between {1} and {2}", 0, name1, name2);
  953. */
  954. return 1;
  955. }
  956. public int TriCallback(IntPtr trimesh, IntPtr refObject, int triangleIndex)
  957. {
  958. String name1 = null;
  959. String name2 = null;
  960. if (!geom_name_map.TryGetValue(trimesh, out name1))
  961. {
  962. name1 = "null";
  963. }
  964. if (!geom_name_map.TryGetValue(refObject, out name2))
  965. {
  966. name2 = "null";
  967. }
  968. // m_log.InfoFormat("TriCallback: A collision was detected between {1} and {2}. Index was {3}", 0, name1, name2, triangleIndex);
  969. d.Vector3 v0 = new d.Vector3();
  970. d.Vector3 v1 = new d.Vector3();
  971. d.Vector3 v2 = new d.Vector3();
  972. d.GeomTriMeshGetTriangle(trimesh, 0, ref v0, ref v1, ref v2);
  973. // m_log.DebugFormat("Triangle {0} is <{1},{2},{3}>, <{4},{5},{6}>, <{7},{8},{9}>", triangleIndex, v0.X, v0.Y, v0.Z, v1.X, v1.Y, v1.Z, v2.X, v2.Y, v2.Z);
  974. return 1;
  975. }
  976. /// <summary>
  977. /// This is our collision testing routine in ODE
  978. /// </summary>
  979. /// <param name="timeStep"></param>
  980. private void collision_optimized(float timeStep)
  981. {
  982. _perloopContact.Clear();
  983. foreach (OdeCharacter chr in _characters)
  984. {
  985. // Reset the collision values to false
  986. // since we don't know if we're colliding yet
  987. // For some reason this can happen. Don't ask...
  988. //
  989. if (chr == null)
  990. continue;
  991. chr.IsColliding = false;
  992. chr.CollidingGround = false;
  993. chr.CollidingObj = false;
  994. // test the avatar's geometry for collision with the space
  995. // This will return near and the space that they are the closest to
  996. // And we'll run this again against the avatar and the space segment
  997. // This will return with a bunch of possible objects in the space segment
  998. // and we'll run it again on all of them.
  999. try
  1000. {
  1001. d.SpaceCollide2(space, chr.Shell, IntPtr.Zero, nearCallback);
  1002. }
  1003. catch (AccessViolationException)
  1004. {
  1005. m_log.Warn("[PHYSICS]: Unable to space collide");
  1006. }
  1007. //float terrainheight = GetTerrainHeightAtXY(chr.Position.X, chr.Position.Y);
  1008. //if (chr.Position.Z + (chr.Velocity.Z * timeStep) < terrainheight + 10)
  1009. //{
  1010. //chr.Position.Z = terrainheight + 10.0f;
  1011. //forcedZ = true;
  1012. //}
  1013. }
  1014. lock (_activeprims)
  1015. {
  1016. List<OdePrim> removeprims = null;
  1017. foreach (OdePrim chr in _activeprims)
  1018. {
  1019. if (chr.Body != IntPtr.Zero && d.BodyIsEnabled(chr.Body) && (!chr.m_disabled))
  1020. {
  1021. try
  1022. {
  1023. lock (chr)
  1024. {
  1025. if (space != IntPtr.Zero && chr.prim_geom != IntPtr.Zero && chr.m_taintremove == false)
  1026. {
  1027. d.SpaceCollide2(space, chr.prim_geom, IntPtr.Zero, nearCallback);
  1028. }
  1029. else
  1030. {
  1031. if (removeprims == null)
  1032. {
  1033. removeprims = new List<OdePrim>();
  1034. }
  1035. removeprims.Add(chr);
  1036. m_log.Debug("[PHYSICS]: unable to collide test active prim against space. The space was zero, the geom was zero or it was in the process of being removed. Removed it from the active prim list. This needs to be fixed!");
  1037. }
  1038. }
  1039. }
  1040. catch (AccessViolationException)
  1041. {
  1042. m_log.Warn("[PHYSICS]: Unable to space collide");
  1043. }
  1044. }
  1045. }
  1046. if (removeprims != null)
  1047. {
  1048. foreach (OdePrim chr in removeprims)
  1049. {
  1050. _activeprims.Remove(chr);
  1051. }
  1052. }
  1053. }
  1054. _perloopContact.Clear();
  1055. }
  1056. #endregion
  1057. // TODO: unused
  1058. // private float GetTerrainHeightAtXY(float x, float y)
  1059. // {
  1060. // return (float)_origheightmap[(int)y * Constants.RegionSize + (int)x];
  1061. // }
  1062. public void addCollisionEventReporting(PhysicsActor obj)
  1063. {
  1064. lock (_collisionEventPrim)
  1065. {
  1066. if (!_collisionEventPrim.Contains(obj))
  1067. _collisionEventPrim.Add(obj);
  1068. }
  1069. }
  1070. public void remCollisionEventReporting(PhysicsActor obj)
  1071. {
  1072. lock (_collisionEventPrim)
  1073. {
  1074. if (!_collisionEventPrim.Contains(obj))
  1075. _collisionEventPrim.Remove(obj);
  1076. }
  1077. }
  1078. #region Add/Remove Entities
  1079. public override PhysicsActor AddAvatar(string avName, PhysicsVector position, PhysicsVector size)
  1080. {
  1081. PhysicsVector pos = new PhysicsVector();
  1082. pos.X = position.X;
  1083. pos.Y = position.Y;
  1084. pos.Z = position.Z;
  1085. OdeCharacter newAv = new OdeCharacter(avName, this, pos, ode, size, avPIDD, avPIDP, avCapRadius, avStandupTensor, avDensity, avHeightFudgeFactor, avMovementDivisorWalk, avMovementDivisorRun);
  1086. _characters.Add(newAv);
  1087. return newAv;
  1088. }
  1089. public override void RemoveAvatar(PhysicsActor actor)
  1090. {
  1091. lock (OdeLock)
  1092. {
  1093. //m_log.Debug("[PHYSICS]:ODELOCK");
  1094. ((OdeCharacter) actor).Destroy();
  1095. _characters.Remove((OdeCharacter) actor);
  1096. }
  1097. }
  1098. private PhysicsActor AddPrim(String name, PhysicsVector position, PhysicsVector size, Quaternion rotation,
  1099. IMesh mesh, PrimitiveBaseShape pbs, bool isphysical)
  1100. {
  1101. PhysicsVector pos = new PhysicsVector(position.X, position.Y, position.Z);
  1102. //pos.X = position.X;
  1103. //pos.Y = position.Y;
  1104. //pos.Z = position.Z;
  1105. PhysicsVector siz = new PhysicsVector();
  1106. siz.X = size.X;
  1107. siz.Y = size.Y;
  1108. siz.Z = size.Z;
  1109. Quaternion rot = rotation;
  1110. OdePrim newPrim;
  1111. lock (OdeLock)
  1112. {
  1113. newPrim = new OdePrim(name, this, pos, siz, rot, mesh, pbs, isphysical, ode);
  1114. lock (_prims)
  1115. _prims.Add(newPrim);
  1116. }
  1117. return newPrim;
  1118. }
  1119. public void addActivePrim(OdePrim activatePrim)
  1120. {
  1121. // adds active prim.. (ones that should be iterated over in collisions_optimized
  1122. lock (_activeprims)
  1123. {
  1124. if (!_activeprims.Contains(activatePrim))
  1125. _activeprims.Add(activatePrim);
  1126. //else
  1127. // m_log.Warn("[PHYSICS]: Double Entry in _activeprims detected, potential crash immenent");
  1128. }
  1129. }
  1130. public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position,
  1131. PhysicsVector size, Quaternion rotation) //To be removed
  1132. {
  1133. return AddPrimShape(primName, pbs, position, size, rotation, false);
  1134. }
  1135. public override PhysicsActor AddPrimShape(string primName, PrimitiveBaseShape pbs, PhysicsVector position,
  1136. PhysicsVector size, Quaternion rotation, bool isPhysical)
  1137. {
  1138. PhysicsActor result;
  1139. IMesh mesh = null;
  1140. //switch (pbs.ProfileShape)
  1141. //{
  1142. // case ProfileShape.Square:
  1143. // //support simple box & hollow box now; later, more shapes
  1144. // if (needsMeshing(pbs))
  1145. // {
  1146. // mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical);
  1147. // }
  1148. // break;
  1149. //}
  1150. if (needsMeshing(pbs))
  1151. mesh = mesher.CreateMesh(primName, pbs, size, 32f, isPhysical);
  1152. result = AddPrim(primName, position, size, rotation, mesh, pbs, isPhysical);
  1153. return result;
  1154. }
  1155. public override bool SupportsNINJAJoints
  1156. {
  1157. get { return m_NINJA_physics_joints_enabled; }
  1158. }
  1159. // internal utility function: must be called within a lock (OdeLock)
  1160. private void InternalAddActiveJoint(PhysicsJoint joint)
  1161. {
  1162. activeJoints.Add(joint);
  1163. SOPName_to_activeJoint.Add(joint.ObjectNameInScene, joint);
  1164. }
  1165. // internal utility function: must be called within a lock (OdeLock)
  1166. private void InternalAddPendingJoint(OdePhysicsJoint joint)
  1167. {
  1168. pendingJoints.Add(joint);
  1169. SOPName_to_pendingJoint.Add(joint.ObjectNameInScene, joint);
  1170. }
  1171. // internal utility function: must be called within a lock (OdeLock)
  1172. private void InternalRemovePendingJoint(PhysicsJoint joint)
  1173. {
  1174. pendingJoints.Remove(joint);
  1175. SOPName_to_pendingJoint.Remove(joint.ObjectNameInScene);
  1176. }
  1177. // internal utility function: must be called within a lock (OdeLock)
  1178. private void InternalRemoveActiveJoint(PhysicsJoint joint)
  1179. {
  1180. activeJoints.Remove(joint);
  1181. SOPName_to_activeJoint.Remove(joint.ObjectNameInScene);
  1182. }
  1183. public override void DumpJointInfo()
  1184. {
  1185. string hdr = "[NINJA] JOINTINFO: ";
  1186. foreach (PhysicsJoint j in pendingJoints)
  1187. {
  1188. m_log.Debug(hdr + " pending joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
  1189. }
  1190. m_log.Debug(hdr + pendingJoints.Count + " total pending joints");
  1191. foreach (string jointName in SOPName_to_pendingJoint.Keys)
  1192. {
  1193. m_log.Debug(hdr + " pending joints dict contains Name: " + jointName);
  1194. }
  1195. m_log.Debug(hdr + SOPName_to_pendingJoint.Keys.Count + " total pending joints dict entries");
  1196. foreach (PhysicsJoint j in activeJoints)
  1197. {
  1198. m_log.Debug(hdr + " active joint, Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
  1199. }
  1200. m_log.Debug(hdr + activeJoints.Count + " total active joints");
  1201. foreach (string jointName in SOPName_to_activeJoint.Keys)
  1202. {
  1203. m_log.Debug(hdr + " active joints dict contains Name: " + jointName);
  1204. }
  1205. m_log.Debug(hdr + SOPName_to_activeJoint.Keys.Count + " total active joints dict entries");
  1206. m_log.Debug(hdr + " Per-body joint connectivity information follows.");
  1207. m_log.Debug(hdr + joints_connecting_actor.Keys.Count + " bodies are connected by joints.");
  1208. foreach (string actorName in joints_connecting_actor.Keys)
  1209. {
  1210. m_log.Debug(hdr + " Actor " + actorName + " has the following joints connecting it");
  1211. foreach (PhysicsJoint j in joints_connecting_actor[actorName])
  1212. {
  1213. m_log.Debug(hdr + " * joint Name: " + j.ObjectNameInScene + " raw parms:" + j.RawParams);
  1214. }
  1215. m_log.Debug(hdr + joints_connecting_actor[actorName].Count + " connecting joints total for this actor");
  1216. }
  1217. }
  1218. public override void RequestJointDeletion(string ObjectNameInScene)
  1219. {
  1220. lock (externalJointRequestsLock)
  1221. {
  1222. if (!requestedJointsToBeDeleted.Contains(ObjectNameInScene)) // forbid same deletion request from entering twice to prevent spurious deletions processed asynchronously
  1223. {
  1224. requestedJointsToBeDeleted.Add(ObjectNameInScene);
  1225. }
  1226. }
  1227. }
  1228. private void DeleteRequestedJoints()
  1229. {
  1230. List<string> myRequestedJointsToBeDeleted;
  1231. lock (externalJointRequestsLock)
  1232. {
  1233. // make a local copy of the shared list for processing (threading issues)
  1234. myRequestedJointsToBeDeleted = new List<string>(requestedJointsToBeDeleted);
  1235. }
  1236. foreach (string jointName in myRequestedJointsToBeDeleted)
  1237. {
  1238. lock (OdeLock)
  1239. {
  1240. //m_log.Debug("[NINJA] trying to deleting requested joint " + jointName);
  1241. if (SOPName_to_activeJoint.ContainsKey(jointName) || SOPName_to_pendingJoint.ContainsKey(jointName))
  1242. {
  1243. OdePhysicsJoint joint = null;
  1244. if (SOPName_to_activeJoint.ContainsKey(jointName))
  1245. {
  1246. joint = SOPName_to_activeJoint[jointName] as OdePhysicsJoint;
  1247. InternalRemoveActiveJoint(joint);
  1248. }
  1249. else if (SOPName_to_pendingJoint.ContainsKey(jointName))
  1250. {
  1251. joint = SOPName_to_pendingJoint[jointName] as OdePhysicsJoint;
  1252. InternalRemovePendingJoint(joint);
  1253. }
  1254. if (joint != null)
  1255. {
  1256. //m_log.Debug("joint.BodyNames.Count is " + joint.BodyNames.Count + " and contents " + joint.BodyNames);
  1257. for (int iBodyName = 0; iBodyName < 2; iBodyName++)
  1258. {
  1259. string bodyName = joint.BodyNames[iBodyName];
  1260. if (bodyName != "NULL")
  1261. {
  1262. joints_connecting_actor[bodyName].Remove(joint);
  1263. if (joints_connecting_actor[bodyName].Count == 0)
  1264. {
  1265. joints_connecting_actor.Remove(bodyName);
  1266. }
  1267. }
  1268. }
  1269. DoJointDeactivated(joint);
  1270. if (joint.jointID != IntPtr.Zero)
  1271. {
  1272. d.JointDestroy(joint.jointID);
  1273. joint.jointID = IntPtr.Zero;
  1274. //DoJointErrorMessage(joint, "successfully destroyed joint " + jointName);
  1275. }
  1276. else
  1277. {
  1278. //m_log.Warn("[NINJA] Ignoring re-request to destroy joint " + jointName);
  1279. }
  1280. }
  1281. else
  1282. {
  1283. // DoJointErrorMessage(joint, "coult not find joint to destroy based on name " + jointName);
  1284. }
  1285. }
  1286. else
  1287. {
  1288. // DoJointErrorMessage(joint, "WARNING - joint removal failed, joint " + jointName);
  1289. }
  1290. }
  1291. }
  1292. // remove processed joints from the shared list
  1293. lock (externalJointRequestsLock)
  1294. {
  1295. foreach (string jointName in myRequestedJointsToBeDeleted)
  1296. {
  1297. requestedJointsToBeDeleted.Remove(jointName);
  1298. }
  1299. }
  1300. }
  1301. // for pending joints we don't know if their associated bodies exist yet or not.
  1302. // the joint is actually created during processing of the taints
  1303. private void CreateRequestedJoints()
  1304. {
  1305. List<PhysicsJoint> myRequestedJointsToBeCreated;
  1306. lock (externalJointRequestsLock)
  1307. {
  1308. // make a local copy of the shared list for processing (threading issues)
  1309. myRequestedJointsToBeCreated = new List<PhysicsJoint>(requestedJointsToBeCreated);
  1310. }
  1311. foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
  1312. {
  1313. lock (OdeLock)
  1314. {
  1315. if (SOPName_to_pendingJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_pendingJoint[joint.ObjectNameInScene] != null)
  1316. {
  1317. DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already pending joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
  1318. continue;
  1319. }
  1320. if (SOPName_to_activeJoint.ContainsKey(joint.ObjectNameInScene) && SOPName_to_activeJoint[joint.ObjectNameInScene] != null)
  1321. {
  1322. DoJointErrorMessage(joint, "WARNING: ignoring request to re-add already active joint Name:" + joint.ObjectNameInScene + " type:" + joint.Type + " parms: " + joint.RawParams + " pos: " + joint.Position + " rot:" + joint.Rotation);
  1323. continue;
  1324. }
  1325. InternalAddPendingJoint(joint as OdePhysicsJoint);
  1326. if (joint.BodyNames.Count >= 2)
  1327. {
  1328. for (int iBodyName = 0; iBodyName < 2; iBodyName++)
  1329. {
  1330. string bodyName = joint.BodyNames[iBodyName];
  1331. if (bodyName != "NULL")
  1332. {
  1333. if (!joints_connecting_actor.ContainsKey(bodyName))
  1334. {
  1335. joints_connecting_actor.Add(bodyName, new List<PhysicsJoint>());
  1336. }
  1337. joints_connecting_actor[bodyName].Add(joint);
  1338. }
  1339. }
  1340. }
  1341. }
  1342. }
  1343. // remove processed joints from shared list
  1344. lock (externalJointRequestsLock)
  1345. {
  1346. foreach (PhysicsJoint joint in myRequestedJointsToBeCreated)
  1347. {
  1348. requestedJointsToBeCreated.Remove(joint);
  1349. }
  1350. }
  1351. }
  1352. // public function to add an request for joint creation
  1353. // this joint will just be added to a waiting list that is NOT processed during the main
  1354. // Simulate() loop (to avoid deadlocks). After Simulate() is finished, we handle unprocessed joint requests.
  1355. public override PhysicsJoint RequestJointCreation(string objectNameInScene, PhysicsJointType jointType, PhysicsVector position,
  1356. Quaternion rotation, string parms, List<string> bodyNames, string trackedBodyName, Quaternion localRotation)
  1357. {
  1358. OdePhysicsJoint joint = new OdePhysicsJoint();
  1359. joint.ObjectNameInScene = objectNameInScene;
  1360. joint.Type = jointType;
  1361. joint.Position = new PhysicsVector(position.X, position.Y, position.Z);
  1362. joint.Rotation = rotation;
  1363. joint.RawParams = parms;
  1364. joint.BodyNames = new List<string>(bodyNames);
  1365. joint.TrackedBodyName = trackedBodyName;
  1366. joint.LocalRotation = localRotation;
  1367. joint.jointID = IntPtr.Zero;
  1368. joint.ErrorMessageCount = 0;
  1369. lock (externalJointRequestsLock)
  1370. {
  1371. if (!requestedJointsToBeCreated.Contains(joint)) // forbid same creation request from entering twice
  1372. {
  1373. requestedJointsToBeCreated.Add(joint);
  1374. }
  1375. }
  1376. return joint;
  1377. }
  1378. private void RemoveAllJointsConnectedToActor(PhysicsActor actor)
  1379. {
  1380. //m_log.Debug("RemoveAllJointsConnectedToActor: start");
  1381. if (actor.SOPName != null && joints_connecting_actor.ContainsKey(actor.SOPName) && joints_connecting_actor[actor.SOPName] != null)
  1382. {
  1383. List<PhysicsJoint> jointsToRemove = new List<PhysicsJoint>();
  1384. //TODO: merge these 2 loops (originally it was needed to avoid altering a list being iterated over, but it is no longer needed due to the joint request queue mechanism)
  1385. foreach (PhysicsJoint j in joints_connecting_actor[actor.SOPName])
  1386. {
  1387. jointsToRemove.Add(j);
  1388. }
  1389. foreach (PhysicsJoint j in jointsToRemove)
  1390. {
  1391. //m_log.Debug("RemoveAllJointsConnectedToActor: about to request deletion of " + j.ObjectNameInScene);
  1392. RequestJointDeletion(j.ObjectNameInScene);
  1393. //m_log.Debug("RemoveAllJointsConnectedToActor: done request deletion of " + j.ObjectNameInScene);
  1394. j.TrackedBodyName = null; // *IMMEDIATELY* prevent any further movement of this joint (else a deleted actor might cause spurious tracking motion of the joint for a few frames, leading to the joint proxy object disappearing)
  1395. }
  1396. }
  1397. }
  1398. public override void RemoveAllJointsConnectedToActorThreadLocked(PhysicsActor actor)
  1399. {
  1400. //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: start");
  1401. lock (OdeLock)
  1402. {
  1403. //m_log.Debug("RemoveAllJointsConnectedToActorThreadLocked: got lock");
  1404. RemoveAllJointsConnectedToActor(actor);
  1405. }
  1406. }
  1407. // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
  1408. public override PhysicsVector GetJointAnchor(PhysicsJoint joint)
  1409. {
  1410. Debug.Assert(joint.IsInPhysicsEngine);
  1411. d.Vector3 pos = new d.Vector3();
  1412. if (!(joint is OdePhysicsJoint))
  1413. {
  1414. DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
  1415. }
  1416. else
  1417. {
  1418. OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
  1419. switch (odeJoint.Type)
  1420. {
  1421. case PhysicsJointType.Ball:
  1422. d.JointGetBallAnchor(odeJoint.jointID, out pos);
  1423. break;
  1424. case PhysicsJointType.Hinge:
  1425. d.JointGetHingeAnchor(odeJoint.jointID, out pos);
  1426. break;
  1427. }
  1428. }
  1429. return new PhysicsVector(pos.X, pos.Y, pos.Z);
  1430. }
  1431. // normally called from within OnJointMoved, which is called from within a lock (OdeLock)
  1432. // WARNING: ODE sometimes returns <0,0,0> as the joint axis! Therefore this function
  1433. // appears to be unreliable. Fortunately we can compute the joint axis ourselves by
  1434. // keeping track of the joint's original orientation relative to one of the involved bodies.
  1435. public override PhysicsVector GetJointAxis(PhysicsJoint joint)
  1436. {
  1437. Debug.Assert(joint.IsInPhysicsEngine);
  1438. d.Vector3 axis = new d.Vector3();
  1439. if (!(joint is OdePhysicsJoint))
  1440. {
  1441. DoJointErrorMessage(joint, "warning: non-ODE joint requesting anchor: " + joint.ObjectNameInScene);
  1442. }
  1443. else
  1444. {
  1445. OdePhysicsJoint odeJoint = (OdePhysicsJoint)joint;
  1446. switch (odeJoint.Type)
  1447. {
  1448. case PhysicsJointType.Ball:
  1449. DoJointErrorMessage(joint, "warning - axis requested for ball joint: " + joint.ObjectNameInScene);
  1450. break;
  1451. case PhysicsJointType.Hinge:
  1452. d.JointGetHingeAxis(odeJoint.jointID, out axis);
  1453. break;
  1454. }
  1455. }
  1456. return new PhysicsVector(axis.X, axis.Y, axis.Z);
  1457. }
  1458. public void remActivePrim(OdePrim deactivatePrim)
  1459. {
  1460. lock (_activeprims)
  1461. {
  1462. _activeprims.Remove(deactivatePrim);
  1463. }
  1464. }
  1465. public override void RemovePrim(PhysicsActor prim)
  1466. {
  1467. if (prim is OdePrim)
  1468. {
  1469. lock (OdeLock)
  1470. {
  1471. OdePrim p = (OdePrim) prim;
  1472. p.setPrimForRemoval();
  1473. AddPhysicsActorTaint(prim);
  1474. //RemovePrimThreadLocked(p);
  1475. }
  1476. }
  1477. }
  1478. /// <summary>
  1479. /// This is called from within simulate but outside the locked portion
  1480. /// We need to do our own locking here
  1481. /// Essentially, we need to remove the prim from our space segment, whatever segment it's in.
  1482. ///
  1483. /// If there are no more prim in the segment, we need to empty (spacedestroy)the segment and reclaim memory
  1484. /// that the space was using.
  1485. /// </summary>
  1486. /// <param name="prim"></param>
  1487. public void RemovePrimThreadLocked(OdePrim prim)
  1488. {
  1489. lock (prim)
  1490. {
  1491. remCollisionEventReporting(prim);
  1492. lock (ode)
  1493. {
  1494. if (prim.prim_geom != IntPtr.Zero)
  1495. {
  1496. prim.ResetTaints();
  1497. if (prim.IsPhysical)
  1498. {
  1499. prim.disableBody();
  1500. if (prim.childPrim)
  1501. {
  1502. prim.childPrim = false;
  1503. prim.Body = IntPtr.Zero;
  1504. prim.m_disabled = true;
  1505. prim.IsPhysical = false;
  1506. }
  1507. }
  1508. // we don't want to remove the main space
  1509. // If the geometry is in the targetspace, remove it from the target space
  1510. //m_log.Warn(prim.m_targetSpace);
  1511. //if (prim.m_targetSpace != IntPtr.Zero)
  1512. //{
  1513. //if (d.SpaceQuery(prim.m_targetSpace, prim.prim_geom))
  1514. //{
  1515. //if (d.GeomIsSpace(prim.m_targetSpace))
  1516. //{
  1517. //waitForSpaceUnlock(prim.m_targetSpace);
  1518. //d.SpaceRemove(prim.m_targetSpace, prim.prim_geom);
  1519. prim.m_targetSpace = IntPtr.Zero;
  1520. //}
  1521. //else
  1522. //{
  1523. // m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
  1524. //((OdePrim)prim).m_targetSpace.ToString());
  1525. //}
  1526. //}
  1527. //}
  1528. //m_log.Warn(prim.prim_geom);
  1529. try
  1530. {
  1531. if (prim.prim_geom != IntPtr.Zero)
  1532. {
  1533. d.GeomDestroy(prim.prim_geom);
  1534. prim.prim_geom = IntPtr.Zero;
  1535. }
  1536. else
  1537. {
  1538. m_log.Warn("[PHYSICS]: Unable to remove prim from physics scene");
  1539. }
  1540. }
  1541. catch (AccessViolationException)
  1542. {
  1543. m_log.Info("[PHYSICS]: Couldn't remove prim from physics scene, it was already be removed.");
  1544. }
  1545. lock (_prims)
  1546. _prims.Remove(prim);
  1547. //If there are no more geometries in the sub-space, we don't need it in the main space anymore
  1548. //if (d.SpaceGetNumGeoms(prim.m_targetSpace) == 0)
  1549. //{
  1550. //if (prim.m_targetSpace != null)
  1551. //{
  1552. //if (d.GeomIsSpace(prim.m_targetSpace))
  1553. //{
  1554. //waitForSpaceUnlock(prim.m_targetSpace);
  1555. //d.SpaceRemove(space, prim.m_targetSpace);
  1556. // free up memory used by the space.
  1557. //d.SpaceDestroy(prim.m_targetSpace);
  1558. //int[] xyspace = calculateSpaceArrayItemFromPos(prim.Position);
  1559. //resetSpaceArrayItemToZero(xyspace[0], xyspace[1]);
  1560. //}
  1561. //else
  1562. //{
  1563. //m_log.Info("[Physics]: Invalid Scene passed to 'removeprim from scene':" +
  1564. //((OdePrim) prim).m_targetSpace.ToString());
  1565. //}
  1566. //}
  1567. //}
  1568. if (SupportsNINJAJoints)
  1569. {
  1570. RemoveAllJointsConnectedToActorThreadLocked(prim);
  1571. }
  1572. }
  1573. }
  1574. }
  1575. }
  1576. #endregion
  1577. #region Space Separation Calculation
  1578. /// <summary>
  1579. /// Takes a space pointer and zeros out the array we're using to hold the spaces
  1580. /// </summary>
  1581. /// <param name="space"></param>
  1582. public void resetSpaceArrayItemToZero(IntPtr space)
  1583. {
  1584. for (int x = 0; x < staticPrimspace.GetLength(0); x++)
  1585. {
  1586. for (int y = 0; y < staticPrimspace.GetLength(1); y++)
  1587. {
  1588. if (staticPrimspace[x, y] == space)
  1589. staticPrimspace[x, y] = IntPtr.Zero;
  1590. }
  1591. }
  1592. }
  1593. public void resetSpaceArrayItemToZero(int arrayitemX, int arrayitemY)
  1594. {
  1595. staticPrimspace[arrayitemX, arrayitemY] = IntPtr.Zero;
  1596. }
  1597. /// <summary>
  1598. /// Called when a static prim moves. Allocates a space for the prim based on it's position
  1599. /// </summary>
  1600. /// <param name="geom">the pointer to the geom that moved</param>
  1601. /// <param name="pos">the position that the geom moved to</param>
  1602. /// <param name="currentspace">a pointer to the space it was in before it was moved.</param>
  1603. /// <returns>a pointer to the new space it's in</returns>
  1604. public IntPtr recalculateSpaceForGeom(IntPtr geom, PhysicsVector pos, IntPtr currentspace)
  1605. {
  1606. // Called from setting the Position and Size of an ODEPrim so
  1607. // it's already in locked space.
  1608. // we don't want to remove the main space
  1609. // we don't need to test physical here because this function should
  1610. // never be called if the prim is physical(active)
  1611. // All physical prim end up in the root space
  1612. //Thread.Sleep(20);
  1613. if (currentspace != space)
  1614. {
  1615. //m_log.Info("[SPACE]: C:" + currentspace.ToString() + " g:" + geom.ToString());
  1616. //if (currentspace == IntPtr.Zero)
  1617. //{
  1618. //int adfadf = 0;
  1619. //}
  1620. if (d.SpaceQuery(currentspace, geom) && currentspace != IntPtr.Zero)
  1621. {
  1622. if (d.GeomIsSpace(currentspace))
  1623. {
  1624. waitForSpaceUnlock(currentspace);
  1625. d.SpaceRemove(currentspace, geom);
  1626. }
  1627. else
  1628. {
  1629. m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" + currentspace.ToString() +
  1630. " Geom:" + geom.ToString());
  1631. }
  1632. }
  1633. else
  1634. {
  1635. IntPtr sGeomIsIn = d.GeomGetSpace(geom);
  1636. if (sGeomIsIn != IntPtr.Zero)
  1637. {
  1638. if (d.GeomIsSpace(currentspace))
  1639. {
  1640. waitForSpaceUnlock(sGeomIsIn);
  1641. d.SpaceRemove(sGeomIsIn, geom);
  1642. }
  1643. else
  1644. {
  1645. m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
  1646. sGeomIsIn.ToString() + " Geom:" + geom.ToString());
  1647. }
  1648. }
  1649. }
  1650. //If there are no more geometries in the sub-space, we don't need it in the main space anymore
  1651. if (d.SpaceGetNumGeoms(currentspace) == 0)
  1652. {
  1653. if (currentspace != IntPtr.Zero)
  1654. {
  1655. if (d.GeomIsSpace(currentspace))
  1656. {
  1657. waitForSpaceUnlock(currentspace);
  1658. waitForSpaceUnlock(space);
  1659. d.SpaceRemove(space, currentspace);
  1660. // free up memory used by the space.
  1661. //d.SpaceDestroy(currentspace);
  1662. resetSpaceArrayItemToZero(currentspace);
  1663. }
  1664. else
  1665. {
  1666. m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
  1667. currentspace.ToString() + " Geom:" + geom.ToString());
  1668. }
  1669. }
  1670. }
  1671. }
  1672. else
  1673. {
  1674. // this is a physical object that got disabled. ;.;
  1675. if (currentspace != IntPtr.Zero && geom != IntPtr.Zero)
  1676. {
  1677. if (d.SpaceQuery(currentspace, geom))
  1678. {
  1679. if (d.GeomIsSpace(currentspace))
  1680. {
  1681. waitForSpaceUnlock(currentspace);
  1682. d.SpaceRemove(currentspace, geom);
  1683. }
  1684. else
  1685. {
  1686. m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
  1687. currentspace.ToString() + " Geom:" + geom.ToString());
  1688. }
  1689. }
  1690. else
  1691. {
  1692. IntPtr sGeomIsIn = d.GeomGetSpace(geom);
  1693. if (sGeomIsIn != IntPtr.Zero)
  1694. {
  1695. if (d.GeomIsSpace(sGeomIsIn))
  1696. {
  1697. waitForSpaceUnlock(sGeomIsIn);
  1698. d.SpaceRemove(sGeomIsIn, geom);
  1699. }
  1700. else
  1701. {
  1702. m_log.Info("[Physics]: Invalid Scene passed to 'recalculatespace':" +
  1703. sGeomIsIn.ToString() + " Geom:" + geom.ToString());
  1704. }
  1705. }
  1706. }
  1707. }
  1708. }
  1709. // The routines in the Position and Size sections do the 'inserting' into the space,
  1710. // so all we have to do is make sure that the space that we're putting the prim into
  1711. // is in the 'main' space.
  1712. int[] iprimspaceArrItem = calculateSpaceArrayItemFromPos(pos);
  1713. IntPtr newspace = calculateSpaceForGeom(pos);
  1714. if (newspace == IntPtr.Zero)
  1715. {
  1716. newspace = createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
  1717. d.HashSpaceSetLevels(newspace, smallHashspaceLow, smallHashspaceHigh);
  1718. }
  1719. return newspace;
  1720. }
  1721. /// <summary>
  1722. /// Creates a new space at X Y
  1723. /// </summary>
  1724. /// <param name="iprimspaceArrItemX"></param>
  1725. /// <param name="iprimspaceArrItemY"></param>
  1726. /// <returns>A pointer to the created space</returns>
  1727. public IntPtr createprimspace(int iprimspaceArrItemX, int iprimspaceArrItemY)
  1728. {
  1729. // creating a new space for prim and inserting it into main space.
  1730. staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY] = d.HashSpaceCreate(IntPtr.Zero);
  1731. d.GeomSetCategoryBits(staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY], (int)CollisionCategories.Space);
  1732. waitForSpaceUnlock(space);
  1733. d.SpaceSetSublevel(space, 1);
  1734. d.SpaceAdd(space, staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY]);
  1735. return staticPrimspace[iprimspaceArrItemX, iprimspaceArrItemY];
  1736. }
  1737. /// <summary>
  1738. /// Calculates the space the prim should be in by its position
  1739. /// </summary>
  1740. /// <param name="pos"></param>
  1741. /// <returns>a pointer to the space. This could be a new space or reused space.</returns>
  1742. public IntPtr calculateSpaceForGeom(PhysicsVector pos)
  1743. {
  1744. int[] xyspace = calculateSpaceArrayItemFromPos(pos);
  1745. //m_log.Info("[Physics]: Attempting to use arrayItem: " + xyspace[0].ToString() + "," + xyspace[1].ToString());
  1746. return staticPrimspace[xyspace[0], xyspace[1]];
  1747. }
  1748. /// <summary>
  1749. /// Holds the space allocation logic
  1750. /// </summary>
  1751. /// <param name="pos"></param>
  1752. /// <returns>an array item based on the position</returns>
  1753. public int[] calculateSpaceArrayItemFromPos(PhysicsVector pos)
  1754. {
  1755. int[] returnint = new int[2];
  1756. returnint[0] = (int) (pos.X/metersInSpace);
  1757. if (returnint[0] > ((int) (259f/metersInSpace)))
  1758. returnint[0] = ((int) (259f/metersInSpace));
  1759. if (returnint[0] < 0)
  1760. returnint[0] = 0;
  1761. returnint[1] = (int) (pos.Y/metersInSpace);
  1762. if (returnint[1] > ((int) (259f/metersInSpace)))
  1763. returnint[1] = ((int) (259f/metersInSpace));
  1764. if (returnint[1] < 0)
  1765. returnint[1] = 0;
  1766. return returnint;
  1767. }
  1768. #endregion
  1769. /// <summary>
  1770. /// Routine to figure out if we need to mesh this prim with our mesher
  1771. /// </summary>
  1772. /// <param name="pbs"></param>
  1773. /// <returns></returns>
  1774. public bool needsMeshing(PrimitiveBaseShape pbs)
  1775. {
  1776. // most of this is redundant now as the mesher will return null if it cant mesh a prim
  1777. // but we still need to check for sculptie meshing being enabled so this is the most
  1778. // convenient place to do it for now...
  1779. // //if (pbs.PathCurve == (byte)Primitive.PathCurve.Circle && pbs.ProfileCurve == (byte)Primitive.ProfileCurve.Circle && pbs.PathScaleY <= 0.75f)
  1780. // //Console.WriteLine("needsMeshing: " + " pathCurve: " + pbs.PathCurve.ToString() + " profileCurve: " + pbs.ProfileCurve.ToString() + " pathScaleY: " + Primitive.UnpackPathScale(pbs.PathScaleY).ToString());
  1781. int iPropertiesNotSupportedDefault = 0;
  1782. if (pbs.SculptEntry && !meshSculptedPrim)
  1783. {
  1784. #if SPAM
  1785. m_log.Warn("NonMesh");
  1786. #endif
  1787. return false;
  1788. }
  1789. // if it's a standard box or sphere with no cuts, hollows, twist or top shear, return false since ODE can use an internal representation for the prim
  1790. if ((pbs.ProfileShape == ProfileShape.Square && pbs.PathCurve == (byte)Extrusion.Straight)
  1791. || (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1
  1792. && pbs.Scale.X == pbs.Scale.Y && pbs.Scale.Y == pbs.Scale.Z))
  1793. {
  1794. if (pbs.ProfileBegin == 0 && pbs.ProfileEnd == 0
  1795. && pbs.ProfileHollow == 0
  1796. && pbs.PathTwist == 0 && pbs.PathTwistBegin == 0
  1797. && pbs.PathBegin == 0 && pbs.PathEnd == 0
  1798. && pbs.PathTaperX == 0 && pbs.PathTaperY == 0
  1799. && pbs.PathScaleX == 100 && pbs.PathScaleY == 100
  1800. && pbs.PathShearX == 0 && pbs.PathShearY == 0)
  1801. {
  1802. #if SPAM
  1803. m_log.Warn("NonMesh");
  1804. #endif
  1805. return false;
  1806. }
  1807. }
  1808. if (pbs.ProfileHollow != 0)
  1809. iPropertiesNotSupportedDefault++;
  1810. if (((Int16)pbs.PathTwistBegin != 0) || ((Int16)pbs.PathTwist != 0))
  1811. iPropertiesNotSupportedDefault++;
  1812. if ((pbs.ProfileBegin != 0) || pbs.ProfileEnd != 0)
  1813. iPropertiesNotSupportedDefault++;
  1814. if ((pbs.PathScaleX != 100) || (pbs.PathScaleY != 100))
  1815. iPropertiesNotSupportedDefault++;
  1816. if ((pbs.PathShearX != 0) || (pbs.PathShearY != 0))
  1817. iPropertiesNotSupportedDefault++;
  1818. if (pbs.ProfileShape == ProfileShape.Circle && pbs.PathCurve == (byte)Extrusion.Straight)
  1819. iPropertiesNotSupportedDefault++;
  1820. if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte)Extrusion.Curve1 && (pbs.Scale.X != pbs.Scale.Y || pbs.Scale.Y != pbs.Scale.Z || pbs.Scale.Z != pbs.Scale.X))
  1821. iPropertiesNotSupportedDefault++;
  1822. if (pbs.ProfileShape == ProfileShape.HalfCircle && pbs.PathCurve == (byte) Extrusion.Curve1)
  1823. iPropertiesNotSupportedDefault++;
  1824. // test for torus
  1825. if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Square)
  1826. {
  1827. if (pbs.PathCurve == (byte)Extrusion.Curve1)
  1828. {
  1829. iPropertiesNotSupportedDefault++;
  1830. }
  1831. }
  1832. else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.Circle)
  1833. {
  1834. if (pbs.PathCurve == (byte)Extrusion.Straight)
  1835. {
  1836. iPropertiesNotSupportedDefault++;
  1837. }
  1838. // ProfileCurve seems to combine hole shape and profile curve so we need to only compare against the lower 3 bits
  1839. else if (pbs.PathCurve == (byte)Extrusion.Curve1)
  1840. {
  1841. iPropertiesNotSupportedDefault++;
  1842. }
  1843. }
  1844. else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.HalfCircle)
  1845. {
  1846. if (pbs.PathCurve == (byte)Extrusion.Curve1 || pbs.PathCurve == (byte)Extrusion.Curve2)
  1847. {
  1848. iPropertiesNotSupportedDefault++;
  1849. }
  1850. }
  1851. else if ((pbs.ProfileCurve & 0x07) == (byte)ProfileShape.EquilateralTriangle)
  1852. {
  1853. if (pbs.PathCurve == (byte)Extrusion.Straight)
  1854. {
  1855. iPropertiesNotSupportedDefault++;
  1856. }
  1857. else if (pbs.PathCurve == (byte)Extrusion.Curve1)
  1858. {
  1859. iPropertiesNotSupportedDefault++;
  1860. }
  1861. }
  1862. if (iPropertiesNotSupportedDefault == 0)
  1863. {
  1864. #if SPAM
  1865. m_log.Warn("NonMesh");
  1866. #endif
  1867. return false;
  1868. }
  1869. #if SPAM
  1870. m_log.Debug("Mesh");
  1871. #endif
  1872. return true;
  1873. }
  1874. /// <summary>
  1875. /// Called after our prim properties are set Scale, position etc.
  1876. /// We use this event queue like method to keep changes to the physical scene occuring in the threadlocked mutex
  1877. /// This assures us that we have no race conditions
  1878. /// </summary>
  1879. /// <param name="prim"></param>
  1880. public override void AddPhysicsActorTaint(PhysicsActor prim)
  1881. {
  1882. if (prim is OdePrim)
  1883. {
  1884. OdePrim taintedprim = ((OdePrim) prim);
  1885. lock (_taintedPrim)
  1886. {
  1887. if (!(_taintedPrim.Contains(taintedprim)))
  1888. _taintedPrim.Add(taintedprim);
  1889. }
  1890. return;
  1891. }
  1892. else if (prim is OdeCharacter)
  1893. {
  1894. OdeCharacter taintedchar = ((OdeCharacter)prim);
  1895. lock (_taintedActors)
  1896. {
  1897. if (!(_taintedActors.Contains(taintedchar)))
  1898. _taintedActors.Add(taintedchar);
  1899. }
  1900. }
  1901. }
  1902. /// <summary>
  1903. /// This is our main simulate loop
  1904. /// It's thread locked by a Mutex in the scene.
  1905. /// It holds Collisions, it instructs ODE to step through the physical reactions
  1906. /// It moves the objects around in memory
  1907. /// It calls the methods that report back to the object owners.. (scenepresence, SceneObjectGroup)
  1908. /// </summary>
  1909. /// <param name="timeStep"></param>
  1910. /// <returns></returns>
  1911. public override float Simulate(float timeStep)
  1912. {
  1913. if (framecount >= int.MaxValue)
  1914. framecount = 0;
  1915. framecount++;
  1916. float fps = 0;
  1917. //m_log.Info(timeStep.ToString());
  1918. step_time += timeStep;
  1919. // If We're loaded down by something else,
  1920. // or debugging with the Visual Studio project on pause
  1921. // skip a few frames to catch up gracefully.
  1922. // without shooting the physicsactors all over the place
  1923. if (step_time >= m_SkipFramesAtms)
  1924. {
  1925. // Instead of trying to catch up, it'll do 5 physics frames only
  1926. step_time = ODE_STEPSIZE;
  1927. m_physicsiterations = 5;
  1928. }
  1929. else
  1930. {
  1931. m_physicsiterations = 10;
  1932. }
  1933. if (SupportsNINJAJoints)
  1934. {
  1935. DeleteRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
  1936. CreateRequestedJoints(); // this must be outside of the lock (OdeLock) to avoid deadlocks
  1937. }
  1938. lock (OdeLock)
  1939. {
  1940. // Process 10 frames if the sim is running normal..
  1941. // process 5 frames if the sim is running slow
  1942. //try
  1943. //{
  1944. //d.WorldSetQuickStepNumIterations(world, m_physicsiterations);
  1945. //}
  1946. //catch (StackOverflowException)
  1947. //{
  1948. // m_log.Error("[PHYSICS]: The operating system wasn't able to allocate enough memory for the simulation. Restarting the sim.");
  1949. // ode.drelease(world);
  1950. //base.TriggerPhysicsBasedRestart();
  1951. //}
  1952. int i = 0;
  1953. // Figure out the Frames Per Second we're going at.
  1954. //(step_time == 0.004f, there's 250 of those per second. Times the step time/step size
  1955. step_time = 0.09375f;
  1956. fps = (step_time/ODE_STEPSIZE) * 1000;
  1957. while (step_time > 0.0f)
  1958. {
  1959. //lock (ode)
  1960. //{
  1961. //if (!ode.lockquery())
  1962. //{
  1963. // ode.dlock(world);
  1964. try
  1965. {
  1966. // Insert, remove Characters
  1967. bool processedtaints = false;
  1968. lock (_taintedActors)
  1969. {
  1970. if (_taintedActors.Count > 0)
  1971. {
  1972. foreach (OdeCharacter character in _taintedActors)
  1973. {
  1974. character.ProcessTaints(timeStep);
  1975. processedtaints = true;
  1976. //character.m_collisionscore = 0;
  1977. }
  1978. if (processedtaints)
  1979. _taintedActors.Clear();
  1980. }
  1981. }
  1982. // Modify other objects in the scene.
  1983. processedtaints = false;
  1984. lock (_taintedPrim)
  1985. {
  1986. foreach (OdePrim prim in _taintedPrim)
  1987. {
  1988. if (prim.m_taintremove)
  1989. {
  1990. RemovePrimThreadLocked(prim);
  1991. }
  1992. else
  1993. {
  1994. prim.ProcessTaints(timeStep);
  1995. }
  1996. processedtaints = true;
  1997. prim.m_collisionscore = 0;
  1998. }
  1999. if (SupportsNINJAJoints)
  2000. {
  2001. // Create pending joints, if possible
  2002. // joints can only be processed after ALL bodies are processed (and exist in ODE), since creating
  2003. // a joint requires specifying the body id of both involved bodies
  2004. if (pendingJoints.Count > 0)
  2005. {
  2006. List<PhysicsJoint> successfullyProcessedPendingJoints = new List<PhysicsJoint>();
  2007. //DoJointErrorMessage(joints_connecting_actor, "taint: " + pendingJoints.Count + " pending joints");
  2008. foreach (PhysicsJoint joint in pendingJoints)
  2009. {
  2010. //DoJointErrorMessage(joint, "taint: time to create joint with parms: " + joint.RawParams);
  2011. string[] jointParams = joint.RawParams.Split(" ".ToCharArray(), System.StringSplitOptions.RemoveEmptyEntries);
  2012. List<IntPtr> jointBodies = new List<IntPtr>();
  2013. bool allJointBodiesAreReady = true;
  2014. foreach (string jointParam in jointParams)
  2015. {
  2016. if (jointParam == "NULL")
  2017. {
  2018. //DoJointErrorMessage(joint, "attaching NULL joint to world");
  2019. jointBodies.Add(IntPtr.Zero);
  2020. }
  2021. else
  2022. {
  2023. //DoJointErrorMessage(joint, "looking for prim name: " + jointParam);
  2024. bool foundPrim = false;
  2025. lock (_prims)
  2026. {
  2027. foreach (OdePrim prim in _prims) // FIXME: inefficient
  2028. {
  2029. if (prim.SOPName == jointParam)
  2030. {
  2031. //DoJointErrorMessage(joint, "found for prim name: " + jointParam);
  2032. if (prim.IsPhysical && prim.Body != IntPtr.Zero)
  2033. {
  2034. jointBodies.Add(prim.Body);
  2035. foundPrim = true;
  2036. break;
  2037. }
  2038. else
  2039. {
  2040. DoJointErrorMessage(joint, "prim name " + jointParam +
  2041. " exists but is not (yet) physical; deferring joint creation. " +
  2042. "IsPhysical property is " + prim.IsPhysical +
  2043. " and body is " + prim.Body);
  2044. foundPrim = false;
  2045. break;
  2046. }
  2047. }
  2048. }
  2049. }
  2050. if (foundPrim)
  2051. {
  2052. // all is fine
  2053. }
  2054. else
  2055. {
  2056. allJointBodiesAreReady = false;
  2057. break;
  2058. }
  2059. }
  2060. }
  2061. if (allJointBodiesAreReady)
  2062. {
  2063. //DoJointErrorMessage(joint, "allJointBodiesAreReady for " + joint.ObjectNameInScene + " with parms " + joint.RawParams);
  2064. if (jointBodies[0] == jointBodies[1])
  2065. {
  2066. DoJointErrorMessage(joint, "ERROR: joint cannot be created; the joint bodies are the same, body1==body2. Raw body is " + jointBodies[0] + ". raw parms: " + joint.RawParams);
  2067. }
  2068. else
  2069. {
  2070. switch (joint.Type)
  2071. {
  2072. case PhysicsJointType.Ball:
  2073. {
  2074. IntPtr odeJoint;
  2075. //DoJointErrorMessage(joint, "ODE creating ball joint ");
  2076. odeJoint = d.JointCreateBall(world, IntPtr.Zero);
  2077. //DoJointErrorMessage(joint, "ODE attaching ball joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
  2078. d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
  2079. //DoJointErrorMessage(joint, "ODE setting ball anchor: " + odeJoint + " to vec:" + joint.Position);
  2080. d.JointSetBallAnchor(odeJoint,
  2081. joint.Position.X,
  2082. joint.Position.Y,
  2083. joint.Position.Z);
  2084. //DoJointErrorMessage(joint, "ODE joint setting OK");
  2085. //DoJointErrorMessage(joint, "The ball joint's bodies are here: b0: ");
  2086. //DoJointErrorMessage(joint, "" + (jointBodies[0] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[0]) : "fixed environment"));
  2087. //DoJointErrorMessage(joint, "The ball joint's bodies are here: b1: ");
  2088. //DoJointErrorMessage(joint, "" + (jointBodies[1] != IntPtr.Zero ? "" + d.BodyGetPosition(jointBodies[1]) : "fixed environment"));
  2089. if (joint is OdePhysicsJoint)
  2090. {
  2091. ((OdePhysicsJoint)joint).jointID = odeJoint;
  2092. }
  2093. else
  2094. {
  2095. DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
  2096. }
  2097. }
  2098. break;
  2099. case PhysicsJointType.Hinge:
  2100. {
  2101. IntPtr odeJoint;
  2102. //DoJointErrorMessage(joint, "ODE creating hinge joint ");
  2103. odeJoint = d.JointCreateHinge(world, IntPtr.Zero);
  2104. //DoJointErrorMessage(joint, "ODE attaching hinge joint: " + odeJoint + " with b1:" + jointBodies[0] + " b2:" + jointBodies[1]);
  2105. d.JointAttach(odeJoint, jointBodies[0], jointBodies[1]);
  2106. //DoJointErrorMessage(joint, "ODE setting hinge anchor: " + odeJoint + " to vec:" + joint.Position);
  2107. d.JointSetHingeAnchor(odeJoint,
  2108. joint.Position.X,
  2109. joint.Position.Y,
  2110. joint.Position.Z);
  2111. // We use the orientation of the x-axis of the joint's coordinate frame
  2112. // as the axis for the hinge.
  2113. // Therefore, we must get the joint's coordinate frame based on the
  2114. // joint.Rotation field, which originates from the orientation of the
  2115. // joint's proxy object in the scene.
  2116. // The joint's coordinate frame is defined as the transformation matrix
  2117. // that converts a vector from joint-local coordinates into world coordinates.
  2118. // World coordinates are defined as the XYZ coordinate system of the sim,
  2119. // as shown in the top status-bar of the viewer.
  2120. // Once we have the joint's coordinate frame, we extract its X axis (AtAxis)
  2121. // and use that as the hinge axis.
  2122. //joint.Rotation.Normalize();
  2123. Matrix4 proxyFrame = Matrix4.CreateFromQuaternion(joint.Rotation);
  2124. // Now extract the X axis of the joint's coordinate frame.
  2125. // Do not try to use proxyFrame.AtAxis or you will become mired in the
  2126. // tar pit of transposed, inverted, and generally messed-up orientations.
  2127. // (In other words, Matrix4.AtAxis() is borked.)
  2128. // Vector3 jointAxis = proxyFrame.AtAxis; <--- this path leadeth to madness
  2129. // Instead, compute the X axis of the coordinate frame by transforming
  2130. // the (1,0,0) vector. At least that works.
  2131. //m_log.Debug("PHY: making axis: complete matrix is " + proxyFrame);
  2132. Vector3 jointAxis = Vector3.Transform(Vector3.UnitX, proxyFrame);
  2133. //m_log.Debug("PHY: making axis: hinge joint axis is " + jointAxis);
  2134. //DoJointErrorMessage(joint, "ODE setting hinge axis: " + odeJoint + " to vec:" + jointAxis);
  2135. d.JointSetHingeAxis(odeJoint,
  2136. jointAxis.X,
  2137. jointAxis.Y,
  2138. jointAxis.Z);
  2139. //d.JointSetHingeParam(odeJoint, (int)dParam.CFM, 0.1f);
  2140. if (joint is OdePhysicsJoint)
  2141. {
  2142. ((OdePhysicsJoint)joint).jointID = odeJoint;
  2143. }
  2144. else
  2145. {
  2146. DoJointErrorMessage(joint, "WARNING: non-ode joint in ODE!");
  2147. }
  2148. }
  2149. break;
  2150. }
  2151. successfullyProcessedPendingJoints.Add(joint);
  2152. }
  2153. }
  2154. else
  2155. {
  2156. DoJointErrorMessage(joint, "joint could not yet be created; still pending");
  2157. }
  2158. }
  2159. foreach (PhysicsJoint successfullyProcessedJoint in successfullyProcessedPendingJoints)
  2160. {
  2161. //DoJointErrorMessage(successfullyProcessedJoint, "finalizing succesfully procsssed joint " + successfullyProcessedJoint.ObjectNameInScene + " parms " + successfullyProcessedJoint.RawParams);
  2162. //DoJointErrorMessage(successfullyProcessedJoint, "removing from pending");
  2163. InternalRemovePendingJoint(successfullyProcessedJoint);
  2164. //DoJointErrorMessage(successfullyProcessedJoint, "adding to active");
  2165. InternalAddActiveJoint(successfullyProcessedJoint);
  2166. //DoJointErrorMessage(successfullyProcessedJoint, "done");
  2167. }
  2168. }
  2169. }
  2170. if (processedtaints)
  2171. _taintedPrim.Clear();
  2172. }
  2173. // Move characters
  2174. lock (_characters)
  2175. {
  2176. foreach (OdeCharacter actor in _characters)
  2177. {
  2178. if (actor != null)
  2179. actor.Move(timeStep);
  2180. }
  2181. }
  2182. // Move other active objects
  2183. lock (_activeprims)
  2184. {
  2185. foreach (OdePrim prim in _activeprims)
  2186. {
  2187. prim.m_collisionscore = 0;
  2188. prim.Move(timeStep);
  2189. }
  2190. }
  2191. //if ((framecount % m_randomizeWater) == 0)
  2192. // randomizeWater(waterlevel);
  2193. collision_optimized(timeStep);
  2194. lock (_collisionEventPrim)
  2195. {
  2196. foreach (PhysicsActor obj in _collisionEventPrim)
  2197. {
  2198. if (obj == null)
  2199. continue;
  2200. switch ((ActorTypes)obj.PhysicsActorType)
  2201. {
  2202. case ActorTypes.Agent:
  2203. OdeCharacter cobj = (OdeCharacter)obj;
  2204. cobj.SendCollisions();
  2205. break;
  2206. case ActorTypes.Prim:
  2207. OdePrim pobj = (OdePrim)obj;
  2208. pobj.SendCollisions();
  2209. break;
  2210. }
  2211. }
  2212. }
  2213. d.WorldQuickStep(world, ODE_STEPSIZE);
  2214. d.JointGroupEmpty(contactgroup);
  2215. //ode.dunlock(world);
  2216. }
  2217. catch (Exception e)
  2218. {
  2219. m_log.ErrorFormat("[PHYSICS]: {0}, {1}, {2}", e.Message, e.TargetSite, e);
  2220. ode.dunlock(world);
  2221. }
  2222. step_time -= ODE_STEPSIZE;
  2223. i++;
  2224. //}
  2225. //else
  2226. //{
  2227. //fps = 0;
  2228. //}
  2229. //}
  2230. }
  2231. lock (_characters)
  2232. {
  2233. foreach (OdeCharacter actor in _characters)
  2234. {
  2235. if (actor != null)
  2236. actor.UpdatePositionAndVelocity();
  2237. }
  2238. }
  2239. lock (_activeprims)
  2240. {
  2241. //if (timeStep < 0.2f)
  2242. {
  2243. foreach (OdePrim actor in _activeprims)
  2244. {
  2245. if (actor.IsPhysical && (d.BodyIsEnabled(actor.Body) || !actor._zeroFlag))
  2246. {
  2247. actor.UpdatePositionAndVelocity();
  2248. if (SupportsNINJAJoints)
  2249. {
  2250. // If an actor moved, move its joint proxy objects as well.
  2251. // There seems to be an event PhysicsActor.OnPositionUpdate that could be used
  2252. // for this purpose but it is never called! So we just do the joint
  2253. // movement code here.
  2254. if (actor.SOPName != null &&
  2255. joints_connecting_actor.ContainsKey(actor.SOPName) &&
  2256. joints_connecting_actor[actor.SOPName] != null &&
  2257. joints_connecting_actor[actor.SOPName].Count > 0)
  2258. {
  2259. foreach (PhysicsJoint affectedJoint in joints_connecting_actor[actor.SOPName])
  2260. {
  2261. if (affectedJoint.IsInPhysicsEngine)
  2262. {
  2263. DoJointMoved(affectedJoint);
  2264. }
  2265. else
  2266. {
  2267. DoJointErrorMessage(affectedJoint, "a body connected to a joint was moved, but the joint doesn't exist yet! this will lead to joint error. joint was: " + affectedJoint.ObjectNameInScene + " parms:" + affectedJoint.RawParams);
  2268. }
  2269. }
  2270. }
  2271. }
  2272. }
  2273. }
  2274. }
  2275. }
  2276. //DumpJointInfo();
  2277. // Finished with all sim stepping. If requested, dump world state to file for debugging.
  2278. // TODO: This call to the export function is already inside lock (OdeLock) - but is an extra lock needed?
  2279. // TODO: This overwrites all dump files in-place. Should this be a growing logfile, or separate snapshots?
  2280. if (physics_logging && (physics_logging_interval>0) && (framecount % physics_logging_interval == 0))
  2281. {
  2282. string fname = "state-" + world.ToString() + ".DIF"; // give each physics world a separate filename
  2283. string prefix = "world" + world.ToString(); // prefix for variable names in exported .DIF file
  2284. if (physics_logging_append_existing_logfile)
  2285. {
  2286. string header = "-------------- START OF PHYSICS FRAME " + framecount.ToString() + " --------------";
  2287. TextWriter fwriter = File.AppendText(fname);
  2288. fwriter.WriteLine(header);
  2289. fwriter.Close();
  2290. }
  2291. d.WorldExportDIF(world, fname, physics_logging_append_existing_logfile, prefix);
  2292. }
  2293. }
  2294. return fps;
  2295. }
  2296. public override void GetResults()
  2297. {
  2298. }
  2299. public override bool IsThreaded
  2300. {
  2301. // for now we won't be multithreaded
  2302. get { return (false); }
  2303. }
  2304. #region ODE Specific Terrain Fixes
  2305. public float[] ResizeTerrain512NearestNeighbour(float[] heightMap)
  2306. {
  2307. float[] returnarr = new float[262144];
  2308. float[,] resultarr = new float[m_regionWidth, m_regionHeight];
  2309. // Filling out the array into it's multi-dimentional components
  2310. for (int y = 0; y < m_regionHeight; y++)
  2311. {
  2312. for (int x = 0; x < m_regionWidth; x++)
  2313. {
  2314. resultarr[y, x] = heightMap[y * m_regionWidth + x];
  2315. }
  2316. }
  2317. // Resize using Nearest Neighbour
  2318. // This particular way is quick but it only works on a multiple of the original
  2319. // The idea behind this method can be described with the following diagrams
  2320. // second pass and third pass happen in the same loop really.. just separated
  2321. // them to show what this does.
  2322. // First Pass
  2323. // ResultArr:
  2324. // 1,1,1,1,1,1
  2325. // 1,1,1,1,1,1
  2326. // 1,1,1,1,1,1
  2327. // 1,1,1,1,1,1
  2328. // 1,1,1,1,1,1
  2329. // 1,1,1,1,1,1
  2330. // Second Pass
  2331. // ResultArr2:
  2332. // 1,,1,,1,,1,,1,,1,
  2333. // ,,,,,,,,,,
  2334. // 1,,1,,1,,1,,1,,1,
  2335. // ,,,,,,,,,,
  2336. // 1,,1,,1,,1,,1,,1,
  2337. // ,,,,,,,,,,
  2338. // 1,,1,,1,,1,,1,,1,
  2339. // ,,,,,,,,,,
  2340. // 1,,1,,1,,1,,1,,1,
  2341. // ,,,,,,,,,,
  2342. // 1,,1,,1,,1,,1,,1,
  2343. // Third pass fills in the blanks
  2344. // ResultArr2:
  2345. // 1,1,1,1,1,1,1,1,1,1,1,1
  2346. // 1,1,1,1,1,1,1,1,1,1,1,1
  2347. // 1,1,1,1,1,1,1,1,1,1,1,1
  2348. // 1,1,1,1,1,1,1,1,1,1,1,1
  2349. // 1,1,1,1,1,1,1,1,1,1,1,1
  2350. // 1,1,1,1,1,1,1,1,1,1,1,1
  2351. // 1,1,1,1,1,1,1,1,1,1,1,1
  2352. // 1,1,1,1,1,1,1,1,1,1,1,1
  2353. // 1,1,1,1,1,1,1,1,1,1,1,1
  2354. // 1,1,1,1,1,1,1,1,1,1,1,1
  2355. // 1,1,1,1,1,1,1,1,1,1,1,1
  2356. // X,Y = .
  2357. // X+1,y = ^
  2358. // X,Y+1 = *
  2359. // X+1,Y+1 = #
  2360. // Filling in like this;
  2361. // .*
  2362. // ^#
  2363. // 1st .
  2364. // 2nd *
  2365. // 3rd ^
  2366. // 4th #
  2367. // on single loop.
  2368. float[,] resultarr2 = new float[512, 512];
  2369. for (int y = 0; y < m_regionHeight; y++)
  2370. {
  2371. for (int x = 0; x < m_regionWidth; x++)
  2372. {
  2373. resultarr2[y * 2, x * 2] = resultarr[y, x];
  2374. if (y < m_regionHeight)
  2375. {
  2376. resultarr2[(y * 2) + 1, x * 2] = resultarr[y, x];
  2377. }
  2378. if (x < m_regionWidth)
  2379. {
  2380. resultarr2[y * 2, (x * 2) + 1] = resultarr[y, x];
  2381. }
  2382. if (x < m_regionWidth && y < m_regionHeight)
  2383. {
  2384. resultarr2[(y * 2) + 1, (x * 2) + 1] = resultarr[y, x];
  2385. }
  2386. }
  2387. }
  2388. //Flatten out the array
  2389. int i = 0;
  2390. for (int y = 0; y < 512; y++)
  2391. {
  2392. for (int x = 0; x < 512; x++)
  2393. {
  2394. if (resultarr2[y, x] <= 0)
  2395. returnarr[i] = 0.0000001f;
  2396. else
  2397. returnarr[i] = resultarr2[y, x];
  2398. i++;
  2399. }
  2400. }
  2401. return returnarr;
  2402. }
  2403. public float[] ResizeTerrain512Interpolation(float[] heightMap)
  2404. {
  2405. float[] returnarr = new float[262144];
  2406. float[,] resultarr = new float[m_regionWidth,m_regionHeight];
  2407. // Filling out the array into it's multi-dimentional components
  2408. for (int y = 0; y < m_regionHeight; y++)
  2409. {
  2410. for (int x = 0; x < m_regionWidth; x++)
  2411. {
  2412. resultarr[y, x] = heightMap[y*m_regionWidth + x];
  2413. }
  2414. }
  2415. // Resize using interpolation
  2416. // This particular way is quick but it only works on a multiple of the original
  2417. // The idea behind this method can be described with the following diagrams
  2418. // second pass and third pass happen in the same loop really.. just separated
  2419. // them to show what this does.
  2420. // First Pass
  2421. // ResultArr:
  2422. // 1,1,1,1,1,1
  2423. // 1,1,1,1,1,1
  2424. // 1,1,1,1,1,1
  2425. // 1,1,1,1,1,1
  2426. // 1,1,1,1,1,1
  2427. // 1,1,1,1,1,1
  2428. // Second Pass
  2429. // ResultArr2:
  2430. // 1,,1,,1,,1,,1,,1,
  2431. // ,,,,,,,,,,
  2432. // 1,,1,,1,,1,,1,,1,
  2433. // ,,,,,,,,,,
  2434. // 1,,1,,1,,1,,1,,1,
  2435. // ,,,,,,,,,,
  2436. // 1,,1,,1,,1,,1,,1,
  2437. // ,,,,,,,,,,
  2438. // 1,,1,,1,,1,,1,,1,
  2439. // ,,,,,,,,,,
  2440. // 1,,1,,1,,1,,1,,1,
  2441. // Third pass fills in the blanks
  2442. // ResultArr2:
  2443. // 1,1,1,1,1,1,1,1,1,1,1,1
  2444. // 1,1,1,1,1,1,1,1,1,1,1,1
  2445. // 1,1,1,1,1,1,1,1,1,1,1,1
  2446. // 1,1,1,1,1,1,1,1,1,1,1,1
  2447. // 1,1,1,1,1,1,1,1,1,1,1,1
  2448. // 1,1,1,1,1,1,1,1,1,1,1,1
  2449. // 1,1,1,1,1,1,1,1,1,1,1,1
  2450. // 1,1,1,1,1,1,1,1,1,1,1,1
  2451. // 1,1,1,1,1,1,1,1,1,1,1,1
  2452. // 1,1,1,1,1,1,1,1,1,1,1,1
  2453. // 1,1,1,1,1,1,1,1,1,1,1,1
  2454. // X,Y = .
  2455. // X+1,y = ^
  2456. // X,Y+1 = *
  2457. // X+1,Y+1 = #
  2458. // Filling in like this;
  2459. // .*
  2460. // ^#
  2461. // 1st .
  2462. // 2nd *
  2463. // 3rd ^
  2464. // 4th #
  2465. // on single loop.
  2466. float[,] resultarr2 = new float[512,512];
  2467. for (int y = 0; y < m_regionHeight; y++)
  2468. {
  2469. for (int x = 0; x < m_regionWidth; x++)
  2470. {
  2471. resultarr2[y*2, x*2] = resultarr[y, x];
  2472. if (y < m_regionHeight)
  2473. {
  2474. if (y + 1 < m_regionHeight)
  2475. {
  2476. if (x + 1 < m_regionWidth)
  2477. {
  2478. resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x] +
  2479. resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
  2480. }
  2481. else
  2482. {
  2483. resultarr2[(y*2) + 1, x*2] = ((resultarr[y, x] + resultarr[y + 1, x])/2);
  2484. }
  2485. }
  2486. else
  2487. {
  2488. resultarr2[(y*2) + 1, x*2] = resultarr[y, x];
  2489. }
  2490. }
  2491. if (x < m_regionWidth)
  2492. {
  2493. if (x + 1 < m_regionWidth)
  2494. {
  2495. if (y + 1 < m_regionHeight)
  2496. {
  2497. resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
  2498. resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
  2499. }
  2500. else
  2501. {
  2502. resultarr2[y*2, (x*2) + 1] = ((resultarr[y, x] + resultarr[y, x + 1])/2);
  2503. }
  2504. }
  2505. else
  2506. {
  2507. resultarr2[y*2, (x*2) + 1] = resultarr[y, x];
  2508. }
  2509. }
  2510. if (x < m_regionWidth && y < m_regionHeight)
  2511. {
  2512. if ((x + 1 < m_regionWidth) && (y + 1 < m_regionHeight))
  2513. {
  2514. resultarr2[(y*2) + 1, (x*2) + 1] = ((resultarr[y, x] + resultarr[y + 1, x] +
  2515. resultarr[y, x + 1] + resultarr[y + 1, x + 1])/4);
  2516. }
  2517. else
  2518. {
  2519. resultarr2[(y*2) + 1, (x*2) + 1] = resultarr[y, x];
  2520. }
  2521. }
  2522. }
  2523. }
  2524. //Flatten out the array
  2525. int i = 0;
  2526. for (int y = 0; y < 512; y++)
  2527. {
  2528. for (int x = 0; x < 512; x++)
  2529. {
  2530. if (Single.IsNaN(resultarr2[y, x]) || Single.IsInfinity(resultarr2[y, x]))
  2531. {
  2532. m_log.Warn("[PHYSICS]: Non finite heightfield element detected. Setting it to 0");
  2533. resultarr2[y, x] = 0;
  2534. }
  2535. if (resultarr2[y, x] <= 0)
  2536. {
  2537. returnarr[i] = 0.0000001f;
  2538. }
  2539. else
  2540. returnarr[i] = resultarr2[y, x];
  2541. i++;
  2542. }
  2543. }
  2544. return returnarr;
  2545. }
  2546. #endregion
  2547. public override void SetTerrain(float[] heightMap)
  2548. {
  2549. // this._heightmap[i] = (double)heightMap[i];
  2550. // dbm (danx0r) -- creating a buffer zone of one extra sample all around
  2551. // _origheightmap = heightMap;
  2552. const uint heightmapWidth = m_regionWidth + 2;
  2553. const uint heightmapHeight = m_regionHeight + 2;
  2554. const uint heightmapWidthSamples = 2*m_regionWidth + 2;
  2555. const uint heightmapHeightSamples = 2*m_regionHeight + 2;
  2556. const float scale = 1.0f;
  2557. const float offset = 0.0f;
  2558. const float thickness = 0.2f;
  2559. const int wrap = 0;
  2560. //Double resolution
  2561. heightMap = ResizeTerrain512Interpolation(heightMap);
  2562. for (int x = 0; x < heightmapWidthSamples; x++)
  2563. {
  2564. for (int y = 0; y < heightmapHeightSamples; y++)
  2565. {
  2566. int xx = Util.Clip(x - 1, 0, 511);
  2567. int yy = Util.Clip(y - 1, 0, 511);
  2568. float val = heightMap[yy*512 + xx];
  2569. _heightmap[x*heightmapHeightSamples + y] = val;
  2570. }
  2571. }
  2572. lock (OdeLock)
  2573. {
  2574. if (LandGeom != IntPtr.Zero)
  2575. {
  2576. d.SpaceRemove(space, LandGeom);
  2577. }
  2578. IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
  2579. d.GeomHeightfieldDataBuildSingle(HeightmapData, _heightmap, 0, heightmapWidth, heightmapHeight,
  2580. (int) heightmapWidthSamples, (int) heightmapHeightSamples, scale,
  2581. offset, thickness, wrap);
  2582. d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
  2583. LandGeom = d.CreateHeightfield(space, HeightmapData, 1);
  2584. if (LandGeom != IntPtr.Zero)
  2585. {
  2586. d.GeomSetCategoryBits(LandGeom, (int)(CollisionCategories.Land));
  2587. d.GeomSetCollideBits(LandGeom, (int)(CollisionCategories.Space));
  2588. }
  2589. geom_name_map[LandGeom] = "Terrain";
  2590. d.Matrix3 R = new d.Matrix3();
  2591. Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
  2592. Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
  2593. //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
  2594. q1 = q1*q2;
  2595. //q1 = q1 * q3;
  2596. Vector3 v3;
  2597. float angle;
  2598. q1.GetAxisAngle(out v3, out angle);
  2599. d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
  2600. d.GeomSetRotation(LandGeom, ref R);
  2601. d.GeomSetPosition(LandGeom, 128, 128, 0);
  2602. }
  2603. }
  2604. public override void DeleteTerrain()
  2605. {
  2606. }
  2607. public override void SetWaterLevel(float baseheight)
  2608. {
  2609. waterlevel = baseheight;
  2610. randomizeWater(waterlevel);
  2611. }
  2612. public void randomizeWater(float baseheight)
  2613. {
  2614. const uint heightmapWidth = m_regionWidth + 2;
  2615. const uint heightmapHeight = m_regionHeight + 2;
  2616. const uint heightmapWidthSamples = m_regionWidth + 2;
  2617. const uint heightmapHeightSamples = m_regionHeight + 2;
  2618. const float scale = 1.0f;
  2619. const float offset = 0.0f;
  2620. const float thickness = 2.9f;
  2621. const int wrap = 0;
  2622. for (int i = 0; i < (258 * 258); i++)
  2623. {
  2624. _watermap[i] = (baseheight-0.1f) + ((float)fluidRandomizer.Next(1,9) / 10f);
  2625. // m_log.Info((baseheight - 0.1f) + ((float)fluidRandomizer.Next(1, 9) / 10f));
  2626. }
  2627. lock (OdeLock)
  2628. {
  2629. if (WaterGeom != IntPtr.Zero)
  2630. {
  2631. d.SpaceRemove(space, WaterGeom);
  2632. }
  2633. IntPtr HeightmapData = d.GeomHeightfieldDataCreate();
  2634. d.GeomHeightfieldDataBuildSingle(HeightmapData, _watermap, 0, heightmapWidth, heightmapHeight,
  2635. (int)heightmapWidthSamples, (int)heightmapHeightSamples, scale,
  2636. offset, thickness, wrap);
  2637. d.GeomHeightfieldDataSetBounds(HeightmapData, m_regionWidth, m_regionHeight);
  2638. WaterGeom = d.CreateHeightfield(space, HeightmapData, 1);
  2639. if (WaterGeom != IntPtr.Zero)
  2640. {
  2641. d.GeomSetCategoryBits(WaterGeom, (int)(CollisionCategories.Water));
  2642. d.GeomSetCollideBits(WaterGeom, (int)(CollisionCategories.Space));
  2643. }
  2644. geom_name_map[WaterGeom] = "Water";
  2645. d.Matrix3 R = new d.Matrix3();
  2646. Quaternion q1 = Quaternion.CreateFromAxisAngle(new Vector3(1, 0, 0), 1.5707f);
  2647. Quaternion q2 = Quaternion.CreateFromAxisAngle(new Vector3(0, 1, 0), 1.5707f);
  2648. //Axiom.Math.Quaternion q3 = Axiom.Math.Quaternion.FromAngleAxis(3.14f, new Axiom.Math.Vector3(0, 0, 1));
  2649. q1 = q1 * q2;
  2650. //q1 = q1 * q3;
  2651. Vector3 v3;
  2652. float angle;
  2653. q1.GetAxisAngle(out v3, out angle);
  2654. d.RFromAxisAndAngle(out R, v3.X, v3.Y, v3.Z, angle);
  2655. d.GeomSetRotation(WaterGeom, ref R);
  2656. d.GeomSetPosition(WaterGeom, 128, 128, 0);
  2657. }
  2658. }
  2659. public override void Dispose()
  2660. {
  2661. lock (OdeLock)
  2662. {
  2663. lock (_prims)
  2664. {
  2665. foreach (OdePrim prm in _prims)
  2666. {
  2667. RemovePrim(prm);
  2668. }
  2669. }
  2670. //foreach (OdeCharacter act in _characters)
  2671. //{
  2672. //RemoveAvatar(act);
  2673. //}
  2674. d.WorldDestroy(world);
  2675. //d.CloseODE();
  2676. }
  2677. }
  2678. public override Dictionary<uint, float> GetTopColliders()
  2679. {
  2680. Dictionary<uint, float> returncolliders = new Dictionary<uint, float>();
  2681. int cnt = 0;
  2682. lock (_prims)
  2683. {
  2684. foreach (OdePrim prm in _prims)
  2685. {
  2686. if (prm.CollisionScore > 0)
  2687. {
  2688. returncolliders.Add(prm.m_localID, prm.CollisionScore);
  2689. cnt++;
  2690. prm.CollisionScore = 0f;
  2691. if (cnt > 25)
  2692. {
  2693. break;
  2694. }
  2695. }
  2696. }
  2697. }
  2698. return returncolliders;
  2699. }
  2700. #if USE_DRAWSTUFF
  2701. // Keyboard callback
  2702. public void command(int cmd)
  2703. {
  2704. IntPtr geom;
  2705. d.Mass mass;
  2706. d.Vector3 sides = new d.Vector3(d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f, d.RandReal() * 0.5f + 0.1f);
  2707. Char ch = Char.ToLower((Char)cmd);
  2708. switch ((Char)ch)
  2709. {
  2710. case 'w':
  2711. try
  2712. {
  2713. Vector3 rotate = (new Vector3(1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
  2714. xyz.X += rotate.X; xyz.Y += rotate.Y; xyz.Z += rotate.Z;
  2715. ds.SetViewpoint(ref xyz, ref hpr);
  2716. }
  2717. catch (ArgumentException)
  2718. { hpr.X = 0; }
  2719. break;
  2720. case 'a':
  2721. hpr.X++;
  2722. ds.SetViewpoint(ref xyz, ref hpr);
  2723. break;
  2724. case 's':
  2725. try
  2726. {
  2727. Vector3 rotate2 = (new Vector3(-1, 0, 0) * Quaternion.CreateFromEulers(hpr.Z * Utils.DEG_TO_RAD, hpr.Y * Utils.DEG_TO_RAD, hpr.X * Utils.DEG_TO_RAD));
  2728. xyz.X += rotate2.X; xyz.Y += rotate2.Y; xyz.Z += rotate2.Z;
  2729. ds.SetViewpoint(ref xyz, ref hpr);
  2730. }
  2731. catch (ArgumentException)
  2732. { hpr.X = 0; }
  2733. break;
  2734. case 'd':
  2735. hpr.X--;
  2736. ds.SetViewpoint(ref xyz, ref hpr);
  2737. break;
  2738. case 'r':
  2739. xyz.Z++;
  2740. ds.SetViewpoint(ref xyz, ref hpr);
  2741. break;
  2742. case 'f':
  2743. xyz.Z--;
  2744. ds.SetViewpoint(ref xyz, ref hpr);
  2745. break;
  2746. case 'e':
  2747. xyz.Y++;
  2748. ds.SetViewpoint(ref xyz, ref hpr);
  2749. break;
  2750. case 'q':
  2751. xyz.Y--;
  2752. ds.SetViewpoint(ref xyz, ref hpr);
  2753. break;
  2754. }
  2755. }
  2756. public void step(int pause)
  2757. {
  2758. ds.SetColor(1.0f, 1.0f, 0.0f);
  2759. ds.SetTexture(ds.Texture.Wood);
  2760. lock (_prims)
  2761. {
  2762. foreach (OdePrim prm in _prims)
  2763. {
  2764. //IntPtr body = d.GeomGetBody(prm.prim_geom);
  2765. if (prm.prim_geom != IntPtr.Zero)
  2766. {
  2767. d.Vector3 pos;
  2768. d.GeomCopyPosition(prm.prim_geom, out pos);
  2769. //d.BodyCopyPosition(body, out pos);
  2770. d.Matrix3 R;
  2771. d.GeomCopyRotation(prm.prim_geom, out R);
  2772. //d.BodyCopyRotation(body, out R);
  2773. d.Vector3 sides = new d.Vector3();
  2774. sides.X = prm.Size.X;
  2775. sides.Y = prm.Size.Y;
  2776. sides.Z = prm.Size.Z;
  2777. ds.DrawBox(ref pos, ref R, ref sides);
  2778. }
  2779. }
  2780. }
  2781. ds.SetColor(1.0f, 0.0f, 0.0f);
  2782. lock (_characters)
  2783. {
  2784. foreach (OdeCharacter chr in _characters)
  2785. {
  2786. if (chr.Shell != IntPtr.Zero)
  2787. {
  2788. IntPtr body = d.GeomGetBody(chr.Shell);
  2789. d.Vector3 pos;
  2790. d.GeomCopyPosition(chr.Shell, out pos);
  2791. //d.BodyCopyPosition(body, out pos);
  2792. d.Matrix3 R;
  2793. d.GeomCopyRotation(chr.Shell, out R);
  2794. //d.BodyCopyRotation(body, out R);
  2795. ds.DrawCapsule(ref pos, ref R, chr.Size.Z, 0.35f);
  2796. d.Vector3 sides = new d.Vector3();
  2797. sides.X = 0.5f;
  2798. sides.Y = 0.5f;
  2799. sides.Z = 0.5f;
  2800. ds.DrawBox(ref pos, ref R, ref sides);
  2801. }
  2802. }
  2803. }
  2804. }
  2805. public void start(int unused)
  2806. {
  2807. ds.SetViewpoint(ref xyz, ref hpr);
  2808. }
  2809. #endif
  2810. }
  2811. }