1
0

OdeScene.cs 185 KB

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