ODEPrim.cs 150 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503504505506507508509510511512513514515516517518519520521522523524525526527528529530531532533534535536537538539540541542543544545546547548549550551552553554555556557558559560561562563564565566567568569570571572573574575576577578579580581582583584585586587588589590591592593594595596597598599600601602603604605606607608609610611612613614615616617618619620621622623624625626627628629630631632633634635636637638639640641642643644645646647648649650651652653654655656657658659660661662663664665666667668669670671672673674675676677678679680681682683684685686687688689690691692693694695696697698699700701702703704705706707708709710711712713714715716717718719720721722723724725726727728729730731732733734735736737738739740741742743744745746747748749750751752753754755756757758759760761762763764765766767768769770771772773774775776777778779780781782783784785786787788789790791792793794795796797798799800801802803804805806807808809810811812813814815816817818819820821822823824825826827828829830831832833834835836837838839840841842843844845846847848849850851852853854855856857858859860861862863864865866867868869870871872873874875876877878879880881882883884885886887888889890891892893894895896897898899900901902903904905906907908909910911912913914915916917918919920921922923924925926927928929930931932933934935936937938939940941942943944945946947948949950951952953954955956957958959960961962963964965966967968969970971972973974975976977978979980981982983984985986987988989990991992993994995996997998999100010011002100310041005100610071008100910101011101210131014101510161017101810191020102110221023102410251026102710281029103010311032103310341035103610371038103910401041104210431044104510461047104810491050105110521053105410551056105710581059106010611062106310641065106610671068106910701071107210731074107510761077107810791080108110821083108410851086108710881089109010911092109310941095109610971098109911001101110211031104110511061107110811091110111111121113111411151116111711181119112011211122112311241125112611271128112911301131113211331134113511361137113811391140114111421143114411451146114711481149115011511152115311541155115611571158115911601161116211631164116511661167116811691170117111721173117411751176117711781179118011811182118311841185118611871188118911901191119211931194119511961197119811991200120112021203120412051206120712081209121012111212121312141215121612171218121912201221122212231224122512261227122812291230123112321233123412351236123712381239124012411242124312441245124612471248124912501251125212531254125512561257125812591260126112621263126412651266126712681269127012711272127312741275127612771278127912801281128212831284128512861287128812891290129112921293129412951296129712981299130013011302130313041305130613071308130913101311131213131314131513161317131813191320132113221323132413251326132713281329133013311332133313341335133613371338133913401341134213431344134513461347134813491350135113521353135413551356135713581359136013611362136313641365136613671368136913701371137213731374137513761377137813791380138113821383138413851386138713881389139013911392139313941395139613971398139914001401140214031404140514061407140814091410141114121413141414151416141714181419142014211422142314241425142614271428142914301431143214331434143514361437143814391440144114421443144414451446144714481449145014511452145314541455145614571458145914601461146214631464146514661467146814691470147114721473147414751476147714781479148014811482148314841485148614871488148914901491149214931494149514961497149814991500150115021503150415051506150715081509151015111512151315141515151615171518151915201521152215231524152515261527152815291530153115321533153415351536153715381539154015411542154315441545154615471548154915501551155215531554155515561557155815591560156115621563156415651566156715681569157015711572157315741575157615771578157915801581158215831584158515861587158815891590159115921593159415951596159715981599160016011602160316041605160616071608160916101611161216131614161516161617161816191620162116221623162416251626162716281629163016311632163316341635163616371638163916401641164216431644164516461647164816491650165116521653165416551656165716581659166016611662166316641665166616671668166916701671167216731674167516761677167816791680168116821683168416851686168716881689169016911692169316941695169616971698169917001701170217031704170517061707170817091710171117121713171417151716171717181719172017211722172317241725172617271728172917301731173217331734173517361737173817391740174117421743174417451746174717481749175017511752175317541755175617571758175917601761176217631764176517661767176817691770177117721773177417751776177717781779178017811782178317841785178617871788178917901791179217931794179517961797179817991800180118021803180418051806180718081809181018111812181318141815181618171818181918201821182218231824182518261827182818291830183118321833183418351836183718381839184018411842184318441845184618471848184918501851185218531854185518561857185818591860186118621863186418651866186718681869187018711872187318741875187618771878187918801881188218831884188518861887188818891890189118921893189418951896189718981899190019011902190319041905190619071908190919101911191219131914191519161917191819191920192119221923192419251926192719281929193019311932193319341935193619371938193919401941194219431944194519461947194819491950195119521953195419551956195719581959196019611962196319641965196619671968196919701971197219731974197519761977197819791980198119821983198419851986198719881989199019911992199319941995199619971998199920002001200220032004200520062007200820092010201120122013201420152016201720182019202020212022202320242025202620272028202920302031203220332034203520362037203820392040204120422043204420452046204720482049205020512052205320542055205620572058205920602061206220632064206520662067206820692070207120722073207420752076207720782079208020812082208320842085208620872088208920902091209220932094209520962097209820992100210121022103210421052106210721082109211021112112211321142115211621172118211921202121212221232124212521262127212821292130213121322133213421352136213721382139214021412142214321442145214621472148214921502151215221532154215521562157215821592160216121622163216421652166216721682169217021712172217321742175217621772178217921802181218221832184218521862187218821892190219121922193219421952196219721982199220022012202220322042205220622072208220922102211221222132214221522162217221822192220222122222223222422252226222722282229223022312232223322342235223622372238223922402241224222432244224522462247224822492250225122522253225422552256225722582259226022612262226322642265226622672268226922702271227222732274227522762277227822792280228122822283228422852286228722882289229022912292229322942295229622972298229923002301230223032304230523062307230823092310231123122313231423152316231723182319232023212322232323242325232623272328232923302331233223332334233523362337233823392340234123422343234423452346234723482349235023512352235323542355235623572358235923602361236223632364236523662367236823692370237123722373237423752376237723782379238023812382238323842385238623872388238923902391239223932394239523962397239823992400240124022403240424052406240724082409241024112412241324142415241624172418241924202421242224232424242524262427242824292430243124322433243424352436243724382439244024412442244324442445244624472448244924502451245224532454245524562457245824592460246124622463246424652466246724682469247024712472247324742475247624772478247924802481248224832484248524862487248824892490249124922493249424952496249724982499250025012502250325042505250625072508250925102511251225132514251525162517251825192520252125222523252425252526252725282529253025312532253325342535253625372538253925402541254225432544254525462547254825492550255125522553255425552556255725582559256025612562256325642565256625672568256925702571257225732574257525762577257825792580258125822583258425852586258725882589259025912592259325942595259625972598259926002601260226032604260526062607260826092610261126122613261426152616261726182619262026212622262326242625262626272628262926302631263226332634263526362637263826392640264126422643264426452646264726482649265026512652265326542655265626572658265926602661266226632664266526662667266826692670267126722673267426752676267726782679268026812682268326842685268626872688268926902691269226932694269526962697269826992700270127022703270427052706270727082709271027112712271327142715271627172718271927202721272227232724272527262727272827292730273127322733273427352736273727382739274027412742274327442745274627472748274927502751275227532754275527562757275827592760276127622763276427652766276727682769277027712772277327742775277627772778277927802781278227832784278527862787278827892790279127922793279427952796279727982799280028012802280328042805280628072808280928102811281228132814281528162817281828192820282128222823282428252826282728282829283028312832283328342835283628372838283928402841284228432844284528462847284828492850285128522853285428552856285728582859286028612862286328642865286628672868286928702871287228732874287528762877287828792880288128822883288428852886288728882889289028912892289328942895289628972898289929002901290229032904290529062907290829092910291129122913291429152916291729182919292029212922292329242925292629272928292929302931293229332934293529362937293829392940294129422943294429452946294729482949295029512952295329542955295629572958295929602961296229632964296529662967296829692970297129722973297429752976297729782979298029812982298329842985298629872988298929902991299229932994299529962997299829993000300130023003300430053006300730083009301030113012301330143015301630173018301930203021302230233024302530263027302830293030303130323033303430353036303730383039304030413042304330443045304630473048304930503051305230533054305530563057305830593060306130623063306430653066306730683069307030713072307330743075307630773078307930803081308230833084308530863087308830893090309130923093309430953096309730983099310031013102310331043105310631073108310931103111311231133114311531163117311831193120312131223123312431253126312731283129313031313132313331343135313631373138313931403141314231433144314531463147314831493150315131523153315431553156315731583159316031613162316331643165316631673168316931703171317231733174317531763177317831793180318131823183318431853186318731883189319031913192319331943195319631973198319932003201320232033204320532063207320832093210321132123213321432153216321732183219322032213222322332243225322632273228322932303231323232333234323532363237323832393240324132423243324432453246324732483249325032513252325332543255325632573258325932603261326232633264326532663267326832693270327132723273327432753276327732783279328032813282328332843285328632873288328932903291329232933294329532963297329832993300330133023303330433053306330733083309331033113312331333143315331633173318331933203321332233233324332533263327332833293330333133323333333433353336333733383339334033413342334333443345334633473348334933503351335233533354335533563357335833593360336133623363336433653366336733683369337033713372337333743375337633773378337933803381338233833384338533863387338833893390339133923393339433953396339733983399340034013402340334043405340634073408340934103411341234133414341534163417341834193420342134223423342434253426342734283429343034313432343334343435343634373438343934403441344234433444344534463447344834493450345134523453345434553456345734583459346034613462346334643465346634673468346934703471347234733474347534763477347834793480348134823483348434853486348734883489349034913492349334943495349634973498349935003501350235033504350535063507350835093510351135123513351435153516351735183519352035213522352335243525352635273528352935303531353235333534353535363537353835393540354135423543354435453546354735483549355035513552355335543555355635573558355935603561356235633564356535663567356835693570357135723573357435753576357735783579358035813582358335843585358635873588358935903591359235933594359535963597359835993600360136023603360436053606360736083609361036113612361336143615361636173618361936203621362236233624362536263627362836293630363136323633363436353636363736383639364036413642364336443645364636473648364936503651365236533654365536563657365836593660366136623663366436653666366736683669367036713672367336743675367636773678367936803681368236833684368536863687368836893690369136923693369436953696369736983699370037013702370337043705370637073708370937103711371237133714371537163717371837193720372137223723372437253726372737283729373037313732373337343735373637373738373937403741374237433744374537463747374837493750375137523753375437553756375737583759376037613762376337643765376637673768376937703771377237733774377537763777377837793780378137823783378437853786378737883789379037913792379337943795379637973798379938003801380238033804380538063807380838093810381138123813381438153816381738183819382038213822382338243825382638273828382938303831383238333834383538363837383838393840384138423843384438453846384738483849385038513852385338543855385638573858385938603861386238633864386538663867386838693870387138723873387438753876387738783879388038813882388338843885388638873888388938903891389238933894389538963897389838993900390139023903390439053906390739083909391039113912391339143915391639173918391939203921392239233924392539263927392839293930393139323933393439353936393739383939394039413942394339443945394639473948394939503951395239533954395539563957395839593960396139623963396439653966396739683969397039713972397339743975397639773978397939803981398239833984398539863987398839893990399139923993399439953996399739983999400040014002400340044005400640074008400940104011401240134014401540164017401840194020402140224023402440254026402740284029403040314032403340344035403640374038403940404041404240434044404540464047404840494050405140524053405440554056405740584059406040614062406340644065406640674068406940704071407240734074407540764077407840794080408140824083408440854086408740884089409040914092409340944095409640974098409941004101410241034104410541064107410841094110411141124113411441154116411741184119412041214122412341244125412641274128412941304131413241334134413541364137413841394140414141424143414441454146414741484149415041514152415341544155415641574158415941604161416241634164416541664167416841694170417141724173417441754176417741784179418041814182418341844185418641874188418941904191419241934194419541964197419841994200420142024203420442054206420742084209421042114212421342144215421642174218421942204221422242234224422542264227422842294230423142324233423442354236423742384239424042414242424342444245424642474248424942504251425242534254425542564257425842594260426142624263426442654266426742684269427042714272427342744275427642774278427942804281428242834284428542864287428842894290429142924293429442954296429742984299430043014302430343044305430643074308430943104311431243134314431543164317431843194320
  1. /*
  2. * Copyright (c) Contributors, http://opensimulator.org/
  3. * See CONTRIBUTORS.TXT for a full list of copyright holders.
  4. *
  5. * Redistribution and use in source and binary forms, with or without
  6. * modification, are permitted provided that the following conditions are met:
  7. * * Redistributions of source code must retain the above copyright
  8. * notice, this list of conditions and the following disclaimer.
  9. * * Redistributions in binary form must reproduce the above copyright
  10. * notice, this list of conditions and the following disclaimer in the
  11. * documentation and/or other materials provided with the distribution.
  12. * * Neither the name of the OpenSimulator Project nor the
  13. * names of its contributors may be used to endorse or promote products
  14. * derived from this software without specific prior written permission.
  15. *
  16. * THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
  17. * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
  18. * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
  19. * DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
  20. * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
  21. * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
  22. * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
  23. * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
  24. * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
  25. * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
  26. */
  27. /*
  28. * Revised August 26 2009 by Kitto Flora. ODEDynamics.cs replaces
  29. * ODEVehicleSettings.cs. It and ODEPrim.cs are re-organised:
  30. * ODEPrim.cs contains methods dealing with Prim editing, Prim
  31. * characteristics and Kinetic motion.
  32. * ODEDynamics.cs contains methods dealing with Prim Physical motion
  33. * (dynamics) and the associated settings. Old Linear and angular
  34. * motors for dynamic motion have been replace with MoveLinear()
  35. * and MoveAngular(); 'Physical' is used only to switch ODE dynamic
  36. * simualtion on/off; VEHICAL_TYPE_NONE/VEHICAL_TYPE_<other> is to
  37. * switch between 'VEHICLE' parameter use and general dynamics
  38. * settings use.
  39. */
  40. //#define SPAM
  41. using System;
  42. using System.Collections.Generic;
  43. using System.Reflection;
  44. using log4net;
  45. using OpenMetaverse;
  46. using OpenSim.Framework;
  47. using OpenSim.Region.PhysicsModules.SharedBase;
  48. namespace OpenSim.Region.PhysicsModule.ubOde
  49. {
  50. public class OdePrim : PhysicsActor
  51. {
  52. private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
  53. private bool m_isphysical;
  54. private bool m_fakeisphysical;
  55. private bool m_isphantom;
  56. private bool m_fakeisphantom;
  57. internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
  58. private bool m_fakeisVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
  59. internal bool m_building;
  60. protected bool m_forcePosOrRotation;
  61. private bool m_iscolliding;
  62. internal bool m_isSelected;
  63. private bool m_delaySelect;
  64. private bool m_lastdoneSelected;
  65. internal bool m_outbounds;
  66. private byte m_angularlocks = 0;
  67. private Quaternion m_lastorientation;
  68. private Quaternion _orientation;
  69. private Vector3 _position;
  70. private Vector3 _velocity;
  71. private Vector3 m_lastVelocity;
  72. private Vector3 m_lastposition;
  73. private Vector3 m_rotationalVelocity;
  74. private Vector3 _size;
  75. private Vector3 m_acceleration;
  76. private IntPtr Amotor;
  77. internal Vector3 m_force;
  78. internal Vector3 m_forceacc;
  79. internal Vector3 m_torque;
  80. internal Vector3 m_angularForceacc;
  81. private float m_invTimeStep;
  82. private float m_timeStep;
  83. private Vector3 m_PIDTarget;
  84. private float m_PIDTau;
  85. private bool m_usePID;
  86. private float m_PIDHoverHeight;
  87. private float m_PIDHoverTau;
  88. private bool m_useHoverPID;
  89. private PIDHoverType m_PIDHoverType;
  90. private float m_targetHoverHeight;
  91. private float m_groundHeight;
  92. private float m_waterHeight;
  93. private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
  94. private int m_body_autodisable_frames;
  95. public int m_bodydisablecontrol = 0;
  96. private float m_gravmod = 1.0f;
  97. // Default we're a Geometry
  98. private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
  99. // Default colide nonphysical don't try to colide with anything
  100. private const CollisionCategories m_default_collisionFlagsNotPhysical = 0;
  101. private const CollisionCategories m_default_collisionFlagsPhysical = (CollisionCategories.Geom |
  102. CollisionCategories.Character |
  103. CollisionCategories.Land |
  104. CollisionCategories.VolumeDtc);
  105. // private bool m_collidesLand = true;
  106. private bool m_collidesWater;
  107. // public bool m_returnCollisions;
  108. private bool m_NoColide; // for now only for internal use for bad meshs
  109. // Default, Collide with Other Geometries, spaces and Bodies
  110. private CollisionCategories m_collisionFlags = m_default_collisionFlagsNotPhysical;
  111. public bool m_disabled;
  112. private uint m_localID;
  113. private IMesh m_mesh;
  114. private object m_meshlock = new object();
  115. private PrimitiveBaseShape _pbs;
  116. private UUID? m_assetID;
  117. private MeshState m_meshState;
  118. public ODEScene _parent_scene;
  119. /// <summary>
  120. /// The physics space which contains prim geometry
  121. /// </summary>
  122. public IntPtr m_targetSpace;
  123. public IntPtr prim_geom;
  124. public IntPtr _triMeshData;
  125. private PhysicsActor _parent;
  126. private List<OdePrim> childrenPrim = new List<OdePrim>();
  127. public float m_collisionscore;
  128. private int m_colliderfilter = 0;
  129. public IntPtr collide_geom; // for objects: geom if single prim space it linkset
  130. private float m_density;
  131. private byte m_shapetype;
  132. private byte m_fakeShapetype;
  133. public bool _zeroFlag;
  134. private bool m_lastUpdateSent;
  135. public IntPtr Body;
  136. private Vector3 _target_velocity;
  137. public Vector3 m_OBBOffset;
  138. public Vector3 m_OBB;
  139. public float primOOBradiusSQ;
  140. private bool m_hasOBB = true;
  141. private float m_physCost;
  142. private float m_streamCost;
  143. internal SafeNativeMethods.Mass primdMass; // prim inertia information on it's own referencial
  144. private PhysicsInertiaData m_InertiaOverride;
  145. float primMass; // prim own mass
  146. float primVolume; // prim own volume;
  147. float m_mass; // object mass acording to case
  148. public int givefakepos;
  149. private Vector3 fakepos;
  150. public int givefakeori;
  151. private Quaternion fakeori;
  152. private PhysicsInertiaData m_fakeInertiaOverride;
  153. private int m_eventsubscription;
  154. private int m_cureventsubscription;
  155. private CollisionEventUpdate CollisionEvents = null;
  156. private CollisionEventUpdate CollisionVDTCEvents = null;
  157. private bool SentEmptyCollisionsEvent;
  158. public volatile bool childPrim;
  159. public ODEDynamics m_vehicle;
  160. internal int m_material = (int)Material.Wood;
  161. private float mu;
  162. private float bounce;
  163. /// <summary>
  164. /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
  165. /// </summary>
  166. public override bool IsPhysical // this is not reliable for internal use
  167. {
  168. get { return m_fakeisphysical; }
  169. set
  170. {
  171. m_fakeisphysical = value; // we show imediatly to outside that we changed physical
  172. // and also to stop imediatly some updates
  173. // but real change will only happen in taintprocessing
  174. if (!value) // Zero the remembered last velocity
  175. m_lastVelocity = Vector3.Zero;
  176. AddChange(changes.Physical, value);
  177. }
  178. }
  179. public override bool IsVolumeDtc
  180. {
  181. get { return m_fakeisVolumeDetect; }
  182. set
  183. {
  184. m_fakeisVolumeDetect = value;
  185. AddChange(changes.VolumeDtc, value);
  186. }
  187. }
  188. public override bool Phantom // this is not reliable for internal use
  189. {
  190. get { return m_fakeisphantom; }
  191. set
  192. {
  193. m_fakeisphantom = value;
  194. AddChange(changes.Phantom, value);
  195. }
  196. }
  197. public override bool Building // this is not reliable for internal use
  198. {
  199. get { return m_building; }
  200. set
  201. {
  202. // if (value)
  203. // m_building = true;
  204. AddChange(changes.building, value);
  205. }
  206. }
  207. public override void getContactData(ref ContactData cdata)
  208. {
  209. cdata.mu = mu;
  210. cdata.bounce = bounce;
  211. // cdata.softcolide = m_softcolide;
  212. cdata.softcolide = false;
  213. if (m_isphysical)
  214. {
  215. ODEDynamics veh;
  216. if (_parent != null)
  217. veh = ((OdePrim)_parent).m_vehicle;
  218. else
  219. veh = m_vehicle;
  220. if (veh != null && veh.Type != Vehicle.TYPE_NONE)
  221. cdata.mu *= veh.FrictionFactor;
  222. // cdata.mu *= 0;
  223. }
  224. }
  225. public override float PhysicsCost
  226. {
  227. get
  228. {
  229. return m_physCost;
  230. }
  231. }
  232. public override float StreamCost
  233. {
  234. get
  235. {
  236. return m_streamCost;
  237. }
  238. }
  239. public override int PhysicsActorType
  240. {
  241. get { return (int)ActorTypes.Prim; }
  242. set { return; }
  243. }
  244. public override bool SetAlwaysRun
  245. {
  246. get { return false; }
  247. set { return; }
  248. }
  249. public override uint LocalID
  250. {
  251. get { return m_localID; }
  252. set
  253. {
  254. uint oldid = m_localID;
  255. m_localID = value;
  256. _parent_scene.changePrimID(this, oldid);
  257. }
  258. }
  259. public override PhysicsActor ParentActor
  260. {
  261. get
  262. {
  263. if (childPrim)
  264. return _parent;
  265. else
  266. return (PhysicsActor)this;
  267. }
  268. }
  269. public override bool Grabbed
  270. {
  271. set { return; }
  272. }
  273. public override bool Selected
  274. {
  275. set
  276. {
  277. if (value)
  278. m_isSelected = value; // if true set imediatly to stop moves etc
  279. AddChange(changes.Selected, value);
  280. }
  281. }
  282. public override bool Flying
  283. {
  284. // no flying prims for you
  285. get { return false; }
  286. set { }
  287. }
  288. public override bool IsColliding
  289. {
  290. get { return m_iscolliding; }
  291. set
  292. {
  293. if (value)
  294. {
  295. m_colliderfilter += 2;
  296. if (m_colliderfilter > 2)
  297. m_colliderfilter = 2;
  298. }
  299. else
  300. {
  301. m_colliderfilter--;
  302. if (m_colliderfilter < 0)
  303. m_colliderfilter = 0;
  304. }
  305. if (m_colliderfilter == 0)
  306. m_iscolliding = false;
  307. else
  308. m_iscolliding = true;
  309. }
  310. }
  311. public override bool CollidingGround
  312. {
  313. get { return false; }
  314. set { return; }
  315. }
  316. public override bool CollidingObj
  317. {
  318. get { return false; }
  319. set { return; }
  320. }
  321. public override bool ThrottleUpdates {get;set;}
  322. public override bool Stopped
  323. {
  324. get { return _zeroFlag; }
  325. }
  326. public override Vector3 Position
  327. {
  328. get
  329. {
  330. if (givefakepos > 0)
  331. return fakepos;
  332. else
  333. return _position;
  334. }
  335. set
  336. {
  337. fakepos = value;
  338. givefakepos++;
  339. AddChange(changes.Position, value);
  340. }
  341. }
  342. public override Vector3 Size
  343. {
  344. get { return _size; }
  345. set
  346. {
  347. if (value.IsFinite())
  348. {
  349. _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, value, m_fakeShapetype);
  350. }
  351. else
  352. {
  353. m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
  354. }
  355. }
  356. }
  357. public override float Mass
  358. {
  359. get { return primMass; }
  360. }
  361. public override Vector3 Force
  362. {
  363. get { return m_force; }
  364. set
  365. {
  366. if (value.IsFinite())
  367. {
  368. AddChange(changes.Force, value);
  369. }
  370. else
  371. {
  372. m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
  373. }
  374. }
  375. }
  376. public override void SetVolumeDetect(int param)
  377. {
  378. m_fakeisVolumeDetect = (param != 0);
  379. AddChange(changes.VolumeDtc, m_fakeisVolumeDetect);
  380. }
  381. public override Vector3 GeometricCenter
  382. {
  383. // this is not real geometric center but a average of positions relative to root prim acording to
  384. // http://wiki.secondlife.com/wiki/llGetGeometricCenter
  385. // ignoring tortured prims details since sl also seems to ignore
  386. // so no real use in doing it on physics
  387. get
  388. {
  389. return Vector3.Zero;
  390. }
  391. }
  392. public override PhysicsInertiaData GetInertiaData()
  393. {
  394. PhysicsInertiaData inertia;
  395. if(childPrim)
  396. {
  397. if(_parent != null)
  398. return _parent.GetInertiaData();
  399. else
  400. {
  401. inertia = new PhysicsInertiaData();
  402. inertia.TotalMass = -1;
  403. return inertia;
  404. }
  405. }
  406. inertia = new PhysicsInertiaData();
  407. // double buffering
  408. if(m_fakeInertiaOverride != null)
  409. {
  410. SafeNativeMethods.Mass objdmass = new SafeNativeMethods.Mass();
  411. objdmass.I.M00 = m_fakeInertiaOverride.Inertia.X;
  412. objdmass.I.M11 = m_fakeInertiaOverride.Inertia.Y;
  413. objdmass.I.M22 = m_fakeInertiaOverride.Inertia.Z;
  414. objdmass.mass = m_fakeInertiaOverride.TotalMass;
  415. if(Math.Abs(m_fakeInertiaOverride.InertiaRotation.W) < 0.999)
  416. {
  417. SafeNativeMethods.Matrix3 inertiarotmat = new SafeNativeMethods.Matrix3();
  418. SafeNativeMethods.Quaternion inertiarot = new SafeNativeMethods.Quaternion();
  419. inertiarot.X = m_fakeInertiaOverride.InertiaRotation.X;
  420. inertiarot.Y = m_fakeInertiaOverride.InertiaRotation.Y;
  421. inertiarot.Z = m_fakeInertiaOverride.InertiaRotation.Z;
  422. inertiarot.W = m_fakeInertiaOverride.InertiaRotation.W;
  423. SafeNativeMethods.RfromQ(out inertiarotmat, ref inertiarot);
  424. SafeNativeMethods.MassRotate(ref objdmass, ref inertiarotmat);
  425. }
  426. inertia.TotalMass = m_fakeInertiaOverride.TotalMass;
  427. inertia.CenterOfMass = m_fakeInertiaOverride.CenterOfMass;
  428. inertia.Inertia.X = objdmass.I.M00;
  429. inertia.Inertia.Y = objdmass.I.M11;
  430. inertia.Inertia.Z = objdmass.I.M22;
  431. inertia.InertiaRotation.X = objdmass.I.M01;
  432. inertia.InertiaRotation.Y = objdmass.I.M02;
  433. inertia.InertiaRotation.Z = objdmass.I.M12;
  434. return inertia;
  435. }
  436. inertia.TotalMass = m_mass;
  437. if(Body == IntPtr.Zero || prim_geom == IntPtr.Zero)
  438. {
  439. inertia.CenterOfMass = Vector3.Zero;
  440. inertia.Inertia = Vector3.Zero;
  441. inertia.InertiaRotation = Vector4.Zero;
  442. return inertia;
  443. }
  444. SafeNativeMethods.Vector3 dtmp;
  445. SafeNativeMethods.Mass m = new SafeNativeMethods.Mass();
  446. lock(_parent_scene.OdeLock)
  447. {
  448. SafeNativeMethods.AllocateODEDataForThread(0);
  449. dtmp = SafeNativeMethods.GeomGetOffsetPosition(prim_geom);
  450. SafeNativeMethods.BodyGetMass(Body, out m);
  451. }
  452. Vector3 cm = new Vector3(-dtmp.X, -dtmp.Y, -dtmp.Z);
  453. inertia.CenterOfMass = cm;
  454. inertia.Inertia = new Vector3(m.I.M00, m.I.M11, m.I.M22);
  455. inertia.InertiaRotation = new Vector4(m.I.M01, m.I.M02 , m.I.M12, 0);
  456. return inertia;
  457. }
  458. public override void SetInertiaData(PhysicsInertiaData inertia)
  459. {
  460. if(childPrim)
  461. {
  462. if(_parent != null)
  463. _parent.SetInertiaData(inertia);
  464. return;
  465. }
  466. if(inertia.TotalMass > 0)
  467. m_fakeInertiaOverride = new PhysicsInertiaData(inertia);
  468. else
  469. m_fakeInertiaOverride = null;
  470. if (inertia.TotalMass > _parent_scene.maximumMassObject)
  471. inertia.TotalMass = _parent_scene.maximumMassObject;
  472. AddChange(changes.SetInertia,(object)m_fakeInertiaOverride);
  473. }
  474. public override Vector3 CenterOfMass
  475. {
  476. get
  477. {
  478. lock (_parent_scene.OdeLock)
  479. {
  480. SafeNativeMethods.AllocateODEDataForThread(0);
  481. SafeNativeMethods.Vector3 dtmp;
  482. if (!childPrim && Body != IntPtr.Zero)
  483. {
  484. dtmp = SafeNativeMethods.BodyGetPosition(Body);
  485. return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
  486. }
  487. else if (prim_geom != IntPtr.Zero)
  488. {
  489. SafeNativeMethods.Quaternion dq;
  490. SafeNativeMethods.GeomCopyQuaternion(prim_geom, out dq);
  491. Quaternion q;
  492. q.X = dq.X;
  493. q.Y = dq.Y;
  494. q.Z = dq.Z;
  495. q.W = dq.W;
  496. Vector3 Ptot = m_OBBOffset * q;
  497. dtmp = SafeNativeMethods.GeomGetPosition(prim_geom);
  498. Ptot.X += dtmp.X;
  499. Ptot.Y += dtmp.Y;
  500. Ptot.Z += dtmp.Z;
  501. // if(childPrim) we only know about physical linksets
  502. return Ptot;
  503. /*
  504. float tmass = _mass;
  505. Ptot *= tmass;
  506. float m;
  507. foreach (OdePrim prm in childrenPrim)
  508. {
  509. m = prm._mass;
  510. Ptot += prm.CenterOfMass * m;
  511. tmass += m;
  512. }
  513. if (tmass == 0)
  514. tmass = 0;
  515. else
  516. tmass = 1.0f / tmass;
  517. Ptot *= tmass;
  518. return Ptot;
  519. */
  520. }
  521. else
  522. return _position;
  523. }
  524. }
  525. }
  526. public override PrimitiveBaseShape Shape
  527. {
  528. set
  529. {
  530. // AddChange(changes.Shape, value);
  531. _parent_scene.m_meshWorker.ChangeActorPhysRep(this, value, _size, m_fakeShapetype);
  532. }
  533. }
  534. public override byte PhysicsShapeType
  535. {
  536. get
  537. {
  538. return m_fakeShapetype;
  539. }
  540. set
  541. {
  542. m_fakeShapetype = value;
  543. _parent_scene.m_meshWorker.ChangeActorPhysRep(this, _pbs, _size, value);
  544. }
  545. }
  546. public override Vector3 rootVelocity
  547. {
  548. get
  549. {
  550. if(_parent != null)
  551. return ((OdePrim)_parent).Velocity;
  552. return Velocity;
  553. }
  554. }
  555. public override Vector3 Velocity
  556. {
  557. get
  558. {
  559. if (_zeroFlag)
  560. return Vector3.Zero;
  561. return _velocity;
  562. }
  563. set
  564. {
  565. if (value.IsFinite())
  566. {
  567. if(m_outbounds)
  568. _velocity = value;
  569. else
  570. AddChange(changes.Velocity, value);
  571. }
  572. else
  573. {
  574. m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
  575. }
  576. }
  577. }
  578. public override Vector3 Torque
  579. {
  580. get
  581. {
  582. if (!IsPhysical || Body == IntPtr.Zero)
  583. return Vector3.Zero;
  584. return m_torque;
  585. }
  586. set
  587. {
  588. if (value.IsFinite())
  589. {
  590. AddChange(changes.Torque, value);
  591. }
  592. else
  593. {
  594. m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
  595. }
  596. }
  597. }
  598. public override float CollisionScore
  599. {
  600. get { return m_collisionscore; }
  601. set { m_collisionscore = value; }
  602. }
  603. public override bool Kinematic
  604. {
  605. get { return false; }
  606. set { }
  607. }
  608. public override Quaternion Orientation
  609. {
  610. get
  611. {
  612. if (givefakeori > 0)
  613. return fakeori;
  614. else
  615. return _orientation;
  616. }
  617. set
  618. {
  619. if (QuaternionIsFinite(value))
  620. {
  621. fakeori = value;
  622. givefakeori++;
  623. value.Normalize();
  624. AddChange(changes.Orientation, value);
  625. }
  626. else
  627. m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
  628. }
  629. }
  630. public override Vector3 Acceleration
  631. {
  632. get { return m_acceleration; }
  633. set
  634. {
  635. if(m_outbounds)
  636. m_acceleration = value;
  637. }
  638. }
  639. public override Vector3 RotationalVelocity
  640. {
  641. get
  642. {
  643. Vector3 pv = Vector3.Zero;
  644. if (_zeroFlag)
  645. return pv;
  646. if (m_rotationalVelocity.ApproxEquals(pv, 0.0001f))
  647. return pv;
  648. return m_rotationalVelocity;
  649. }
  650. set
  651. {
  652. if (value.IsFinite())
  653. {
  654. if(m_outbounds)
  655. m_rotationalVelocity = value;
  656. else
  657. AddChange(changes.AngVelocity, value);
  658. }
  659. else
  660. {
  661. m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
  662. }
  663. }
  664. }
  665. public override float Buoyancy
  666. {
  667. get { return m_buoyancy; }
  668. set
  669. {
  670. AddChange(changes.Buoyancy,value);
  671. }
  672. }
  673. public override bool FloatOnWater
  674. {
  675. set
  676. {
  677. AddChange(changes.CollidesWater, value);
  678. }
  679. }
  680. public override Vector3 PIDTarget
  681. {
  682. set
  683. {
  684. if (value.IsFinite())
  685. {
  686. AddChange(changes.PIDTarget,value);
  687. }
  688. else
  689. m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
  690. }
  691. }
  692. public override bool PIDActive
  693. {
  694. get
  695. {
  696. return m_usePID;
  697. }
  698. set
  699. {
  700. AddChange(changes.PIDActive,value);
  701. }
  702. }
  703. public override float PIDTau
  704. {
  705. set
  706. {
  707. float tmp = 0;
  708. if (value > 0)
  709. {
  710. float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
  711. if (value < mint)
  712. tmp = mint;
  713. else
  714. tmp = value;
  715. }
  716. AddChange(changes.PIDTau,tmp);
  717. }
  718. }
  719. public override float PIDHoverHeight
  720. {
  721. set
  722. {
  723. AddChange(changes.PIDHoverHeight,value);
  724. }
  725. }
  726. public override bool PIDHoverActive
  727. {
  728. get
  729. {
  730. return m_useHoverPID;
  731. }
  732. set
  733. {
  734. AddChange(changes.PIDHoverActive, value);
  735. }
  736. }
  737. public override PIDHoverType PIDHoverType
  738. {
  739. set
  740. {
  741. AddChange(changes.PIDHoverType,value);
  742. }
  743. }
  744. public override float PIDHoverTau
  745. {
  746. set
  747. {
  748. float tmp =0;
  749. if (value > 0)
  750. {
  751. float mint = (0.05f > m_timeStep ? 0.05f : m_timeStep);
  752. if (value < mint)
  753. tmp = mint;
  754. else
  755. tmp = value;
  756. }
  757. AddChange(changes.PIDHoverTau, tmp);
  758. }
  759. }
  760. public override Quaternion APIDTarget { set { return; } }
  761. public override bool APIDActive { set { return; } }
  762. public override float APIDStrength { set { return; } }
  763. public override float APIDDamping { set { return; } }
  764. public override int VehicleType
  765. {
  766. // we may need to put a fake on this
  767. get
  768. {
  769. if (m_vehicle == null)
  770. return (int)Vehicle.TYPE_NONE;
  771. else
  772. return (int)m_vehicle.Type;
  773. }
  774. set
  775. {
  776. AddChange(changes.VehicleType, value);
  777. }
  778. }
  779. public override void VehicleFloatParam(int param, float value)
  780. {
  781. strVehicleFloatParam fp = new strVehicleFloatParam();
  782. fp.param = param;
  783. fp.value = value;
  784. AddChange(changes.VehicleFloatParam, fp);
  785. }
  786. public override void VehicleVectorParam(int param, Vector3 value)
  787. {
  788. strVehicleVectorParam fp = new strVehicleVectorParam();
  789. fp.param = param;
  790. fp.value = value;
  791. AddChange(changes.VehicleVectorParam, fp);
  792. }
  793. public override void VehicleRotationParam(int param, Quaternion value)
  794. {
  795. strVehicleQuatParam fp = new strVehicleQuatParam();
  796. fp.param = param;
  797. fp.value = value;
  798. AddChange(changes.VehicleRotationParam, fp);
  799. }
  800. public override void VehicleFlags(int param, bool value)
  801. {
  802. strVehicleBoolParam bp = new strVehicleBoolParam();
  803. bp.param = param;
  804. bp.value = value;
  805. AddChange(changes.VehicleFlags, bp);
  806. }
  807. public override void SetVehicle(object vdata)
  808. {
  809. AddChange(changes.SetVehicle, vdata);
  810. }
  811. public void SetAcceleration(Vector3 accel)
  812. {
  813. m_acceleration = accel;
  814. }
  815. public override void AddForce(Vector3 force, bool pushforce)
  816. {
  817. if (force.IsFinite())
  818. {
  819. if(pushforce)
  820. AddChange(changes.AddForce, force);
  821. else // a impulse
  822. AddChange(changes.AddForce, force * m_invTimeStep);
  823. }
  824. else
  825. {
  826. m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
  827. }
  828. //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
  829. }
  830. public override void AddAngularForce(Vector3 force, bool pushforce)
  831. {
  832. if (force.IsFinite())
  833. {
  834. // if(pushforce) for now applyrotationimpulse seems more happy applied as a force
  835. AddChange(changes.AddAngForce, force);
  836. // else // a impulse
  837. // AddChange(changes.AddAngForce, force * m_invTimeStep);
  838. }
  839. else
  840. {
  841. m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
  842. }
  843. }
  844. public override void CrossingFailure()
  845. {
  846. lock(_parent_scene.OdeLock)
  847. {
  848. if (m_outbounds)
  849. {
  850. _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
  851. _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
  852. _position.Z = Util.Clip(_position.Z + 0.2f, Constants.MinSimulationHeight, Constants.MaxSimulationHeight);
  853. m_lastposition = _position;
  854. _velocity.X = 0;
  855. _velocity.Y = 0;
  856. _velocity.Z = 0;
  857. SafeNativeMethods.AllocateODEDataForThread(0);
  858. m_lastVelocity = _velocity;
  859. if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
  860. m_vehicle.Stop();
  861. if(Body != IntPtr.Zero)
  862. SafeNativeMethods.BodySetLinearVel(Body, 0, 0, 0); // stop it
  863. if (prim_geom != IntPtr.Zero)
  864. SafeNativeMethods.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
  865. m_outbounds = false;
  866. changeDisable(false);
  867. base.RequestPhysicsterseUpdate();
  868. }
  869. }
  870. }
  871. public override void CrossingStart()
  872. {
  873. lock(_parent_scene.OdeLock)
  874. {
  875. if (m_outbounds || childPrim)
  876. return;
  877. m_outbounds = true;
  878. m_lastposition = _position;
  879. m_lastorientation = _orientation;
  880. SafeNativeMethods.AllocateODEDataForThread(0);
  881. if(Body != IntPtr.Zero)
  882. {
  883. SafeNativeMethods.Vector3 dtmp = SafeNativeMethods.BodyGetAngularVel(Body);
  884. m_rotationalVelocity.X = dtmp.X;
  885. m_rotationalVelocity.Y = dtmp.Y;
  886. m_rotationalVelocity.Z = dtmp.Z;
  887. dtmp = SafeNativeMethods.BodyGetLinearVel(Body);
  888. _velocity.X = dtmp.X;
  889. _velocity.Y = dtmp.Y;
  890. _velocity.Z = dtmp.Z;
  891. SafeNativeMethods.BodySetLinearVel(Body, 0, 0, 0); // stop it
  892. SafeNativeMethods.BodySetAngularVel(Body, 0, 0, 0);
  893. }
  894. if(prim_geom != IntPtr.Zero)
  895. SafeNativeMethods.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
  896. disableBodySoft(); // stop collisions
  897. UnSubscribeEvents();
  898. }
  899. }
  900. public override void SetMomentum(Vector3 momentum)
  901. {
  902. }
  903. public override void SetMaterial(int pMaterial)
  904. {
  905. m_material = pMaterial;
  906. mu = _parent_scene.m_materialContactsData[pMaterial].mu;
  907. bounce = _parent_scene.m_materialContactsData[pMaterial].bounce;
  908. }
  909. public override float Density
  910. {
  911. get
  912. {
  913. return m_density * 100f;
  914. }
  915. set
  916. {
  917. float old = m_density;
  918. m_density = value / 100f;
  919. // if(m_density != old)
  920. // UpdatePrimBodyData();
  921. }
  922. }
  923. public override float GravModifier
  924. {
  925. get
  926. {
  927. return m_gravmod;
  928. }
  929. set
  930. {
  931. m_gravmod = value;
  932. if (m_vehicle != null)
  933. m_vehicle.GravMod = m_gravmod;
  934. }
  935. }
  936. public override float Friction
  937. {
  938. get
  939. {
  940. return mu;
  941. }
  942. set
  943. {
  944. mu = value;
  945. }
  946. }
  947. public override float Restitution
  948. {
  949. get
  950. {
  951. return bounce;
  952. }
  953. set
  954. {
  955. bounce = value;
  956. }
  957. }
  958. public void setPrimForRemoval()
  959. {
  960. AddChange(changes.Remove, null);
  961. }
  962. public override void link(PhysicsActor obj)
  963. {
  964. AddChange(changes.Link, obj);
  965. }
  966. public override void delink()
  967. {
  968. AddChange(changes.DeLink, null);
  969. }
  970. public override void LockAngularMotion(byte axislock)
  971. {
  972. // m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
  973. AddChange(changes.AngLock, axislock);
  974. }
  975. public override void SubscribeEvents(int ms)
  976. {
  977. m_eventsubscription = ms;
  978. m_cureventsubscription = 0;
  979. if (CollisionEvents == null)
  980. CollisionEvents = new CollisionEventUpdate();
  981. if (CollisionVDTCEvents == null)
  982. CollisionVDTCEvents = new CollisionEventUpdate();
  983. SentEmptyCollisionsEvent = false;
  984. }
  985. public override void UnSubscribeEvents()
  986. {
  987. if (CollisionVDTCEvents != null)
  988. {
  989. CollisionVDTCEvents.Clear();
  990. CollisionVDTCEvents = null;
  991. }
  992. if (CollisionEvents != null)
  993. {
  994. CollisionEvents.Clear();
  995. CollisionEvents = null;
  996. }
  997. m_eventsubscription = 0;
  998. _parent_scene.RemoveCollisionEventReporting(this);
  999. }
  1000. public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
  1001. {
  1002. if (CollisionEvents == null)
  1003. CollisionEvents = new CollisionEventUpdate();
  1004. CollisionEvents.AddCollider(CollidedWith, contact);
  1005. _parent_scene.AddCollisionEventReporting(this);
  1006. }
  1007. public override void AddVDTCCollisionEvent(uint CollidedWith, ContactPoint contact)
  1008. {
  1009. if (CollisionVDTCEvents == null)
  1010. CollisionVDTCEvents = new CollisionEventUpdate();
  1011. CollisionVDTCEvents.AddCollider(CollidedWith, contact);
  1012. _parent_scene.AddCollisionEventReporting(this);
  1013. }
  1014. internal void SleeperAddCollisionEvents()
  1015. {
  1016. if(CollisionEvents != null && CollisionEvents.m_objCollisionList.Count != 0)
  1017. {
  1018. foreach(KeyValuePair<uint,ContactPoint> kvp in CollisionEvents.m_objCollisionList)
  1019. {
  1020. if(kvp.Key == 0)
  1021. continue;
  1022. OdePrim other = _parent_scene.getPrim(kvp.Key);
  1023. if(other == null)
  1024. continue;
  1025. ContactPoint cp = kvp.Value;
  1026. cp.SurfaceNormal = - cp.SurfaceNormal;
  1027. cp.RelativeSpeed = -cp.RelativeSpeed;
  1028. other.AddCollisionEvent(ParentActor.LocalID,cp);
  1029. }
  1030. }
  1031. if(CollisionVDTCEvents != null && CollisionVDTCEvents.m_objCollisionList.Count != 0)
  1032. {
  1033. foreach(KeyValuePair<uint,ContactPoint> kvp in CollisionVDTCEvents.m_objCollisionList)
  1034. {
  1035. OdePrim other = _parent_scene.getPrim(kvp.Key);
  1036. if(other == null)
  1037. continue;
  1038. ContactPoint cp = kvp.Value;
  1039. cp.SurfaceNormal = - cp.SurfaceNormal;
  1040. cp.RelativeSpeed = -cp.RelativeSpeed;
  1041. other.AddCollisionEvent(ParentActor.LocalID,cp);
  1042. }
  1043. }
  1044. }
  1045. internal void clearSleeperCollisions()
  1046. {
  1047. if(CollisionVDTCEvents != null && CollisionVDTCEvents.Count >0 )
  1048. CollisionVDTCEvents.Clear();
  1049. }
  1050. public void SendCollisions(int timestep)
  1051. {
  1052. if (m_cureventsubscription < 50000)
  1053. m_cureventsubscription += timestep;
  1054. if (m_cureventsubscription < m_eventsubscription)
  1055. return;
  1056. if (CollisionEvents == null)
  1057. return;
  1058. int ncolisions = CollisionEvents.m_objCollisionList.Count;
  1059. if (!SentEmptyCollisionsEvent || ncolisions > 0)
  1060. {
  1061. base.SendCollisionUpdate(CollisionEvents);
  1062. m_cureventsubscription = 0;
  1063. if (ncolisions == 0)
  1064. {
  1065. SentEmptyCollisionsEvent = true;
  1066. // _parent_scene.RemoveCollisionEventReporting(this);
  1067. }
  1068. else if(Body == IntPtr.Zero || (SafeNativeMethods.BodyIsEnabled(Body) && m_bodydisablecontrol >= 0 ))
  1069. {
  1070. SentEmptyCollisionsEvent = false;
  1071. CollisionEvents.Clear();
  1072. }
  1073. }
  1074. }
  1075. public override bool SubscribedEvents()
  1076. {
  1077. if (m_eventsubscription > 0)
  1078. return true;
  1079. return false;
  1080. }
  1081. public OdePrim(String primName, ODEScene parent_scene, Vector3 pos, Vector3 size,
  1082. Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical,bool pisPhantom,byte _shapeType,uint plocalID)
  1083. {
  1084. _parent_scene = parent_scene;
  1085. Name = primName;
  1086. m_localID = plocalID;
  1087. m_vehicle = null;
  1088. if (!pos.IsFinite())
  1089. {
  1090. pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
  1091. parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
  1092. m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
  1093. }
  1094. _position = pos;
  1095. givefakepos = 0;
  1096. m_timeStep = parent_scene.ODE_STEPSIZE;
  1097. m_invTimeStep = 1f / m_timeStep;
  1098. m_density = parent_scene.geomDefaultDensity;
  1099. m_body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
  1100. prim_geom = IntPtr.Zero;
  1101. collide_geom = IntPtr.Zero;
  1102. Body = IntPtr.Zero;
  1103. if (!size.IsFinite())
  1104. {
  1105. size = new Vector3(0.5f, 0.5f, 0.5f);
  1106. m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
  1107. }
  1108. if (size.X <= 0) size.X = 0.01f;
  1109. if (size.Y <= 0) size.Y = 0.01f;
  1110. if (size.Z <= 0) size.Z = 0.01f;
  1111. _size = size;
  1112. if (!QuaternionIsFinite(rotation))
  1113. {
  1114. rotation = Quaternion.Identity;
  1115. m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
  1116. }
  1117. _orientation = rotation;
  1118. givefakeori = 0;
  1119. _pbs = pbs;
  1120. m_targetSpace = IntPtr.Zero;
  1121. if (pos.Z < 0)
  1122. {
  1123. m_isphysical = false;
  1124. }
  1125. else
  1126. {
  1127. m_isphysical = pisPhysical;
  1128. }
  1129. m_fakeisphysical = m_isphysical;
  1130. m_isVolumeDetect = false;
  1131. m_fakeisVolumeDetect = false;
  1132. m_force = Vector3.Zero;
  1133. m_iscolliding = false;
  1134. m_colliderfilter = 0;
  1135. m_NoColide = false;
  1136. _triMeshData = IntPtr.Zero;
  1137. m_fakeShapetype = _shapeType;
  1138. m_lastdoneSelected = false;
  1139. m_isSelected = false;
  1140. m_delaySelect = false;
  1141. m_isphantom = pisPhantom;
  1142. m_fakeisphantom = pisPhantom;
  1143. mu = parent_scene.m_materialContactsData[(int)Material.Wood].mu;
  1144. bounce = parent_scene.m_materialContactsData[(int)Material.Wood].bounce;
  1145. m_building = true; // control must set this to false when done
  1146. AddChange(changes.Add, null);
  1147. // get basic mass parameters
  1148. ODEPhysRepData repData = _parent_scene.m_meshWorker.NewActorPhysRep(this, _pbs, _size, _shapeType);
  1149. primVolume = repData.volume;
  1150. m_OBB = repData.OBB;
  1151. m_OBBOffset = repData.OBBOffset;
  1152. UpdatePrimBodyData();
  1153. }
  1154. private void resetCollisionAccounting()
  1155. {
  1156. m_collisionscore = 0;
  1157. }
  1158. private void UpdateCollisionCatFlags()
  1159. {
  1160. if(m_isphysical && m_disabled)
  1161. {
  1162. m_collisionCategories = 0;
  1163. m_collisionFlags = 0;
  1164. }
  1165. else if (m_isSelected)
  1166. {
  1167. m_collisionCategories = CollisionCategories.Selected;
  1168. m_collisionFlags = 0;
  1169. }
  1170. else if (m_isVolumeDetect)
  1171. {
  1172. m_collisionCategories = CollisionCategories.VolumeDtc;
  1173. if (m_isphysical)
  1174. m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
  1175. else
  1176. m_collisionFlags = 0;
  1177. }
  1178. else if (m_isphantom)
  1179. {
  1180. m_collisionCategories = CollisionCategories.Phantom;
  1181. if (m_isphysical)
  1182. m_collisionFlags = CollisionCategories.Land;
  1183. else
  1184. m_collisionFlags = 0;
  1185. }
  1186. else
  1187. {
  1188. m_collisionCategories = CollisionCategories.Geom;
  1189. if (m_isphysical)
  1190. m_collisionFlags = m_default_collisionFlagsPhysical;
  1191. else
  1192. m_collisionFlags = m_default_collisionFlagsNotPhysical;
  1193. }
  1194. }
  1195. private void ApplyCollisionCatFlags()
  1196. {
  1197. if (prim_geom != IntPtr.Zero)
  1198. {
  1199. if (!childPrim && childrenPrim.Count > 0)
  1200. {
  1201. foreach (OdePrim prm in childrenPrim)
  1202. {
  1203. if (m_isphysical && m_disabled)
  1204. {
  1205. prm.m_collisionCategories = 0;
  1206. prm.m_collisionFlags = 0;
  1207. }
  1208. else
  1209. {
  1210. // preserve some
  1211. if (prm.m_isSelected)
  1212. {
  1213. prm.m_collisionCategories = CollisionCategories.Selected;
  1214. prm.m_collisionFlags = 0;
  1215. }
  1216. else if (prm.m_isVolumeDetect)
  1217. {
  1218. prm.m_collisionCategories = CollisionCategories.VolumeDtc;
  1219. if (m_isphysical)
  1220. prm.m_collisionFlags = CollisionCategories.Geom | CollisionCategories.Character;
  1221. else
  1222. prm.m_collisionFlags = 0;
  1223. }
  1224. else if (prm.m_isphantom)
  1225. {
  1226. prm.m_collisionCategories = CollisionCategories.Phantom;
  1227. if (m_isphysical)
  1228. prm.m_collisionFlags = CollisionCategories.Land;
  1229. else
  1230. prm.m_collisionFlags = 0;
  1231. }
  1232. else
  1233. {
  1234. prm.m_collisionCategories = m_collisionCategories;
  1235. prm.m_collisionFlags = m_collisionFlags;
  1236. }
  1237. }
  1238. if (prm.prim_geom != IntPtr.Zero)
  1239. {
  1240. if (prm.m_NoColide)
  1241. {
  1242. SafeNativeMethods.GeomSetCategoryBits(prm.prim_geom, 0);
  1243. if (m_isphysical)
  1244. SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, (int)CollisionCategories.Land);
  1245. else
  1246. SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, 0);
  1247. }
  1248. else
  1249. {
  1250. SafeNativeMethods.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
  1251. SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
  1252. }
  1253. }
  1254. }
  1255. }
  1256. if (m_NoColide)
  1257. {
  1258. SafeNativeMethods.GeomSetCategoryBits(prim_geom, 0);
  1259. SafeNativeMethods.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
  1260. if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
  1261. {
  1262. SafeNativeMethods.GeomSetCategoryBits(collide_geom, 0);
  1263. SafeNativeMethods.GeomSetCollideBits(collide_geom, (uint)CollisionCategories.Land);
  1264. }
  1265. }
  1266. else
  1267. {
  1268. SafeNativeMethods.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
  1269. SafeNativeMethods.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
  1270. if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
  1271. {
  1272. SafeNativeMethods.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
  1273. SafeNativeMethods.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
  1274. }
  1275. }
  1276. }
  1277. }
  1278. private void createAMotor(byte axislock)
  1279. {
  1280. if (Body == IntPtr.Zero)
  1281. return;
  1282. if (Amotor != IntPtr.Zero)
  1283. {
  1284. SafeNativeMethods.JointDestroy(Amotor);
  1285. Amotor = IntPtr.Zero;
  1286. }
  1287. int axisnum = 0;
  1288. bool axisX = false;
  1289. bool axisY = false;
  1290. bool axisZ = false;
  1291. if((axislock & 0x02) != 0)
  1292. {
  1293. axisnum++;
  1294. axisX = true;
  1295. }
  1296. if((axislock & 0x04) != 0)
  1297. {
  1298. axisnum++;
  1299. axisY = true;
  1300. }
  1301. if((axislock & 0x08) != 0)
  1302. {
  1303. axisnum++;
  1304. axisZ = true;
  1305. }
  1306. if(axisnum == 0)
  1307. return;
  1308. // stop it
  1309. SafeNativeMethods.BodySetTorque(Body, 0, 0, 0);
  1310. SafeNativeMethods.BodySetAngularVel(Body, 0, 0, 0);
  1311. Amotor = SafeNativeMethods.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
  1312. SafeNativeMethods.JointAttach(Amotor, Body, IntPtr.Zero);
  1313. SafeNativeMethods.JointSetAMotorMode(Amotor, 0);
  1314. SafeNativeMethods.JointSetAMotorNumAxes(Amotor, axisnum);
  1315. // get current orientation to lock
  1316. SafeNativeMethods.Quaternion dcur = SafeNativeMethods.BodyGetQuaternion(Body);
  1317. Quaternion curr; // crap convertion between identical things
  1318. curr.X = dcur.X;
  1319. curr.Y = dcur.Y;
  1320. curr.Z = dcur.Z;
  1321. curr.W = dcur.W;
  1322. Vector3 ax;
  1323. int i = 0;
  1324. int j = 0;
  1325. if (axisX)
  1326. {
  1327. ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
  1328. SafeNativeMethods.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
  1329. SafeNativeMethods.JointSetAMotorAngle(Amotor, 0, 0);
  1330. SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.LoStop, 0f);
  1331. SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.HiStop, 0f);
  1332. SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.Vel, 0);
  1333. SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.FudgeFactor, 0.0001f);
  1334. SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.Bounce, 0f);
  1335. SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.CFM, 0f);
  1336. SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.FMax, 5e8f);
  1337. SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.StopCFM, 0f);
  1338. SafeNativeMethods.JointSetAMotorParam(Amotor, (int)SafeNativeMethods.JointParam.StopERP, 0.8f);
  1339. i++;
  1340. j = 256; // move to next axis set
  1341. }
  1342. if (axisY)
  1343. {
  1344. ax = (new Vector3(0, 1, 0)) * curr;
  1345. SafeNativeMethods.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
  1346. SafeNativeMethods.JointSetAMotorAngle(Amotor, i, 0);
  1347. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.LoStop, 0f);
  1348. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.HiStop, 0f);
  1349. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.Vel, 0);
  1350. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.FudgeFactor, 0.0001f);
  1351. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.Bounce, 0f);
  1352. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.CFM, 0f);
  1353. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.FMax, 5e8f);
  1354. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.StopCFM, 0f);
  1355. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.StopERP, 0.8f);
  1356. i++;
  1357. j += 256;
  1358. }
  1359. if (axisZ)
  1360. {
  1361. ax = (new Vector3(0, 0, 1)) * curr;
  1362. SafeNativeMethods.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
  1363. SafeNativeMethods.JointSetAMotorAngle(Amotor, i, 0);
  1364. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.LoStop, 0f);
  1365. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.HiStop, 0f);
  1366. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.Vel, 0);
  1367. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.FudgeFactor, 0.0001f);
  1368. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.Bounce, 0f);
  1369. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.CFM, 0f);
  1370. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.FMax, 5e8f);
  1371. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.StopCFM, 0f);
  1372. SafeNativeMethods.JointSetAMotorParam(Amotor, j + (int)SafeNativeMethods.JointParam.StopERP, 0.8f);
  1373. }
  1374. }
  1375. private void SetGeom(IntPtr geom)
  1376. {
  1377. prim_geom = geom;
  1378. //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
  1379. if (prim_geom != IntPtr.Zero)
  1380. {
  1381. if (m_NoColide)
  1382. {
  1383. SafeNativeMethods.GeomSetCategoryBits(prim_geom, 0);
  1384. if (m_isphysical)
  1385. {
  1386. SafeNativeMethods.GeomSetCollideBits(prim_geom, (uint)CollisionCategories.Land);
  1387. }
  1388. else
  1389. {
  1390. SafeNativeMethods.GeomSetCollideBits(prim_geom, 0);
  1391. SafeNativeMethods.GeomDisable(prim_geom);
  1392. }
  1393. }
  1394. else
  1395. {
  1396. SafeNativeMethods.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
  1397. SafeNativeMethods.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
  1398. }
  1399. UpdatePrimBodyData();
  1400. _parent_scene.actor_name_map[prim_geom] = this;
  1401. /*
  1402. // debug
  1403. d.AABB aabb;
  1404. d.GeomGetAABB(prim_geom, out aabb);
  1405. float x = aabb.MaxX - aabb.MinX;
  1406. float y = aabb.MaxY - aabb.MinY;
  1407. float z = aabb.MaxZ - aabb.MinZ;
  1408. if( x > 60.0f || y > 60.0f || z > 60.0f)
  1409. m_log.WarnFormat("[PHYSICS]: large prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
  1410. Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
  1411. else if (x < 0.001f || y < 0.001f || z < 0.001f)
  1412. m_log.WarnFormat("[PHYSICS]: small prim geo {0},size {1}, AABBsize <{2},{3},{4}, mesh {5} at {6}",
  1413. Name, _size.ToString(), x, y, z, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh", _position.ToString());
  1414. */
  1415. }
  1416. else
  1417. m_log.Warn("Setting bad Geom");
  1418. }
  1419. private bool GetMeshGeom()
  1420. {
  1421. IntPtr vertices, indices;
  1422. int vertexCount, indexCount;
  1423. int vertexStride, triStride;
  1424. IMesh mesh = m_mesh;
  1425. if (mesh == null)
  1426. return false;
  1427. mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount);
  1428. mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
  1429. if (vertexCount == 0 || indexCount == 0)
  1430. {
  1431. m_log.WarnFormat("[PHYSICS]: Invalid mesh data on OdePrim {0}, mesh {1} at {2}",
  1432. Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",_position.ToString());
  1433. m_hasOBB = false;
  1434. m_OBBOffset = Vector3.Zero;
  1435. m_OBB = _size * 0.5f;
  1436. m_physCost = 0.1f;
  1437. m_streamCost = 1.0f;
  1438. _parent_scene.mesher.ReleaseMesh(mesh);
  1439. m_meshState = MeshState.MeshFailed;
  1440. m_mesh = null;
  1441. return false;
  1442. }
  1443. if (vertexCount > 64000 || indexCount > 64000)
  1444. {
  1445. m_log.WarnFormat("[PHYSICS]: large mesh data on OdePrim {0}, mesh {1} at {2}, {3} vertices, {4} indexes",
  1446. Name, _pbs.SculptEntry ? _pbs.SculptTexture.ToString() : "primMesh",
  1447. _position.ToString() ,vertexCount , indexCount );
  1448. }
  1449. IntPtr geo = IntPtr.Zero;
  1450. try
  1451. {
  1452. _triMeshData = SafeNativeMethods.GeomTriMeshDataCreate();
  1453. SafeNativeMethods.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
  1454. SafeNativeMethods.GeomTriMeshDataPreprocess(_triMeshData);
  1455. geo = SafeNativeMethods.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null);
  1456. }
  1457. catch (Exception e)
  1458. {
  1459. m_log.ErrorFormat("[PHYSICS]: SetGeom Mesh failed for {0} exception: {1}", Name, e);
  1460. if (_triMeshData != IntPtr.Zero)
  1461. {
  1462. try
  1463. {
  1464. SafeNativeMethods.GeomTriMeshDataDestroy(_triMeshData);
  1465. }
  1466. catch
  1467. {
  1468. }
  1469. }
  1470. _triMeshData = IntPtr.Zero;
  1471. m_hasOBB = false;
  1472. m_OBBOffset = Vector3.Zero;
  1473. m_OBB = _size * 0.5f;
  1474. m_physCost = 0.1f;
  1475. m_streamCost = 1.0f;
  1476. _parent_scene.mesher.ReleaseMesh(mesh);
  1477. m_meshState = MeshState.MeshFailed;
  1478. m_mesh = null;
  1479. return false;
  1480. }
  1481. m_physCost = 0.0013f * (float)indexCount;
  1482. // todo
  1483. m_streamCost = 1.0f;
  1484. SetGeom(geo);
  1485. return true;
  1486. }
  1487. private void CreateGeom(bool OverrideToBox)
  1488. {
  1489. bool hasMesh = false;
  1490. m_NoColide = false;
  1491. if ((m_meshState & MeshState.MeshNoColide) != 0)
  1492. m_NoColide = true;
  1493. else if(!OverrideToBox && m_mesh != null)
  1494. {
  1495. if (GetMeshGeom())
  1496. hasMesh = true;
  1497. else
  1498. m_NoColide = true;
  1499. }
  1500. if (!hasMesh)
  1501. {
  1502. IntPtr geo = IntPtr.Zero;
  1503. if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1
  1504. && _size.X == _size.Y && _size.Y == _size.Z)
  1505. { // it's a sphere
  1506. try
  1507. {
  1508. geo = SafeNativeMethods.CreateSphere(m_targetSpace, _size.X * 0.5f);
  1509. }
  1510. catch (Exception e)
  1511. {
  1512. m_log.WarnFormat("[PHYSICS]: Create sphere failed: {0}", e);
  1513. return;
  1514. }
  1515. }
  1516. else
  1517. {// do it as a box
  1518. try
  1519. {
  1520. geo = SafeNativeMethods.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z);
  1521. }
  1522. catch (Exception e)
  1523. {
  1524. m_log.Warn("[PHYSICS]: Create box failed: {0}", e);
  1525. return;
  1526. }
  1527. }
  1528. m_physCost = 0.1f;
  1529. m_streamCost = 1.0f;
  1530. SetGeom(geo);
  1531. }
  1532. }
  1533. private void RemoveGeom()
  1534. {
  1535. if (prim_geom != IntPtr.Zero)
  1536. {
  1537. _parent_scene.actor_name_map.Remove(prim_geom);
  1538. try
  1539. {
  1540. SafeNativeMethods.GeomDestroy(prim_geom);
  1541. if (_triMeshData != IntPtr.Zero)
  1542. {
  1543. SafeNativeMethods.GeomTriMeshDataDestroy(_triMeshData);
  1544. _triMeshData = IntPtr.Zero;
  1545. }
  1546. }
  1547. catch (Exception e)
  1548. {
  1549. m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction failed for {0} exception {1}", Name, e);
  1550. }
  1551. prim_geom = IntPtr.Zero;
  1552. collide_geom = IntPtr.Zero;
  1553. m_targetSpace = IntPtr.Zero;
  1554. }
  1555. else
  1556. {
  1557. m_log.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD {0}", Name);
  1558. }
  1559. lock (m_meshlock)
  1560. {
  1561. if (m_mesh != null)
  1562. {
  1563. _parent_scene.mesher.ReleaseMesh(m_mesh);
  1564. m_mesh = null;
  1565. }
  1566. }
  1567. Body = IntPtr.Zero;
  1568. m_hasOBB = false;
  1569. }
  1570. //sets non physical prim m_targetSpace to right space in spaces grid for static prims
  1571. // should only be called for non physical prims unless they are becoming non physical
  1572. private void SetInStaticSpace(OdePrim prim)
  1573. {
  1574. IntPtr targetSpace = _parent_scene.MoveGeomToStaticSpace(prim.prim_geom, prim.m_targetSpace);
  1575. prim.m_targetSpace = targetSpace;
  1576. collide_geom = IntPtr.Zero;
  1577. }
  1578. public void enableBodySoft()
  1579. {
  1580. m_disabled = false;
  1581. if (!childPrim && !m_isSelected)
  1582. {
  1583. if (m_isphysical && Body != IntPtr.Zero)
  1584. {
  1585. UpdateCollisionCatFlags();
  1586. ApplyCollisionCatFlags();
  1587. _zeroFlag = true;
  1588. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  1589. SafeNativeMethods.BodyEnable(Body);
  1590. }
  1591. }
  1592. resetCollisionAccounting();
  1593. }
  1594. private void disableBodySoft()
  1595. {
  1596. m_disabled = true;
  1597. if (!childPrim)
  1598. {
  1599. if (m_isphysical && Body != IntPtr.Zero)
  1600. {
  1601. if (m_isSelected)
  1602. m_collisionFlags = CollisionCategories.Selected;
  1603. else
  1604. m_collisionCategories = 0;
  1605. m_collisionFlags = 0;
  1606. ApplyCollisionCatFlags();
  1607. SafeNativeMethods.BodyDisable(Body);
  1608. }
  1609. }
  1610. }
  1611. private void MakeBody()
  1612. {
  1613. if (!m_isphysical) // only physical get bodies
  1614. return;
  1615. if (childPrim) // child prims don't get bodies;
  1616. return;
  1617. if (m_building)
  1618. return;
  1619. if (prim_geom == IntPtr.Zero)
  1620. {
  1621. m_log.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
  1622. return;
  1623. }
  1624. if (Body != IntPtr.Zero)
  1625. {
  1626. DestroyBody();
  1627. m_log.Warn("[PHYSICS]: MakeBody called having a body");
  1628. }
  1629. if (SafeNativeMethods.GeomGetBody(prim_geom) != IntPtr.Zero)
  1630. {
  1631. SafeNativeMethods.GeomSetBody(prim_geom, IntPtr.Zero);
  1632. m_log.Warn("[PHYSICS]: MakeBody root geom already had a body");
  1633. }
  1634. bool noInertiaOverride = (m_InertiaOverride == null);
  1635. Body = SafeNativeMethods.BodyCreate(_parent_scene.world);
  1636. SafeNativeMethods.Matrix3 mymat = new SafeNativeMethods.Matrix3();
  1637. SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
  1638. SafeNativeMethods.Mass objdmass = new SafeNativeMethods.Mass { };
  1639. myrot.X = _orientation.X;
  1640. myrot.Y = _orientation.Y;
  1641. myrot.Z = _orientation.Z;
  1642. myrot.W = _orientation.W;
  1643. SafeNativeMethods.RfromQ(out mymat, ref myrot);
  1644. // set the body rotation
  1645. SafeNativeMethods.BodySetRotation(Body, ref mymat);
  1646. if(noInertiaOverride)
  1647. {
  1648. objdmass = primdMass;
  1649. SafeNativeMethods.MassRotate(ref objdmass, ref mymat);
  1650. }
  1651. // recompute full object inertia if needed
  1652. if (childrenPrim.Count > 0)
  1653. {
  1654. SafeNativeMethods.Matrix3 mat = new SafeNativeMethods.Matrix3();
  1655. SafeNativeMethods.Quaternion quat = new SafeNativeMethods.Quaternion();
  1656. SafeNativeMethods.Mass tmpdmass = new SafeNativeMethods.Mass { };
  1657. Vector3 rcm;
  1658. rcm.X = _position.X;
  1659. rcm.Y = _position.Y;
  1660. rcm.Z = _position.Z;
  1661. lock (childrenPrim)
  1662. {
  1663. foreach (OdePrim prm in childrenPrim)
  1664. {
  1665. if (prm.prim_geom == IntPtr.Zero)
  1666. {
  1667. m_log.Warn("[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet");
  1668. continue;
  1669. }
  1670. quat.X = prm._orientation.X;
  1671. quat.Y = prm._orientation.Y;
  1672. quat.Z = prm._orientation.Z;
  1673. quat.W = prm._orientation.W;
  1674. SafeNativeMethods.RfromQ(out mat, ref quat);
  1675. // fix prim colision cats
  1676. if (SafeNativeMethods.GeomGetBody(prm.prim_geom) != IntPtr.Zero)
  1677. {
  1678. SafeNativeMethods.GeomSetBody(prm.prim_geom, IntPtr.Zero);
  1679. m_log.Warn("[PHYSICS]: MakeBody child geom already had a body");
  1680. }
  1681. SafeNativeMethods.GeomClearOffset(prm.prim_geom);
  1682. SafeNativeMethods.GeomSetBody(prm.prim_geom, Body);
  1683. prm.Body = Body;
  1684. SafeNativeMethods.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
  1685. if(noInertiaOverride)
  1686. {
  1687. tmpdmass = prm.primdMass;
  1688. SafeNativeMethods.MassRotate(ref tmpdmass, ref mat);
  1689. Vector3 ppos = prm._position;
  1690. ppos.X -= rcm.X;
  1691. ppos.Y -= rcm.Y;
  1692. ppos.Z -= rcm.Z;
  1693. // refer inertia to root prim center of mass position
  1694. SafeNativeMethods.MassTranslate(ref tmpdmass,
  1695. ppos.X,
  1696. ppos.Y,
  1697. ppos.Z);
  1698. SafeNativeMethods.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
  1699. }
  1700. }
  1701. }
  1702. }
  1703. SafeNativeMethods.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
  1704. // associate root geom with body
  1705. SafeNativeMethods.GeomSetBody(prim_geom, Body);
  1706. if(noInertiaOverride)
  1707. SafeNativeMethods.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
  1708. else
  1709. {
  1710. Vector3 ncm = m_InertiaOverride.CenterOfMass * _orientation;
  1711. SafeNativeMethods.BodySetPosition(Body,
  1712. _position.X + ncm.X,
  1713. _position.Y + ncm.Y,
  1714. _position.Z + ncm.Z);
  1715. }
  1716. SafeNativeMethods.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
  1717. if(noInertiaOverride)
  1718. {
  1719. SafeNativeMethods.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
  1720. myrot.X = -myrot.X;
  1721. myrot.Y = -myrot.Y;
  1722. myrot.Z = -myrot.Z;
  1723. SafeNativeMethods.RfromQ(out mymat, ref myrot);
  1724. SafeNativeMethods.MassRotate(ref objdmass, ref mymat);
  1725. SafeNativeMethods.BodySetMass(Body, ref objdmass);
  1726. m_mass = objdmass.mass;
  1727. }
  1728. else
  1729. {
  1730. objdmass.c.X = 0;
  1731. objdmass.c.Y = 0;
  1732. objdmass.c.Z = 0;
  1733. objdmass.I.M00 = m_InertiaOverride.Inertia.X;
  1734. objdmass.I.M11 = m_InertiaOverride.Inertia.Y;
  1735. objdmass.I.M22 = m_InertiaOverride.Inertia.Z;
  1736. objdmass.mass = m_InertiaOverride.TotalMass;
  1737. if(Math.Abs(m_InertiaOverride.InertiaRotation.W) < 0.999)
  1738. {
  1739. SafeNativeMethods.Matrix3 inertiarotmat = new SafeNativeMethods.Matrix3();
  1740. SafeNativeMethods.Quaternion inertiarot = new SafeNativeMethods.Quaternion();
  1741. inertiarot.X = m_InertiaOverride.InertiaRotation.X;
  1742. inertiarot.Y = m_InertiaOverride.InertiaRotation.Y;
  1743. inertiarot.Z = m_InertiaOverride.InertiaRotation.Z;
  1744. inertiarot.W = m_InertiaOverride.InertiaRotation.W;
  1745. SafeNativeMethods.RfromQ(out inertiarotmat, ref inertiarot);
  1746. SafeNativeMethods.MassRotate(ref objdmass, ref inertiarotmat);
  1747. }
  1748. SafeNativeMethods.BodySetMass(Body, ref objdmass);
  1749. m_mass = objdmass.mass;
  1750. }
  1751. // disconnect from world gravity so we can apply buoyancy
  1752. SafeNativeMethods.BodySetGravityMode(Body, false);
  1753. SafeNativeMethods.BodySetAutoDisableFlag(Body, true);
  1754. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  1755. SafeNativeMethods.BodySetAutoDisableAngularThreshold(Body, 0.05f);
  1756. SafeNativeMethods.BodySetAutoDisableLinearThreshold(Body, 0.05f);
  1757. SafeNativeMethods.BodySetDamping(Body, .004f, .001f);
  1758. if (m_targetSpace != IntPtr.Zero)
  1759. {
  1760. _parent_scene.waitForSpaceUnlock(m_targetSpace);
  1761. if (SafeNativeMethods.SpaceQuery(m_targetSpace, prim_geom))
  1762. SafeNativeMethods.SpaceRemove(m_targetSpace, prim_geom);
  1763. }
  1764. if (childrenPrim.Count == 0)
  1765. {
  1766. collide_geom = prim_geom;
  1767. m_targetSpace = _parent_scene.ActiveSpace;
  1768. }
  1769. else
  1770. {
  1771. m_targetSpace = SafeNativeMethods.SimpleSpaceCreate(_parent_scene.ActiveSpace);
  1772. SafeNativeMethods.SpaceSetSublevel(m_targetSpace, 3);
  1773. SafeNativeMethods.SpaceSetCleanup(m_targetSpace, false);
  1774. SafeNativeMethods.GeomSetCategoryBits(m_targetSpace, (uint)(CollisionCategories.Space |
  1775. CollisionCategories.Geom |
  1776. CollisionCategories.Phantom |
  1777. CollisionCategories.VolumeDtc
  1778. ));
  1779. SafeNativeMethods.GeomSetCollideBits(m_targetSpace, 0);
  1780. collide_geom = m_targetSpace;
  1781. }
  1782. if (SafeNativeMethods.SpaceQuery(m_targetSpace, prim_geom))
  1783. m_log.Debug("[PRIM]: parent already in target space");
  1784. else
  1785. SafeNativeMethods.SpaceAdd(m_targetSpace, prim_geom);
  1786. if (m_delaySelect)
  1787. {
  1788. m_isSelected = true;
  1789. m_delaySelect = false;
  1790. }
  1791. m_collisionscore = 0;
  1792. UpdateCollisionCatFlags();
  1793. ApplyCollisionCatFlags();
  1794. _parent_scene.addActivePrim(this);
  1795. lock (childrenPrim)
  1796. {
  1797. foreach (OdePrim prm in childrenPrim)
  1798. {
  1799. IntPtr prmgeom = prm.prim_geom;
  1800. if (prmgeom == IntPtr.Zero)
  1801. continue;
  1802. Vector3 ppos = prm._position;
  1803. SafeNativeMethods.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
  1804. IntPtr prmspace = prm.m_targetSpace;
  1805. if (prmspace != m_targetSpace)
  1806. {
  1807. if (prmspace != IntPtr.Zero)
  1808. {
  1809. _parent_scene.waitForSpaceUnlock(prmspace);
  1810. if (SafeNativeMethods.SpaceQuery(prmspace, prmgeom))
  1811. SafeNativeMethods.SpaceRemove(prmspace, prmgeom);
  1812. }
  1813. prm.m_targetSpace = m_targetSpace;
  1814. if (SafeNativeMethods.SpaceQuery(m_targetSpace, prmgeom))
  1815. m_log.Debug("[PRIM]: child already in target space");
  1816. else
  1817. SafeNativeMethods.SpaceAdd(m_targetSpace, prmgeom);
  1818. }
  1819. prm.m_collisionscore = 0;
  1820. if(!m_disabled)
  1821. prm.m_disabled = false;
  1822. _parent_scene.addActivePrim(prm);
  1823. }
  1824. }
  1825. // The body doesn't already have a finite rotation mode set here
  1826. if (m_angularlocks != 0 && _parent == null)
  1827. {
  1828. createAMotor(m_angularlocks);
  1829. }
  1830. if (m_isSelected || m_disabled)
  1831. {
  1832. SafeNativeMethods.BodyDisable(Body);
  1833. _zeroFlag = true;
  1834. }
  1835. else
  1836. {
  1837. SafeNativeMethods.BodySetAngularVel(Body, m_rotationalVelocity.X, m_rotationalVelocity.Y, m_rotationalVelocity.Z);
  1838. SafeNativeMethods.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
  1839. _zeroFlag = false;
  1840. m_bodydisablecontrol = 0;
  1841. }
  1842. _parent_scene.addActiveGroups(this);
  1843. }
  1844. private void DestroyBody()
  1845. {
  1846. if (Body != IntPtr.Zero)
  1847. {
  1848. _parent_scene.remActivePrim(this);
  1849. collide_geom = IntPtr.Zero;
  1850. if (m_disabled)
  1851. m_collisionCategories = 0;
  1852. else if (m_isSelected)
  1853. m_collisionCategories = CollisionCategories.Selected;
  1854. else if (m_isVolumeDetect)
  1855. m_collisionCategories = CollisionCategories.VolumeDtc;
  1856. else if (m_isphantom)
  1857. m_collisionCategories = CollisionCategories.Phantom;
  1858. else
  1859. m_collisionCategories = CollisionCategories.Geom;
  1860. m_collisionFlags = 0;
  1861. if (prim_geom != IntPtr.Zero)
  1862. {
  1863. if (m_NoColide)
  1864. {
  1865. SafeNativeMethods.GeomSetCategoryBits(prim_geom, 0);
  1866. SafeNativeMethods.GeomSetCollideBits(prim_geom, 0);
  1867. }
  1868. else
  1869. {
  1870. SafeNativeMethods.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
  1871. SafeNativeMethods.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
  1872. }
  1873. UpdateDataFromGeom();
  1874. SafeNativeMethods.GeomSetBody(prim_geom, IntPtr.Zero);
  1875. SetInStaticSpace(this);
  1876. }
  1877. if (!childPrim)
  1878. {
  1879. lock (childrenPrim)
  1880. {
  1881. foreach (OdePrim prm in childrenPrim)
  1882. {
  1883. _parent_scene.remActivePrim(prm);
  1884. if (prm.m_isSelected)
  1885. prm.m_collisionCategories = CollisionCategories.Selected;
  1886. else if (prm.m_isVolumeDetect)
  1887. prm.m_collisionCategories = CollisionCategories.VolumeDtc;
  1888. else if (prm.m_isphantom)
  1889. prm.m_collisionCategories = CollisionCategories.Phantom;
  1890. else
  1891. prm.m_collisionCategories = CollisionCategories.Geom;
  1892. prm.m_collisionFlags = 0;
  1893. if (prm.prim_geom != IntPtr.Zero)
  1894. {
  1895. if (prm.m_NoColide)
  1896. {
  1897. SafeNativeMethods.GeomSetCategoryBits(prm.prim_geom, 0);
  1898. SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, 0);
  1899. }
  1900. else
  1901. {
  1902. SafeNativeMethods.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
  1903. SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
  1904. }
  1905. prm.UpdateDataFromGeom();
  1906. SetInStaticSpace(prm);
  1907. }
  1908. prm.Body = IntPtr.Zero;
  1909. prm.m_mass = prm.primMass;
  1910. prm.m_collisionscore = 0;
  1911. }
  1912. }
  1913. if (Amotor != IntPtr.Zero)
  1914. {
  1915. SafeNativeMethods.JointDestroy(Amotor);
  1916. Amotor = IntPtr.Zero;
  1917. }
  1918. _parent_scene.remActiveGroup(this);
  1919. SafeNativeMethods.BodyDestroy(Body);
  1920. }
  1921. Body = IntPtr.Zero;
  1922. }
  1923. m_mass = primMass;
  1924. m_collisionscore = 0;
  1925. }
  1926. private void FixInertia(Vector3 NewPos,Quaternion newrot)
  1927. {
  1928. SafeNativeMethods.Matrix3 mat = new SafeNativeMethods.Matrix3();
  1929. SafeNativeMethods.Quaternion quat = new SafeNativeMethods.Quaternion();
  1930. SafeNativeMethods.Mass tmpdmass = new SafeNativeMethods.Mass { };
  1931. SafeNativeMethods.Mass objdmass = new SafeNativeMethods.Mass { };
  1932. SafeNativeMethods.BodyGetMass(Body, out tmpdmass);
  1933. objdmass = tmpdmass;
  1934. SafeNativeMethods.Vector3 dobjpos;
  1935. SafeNativeMethods.Vector3 thispos;
  1936. // get current object position and rotation
  1937. dobjpos = SafeNativeMethods.BodyGetPosition(Body);
  1938. // get prim own inertia in its local frame
  1939. tmpdmass = primdMass;
  1940. // transform to object frame
  1941. mat = SafeNativeMethods.GeomGetOffsetRotation(prim_geom);
  1942. SafeNativeMethods.MassRotate(ref tmpdmass, ref mat);
  1943. thispos = SafeNativeMethods.GeomGetOffsetPosition(prim_geom);
  1944. SafeNativeMethods.MassTranslate(ref tmpdmass,
  1945. thispos.X,
  1946. thispos.Y,
  1947. thispos.Z);
  1948. // subtract current prim inertia from object
  1949. DMassSubPartFromObj(ref tmpdmass, ref objdmass);
  1950. // back prim own inertia
  1951. tmpdmass = primdMass;
  1952. // update to new position and orientation
  1953. _position = NewPos;
  1954. SafeNativeMethods.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
  1955. _orientation = newrot;
  1956. quat.X = newrot.X;
  1957. quat.Y = newrot.Y;
  1958. quat.Z = newrot.Z;
  1959. quat.W = newrot.W;
  1960. SafeNativeMethods.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
  1961. mat = SafeNativeMethods.GeomGetOffsetRotation(prim_geom);
  1962. SafeNativeMethods.MassRotate(ref tmpdmass, ref mat);
  1963. thispos = SafeNativeMethods.GeomGetOffsetPosition(prim_geom);
  1964. SafeNativeMethods.MassTranslate(ref tmpdmass,
  1965. thispos.X,
  1966. thispos.Y,
  1967. thispos.Z);
  1968. SafeNativeMethods.MassAdd(ref objdmass, ref tmpdmass);
  1969. // fix all positions
  1970. IntPtr g = SafeNativeMethods.BodyGetFirstGeom(Body);
  1971. while (g != IntPtr.Zero)
  1972. {
  1973. thispos = SafeNativeMethods.GeomGetOffsetPosition(g);
  1974. thispos.X -= objdmass.c.X;
  1975. thispos.Y -= objdmass.c.Y;
  1976. thispos.Z -= objdmass.c.Z;
  1977. SafeNativeMethods.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
  1978. g = SafeNativeMethods.dBodyGetNextGeom(g);
  1979. }
  1980. SafeNativeMethods.BodyVectorToWorld(Body,objdmass.c.X, objdmass.c.Y, objdmass.c.Z,out thispos);
  1981. SafeNativeMethods.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
  1982. SafeNativeMethods.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
  1983. SafeNativeMethods.BodySetMass(Body, ref objdmass);
  1984. m_mass = objdmass.mass;
  1985. }
  1986. private void FixInertia(Vector3 NewPos)
  1987. {
  1988. SafeNativeMethods.Matrix3 primmat = new SafeNativeMethods.Matrix3();
  1989. SafeNativeMethods.Mass tmpdmass = new SafeNativeMethods.Mass { };
  1990. SafeNativeMethods.Mass objdmass = new SafeNativeMethods.Mass { };
  1991. SafeNativeMethods.Mass primmass = new SafeNativeMethods.Mass { };
  1992. SafeNativeMethods.Vector3 dobjpos;
  1993. SafeNativeMethods.Vector3 thispos;
  1994. SafeNativeMethods.BodyGetMass(Body, out objdmass);
  1995. // get prim own inertia in its local frame
  1996. primmass = primdMass;
  1997. // transform to object frame
  1998. primmat = SafeNativeMethods.GeomGetOffsetRotation(prim_geom);
  1999. SafeNativeMethods.MassRotate(ref primmass, ref primmat);
  2000. tmpdmass = primmass;
  2001. thispos = SafeNativeMethods.GeomGetOffsetPosition(prim_geom);
  2002. SafeNativeMethods.MassTranslate(ref tmpdmass,
  2003. thispos.X,
  2004. thispos.Y,
  2005. thispos.Z);
  2006. // subtract current prim inertia from object
  2007. DMassSubPartFromObj(ref tmpdmass, ref objdmass);
  2008. // update to new position
  2009. _position = NewPos;
  2010. SafeNativeMethods.GeomSetOffsetWorldPosition(prim_geom, NewPos.X, NewPos.Y, NewPos.Z);
  2011. thispos = SafeNativeMethods.GeomGetOffsetPosition(prim_geom);
  2012. SafeNativeMethods.MassTranslate(ref primmass,
  2013. thispos.X,
  2014. thispos.Y,
  2015. thispos.Z);
  2016. SafeNativeMethods.MassAdd(ref objdmass, ref primmass);
  2017. // fix all positions
  2018. IntPtr g = SafeNativeMethods.BodyGetFirstGeom(Body);
  2019. while (g != IntPtr.Zero)
  2020. {
  2021. thispos = SafeNativeMethods.GeomGetOffsetPosition(g);
  2022. thispos.X -= objdmass.c.X;
  2023. thispos.Y -= objdmass.c.Y;
  2024. thispos.Z -= objdmass.c.Z;
  2025. SafeNativeMethods.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
  2026. g = SafeNativeMethods.dBodyGetNextGeom(g);
  2027. }
  2028. SafeNativeMethods.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
  2029. // get current object position and rotation
  2030. dobjpos = SafeNativeMethods.BodyGetPosition(Body);
  2031. SafeNativeMethods.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
  2032. SafeNativeMethods.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
  2033. SafeNativeMethods.BodySetMass(Body, ref objdmass);
  2034. m_mass = objdmass.mass;
  2035. }
  2036. private void FixInertia(Quaternion newrot)
  2037. {
  2038. SafeNativeMethods.Matrix3 mat = new SafeNativeMethods.Matrix3();
  2039. SafeNativeMethods.Quaternion quat = new SafeNativeMethods.Quaternion();
  2040. SafeNativeMethods.Mass tmpdmass = new SafeNativeMethods.Mass { };
  2041. SafeNativeMethods.Mass objdmass = new SafeNativeMethods.Mass { };
  2042. SafeNativeMethods.Vector3 dobjpos;
  2043. SafeNativeMethods.Vector3 thispos;
  2044. SafeNativeMethods.BodyGetMass(Body, out objdmass);
  2045. // get prim own inertia in its local frame
  2046. tmpdmass = primdMass;
  2047. mat = SafeNativeMethods.GeomGetOffsetRotation(prim_geom);
  2048. SafeNativeMethods.MassRotate(ref tmpdmass, ref mat);
  2049. // transform to object frame
  2050. thispos = SafeNativeMethods.GeomGetOffsetPosition(prim_geom);
  2051. SafeNativeMethods.MassTranslate(ref tmpdmass,
  2052. thispos.X,
  2053. thispos.Y,
  2054. thispos.Z);
  2055. // subtract current prim inertia from object
  2056. DMassSubPartFromObj(ref tmpdmass, ref objdmass);
  2057. // update to new orientation
  2058. _orientation = newrot;
  2059. quat.X = newrot.X;
  2060. quat.Y = newrot.Y;
  2061. quat.Z = newrot.Z;
  2062. quat.W = newrot.W;
  2063. SafeNativeMethods.GeomSetOffsetWorldQuaternion(prim_geom, ref quat);
  2064. tmpdmass = primdMass;
  2065. mat = SafeNativeMethods.GeomGetOffsetRotation(prim_geom);
  2066. SafeNativeMethods.MassRotate(ref tmpdmass, ref mat);
  2067. SafeNativeMethods.MassTranslate(ref tmpdmass,
  2068. thispos.X,
  2069. thispos.Y,
  2070. thispos.Z);
  2071. SafeNativeMethods.MassAdd(ref objdmass, ref tmpdmass);
  2072. // fix all positions
  2073. IntPtr g = SafeNativeMethods.BodyGetFirstGeom(Body);
  2074. while (g != IntPtr.Zero)
  2075. {
  2076. thispos = SafeNativeMethods.GeomGetOffsetPosition(g);
  2077. thispos.X -= objdmass.c.X;
  2078. thispos.Y -= objdmass.c.Y;
  2079. thispos.Z -= objdmass.c.Z;
  2080. SafeNativeMethods.GeomSetOffsetPosition(g, thispos.X, thispos.Y, thispos.Z);
  2081. g = SafeNativeMethods.dBodyGetNextGeom(g);
  2082. }
  2083. SafeNativeMethods.BodyVectorToWorld(Body, objdmass.c.X, objdmass.c.Y, objdmass.c.Z, out thispos);
  2084. // get current object position and rotation
  2085. dobjpos = SafeNativeMethods.BodyGetPosition(Body);
  2086. SafeNativeMethods.BodySetPosition(Body, dobjpos.X + thispos.X, dobjpos.Y + thispos.Y, dobjpos.Z + thispos.Z);
  2087. SafeNativeMethods.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z); // ode wants inertia at center of body
  2088. SafeNativeMethods.BodySetMass(Body, ref objdmass);
  2089. m_mass = objdmass.mass;
  2090. }
  2091. #region Mass Calculation
  2092. private void UpdatePrimBodyData()
  2093. {
  2094. primMass = m_density * primVolume;
  2095. if (primMass <= 0)
  2096. primMass = 0.0001f;//ckrinke: Mass must be greater then zero.
  2097. if (primMass > _parent_scene.maximumMassObject)
  2098. primMass = _parent_scene.maximumMassObject;
  2099. m_mass = primMass; // just in case
  2100. SafeNativeMethods.MassSetBoxTotal(out primdMass, primMass, 2.0f * m_OBB.X, 2.0f * m_OBB.Y, 2.0f * m_OBB.Z);
  2101. SafeNativeMethods.MassTranslate(ref primdMass,
  2102. m_OBBOffset.X,
  2103. m_OBBOffset.Y,
  2104. m_OBBOffset.Z);
  2105. primOOBradiusSQ = m_OBB.LengthSquared();
  2106. if (_triMeshData != IntPtr.Zero)
  2107. {
  2108. float pc = m_physCost;
  2109. float psf = primOOBradiusSQ;
  2110. psf *= 1.33f * .2f;
  2111. pc *= psf;
  2112. if (pc < 0.1f)
  2113. pc = 0.1f;
  2114. m_physCost = pc;
  2115. }
  2116. else
  2117. m_physCost = 0.1f;
  2118. m_streamCost = 1.0f;
  2119. }
  2120. #endregion
  2121. /// <summary>
  2122. /// Add a child prim to this parent prim.
  2123. /// </summary>
  2124. /// <param name="prim">Child prim</param>
  2125. // I'm the parent
  2126. // prim is the child
  2127. public void ParentPrim(OdePrim prim)
  2128. {
  2129. //Console.WriteLine("ParentPrim " + m_primName);
  2130. if (this.m_localID != prim.m_localID)
  2131. {
  2132. DestroyBody(); // for now we need to rebuil entire object on link change
  2133. lock (childrenPrim)
  2134. {
  2135. // adopt the prim
  2136. if (!childrenPrim.Contains(prim))
  2137. childrenPrim.Add(prim);
  2138. // see if this prim has kids and adopt them also
  2139. // should not happen for now
  2140. foreach (OdePrim prm in prim.childrenPrim)
  2141. {
  2142. if (!childrenPrim.Contains(prm))
  2143. {
  2144. if (prm.Body != IntPtr.Zero)
  2145. {
  2146. if (prm.prim_geom != IntPtr.Zero)
  2147. SafeNativeMethods.GeomSetBody(prm.prim_geom, IntPtr.Zero);
  2148. if (prm.Body != prim.Body)
  2149. prm.DestroyBody(); // don't loose bodies around
  2150. prm.Body = IntPtr.Zero;
  2151. }
  2152. childrenPrim.Add(prm);
  2153. prm._parent = this;
  2154. }
  2155. }
  2156. }
  2157. //Remove old children from the prim
  2158. prim.childrenPrim.Clear();
  2159. if (prim.Body != IntPtr.Zero)
  2160. {
  2161. if (prim.prim_geom != IntPtr.Zero)
  2162. SafeNativeMethods.GeomSetBody(prim.prim_geom, IntPtr.Zero);
  2163. prim.DestroyBody(); // don't loose bodies around
  2164. prim.Body = IntPtr.Zero;
  2165. }
  2166. prim.childPrim = true;
  2167. prim._parent = this;
  2168. MakeBody(); // full nasty reconstruction
  2169. }
  2170. }
  2171. private void UpdateChildsfromgeom()
  2172. {
  2173. if (childrenPrim.Count > 0)
  2174. {
  2175. foreach (OdePrim prm in childrenPrim)
  2176. prm.UpdateDataFromGeom();
  2177. }
  2178. }
  2179. private void UpdateDataFromGeom()
  2180. {
  2181. if (prim_geom != IntPtr.Zero)
  2182. {
  2183. SafeNativeMethods.Quaternion qtmp;
  2184. SafeNativeMethods.GeomCopyQuaternion(prim_geom, out qtmp);
  2185. _orientation.X = qtmp.X;
  2186. _orientation.Y = qtmp.Y;
  2187. _orientation.Z = qtmp.Z;
  2188. _orientation.W = qtmp.W;
  2189. /*
  2190. // Debug
  2191. float qlen = _orientation.Length();
  2192. if (qlen > 1.01f || qlen < 0.99)
  2193. m_log.WarnFormat("[PHYSICS]: Got nonnorm quaternion from geom in Object {0} norm {1}", Name, qlen);
  2194. //
  2195. */
  2196. _orientation.Normalize();
  2197. SafeNativeMethods.Vector3 lpos = SafeNativeMethods.GeomGetPosition(prim_geom);
  2198. _position.X = lpos.X;
  2199. _position.Y = lpos.Y;
  2200. _position.Z = lpos.Z;
  2201. }
  2202. }
  2203. private void ChildDelink(OdePrim odePrim, bool remakebodies)
  2204. {
  2205. // Okay, we have a delinked child.. destroy all body and remake
  2206. if (odePrim != this && !childrenPrim.Contains(odePrim))
  2207. return;
  2208. DestroyBody();
  2209. if (odePrim == this) // delinking the root prim
  2210. {
  2211. OdePrim newroot = null;
  2212. lock (childrenPrim)
  2213. {
  2214. if (childrenPrim.Count > 0)
  2215. {
  2216. newroot = childrenPrim[0];
  2217. childrenPrim.RemoveAt(0);
  2218. foreach (OdePrim prm in childrenPrim)
  2219. {
  2220. newroot.childrenPrim.Add(prm);
  2221. }
  2222. childrenPrim.Clear();
  2223. }
  2224. if (newroot != null)
  2225. {
  2226. newroot.childPrim = false;
  2227. newroot._parent = null;
  2228. if (remakebodies)
  2229. newroot.MakeBody();
  2230. }
  2231. }
  2232. }
  2233. else
  2234. {
  2235. lock (childrenPrim)
  2236. {
  2237. childrenPrim.Remove(odePrim);
  2238. odePrim.childPrim = false;
  2239. odePrim._parent = null;
  2240. // odePrim.UpdateDataFromGeom();
  2241. if (remakebodies)
  2242. odePrim.MakeBody();
  2243. }
  2244. }
  2245. if (remakebodies)
  2246. MakeBody();
  2247. }
  2248. protected void ChildRemove(OdePrim odePrim, bool reMakeBody)
  2249. {
  2250. // Okay, we have a delinked child.. destroy all body and remake
  2251. if (odePrim != this && !childrenPrim.Contains(odePrim))
  2252. return;
  2253. DestroyBody();
  2254. if (odePrim == this)
  2255. {
  2256. OdePrim newroot = null;
  2257. lock (childrenPrim)
  2258. {
  2259. if (childrenPrim.Count > 0)
  2260. {
  2261. newroot = childrenPrim[0];
  2262. childrenPrim.RemoveAt(0);
  2263. foreach (OdePrim prm in childrenPrim)
  2264. {
  2265. newroot.childrenPrim.Add(prm);
  2266. }
  2267. childrenPrim.Clear();
  2268. }
  2269. if (newroot != null)
  2270. {
  2271. newroot.childPrim = false;
  2272. newroot._parent = null;
  2273. newroot.MakeBody();
  2274. }
  2275. }
  2276. if (reMakeBody)
  2277. MakeBody();
  2278. return;
  2279. }
  2280. else
  2281. {
  2282. lock (childrenPrim)
  2283. {
  2284. childrenPrim.Remove(odePrim);
  2285. odePrim.childPrim = false;
  2286. odePrim._parent = null;
  2287. if (reMakeBody)
  2288. odePrim.MakeBody();
  2289. }
  2290. }
  2291. MakeBody();
  2292. }
  2293. #region changes
  2294. private void changeadd()
  2295. {
  2296. _parent_scene.addToPrims(this);
  2297. }
  2298. private void changeAngularLock(byte newLocks)
  2299. {
  2300. // do we have a Physical object?
  2301. if (Body != IntPtr.Zero)
  2302. {
  2303. //Check that we have a Parent
  2304. //If we have a parent then we're not authorative here
  2305. if (_parent == null)
  2306. {
  2307. if (newLocks != 0)
  2308. {
  2309. createAMotor(newLocks);
  2310. }
  2311. else
  2312. {
  2313. if (Amotor != IntPtr.Zero)
  2314. {
  2315. SafeNativeMethods.JointDestroy(Amotor);
  2316. Amotor = IntPtr.Zero;
  2317. }
  2318. }
  2319. }
  2320. }
  2321. // Store this for later in case we get turned into a separate body
  2322. m_angularlocks = newLocks;
  2323. }
  2324. private void changeLink(OdePrim NewParent)
  2325. {
  2326. if (_parent == null && NewParent != null)
  2327. {
  2328. NewParent.ParentPrim(this);
  2329. }
  2330. else if (_parent != null)
  2331. {
  2332. if (_parent is OdePrim)
  2333. {
  2334. if (NewParent != _parent)
  2335. {
  2336. (_parent as OdePrim).ChildDelink(this, false); // for now...
  2337. childPrim = false;
  2338. if (NewParent != null)
  2339. {
  2340. NewParent.ParentPrim(this);
  2341. }
  2342. }
  2343. }
  2344. }
  2345. _parent = NewParent;
  2346. }
  2347. private void Stop()
  2348. {
  2349. if (!childPrim)
  2350. {
  2351. // m_force = Vector3.Zero;
  2352. m_forceacc = Vector3.Zero;
  2353. m_angularForceacc = Vector3.Zero;
  2354. // m_torque = Vector3.Zero;
  2355. _velocity = Vector3.Zero;
  2356. m_acceleration = Vector3.Zero;
  2357. m_rotationalVelocity = Vector3.Zero;
  2358. _target_velocity = Vector3.Zero;
  2359. if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
  2360. m_vehicle.Stop();
  2361. _zeroFlag = false;
  2362. base.RequestPhysicsterseUpdate();
  2363. }
  2364. if (Body != IntPtr.Zero)
  2365. {
  2366. SafeNativeMethods.BodySetForce(Body, 0f, 0f, 0f);
  2367. SafeNativeMethods.BodySetTorque(Body, 0f, 0f, 0f);
  2368. SafeNativeMethods.BodySetLinearVel(Body, 0f, 0f, 0f);
  2369. SafeNativeMethods.BodySetAngularVel(Body, 0f, 0f, 0f);
  2370. }
  2371. }
  2372. private void changePhantomStatus(bool newval)
  2373. {
  2374. m_isphantom = newval;
  2375. UpdateCollisionCatFlags();
  2376. ApplyCollisionCatFlags();
  2377. }
  2378. /* not in use
  2379. internal void ChildSelectedChange(bool childSelect)
  2380. {
  2381. if(childPrim)
  2382. return;
  2383. if (childSelect == m_isSelected)
  2384. return;
  2385. if (childSelect)
  2386. {
  2387. DoSelectedStatus(true);
  2388. }
  2389. else
  2390. {
  2391. foreach (OdePrim prm in childrenPrim)
  2392. {
  2393. if (prm.m_isSelected)
  2394. return;
  2395. }
  2396. DoSelectedStatus(false);
  2397. }
  2398. }
  2399. */
  2400. private void changeSelectedStatus(bool newval)
  2401. {
  2402. if (m_lastdoneSelected == newval)
  2403. return;
  2404. m_lastdoneSelected = newval;
  2405. DoSelectedStatus(newval);
  2406. }
  2407. private void CheckDelaySelect()
  2408. {
  2409. if (m_delaySelect)
  2410. {
  2411. DoSelectedStatus(m_isSelected);
  2412. }
  2413. }
  2414. private void DoSelectedStatus(bool newval)
  2415. {
  2416. m_isSelected = newval;
  2417. Stop();
  2418. if (newval)
  2419. {
  2420. if (!childPrim && Body != IntPtr.Zero)
  2421. SafeNativeMethods.BodyDisable(Body);
  2422. if (m_delaySelect || m_isphysical)
  2423. {
  2424. m_collisionCategories = CollisionCategories.Selected;
  2425. m_collisionFlags = 0;
  2426. if (!childPrim)
  2427. {
  2428. foreach (OdePrim prm in childrenPrim)
  2429. {
  2430. prm.m_collisionCategories = m_collisionCategories;
  2431. prm.m_collisionFlags = m_collisionFlags;
  2432. if (prm.prim_geom != IntPtr.Zero)
  2433. {
  2434. if (prm.m_NoColide)
  2435. {
  2436. SafeNativeMethods.GeomSetCategoryBits(prm.prim_geom, 0);
  2437. SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, 0);
  2438. }
  2439. else
  2440. {
  2441. SafeNativeMethods.GeomSetCategoryBits(prm.prim_geom, (uint)m_collisionCategories);
  2442. SafeNativeMethods.GeomSetCollideBits(prm.prim_geom, (uint)m_collisionFlags);
  2443. }
  2444. }
  2445. prm.m_delaySelect = false;
  2446. }
  2447. }
  2448. // else if (_parent != null)
  2449. // ((OdePrim)_parent).ChildSelectedChange(true);
  2450. if (prim_geom != IntPtr.Zero)
  2451. {
  2452. if (m_NoColide)
  2453. {
  2454. SafeNativeMethods.GeomSetCategoryBits(prim_geom, 0);
  2455. SafeNativeMethods.GeomSetCollideBits(prim_geom, 0);
  2456. if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
  2457. {
  2458. SafeNativeMethods.GeomSetCategoryBits(collide_geom, 0);
  2459. SafeNativeMethods.GeomSetCollideBits(collide_geom, 0);
  2460. }
  2461. }
  2462. else
  2463. {
  2464. SafeNativeMethods.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
  2465. SafeNativeMethods.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
  2466. if (collide_geom != prim_geom && collide_geom != IntPtr.Zero)
  2467. {
  2468. SafeNativeMethods.GeomSetCategoryBits(collide_geom, (uint)m_collisionCategories);
  2469. SafeNativeMethods.GeomSetCollideBits(collide_geom, (uint)m_collisionFlags);
  2470. }
  2471. }
  2472. }
  2473. m_delaySelect = false;
  2474. }
  2475. else if(!m_isphysical)
  2476. {
  2477. m_delaySelect = true;
  2478. }
  2479. }
  2480. else
  2481. {
  2482. if (!childPrim)
  2483. {
  2484. if (Body != IntPtr.Zero && !m_disabled)
  2485. {
  2486. _zeroFlag = true;
  2487. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  2488. SafeNativeMethods.BodyEnable(Body);
  2489. }
  2490. }
  2491. // else if (_parent != null)
  2492. // ((OdePrim)_parent).ChildSelectedChange(false);
  2493. UpdateCollisionCatFlags();
  2494. ApplyCollisionCatFlags();
  2495. m_delaySelect = false;
  2496. }
  2497. resetCollisionAccounting();
  2498. }
  2499. private void changePosition(Vector3 newPos)
  2500. {
  2501. CheckDelaySelect();
  2502. if (m_isphysical)
  2503. {
  2504. if (childPrim) // inertia is messed, must rebuild
  2505. {
  2506. if (m_building)
  2507. {
  2508. _position = newPos;
  2509. }
  2510. else if (m_forcePosOrRotation && _position != newPos && Body != IntPtr.Zero)
  2511. {
  2512. FixInertia(newPos);
  2513. if (!SafeNativeMethods.BodyIsEnabled(Body))
  2514. {
  2515. _zeroFlag = true;
  2516. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  2517. SafeNativeMethods.BodyEnable(Body);
  2518. }
  2519. }
  2520. }
  2521. else
  2522. {
  2523. if (_position != newPos)
  2524. {
  2525. SafeNativeMethods.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
  2526. _position = newPos;
  2527. }
  2528. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  2529. {
  2530. _zeroFlag = true;
  2531. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  2532. SafeNativeMethods.BodyEnable(Body);
  2533. }
  2534. }
  2535. }
  2536. else
  2537. {
  2538. if (prim_geom != IntPtr.Zero)
  2539. {
  2540. if (newPos != _position)
  2541. {
  2542. SafeNativeMethods.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
  2543. _position = newPos;
  2544. m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, m_targetSpace);
  2545. }
  2546. }
  2547. }
  2548. givefakepos--;
  2549. if (givefakepos < 0)
  2550. givefakepos = 0;
  2551. // changeSelectedStatus();
  2552. resetCollisionAccounting();
  2553. }
  2554. private void changeOrientation(Quaternion newOri)
  2555. {
  2556. CheckDelaySelect();
  2557. if (m_isphysical)
  2558. {
  2559. if (childPrim) // inertia is messed, must rebuild
  2560. {
  2561. if (m_building)
  2562. {
  2563. _orientation = newOri;
  2564. }
  2565. /*
  2566. else if (m_forcePosOrRotation && _orientation != newOri && Body != IntPtr.Zero)
  2567. {
  2568. FixInertia(_position, newOri);
  2569. if (!d.BodyIsEnabled(Body))
  2570. d.BodyEnable(Body);
  2571. }
  2572. */
  2573. }
  2574. else
  2575. {
  2576. if (newOri != _orientation)
  2577. {
  2578. SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
  2579. myrot.X = newOri.X;
  2580. myrot.Y = newOri.Y;
  2581. myrot.Z = newOri.Z;
  2582. myrot.W = newOri.W;
  2583. SafeNativeMethods.GeomSetQuaternion(prim_geom, ref myrot);
  2584. _orientation = newOri;
  2585. if (Body != IntPtr.Zero)
  2586. {
  2587. if(m_angularlocks != 0)
  2588. createAMotor(m_angularlocks);
  2589. }
  2590. }
  2591. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  2592. {
  2593. _zeroFlag = true;
  2594. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  2595. SafeNativeMethods.BodyEnable(Body);
  2596. }
  2597. }
  2598. }
  2599. else
  2600. {
  2601. if (prim_geom != IntPtr.Zero)
  2602. {
  2603. if (newOri != _orientation)
  2604. {
  2605. SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
  2606. myrot.X = newOri.X;
  2607. myrot.Y = newOri.Y;
  2608. myrot.Z = newOri.Z;
  2609. myrot.W = newOri.W;
  2610. SafeNativeMethods.GeomSetQuaternion(prim_geom, ref myrot);
  2611. _orientation = newOri;
  2612. }
  2613. }
  2614. }
  2615. givefakeori--;
  2616. if (givefakeori < 0)
  2617. givefakeori = 0;
  2618. resetCollisionAccounting();
  2619. }
  2620. private void changePositionAndOrientation(Vector3 newPos, Quaternion newOri)
  2621. {
  2622. CheckDelaySelect();
  2623. if (m_isphysical)
  2624. {
  2625. if (childPrim && m_building) // inertia is messed, must rebuild
  2626. {
  2627. _position = newPos;
  2628. _orientation = newOri;
  2629. }
  2630. else
  2631. {
  2632. if (newOri != _orientation)
  2633. {
  2634. SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
  2635. myrot.X = newOri.X;
  2636. myrot.Y = newOri.Y;
  2637. myrot.Z = newOri.Z;
  2638. myrot.W = newOri.W;
  2639. SafeNativeMethods.GeomSetQuaternion(prim_geom, ref myrot);
  2640. _orientation = newOri;
  2641. if (Body != IntPtr.Zero && m_angularlocks != 0)
  2642. createAMotor(m_angularlocks);
  2643. }
  2644. if (_position != newPos)
  2645. {
  2646. SafeNativeMethods.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
  2647. _position = newPos;
  2648. }
  2649. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  2650. {
  2651. _zeroFlag = true;
  2652. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  2653. SafeNativeMethods.BodyEnable(Body);
  2654. }
  2655. }
  2656. }
  2657. else
  2658. {
  2659. // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
  2660. // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
  2661. if (prim_geom != IntPtr.Zero)
  2662. {
  2663. if (newOri != _orientation)
  2664. {
  2665. SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
  2666. myrot.X = newOri.X;
  2667. myrot.Y = newOri.Y;
  2668. myrot.Z = newOri.Z;
  2669. myrot.W = newOri.W;
  2670. SafeNativeMethods.GeomSetQuaternion(prim_geom, ref myrot);
  2671. _orientation = newOri;
  2672. }
  2673. if (newPos != _position)
  2674. {
  2675. SafeNativeMethods.GeomSetPosition(prim_geom, newPos.X, newPos.Y, newPos.Z);
  2676. _position = newPos;
  2677. m_targetSpace = _parent_scene.MoveGeomToStaticSpace(prim_geom, m_targetSpace);
  2678. }
  2679. }
  2680. }
  2681. givefakepos--;
  2682. if (givefakepos < 0)
  2683. givefakepos = 0;
  2684. givefakeori--;
  2685. if (givefakeori < 0)
  2686. givefakeori = 0;
  2687. resetCollisionAccounting();
  2688. }
  2689. private void changeDisable(bool disable)
  2690. {
  2691. if (disable)
  2692. {
  2693. if (!m_disabled)
  2694. disableBodySoft();
  2695. }
  2696. else
  2697. {
  2698. if (m_disabled)
  2699. enableBodySoft();
  2700. }
  2701. }
  2702. private void changePhysicsStatus(bool NewStatus)
  2703. {
  2704. CheckDelaySelect();
  2705. m_isphysical = NewStatus;
  2706. if (!childPrim)
  2707. {
  2708. if (NewStatus)
  2709. {
  2710. if (Body == IntPtr.Zero)
  2711. MakeBody();
  2712. }
  2713. else
  2714. {
  2715. if (Body != IntPtr.Zero)
  2716. {
  2717. DestroyBody();
  2718. }
  2719. Stop();
  2720. }
  2721. }
  2722. resetCollisionAccounting();
  2723. }
  2724. private void changeSize(Vector3 newSize)
  2725. {
  2726. }
  2727. private void changeShape(PrimitiveBaseShape newShape)
  2728. {
  2729. }
  2730. private void changeAddPhysRep(ODEPhysRepData repData)
  2731. {
  2732. _size = repData.size; //??
  2733. _pbs = repData.pbs;
  2734. m_mesh = repData.mesh;
  2735. m_assetID = repData.assetID;
  2736. m_meshState = repData.meshState;
  2737. m_hasOBB = repData.hasOBB;
  2738. m_OBBOffset = repData.OBBOffset;
  2739. m_OBB = repData.OBB;
  2740. primVolume = repData.volume;
  2741. CreateGeom(repData.isTooSmall);
  2742. if (prim_geom != IntPtr.Zero)
  2743. {
  2744. SafeNativeMethods.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
  2745. SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
  2746. myrot.X = _orientation.X;
  2747. myrot.Y = _orientation.Y;
  2748. myrot.Z = _orientation.Z;
  2749. myrot.W = _orientation.W;
  2750. SafeNativeMethods.GeomSetQuaternion(prim_geom, ref myrot);
  2751. }
  2752. if (!m_isphysical)
  2753. {
  2754. SetInStaticSpace(this);
  2755. UpdateCollisionCatFlags();
  2756. ApplyCollisionCatFlags();
  2757. }
  2758. else
  2759. MakeBody();
  2760. if ((m_meshState & MeshState.NeedMask) != 0)
  2761. {
  2762. repData.size = _size;
  2763. repData.pbs = _pbs;
  2764. repData.shapetype = m_fakeShapetype;
  2765. _parent_scene.m_meshWorker.RequestMesh(repData);
  2766. }
  2767. else
  2768. m_shapetype = repData.shapetype;
  2769. }
  2770. private void changePhysRepData(ODEPhysRepData repData)
  2771. {
  2772. if(_size == repData.size &&
  2773. _pbs == repData.pbs &&
  2774. m_shapetype == repData.shapetype &&
  2775. m_mesh == repData.mesh &&
  2776. primVolume == repData.volume)
  2777. return;
  2778. CheckDelaySelect();
  2779. OdePrim parent = (OdePrim)_parent;
  2780. bool chp = childPrim;
  2781. if (chp)
  2782. {
  2783. if (parent != null)
  2784. {
  2785. parent.DestroyBody();
  2786. }
  2787. }
  2788. else
  2789. {
  2790. DestroyBody();
  2791. }
  2792. RemoveGeom();
  2793. _size = repData.size;
  2794. _pbs = repData.pbs;
  2795. m_mesh = repData.mesh;
  2796. m_assetID = repData.assetID;
  2797. m_meshState = repData.meshState;
  2798. m_hasOBB = repData.hasOBB;
  2799. m_OBBOffset = repData.OBBOffset;
  2800. m_OBB = repData.OBB;
  2801. primVolume = repData.volume;
  2802. CreateGeom(repData.isTooSmall);
  2803. if (prim_geom != IntPtr.Zero)
  2804. {
  2805. SafeNativeMethods.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
  2806. SafeNativeMethods.Quaternion myrot = new SafeNativeMethods.Quaternion();
  2807. myrot.X = _orientation.X;
  2808. myrot.Y = _orientation.Y;
  2809. myrot.Z = _orientation.Z;
  2810. myrot.W = _orientation.W;
  2811. SafeNativeMethods.GeomSetQuaternion(prim_geom, ref myrot);
  2812. }
  2813. if (m_isphysical)
  2814. {
  2815. if (chp)
  2816. {
  2817. if (parent != null)
  2818. {
  2819. parent.MakeBody();
  2820. }
  2821. }
  2822. else
  2823. MakeBody();
  2824. }
  2825. else
  2826. {
  2827. SetInStaticSpace(this);
  2828. UpdateCollisionCatFlags();
  2829. ApplyCollisionCatFlags();
  2830. }
  2831. resetCollisionAccounting();
  2832. if ((m_meshState & MeshState.NeedMask) != 0)
  2833. {
  2834. repData.size = _size;
  2835. repData.pbs = _pbs;
  2836. repData.shapetype = m_fakeShapetype;
  2837. _parent_scene.m_meshWorker.RequestMesh(repData);
  2838. }
  2839. else
  2840. m_shapetype = repData.shapetype;
  2841. }
  2842. private void changeFloatOnWater(bool newval)
  2843. {
  2844. m_collidesWater = newval;
  2845. UpdateCollisionCatFlags();
  2846. ApplyCollisionCatFlags();
  2847. }
  2848. private void changeSetTorque(Vector3 newtorque)
  2849. {
  2850. if (!m_isSelected && !m_outbounds)
  2851. {
  2852. if (m_isphysical && Body != IntPtr.Zero)
  2853. {
  2854. if (m_disabled)
  2855. enableBodySoft();
  2856. else if (!SafeNativeMethods.BodyIsEnabled(Body))
  2857. {
  2858. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  2859. SafeNativeMethods.BodyEnable(Body);
  2860. }
  2861. }
  2862. m_torque = newtorque;
  2863. }
  2864. }
  2865. private void changeForce(Vector3 force)
  2866. {
  2867. m_force = force;
  2868. if (!m_isSelected && !m_outbounds && Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  2869. {
  2870. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  2871. SafeNativeMethods.BodyEnable(Body);
  2872. }
  2873. }
  2874. private void changeAddForce(Vector3 theforce)
  2875. {
  2876. m_forceacc += theforce;
  2877. if (!m_isSelected && !m_outbounds)
  2878. {
  2879. lock (this)
  2880. {
  2881. //m_log.Info("[PHYSICS]: dequeing forcelist");
  2882. if (m_isphysical && Body != IntPtr.Zero)
  2883. {
  2884. if (m_disabled)
  2885. enableBodySoft();
  2886. else if (!SafeNativeMethods.BodyIsEnabled(Body))
  2887. {
  2888. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  2889. SafeNativeMethods.BodyEnable(Body);
  2890. }
  2891. }
  2892. }
  2893. m_collisionscore = 0;
  2894. }
  2895. }
  2896. // actually angular impulse
  2897. private void changeAddAngularImpulse(Vector3 aimpulse)
  2898. {
  2899. m_angularForceacc += aimpulse * m_invTimeStep;
  2900. if (!m_isSelected && !m_outbounds)
  2901. {
  2902. lock (this)
  2903. {
  2904. if (m_isphysical && Body != IntPtr.Zero)
  2905. {
  2906. if (m_disabled)
  2907. enableBodySoft();
  2908. else if (!SafeNativeMethods.BodyIsEnabled(Body))
  2909. {
  2910. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  2911. SafeNativeMethods.BodyEnable(Body);
  2912. }
  2913. }
  2914. }
  2915. m_collisionscore = 0;
  2916. }
  2917. }
  2918. private void changevelocity(Vector3 newVel)
  2919. {
  2920. float len = newVel.LengthSquared();
  2921. if (len > 100000.0f) // limit to 100m/s
  2922. {
  2923. len = 100.0f / (float)Math.Sqrt(len);
  2924. newVel *= len;
  2925. }
  2926. if (!m_isSelected && !m_outbounds)
  2927. {
  2928. if (Body != IntPtr.Zero)
  2929. {
  2930. if (m_disabled)
  2931. enableBodySoft();
  2932. else if (!SafeNativeMethods.BodyIsEnabled(Body))
  2933. {
  2934. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  2935. SafeNativeMethods.BodyEnable(Body);
  2936. }
  2937. SafeNativeMethods.BodySetLinearVel(Body, newVel.X, newVel.Y, newVel.Z);
  2938. }
  2939. //resetCollisionAccounting();
  2940. }
  2941. _velocity = newVel;
  2942. }
  2943. private void changeangvelocity(Vector3 newAngVel)
  2944. {
  2945. float len = newAngVel.LengthSquared();
  2946. if (len > _parent_scene.maxAngVelocitySQ)
  2947. {
  2948. len = _parent_scene.maximumAngularVelocity / (float)Math.Sqrt(len);
  2949. newAngVel *= len;
  2950. }
  2951. if (!m_isSelected && !m_outbounds)
  2952. {
  2953. if (Body != IntPtr.Zero)
  2954. {
  2955. if (m_disabled)
  2956. enableBodySoft();
  2957. else if (!SafeNativeMethods.BodyIsEnabled(Body))
  2958. {
  2959. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  2960. SafeNativeMethods.BodyEnable(Body);
  2961. }
  2962. SafeNativeMethods.BodySetAngularVel(Body, newAngVel.X, newAngVel.Y, newAngVel.Z);
  2963. }
  2964. //resetCollisionAccounting();
  2965. }
  2966. m_rotationalVelocity = newAngVel;
  2967. }
  2968. private void changeVolumedetetion(bool newVolDtc)
  2969. {
  2970. m_isVolumeDetect = newVolDtc;
  2971. m_fakeisVolumeDetect = newVolDtc;
  2972. UpdateCollisionCatFlags();
  2973. ApplyCollisionCatFlags();
  2974. }
  2975. protected void changeBuilding(bool newbuilding)
  2976. {
  2977. // Check if we need to do anything
  2978. if (newbuilding == m_building)
  2979. return;
  2980. if ((bool)newbuilding)
  2981. {
  2982. m_building = true;
  2983. if (!childPrim)
  2984. DestroyBody();
  2985. }
  2986. else
  2987. {
  2988. m_building = false;
  2989. CheckDelaySelect();
  2990. if (!childPrim)
  2991. MakeBody();
  2992. }
  2993. if (!childPrim && childrenPrim.Count > 0)
  2994. {
  2995. foreach (OdePrim prm in childrenPrim)
  2996. prm.changeBuilding(m_building); // call directly
  2997. }
  2998. }
  2999. public void changeSetVehicle(VehicleData vdata)
  3000. {
  3001. if (m_vehicle == null)
  3002. m_vehicle = new ODEDynamics(this);
  3003. m_vehicle.DoSetVehicle(vdata);
  3004. }
  3005. private void changeVehicleType(int value)
  3006. {
  3007. if (value == (int)Vehicle.TYPE_NONE)
  3008. {
  3009. if (m_vehicle != null)
  3010. m_vehicle = null;
  3011. }
  3012. else
  3013. {
  3014. if (m_vehicle == null)
  3015. m_vehicle = new ODEDynamics(this);
  3016. m_vehicle.ProcessTypeChange((Vehicle)value);
  3017. }
  3018. }
  3019. private void changeVehicleFloatParam(strVehicleFloatParam fp)
  3020. {
  3021. if (m_vehicle == null)
  3022. return;
  3023. m_vehicle.ProcessFloatVehicleParam((Vehicle)fp.param, fp.value);
  3024. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  3025. {
  3026. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  3027. SafeNativeMethods.BodyEnable(Body);
  3028. }
  3029. }
  3030. private void changeVehicleVectorParam(strVehicleVectorParam vp)
  3031. {
  3032. if (m_vehicle == null)
  3033. return;
  3034. m_vehicle.ProcessVectorVehicleParam((Vehicle)vp.param, vp.value);
  3035. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  3036. {
  3037. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  3038. SafeNativeMethods.BodyEnable(Body);
  3039. }
  3040. }
  3041. private void changeVehicleRotationParam(strVehicleQuatParam qp)
  3042. {
  3043. if (m_vehicle == null)
  3044. return;
  3045. m_vehicle.ProcessRotationVehicleParam((Vehicle)qp.param, qp.value);
  3046. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  3047. {
  3048. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  3049. SafeNativeMethods.BodyEnable(Body);
  3050. }
  3051. }
  3052. private void changeVehicleFlags(strVehicleBoolParam bp)
  3053. {
  3054. if (m_vehicle == null)
  3055. return;
  3056. m_vehicle.ProcessVehicleFlags(bp.param, bp.value);
  3057. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  3058. {
  3059. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  3060. SafeNativeMethods.BodyEnable(Body);
  3061. }
  3062. }
  3063. private void changeBuoyancy(float b)
  3064. {
  3065. m_buoyancy = b;
  3066. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  3067. {
  3068. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  3069. SafeNativeMethods.BodyEnable(Body);
  3070. }
  3071. }
  3072. private void changePIDTarget(Vector3 trg)
  3073. {
  3074. m_PIDTarget = trg;
  3075. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  3076. {
  3077. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  3078. SafeNativeMethods.BodyEnable(Body);
  3079. }
  3080. }
  3081. private void changePIDTau(float tau)
  3082. {
  3083. m_PIDTau = tau;
  3084. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  3085. {
  3086. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  3087. SafeNativeMethods.BodyEnable(Body);
  3088. }
  3089. }
  3090. private void changePIDActive(bool val)
  3091. {
  3092. m_usePID = val;
  3093. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  3094. {
  3095. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  3096. SafeNativeMethods.BodyEnable(Body);
  3097. }
  3098. }
  3099. private void changePIDHoverHeight(float val)
  3100. {
  3101. m_PIDHoverHeight = val;
  3102. if (val == 0)
  3103. m_useHoverPID = false;
  3104. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  3105. {
  3106. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  3107. SafeNativeMethods.BodyEnable(Body);
  3108. }
  3109. }
  3110. private void changePIDHoverType(PIDHoverType type)
  3111. {
  3112. m_PIDHoverType = type;
  3113. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  3114. {
  3115. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  3116. SafeNativeMethods.BodyEnable(Body);
  3117. }
  3118. }
  3119. private void changePIDHoverTau(float tau)
  3120. {
  3121. m_PIDHoverTau = tau;
  3122. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  3123. {
  3124. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  3125. SafeNativeMethods.BodyEnable(Body);
  3126. }
  3127. }
  3128. private void changePIDHoverActive(bool active)
  3129. {
  3130. m_useHoverPID = active;
  3131. if (Body != IntPtr.Zero && !SafeNativeMethods.BodyIsEnabled(Body))
  3132. {
  3133. SafeNativeMethods.BodySetAutoDisableSteps(Body, m_body_autodisable_frames);
  3134. SafeNativeMethods.BodyEnable(Body);
  3135. }
  3136. }
  3137. private void changeInertia(PhysicsInertiaData inertia)
  3138. {
  3139. m_InertiaOverride = inertia;
  3140. if (Body != IntPtr.Zero)
  3141. DestroyBody();
  3142. MakeBody();
  3143. }
  3144. #endregion
  3145. public void Move()
  3146. {
  3147. if (!childPrim && m_isphysical && Body != IntPtr.Zero &&
  3148. !m_disabled && !m_isSelected && !m_building && !m_outbounds)
  3149. {
  3150. if (!SafeNativeMethods.BodyIsEnabled(Body))
  3151. {
  3152. // let vehicles sleep
  3153. if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
  3154. return;
  3155. if (++m_bodydisablecontrol < 50)
  3156. return;
  3157. // clear residuals
  3158. SafeNativeMethods.BodySetAngularVel(Body,0f,0f,0f);
  3159. SafeNativeMethods.BodySetLinearVel(Body,0f,0f,0f);
  3160. _zeroFlag = true;
  3161. SafeNativeMethods.BodySetAutoDisableSteps(Body, 1);
  3162. SafeNativeMethods.BodyEnable(Body);
  3163. m_bodydisablecontrol = -3;
  3164. }
  3165. if(m_bodydisablecontrol < 0)
  3166. m_bodydisablecontrol++;
  3167. SafeNativeMethods.Vector3 lpos = SafeNativeMethods.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
  3168. if (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
  3169. {
  3170. // 'VEHICLES' are dealt with in ODEDynamics.cs
  3171. m_vehicle.Step();
  3172. return;
  3173. }
  3174. float fx = 0;
  3175. float fy = 0;
  3176. float fz = 0;
  3177. float mass = m_mass;
  3178. if (m_usePID && m_PIDTau > 0)
  3179. {
  3180. // for now position error
  3181. _target_velocity =
  3182. new Vector3(
  3183. (m_PIDTarget.X - lpos.X),
  3184. (m_PIDTarget.Y - lpos.Y),
  3185. (m_PIDTarget.Z - lpos.Z)
  3186. );
  3187. if (_target_velocity.ApproxEquals(Vector3.Zero, 0.02f))
  3188. {
  3189. SafeNativeMethods.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
  3190. SafeNativeMethods.BodySetLinearVel(Body, 0, 0, 0);
  3191. return;
  3192. }
  3193. else
  3194. {
  3195. _zeroFlag = false;
  3196. float tmp = 1 / m_PIDTau;
  3197. _target_velocity *= tmp;
  3198. // apply limits
  3199. tmp = _target_velocity.Length();
  3200. if (tmp > 50.0f)
  3201. {
  3202. tmp = 50 / tmp;
  3203. _target_velocity *= tmp;
  3204. }
  3205. else if (tmp < 0.05f)
  3206. {
  3207. tmp = 0.05f / tmp;
  3208. _target_velocity *= tmp;
  3209. }
  3210. SafeNativeMethods.Vector3 vel = SafeNativeMethods.BodyGetLinearVel(Body);
  3211. fx = (_target_velocity.X - vel.X) * m_invTimeStep;
  3212. fy = (_target_velocity.Y - vel.Y) * m_invTimeStep;
  3213. fz = (_target_velocity.Z - vel.Z) * m_invTimeStep;
  3214. // d.BodySetLinearVel(Body, _target_velocity.X, _target_velocity.Y, _target_velocity.Z);
  3215. }
  3216. } // end if (m_usePID)
  3217. // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
  3218. else if (m_useHoverPID && m_PIDHoverTau != 0 && m_PIDHoverHeight != 0)
  3219. {
  3220. // Non-Vehicles have a limited set of Hover options.
  3221. // determine what our target height really is based on HoverType
  3222. m_groundHeight = _parent_scene.GetTerrainHeightAtXY(lpos.X, lpos.Y);
  3223. switch (m_PIDHoverType)
  3224. {
  3225. case PIDHoverType.Ground:
  3226. m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
  3227. break;
  3228. case PIDHoverType.GroundAndWater:
  3229. m_waterHeight = _parent_scene.GetWaterLevel();
  3230. if (m_groundHeight > m_waterHeight)
  3231. m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
  3232. else
  3233. m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
  3234. break;
  3235. } // end switch (m_PIDHoverType)
  3236. // don't go underground unless volumedetector
  3237. if (m_targetHoverHeight > m_groundHeight || m_isVolumeDetect)
  3238. {
  3239. SafeNativeMethods.Vector3 vel = SafeNativeMethods.BodyGetLinearVel(Body);
  3240. fz = (m_targetHoverHeight - lpos.Z);
  3241. // if error is zero, use position control; otherwise, velocity control
  3242. if (Math.Abs(fz) < 0.01f)
  3243. {
  3244. SafeNativeMethods.BodySetPosition(Body, lpos.X, lpos.Y, m_targetHoverHeight);
  3245. SafeNativeMethods.BodySetLinearVel(Body, vel.X, vel.Y, 0);
  3246. }
  3247. else
  3248. {
  3249. _zeroFlag = false;
  3250. fz /= m_PIDHoverTau;
  3251. float tmp = Math.Abs(fz);
  3252. if (tmp > 50)
  3253. fz = 50 * Math.Sign(fz);
  3254. else if (tmp < 0.1)
  3255. fz = 0.1f * Math.Sign(fz);
  3256. fz = ((fz - vel.Z) * m_invTimeStep);
  3257. }
  3258. }
  3259. }
  3260. else
  3261. {
  3262. float b = (1.0f - m_buoyancy) * m_gravmod;
  3263. fx = _parent_scene.gravityx * b;
  3264. fy = _parent_scene.gravityy * b;
  3265. fz = _parent_scene.gravityz * b;
  3266. }
  3267. fx *= mass;
  3268. fy *= mass;
  3269. fz *= mass;
  3270. // constant force
  3271. fx += m_force.X;
  3272. fy += m_force.Y;
  3273. fz += m_force.Z;
  3274. fx += m_forceacc.X;
  3275. fy += m_forceacc.Y;
  3276. fz += m_forceacc.Z;
  3277. m_forceacc = Vector3.Zero;
  3278. //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
  3279. if (fx != 0 || fy != 0 || fz != 0)
  3280. {
  3281. SafeNativeMethods.BodyAddForce(Body, fx, fy, fz);
  3282. //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
  3283. }
  3284. Vector3 trq;
  3285. trq = m_torque;
  3286. trq += m_angularForceacc;
  3287. m_angularForceacc = Vector3.Zero;
  3288. if (trq.X != 0 || trq.Y != 0 || trq.Z != 0)
  3289. {
  3290. SafeNativeMethods.BodyAddTorque(Body, trq.X, trq.Y, trq.Z);
  3291. }
  3292. }
  3293. else
  3294. { // is not physical, or is not a body or is selected
  3295. // _zeroPosition = d.BodyGetPosition(Body);
  3296. return;
  3297. //Console.WriteLine("Nothing " + Name);
  3298. }
  3299. }
  3300. public void UpdatePositionAndVelocity(int frame)
  3301. {
  3302. if (_parent == null && !m_isSelected && !m_disabled && !m_building && !m_outbounds && Body != IntPtr.Zero)
  3303. {
  3304. if(m_bodydisablecontrol < 0)
  3305. return;
  3306. bool bodyenabled = SafeNativeMethods.BodyIsEnabled(Body);
  3307. if (bodyenabled || !_zeroFlag)
  3308. {
  3309. bool lastZeroFlag = _zeroFlag;
  3310. SafeNativeMethods.Vector3 lpos = SafeNativeMethods.GeomGetPosition(prim_geom);
  3311. // check outside region
  3312. if (lpos.Z < -100 || lpos.Z > 100000f)
  3313. {
  3314. m_outbounds = true;
  3315. lpos.Z = Util.Clip(lpos.Z, -100f, 100000f);
  3316. m_acceleration.X = 0;
  3317. m_acceleration.Y = 0;
  3318. m_acceleration.Z = 0;
  3319. _velocity.X = 0;
  3320. _velocity.Y = 0;
  3321. _velocity.Z = 0;
  3322. m_rotationalVelocity.X = 0;
  3323. m_rotationalVelocity.Y = 0;
  3324. m_rotationalVelocity.Z = 0;
  3325. SafeNativeMethods.BodySetLinearVel(Body, 0, 0, 0); // stop it
  3326. SafeNativeMethods.BodySetAngularVel(Body, 0, 0, 0); // stop it
  3327. SafeNativeMethods.BodySetPosition(Body, lpos.X, lpos.Y, lpos.Z); // put it somewhere
  3328. m_lastposition = _position;
  3329. m_lastorientation = _orientation;
  3330. base.RequestPhysicsterseUpdate();
  3331. // throttleCounter = 0;
  3332. _zeroFlag = true;
  3333. disableBodySoft(); // disable it and colisions
  3334. base.RaiseOutOfBounds(_position);
  3335. return;
  3336. }
  3337. if (lpos.X < 0f)
  3338. {
  3339. _position.X = Util.Clip(lpos.X, -2f, -0.1f);
  3340. m_outbounds = true;
  3341. }
  3342. else if (lpos.X > _parent_scene.WorldExtents.X)
  3343. {
  3344. _position.X = Util.Clip(lpos.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
  3345. m_outbounds = true;
  3346. }
  3347. if (lpos.Y < 0f)
  3348. {
  3349. _position.Y = Util.Clip(lpos.Y, -2f, -0.1f);
  3350. m_outbounds = true;
  3351. }
  3352. else if (lpos.Y > _parent_scene.WorldExtents.Y)
  3353. {
  3354. _position.Y = Util.Clip(lpos.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
  3355. m_outbounds = true;
  3356. }
  3357. if (m_outbounds)
  3358. {
  3359. m_lastposition = _position;
  3360. m_lastorientation = _orientation;
  3361. SafeNativeMethods.Vector3 dtmp = SafeNativeMethods.BodyGetAngularVel(Body);
  3362. m_rotationalVelocity.X = dtmp.X;
  3363. m_rotationalVelocity.Y = dtmp.Y;
  3364. m_rotationalVelocity.Z = dtmp.Z;
  3365. dtmp = SafeNativeMethods.BodyGetLinearVel(Body);
  3366. _velocity.X = dtmp.X;
  3367. _velocity.Y = dtmp.Y;
  3368. _velocity.Z = dtmp.Z;
  3369. SafeNativeMethods.BodySetLinearVel(Body, 0, 0, 0); // stop it
  3370. SafeNativeMethods.BodySetAngularVel(Body, 0, 0, 0);
  3371. SafeNativeMethods.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
  3372. disableBodySoft(); // stop collisions
  3373. UnSubscribeEvents();
  3374. base.RequestPhysicsterseUpdate();
  3375. return;
  3376. }
  3377. SafeNativeMethods.Quaternion ori;
  3378. SafeNativeMethods.GeomCopyQuaternion(prim_geom, out ori);
  3379. // decide if moving
  3380. // use positions since this are integrated quantities
  3381. // tolerance values depende a lot on simulation noise...
  3382. // use simple math.abs since we dont need to be exact
  3383. if(!bodyenabled)
  3384. {
  3385. _zeroFlag = true;
  3386. }
  3387. else
  3388. {
  3389. float poserror;
  3390. float angerror;
  3391. if(_zeroFlag)
  3392. {
  3393. poserror = 0.01f;
  3394. angerror = 0.001f;
  3395. }
  3396. else
  3397. {
  3398. poserror = 0.005f;
  3399. angerror = 0.0005f;
  3400. }
  3401. if (
  3402. (Math.Abs(_position.X - lpos.X) < poserror)
  3403. && (Math.Abs(_position.Y - lpos.Y) < poserror)
  3404. && (Math.Abs(_position.Z - lpos.Z) < poserror)
  3405. && (Math.Abs(_orientation.X - ori.X) < angerror)
  3406. && (Math.Abs(_orientation.Y - ori.Y) < angerror)
  3407. && (Math.Abs(_orientation.Z - ori.Z) < angerror) // ignore W
  3408. )
  3409. _zeroFlag = true;
  3410. else
  3411. _zeroFlag = false;
  3412. }
  3413. // update position
  3414. if (!(_zeroFlag && lastZeroFlag))
  3415. {
  3416. _position.X = lpos.X;
  3417. _position.Y = lpos.Y;
  3418. _position.Z = lpos.Z;
  3419. _orientation.X = ori.X;
  3420. _orientation.Y = ori.Y;
  3421. _orientation.Z = ori.Z;
  3422. _orientation.W = ori.W;
  3423. }
  3424. // update velocities and acceleration
  3425. if (_zeroFlag || lastZeroFlag)
  3426. {
  3427. // disable interpolators
  3428. _velocity = Vector3.Zero;
  3429. m_acceleration = Vector3.Zero;
  3430. m_rotationalVelocity = Vector3.Zero;
  3431. }
  3432. else
  3433. {
  3434. SafeNativeMethods.Vector3 vel = SafeNativeMethods.BodyGetLinearVel(Body);
  3435. m_acceleration = _velocity;
  3436. if ((Math.Abs(vel.X) < 0.005f) &&
  3437. (Math.Abs(vel.Y) < 0.005f) &&
  3438. (Math.Abs(vel.Z) < 0.005f))
  3439. {
  3440. _velocity = Vector3.Zero;
  3441. float t = -m_invTimeStep;
  3442. m_acceleration = m_acceleration * t;
  3443. }
  3444. else
  3445. {
  3446. _velocity.X = vel.X;
  3447. _velocity.Y = vel.Y;
  3448. _velocity.Z = vel.Z;
  3449. m_acceleration = (_velocity - m_acceleration) * m_invTimeStep;
  3450. }
  3451. if ((Math.Abs(m_acceleration.X) < 0.01f) &&
  3452. (Math.Abs(m_acceleration.Y) < 0.01f) &&
  3453. (Math.Abs(m_acceleration.Z) < 0.01f))
  3454. {
  3455. m_acceleration = Vector3.Zero;
  3456. }
  3457. vel = SafeNativeMethods.BodyGetAngularVel(Body);
  3458. if ((Math.Abs(vel.X) < 0.0001) &&
  3459. (Math.Abs(vel.Y) < 0.0001) &&
  3460. (Math.Abs(vel.Z) < 0.0001)
  3461. )
  3462. {
  3463. m_rotationalVelocity = Vector3.Zero;
  3464. }
  3465. else
  3466. {
  3467. m_rotationalVelocity.X = vel.X;
  3468. m_rotationalVelocity.Y = vel.Y;
  3469. m_rotationalVelocity.Z = vel.Z;
  3470. }
  3471. }
  3472. if (_zeroFlag)
  3473. {
  3474. if (!m_lastUpdateSent)
  3475. {
  3476. base.RequestPhysicsterseUpdate();
  3477. if (lastZeroFlag)
  3478. m_lastUpdateSent = true;
  3479. }
  3480. return;
  3481. }
  3482. base.RequestPhysicsterseUpdate();
  3483. m_lastUpdateSent = false;
  3484. }
  3485. }
  3486. }
  3487. internal static bool QuaternionIsFinite(Quaternion q)
  3488. {
  3489. if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
  3490. return false;
  3491. if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
  3492. return false;
  3493. if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
  3494. return false;
  3495. if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
  3496. return false;
  3497. return true;
  3498. }
  3499. internal static void DMassSubPartFromObj(ref SafeNativeMethods.Mass part, ref SafeNativeMethods.Mass theobj)
  3500. {
  3501. // assumes object center of mass is zero
  3502. float smass = part.mass;
  3503. theobj.mass -= smass;
  3504. smass *= 1.0f / (theobj.mass); ;
  3505. theobj.c.X -= part.c.X * smass;
  3506. theobj.c.Y -= part.c.Y * smass;
  3507. theobj.c.Z -= part.c.Z * smass;
  3508. theobj.I.M00 -= part.I.M00;
  3509. theobj.I.M01 -= part.I.M01;
  3510. theobj.I.M02 -= part.I.M02;
  3511. theobj.I.M10 -= part.I.M10;
  3512. theobj.I.M11 -= part.I.M11;
  3513. theobj.I.M12 -= part.I.M12;
  3514. theobj.I.M20 -= part.I.M20;
  3515. theobj.I.M21 -= part.I.M21;
  3516. theobj.I.M22 -= part.I.M22;
  3517. }
  3518. private void donullchange()
  3519. {
  3520. }
  3521. public bool DoAChange(changes what, object arg)
  3522. {
  3523. if (prim_geom == IntPtr.Zero && what != changes.Add && what != changes.AddPhysRep && what != changes.Remove)
  3524. {
  3525. return false;
  3526. }
  3527. // nasty switch
  3528. switch (what)
  3529. {
  3530. case changes.Add:
  3531. changeadd();
  3532. break;
  3533. case changes.AddPhysRep:
  3534. changeAddPhysRep((ODEPhysRepData)arg);
  3535. break;
  3536. case changes.Remove:
  3537. //If its being removed, we don't want to rebuild the physical rep at all, so ignore this stuff...
  3538. //When we return true, it destroys all of the prims in the linkset anyway
  3539. if (_parent != null)
  3540. {
  3541. OdePrim parent = (OdePrim)_parent;
  3542. parent.ChildRemove(this, false);
  3543. }
  3544. else
  3545. ChildRemove(this, false);
  3546. m_vehicle = null;
  3547. RemoveGeom();
  3548. m_targetSpace = IntPtr.Zero;
  3549. UnSubscribeEvents();
  3550. return true;
  3551. case changes.Link:
  3552. OdePrim tmp = (OdePrim)arg;
  3553. changeLink(tmp);
  3554. break;
  3555. case changes.DeLink:
  3556. changeLink(null);
  3557. break;
  3558. case changes.Position:
  3559. changePosition((Vector3)arg);
  3560. break;
  3561. case changes.Orientation:
  3562. changeOrientation((Quaternion)arg);
  3563. break;
  3564. case changes.PosOffset:
  3565. donullchange();
  3566. break;
  3567. case changes.OriOffset:
  3568. donullchange();
  3569. break;
  3570. case changes.Velocity:
  3571. changevelocity((Vector3)arg);
  3572. break;
  3573. case changes.TargetVelocity:
  3574. break;
  3575. // case changes.Acceleration:
  3576. // changeacceleration((Vector3)arg);
  3577. // break;
  3578. case changes.AngVelocity:
  3579. changeangvelocity((Vector3)arg);
  3580. break;
  3581. case changes.Force:
  3582. changeForce((Vector3)arg);
  3583. break;
  3584. case changes.Torque:
  3585. changeSetTorque((Vector3)arg);
  3586. break;
  3587. case changes.AddForce:
  3588. changeAddForce((Vector3)arg);
  3589. break;
  3590. case changes.AddAngForce:
  3591. changeAddAngularImpulse((Vector3)arg);
  3592. break;
  3593. case changes.AngLock:
  3594. changeAngularLock((byte)arg);
  3595. break;
  3596. case changes.Size:
  3597. changeSize((Vector3)arg);
  3598. break;
  3599. case changes.Shape:
  3600. changeShape((PrimitiveBaseShape)arg);
  3601. break;
  3602. case changes.PhysRepData:
  3603. changePhysRepData((ODEPhysRepData) arg);
  3604. break;
  3605. case changes.CollidesWater:
  3606. changeFloatOnWater((bool)arg);
  3607. break;
  3608. case changes.VolumeDtc:
  3609. changeVolumedetetion((bool)arg);
  3610. break;
  3611. case changes.Phantom:
  3612. changePhantomStatus((bool)arg);
  3613. break;
  3614. case changes.Physical:
  3615. changePhysicsStatus((bool)arg);
  3616. break;
  3617. case changes.Selected:
  3618. changeSelectedStatus((bool)arg);
  3619. break;
  3620. case changes.disabled:
  3621. changeDisable((bool)arg);
  3622. break;
  3623. case changes.building:
  3624. changeBuilding((bool)arg);
  3625. break;
  3626. case changes.VehicleType:
  3627. changeVehicleType((int)arg);
  3628. break;
  3629. case changes.VehicleFlags:
  3630. changeVehicleFlags((strVehicleBoolParam) arg);
  3631. break;
  3632. case changes.VehicleFloatParam:
  3633. changeVehicleFloatParam((strVehicleFloatParam) arg);
  3634. break;
  3635. case changes.VehicleVectorParam:
  3636. changeVehicleVectorParam((strVehicleVectorParam) arg);
  3637. break;
  3638. case changes.VehicleRotationParam:
  3639. changeVehicleRotationParam((strVehicleQuatParam) arg);
  3640. break;
  3641. case changes.SetVehicle:
  3642. changeSetVehicle((VehicleData) arg);
  3643. break;
  3644. case changes.Buoyancy:
  3645. changeBuoyancy((float)arg);
  3646. break;
  3647. case changes.PIDTarget:
  3648. changePIDTarget((Vector3)arg);
  3649. break;
  3650. case changes.PIDTau:
  3651. changePIDTau((float)arg);
  3652. break;
  3653. case changes.PIDActive:
  3654. changePIDActive((bool)arg);
  3655. break;
  3656. case changes.PIDHoverHeight:
  3657. changePIDHoverHeight((float)arg);
  3658. break;
  3659. case changes.PIDHoverType:
  3660. changePIDHoverType((PIDHoverType)arg);
  3661. break;
  3662. case changes.PIDHoverTau:
  3663. changePIDHoverTau((float)arg);
  3664. break;
  3665. case changes.PIDHoverActive:
  3666. changePIDHoverActive((bool)arg);
  3667. break;
  3668. case changes.SetInertia:
  3669. changeInertia((PhysicsInertiaData) arg);
  3670. break;
  3671. case changes.Null:
  3672. donullchange();
  3673. break;
  3674. default:
  3675. donullchange();
  3676. break;
  3677. }
  3678. return false;
  3679. }
  3680. public void AddChange(changes what, object arg)
  3681. {
  3682. _parent_scene.AddChange((PhysicsActor) this, what, arg);
  3683. }
  3684. private struct strVehicleBoolParam
  3685. {
  3686. public int param;
  3687. public bool value;
  3688. }
  3689. private struct strVehicleFloatParam
  3690. {
  3691. public int param;
  3692. public float value;
  3693. }
  3694. private struct strVehicleQuatParam
  3695. {
  3696. public int param;
  3697. public Quaternion value;
  3698. }
  3699. private struct strVehicleVectorParam
  3700. {
  3701. public int param;
  3702. public Vector3 value;
  3703. }
  3704. }
  3705. }