ODEPrim.cs 116 KB

12345678910111213141516171819202122232425262728293031323334353637383940414243444546474849505152535455565758596061626364656667686970717273747576777879808182838485868788899091929394959697989910010110210310410510610710810911011111211311411511611711811912012112212312412512612712812913013113213313413513613713813914014114214314414514614714814915015115215315415515615715815916016116216316416516616716816917017117217317417517617717817918018118218318418518618718818919019119219319419519619719819920020120220320420520620720820921021121221321421521621721821922022122222322422522622722822923023123223323423523623723823924024124224324424524624724824925025125225325425525625725825926026126226326426526626726826927027127227327427527627727827928028128228328428528628728828929029129229329429529629729829930030130230330430530630730830931031131231331431531631731831932032132232332432532632732832933033133233333433533633733833934034134234334434534634734834935035135235335435535635735835936036136236336436536636736836937037137237337437537637737837938038138238338438538638738838939039139239339439539639739839940040140240340440540640740840941041141241341441541641741841942042142242342442542642742842943043143243343443543643743843944044144244344444544644744844945045145245345445545645745845946046146246346446546646746846947047147247347447547647747847948048148248348448548648748848949049149249349449549649749849950050150250350450550650750850951051151251351451551651751851952052152252352452552652752852953053153253353453553653753853954054154254354454554654754854955055155255355455555655755855956056156256356456556656756856957057157257357457557657757857958058158258358458558658758858959059159259359459559659759859960060160260360460560660760860961061161261361461561661761861962062162262362462562662762862963063163263363463563663763863964064164264364464564664764864965065165265365465565665765865966066166266366466566666766866967067167267367467567667767867968068168268368468568668768868969069169269369469569669769869970070170270370470570670770870971071171271371471571671771871972072172272372472572672772872973073173273373473573673773873974074174274374474574674774874975075175275375475575675775875976076176276376476576676776876977077177277377477577677777877978078178278378478578678778878979079179279379479579679779879980080180280380480580680780880981081181281381481581681781881982082182282382482582682782882983083183283383483583683783883984084184284384484584684784884985085185285385485585685785885986086186286386486586686786886987087187287387487587687787887988088188288388488588688788888989089189289389489589689789889990090190290390490590690790890991091191291391491591691791891992092192292392492592692792892993093193293393493593693793893994094194294394494594694794894995095195295395495595695795895996096196296396496596696796896997097197297397497597697797897998098198298398498598698798898999099199299399499599699799899910001001100210031004100510061007100810091010101110121013101410151016101710181019102010211022102310241025102610271028102910301031103210331034103510361037103810391040104110421043104410451046104710481049105010511052105310541055105610571058105910601061106210631064106510661067106810691070107110721073107410751076107710781079108010811082108310841085108610871088108910901091109210931094109510961097109810991100110111021103110411051106110711081109111011111112111311141115111611171118111911201121112211231124112511261127112811291130113111321133113411351136113711381139114011411142114311441145114611471148114911501151115211531154115511561157115811591160116111621163116411651166116711681169117011711172117311741175117611771178117911801181118211831184118511861187118811891190119111921193119411951196119711981199120012011202120312041205120612071208120912101211121212131214121512161217121812191220122112221223122412251226122712281229123012311232123312341235123612371238123912401241124212431244124512461247124812491250125112521253125412551256125712581259126012611262126312641265126612671268126912701271127212731274127512761277127812791280128112821283128412851286128712881289129012911292129312941295129612971298129913001301130213031304130513061307130813091310131113121313131413151316131713181319132013211322132313241325132613271328132913301331133213331334133513361337133813391340134113421343134413451346134713481349135013511352135313541355135613571358135913601361136213631364136513661367136813691370137113721373137413751376137713781379138013811382138313841385138613871388138913901391139213931394139513961397139813991400140114021403140414051406140714081409141014111412141314141415141614171418141914201421142214231424142514261427142814291430143114321433143414351436143714381439144014411442144314441445144614471448144914501451145214531454145514561457145814591460146114621463146414651466146714681469147014711472147314741475147614771478147914801481148214831484148514861487148814891490149114921493149414951496149714981499150015011502150315041505150615071508150915101511151215131514151515161517151815191520152115221523152415251526152715281529153015311532153315341535153615371538153915401541154215431544154515461547154815491550155115521553155415551556155715581559156015611562156315641565156615671568156915701571157215731574157515761577157815791580158115821583158415851586158715881589159015911592159315941595159615971598159916001601160216031604160516061607160816091610161116121613161416151616161716181619162016211622162316241625162616271628162916301631163216331634163516361637163816391640164116421643164416451646164716481649165016511652165316541655165616571658165916601661166216631664166516661667166816691670167116721673167416751676167716781679168016811682168316841685168616871688168916901691169216931694169516961697169816991700170117021703170417051706170717081709171017111712171317141715171617171718171917201721172217231724172517261727172817291730173117321733173417351736173717381739174017411742174317441745174617471748174917501751175217531754175517561757175817591760176117621763176417651766176717681769177017711772177317741775177617771778177917801781178217831784178517861787178817891790179117921793179417951796179717981799180018011802180318041805180618071808180918101811181218131814181518161817181818191820182118221823182418251826182718281829183018311832183318341835183618371838183918401841184218431844184518461847184818491850185118521853185418551856185718581859186018611862186318641865186618671868186918701871187218731874187518761877187818791880188118821883188418851886188718881889189018911892189318941895189618971898189919001901190219031904190519061907190819091910191119121913191419151916191719181919192019211922192319241925192619271928192919301931193219331934193519361937193819391940194119421943194419451946194719481949195019511952195319541955195619571958195919601961196219631964196519661967196819691970197119721973197419751976197719781979198019811982198319841985198619871988198919901991199219931994199519961997199819992000200120022003200420052006200720082009201020112012201320142015201620172018201920202021202220232024202520262027202820292030203120322033203420352036203720382039204020412042204320442045204620472048204920502051205220532054205520562057205820592060206120622063206420652066206720682069207020712072207320742075207620772078207920802081208220832084208520862087208820892090209120922093209420952096209720982099210021012102210321042105210621072108210921102111211221132114211521162117211821192120212121222123212421252126212721282129213021312132213321342135213621372138213921402141214221432144214521462147214821492150215121522153215421552156215721582159216021612162216321642165216621672168216921702171217221732174217521762177217821792180218121822183218421852186218721882189219021912192219321942195219621972198219922002201220222032204220522062207220822092210221122122213221422152216221722182219222022212222222322242225222622272228222922302231223222332234223522362237223822392240224122422243224422452246224722482249225022512252225322542255225622572258225922602261226222632264226522662267226822692270227122722273227422752276227722782279228022812282228322842285228622872288228922902291229222932294229522962297229822992300230123022303230423052306230723082309231023112312231323142315231623172318231923202321232223232324232523262327232823292330233123322333233423352336233723382339234023412342234323442345234623472348234923502351235223532354235523562357235823592360236123622363236423652366236723682369237023712372237323742375237623772378237923802381238223832384238523862387238823892390239123922393239423952396239723982399240024012402240324042405240624072408240924102411241224132414241524162417241824192420242124222423242424252426242724282429243024312432243324342435243624372438243924402441244224432444244524462447244824492450245124522453245424552456245724582459246024612462246324642465246624672468246924702471247224732474247524762477247824792480248124822483248424852486248724882489249024912492249324942495249624972498249925002501250225032504250525062507250825092510251125122513251425152516251725182519252025212522252325242525252625272528252925302531253225332534253525362537253825392540254125422543254425452546254725482549255025512552255325542555255625572558255925602561256225632564256525662567256825692570257125722573257425752576257725782579258025812582258325842585258625872588258925902591259225932594259525962597259825992600260126022603260426052606260726082609261026112612261326142615261626172618261926202621262226232624262526262627262826292630263126322633263426352636263726382639264026412642264326442645264626472648264926502651265226532654265526562657265826592660266126622663266426652666266726682669267026712672267326742675267626772678267926802681268226832684268526862687268826892690269126922693269426952696269726982699270027012702270327042705270627072708270927102711271227132714271527162717271827192720272127222723272427252726272727282729273027312732273327342735273627372738273927402741274227432744274527462747274827492750275127522753275427552756275727582759276027612762276327642765276627672768276927702771277227732774277527762777277827792780278127822783278427852786278727882789279027912792279327942795279627972798279928002801280228032804280528062807280828092810281128122813281428152816281728182819282028212822282328242825282628272828282928302831283228332834283528362837283828392840284128422843284428452846284728482849285028512852285328542855285628572858285928602861286228632864286528662867286828692870287128722873287428752876287728782879288028812882288328842885288628872888288928902891289228932894289528962897289828992900290129022903290429052906290729082909291029112912291329142915291629172918291929202921292229232924292529262927292829292930293129322933293429352936293729382939294029412942294329442945294629472948294929502951295229532954295529562957295829592960296129622963296429652966296729682969297029712972297329742975297629772978297929802981298229832984298529862987298829892990299129922993299429952996299729982999300030013002300330043005300630073008300930103011301230133014301530163017301830193020302130223023302430253026302730283029303030313032303330343035303630373038303930403041304230433044304530463047304830493050305130523053305430553056305730583059306030613062306330643065306630673068306930703071307230733074307530763077307830793080308130823083308430853086308730883089309030913092309330943095309630973098309931003101310231033104310531063107310831093110311131123113311431153116311731183119312031213122312331243125312631273128312931303131313231333134313531363137313831393140314131423143314431453146314731483149315031513152315331543155315631573158315931603161316231633164316531663167316831693170317131723173317431753176317731783179318031813182318331843185318631873188318931903191319231933194319531963197319831993200320132023203320432053206
  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 System.Runtime.InteropServices;
  45. using System.Threading;
  46. using log4net;
  47. using OpenMetaverse;
  48. using OpenSim.Framework;
  49. using OpenSim.Region.PhysicsModules.SharedBase;
  50. namespace OpenSim.Region.PhysicsModule.ODE
  51. {
  52. /// <summary>
  53. /// Various properties that ODE uses for AMotors but isn't exposed in ODE.NET so we must define them ourselves.
  54. /// </summary>
  55. public class OdePrim : PhysicsActor
  56. {
  57. private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
  58. private bool m_isphysical;
  59. public int ExpectedCollisionContacts { get { return m_expectedCollisionContacts; } }
  60. private int m_expectedCollisionContacts = 0;
  61. /// <summary>
  62. /// Gets collide bits so that we can still perform land collisions if a mesh fails to load.
  63. /// </summary>
  64. private int BadMeshAssetCollideBits
  65. {
  66. get { return m_isphysical ? (int)CollisionCategories.Land : 0; }
  67. }
  68. /// <summary>
  69. /// Is this prim subject to physics? Even if not, it's still solid for collision purposes.
  70. /// </summary>
  71. public override bool IsPhysical
  72. {
  73. get { return m_isphysical; }
  74. set
  75. {
  76. m_isphysical = value;
  77. if (!m_isphysical)
  78. {
  79. _zeroFlag = true; // Zero the remembered last velocity
  80. m_lastVelocity = Vector3.Zero;
  81. _acceleration = Vector3.Zero;
  82. _velocity = Vector3.Zero;
  83. m_taintVelocity = Vector3.Zero;
  84. m_rotationalVelocity = Vector3.Zero;
  85. }
  86. }
  87. }
  88. private Vector3 _position;
  89. private Vector3 _velocity;
  90. private Vector3 _torque;
  91. private Vector3 m_lastVelocity;
  92. private Vector3 m_lastposition;
  93. private Quaternion m_lastorientation = new Quaternion();
  94. private Vector3 m_rotationalVelocity;
  95. private Vector3 _size;
  96. private Vector3 _acceleration;
  97. // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
  98. private Quaternion _orientation;
  99. private Vector3 m_taintposition;
  100. private Vector3 m_taintsize;
  101. private Vector3 m_taintVelocity;
  102. private Vector3 m_taintTorque;
  103. private Quaternion m_taintrot;
  104. private IntPtr Amotor = IntPtr.Zero;
  105. private byte m_taintAngularLock = 0;
  106. private byte m_angularlock = 0;
  107. private bool m_assetFailed = false;
  108. private Vector3 m_PIDTarget;
  109. private float m_PIDTau;
  110. private float PID_D = 35f;
  111. private float PID_G = 25f;
  112. // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
  113. // and are for non-VEHICLES only.
  114. private float m_PIDHoverHeight;
  115. private float m_PIDHoverTau;
  116. private bool m_useHoverPID;
  117. private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
  118. private float m_targetHoverHeight;
  119. private float m_groundHeight;
  120. private float m_waterHeight;
  121. private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
  122. // private float m_tensor = 5f;
  123. private int body_autodisable_frames = 20;
  124. private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
  125. | CollisionCategories.Space
  126. | CollisionCategories.Body
  127. | CollisionCategories.Character
  128. );
  129. private bool m_taintshape;
  130. private bool m_taintPhysics;
  131. private bool m_collidesLand = true;
  132. private bool m_collidesWater;
  133. // Default we're a Geometry
  134. private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
  135. // Default, Collide with Other Geometries, spaces and Bodies
  136. private CollisionCategories m_collisionFlags = m_default_collisionFlags;
  137. public bool m_taintremove { get; private set; }
  138. public bool m_taintdisable { get; private set; }
  139. internal bool m_disabled;
  140. public bool m_taintadd { get; private set; }
  141. public bool m_taintselected { get; private set; }
  142. public bool m_taintCollidesWater { get; private set; }
  143. private bool m_taintforce = false;
  144. private bool m_taintaddangularforce = false;
  145. private Vector3 m_force;
  146. private List<Vector3> m_forcelist = new List<Vector3>();
  147. private List<Vector3> m_angularforcelist = new List<Vector3>();
  148. private PrimitiveBaseShape _pbs;
  149. private OdeScene _parent_scene;
  150. /// <summary>
  151. /// The physics space which contains prim geometries
  152. /// </summary>
  153. public IntPtr m_targetSpace = IntPtr.Zero;
  154. /// <summary>
  155. /// The prim geometry, used for collision detection.
  156. /// </summary>
  157. /// <remarks>
  158. /// This is never null except for a brief period when the geometry needs to be replaced (due to resizing or
  159. /// mesh change) or when the physical prim is being removed from the scene.
  160. /// </remarks>
  161. public IntPtr prim_geom { get; private set; }
  162. public IntPtr _triMeshData { get; private set; }
  163. private IntPtr _linkJointGroup = IntPtr.Zero;
  164. private PhysicsActor _parent;
  165. private PhysicsActor m_taintparent;
  166. private List<OdePrim> childrenPrim = new List<OdePrim>();
  167. private bool iscolliding;
  168. private bool m_isSelected;
  169. internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
  170. private bool m_throttleUpdates;
  171. private int throttleCounter;
  172. public int m_interpenetrationcount { get; private set; }
  173. internal float m_collisionscore;
  174. public int m_roundsUnderMotionThreshold { get; private set; }
  175. public bool outofBounds { get; private set; }
  176. private float m_density = 10.000006836f; // Aluminum g/cm3;
  177. public bool _zeroFlag { get; private set; }
  178. private bool m_lastUpdateSent;
  179. public IntPtr Body = IntPtr.Zero;
  180. private Vector3 _target_velocity;
  181. private d.Mass pMass;
  182. private int m_eventsubscription;
  183. private CollisionEventUpdate CollisionEventsThisFrame = new CollisionEventUpdate();
  184. /// <summary>
  185. /// Signal whether there were collisions on the previous frame, so we know if we need to send the
  186. /// empty CollisionEventsThisFrame to the prim so that it can detect the end of a collision.
  187. /// </summary>
  188. /// <remarks>
  189. /// This is probably a temporary measure, pending storing this information consistently in CollisionEventUpdate itself.
  190. /// </remarks>
  191. private bool m_collisionsOnPreviousFrame;
  192. private IntPtr m_linkJoint = IntPtr.Zero;
  193. internal volatile bool childPrim;
  194. private ODEDynamics m_vehicle;
  195. internal int m_material = (int)Material.Wood;
  196. public OdePrim(
  197. String primName, OdeScene parent_scene, Vector3 pos, Vector3 size,
  198. Quaternion rotation, PrimitiveBaseShape pbs, bool pisPhysical)
  199. {
  200. Name = primName;
  201. m_vehicle = new ODEDynamics();
  202. //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
  203. if (!pos.IsFinite())
  204. {
  205. pos = new Vector3(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f),
  206. parent_scene.GetTerrainHeightAtXY(((float)Constants.RegionSize * 0.5f), ((float)Constants.RegionSize * 0.5f)) + 0.5f);
  207. m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Position for {0}", Name);
  208. }
  209. _position = pos;
  210. m_taintposition = pos;
  211. PID_D = parent_scene.bodyPIDD;
  212. PID_G = parent_scene.bodyPIDG;
  213. m_density = parent_scene.geomDefaultDensity;
  214. // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
  215. body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
  216. prim_geom = IntPtr.Zero;
  217. if (!pos.IsFinite())
  218. {
  219. size = new Vector3(0.5f, 0.5f, 0.5f);
  220. m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Size for {0}", Name);
  221. }
  222. if (size.X <= 0) size.X = 0.01f;
  223. if (size.Y <= 0) size.Y = 0.01f;
  224. if (size.Z <= 0) size.Z = 0.01f;
  225. _size = size;
  226. m_taintsize = _size;
  227. if (!QuaternionIsFinite(rotation))
  228. {
  229. rotation = Quaternion.Identity;
  230. m_log.WarnFormat("[PHYSICS]: Got nonFinite Object create Rotation for {0}", Name);
  231. }
  232. _orientation = rotation;
  233. m_taintrot = _orientation;
  234. _pbs = pbs;
  235. _parent_scene = parent_scene;
  236. m_targetSpace = (IntPtr)0;
  237. if (pos.Z < 0)
  238. {
  239. IsPhysical = false;
  240. }
  241. else
  242. {
  243. IsPhysical = pisPhysical;
  244. // If we're physical, we need to be in the master space for now.
  245. // linksets *should* be in a space together.. but are not currently
  246. if (IsPhysical)
  247. m_targetSpace = _parent_scene.space;
  248. }
  249. m_taintadd = true;
  250. m_assetFailed = false;
  251. _parent_scene.AddPhysicsActorTaint(this);
  252. }
  253. public override int PhysicsActorType
  254. {
  255. get { return (int) ActorTypes.Prim; }
  256. set { return; }
  257. }
  258. public override bool SetAlwaysRun
  259. {
  260. get { return false; }
  261. set { return; }
  262. }
  263. public override bool Grabbed
  264. {
  265. set { return; }
  266. }
  267. public override bool Selected
  268. {
  269. set
  270. {
  271. // This only makes the object not collidable if the object
  272. // is physical or the object is modified somehow *IN THE FUTURE*
  273. // without this, if an avatar selects prim, they can walk right
  274. // through it while it's selected
  275. m_collisionscore = 0;
  276. if ((IsPhysical && !_zeroFlag) || !value)
  277. {
  278. m_taintselected = value;
  279. _parent_scene.AddPhysicsActorTaint(this);
  280. }
  281. else
  282. {
  283. m_taintselected = value;
  284. m_isSelected = value;
  285. }
  286. if (m_isSelected)
  287. disableBodySoft();
  288. }
  289. }
  290. /// <summary>
  291. /// Set a new geometry for this prim.
  292. /// </summary>
  293. /// <param name="geom"></param>
  294. private void SetGeom(IntPtr geom)
  295. {
  296. prim_geom = geom;
  297. //Console.WriteLine("SetGeom to " + prim_geom + " for " + Name);
  298. if (m_assetFailed)
  299. {
  300. d.GeomSetCategoryBits(prim_geom, 0);
  301. d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
  302. }
  303. else
  304. {
  305. d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
  306. d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
  307. }
  308. _parent_scene.geom_name_map[prim_geom] = Name;
  309. _parent_scene.actor_name_map[prim_geom] = this;
  310. if (childPrim)
  311. {
  312. if (_parent != null && _parent is OdePrim)
  313. {
  314. OdePrim parent = (OdePrim)_parent;
  315. //Console.WriteLine("SetGeom calls ChildSetGeom");
  316. parent.ChildSetGeom(this);
  317. }
  318. }
  319. //m_log.Warn("Setting Geom to: " + prim_geom);
  320. }
  321. private void enableBodySoft()
  322. {
  323. if (!childPrim)
  324. {
  325. if (IsPhysical && Body != IntPtr.Zero)
  326. {
  327. d.BodyEnable(Body);
  328. if (m_vehicle.Type != Vehicle.TYPE_NONE)
  329. m_vehicle.Enable(Body, _parent_scene);
  330. }
  331. m_disabled = false;
  332. }
  333. }
  334. private void disableBodySoft()
  335. {
  336. m_disabled = true;
  337. if (IsPhysical && Body != IntPtr.Zero)
  338. {
  339. d.BodyDisable(Body);
  340. }
  341. }
  342. /// <summary>
  343. /// Make a prim subject to physics.
  344. /// </summary>
  345. private void enableBody()
  346. {
  347. // Don't enable this body if we're a child prim
  348. // this should be taken care of in the parent function not here
  349. if (!childPrim)
  350. {
  351. // Sets the geom to a body
  352. Body = d.BodyCreate(_parent_scene.world);
  353. setMass();
  354. d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
  355. d.Quaternion myrot = new d.Quaternion();
  356. myrot.X = _orientation.X;
  357. myrot.Y = _orientation.Y;
  358. myrot.Z = _orientation.Z;
  359. myrot.W = _orientation.W;
  360. d.BodySetQuaternion(Body, ref myrot);
  361. d.GeomSetBody(prim_geom, Body);
  362. if (m_assetFailed)
  363. {
  364. d.GeomSetCategoryBits(prim_geom, 0);
  365. d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
  366. }
  367. else
  368. {
  369. m_collisionCategories |= CollisionCategories.Body;
  370. m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
  371. }
  372. d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
  373. d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
  374. d.BodySetAutoDisableFlag(Body, true);
  375. d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
  376. // disconnect from world gravity so we can apply buoyancy
  377. d.BodySetGravityMode (Body, false);
  378. m_interpenetrationcount = 0;
  379. m_collisionscore = 0;
  380. m_disabled = false;
  381. // The body doesn't already have a finite rotation mode set here
  382. if (m_angularlock != 0 && _parent == null)
  383. {
  384. createAMotor(m_angularlock);
  385. }
  386. if (m_vehicle.Type != Vehicle.TYPE_NONE)
  387. {
  388. m_vehicle.Enable(Body, _parent_scene);
  389. }
  390. _parent_scene.ActivatePrim(this);
  391. }
  392. }
  393. #region Mass Calculation
  394. private float CalculateMass()
  395. {
  396. float volume = _size.X * _size.Y * _size.Z; // default
  397. float tmp;
  398. float returnMass = 0;
  399. float hollowAmount = (float)_pbs.ProfileHollow * 2.0e-5f;
  400. float hollowVolume = hollowAmount * hollowAmount;
  401. switch (_pbs.ProfileShape)
  402. {
  403. case ProfileShape.Square:
  404. // default box
  405. if (_pbs.PathCurve == (byte)Extrusion.Straight)
  406. {
  407. if (hollowAmount > 0.0)
  408. {
  409. switch (_pbs.HollowShape)
  410. {
  411. case HollowShape.Square:
  412. case HollowShape.Same:
  413. break;
  414. case HollowShape.Circle:
  415. hollowVolume *= 0.78539816339f;
  416. break;
  417. case HollowShape.Triangle:
  418. hollowVolume *= (0.5f * .5f);
  419. break;
  420. default:
  421. hollowVolume = 0;
  422. break;
  423. }
  424. volume *= (1.0f - hollowVolume);
  425. }
  426. }
  427. else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
  428. {
  429. //a tube
  430. volume *= 0.78539816339e-2f * (float)(200 - _pbs.PathScaleX);
  431. tmp= 1.0f -2.0e-2f * (float)(200 - _pbs.PathScaleY);
  432. volume -= volume*tmp*tmp;
  433. if (hollowAmount > 0.0)
  434. {
  435. hollowVolume *= hollowAmount;
  436. switch (_pbs.HollowShape)
  437. {
  438. case HollowShape.Square:
  439. case HollowShape.Same:
  440. break;
  441. case HollowShape.Circle:
  442. hollowVolume *= 0.78539816339f;;
  443. break;
  444. case HollowShape.Triangle:
  445. hollowVolume *= 0.5f * 0.5f;
  446. break;
  447. default:
  448. hollowVolume = 0;
  449. break;
  450. }
  451. volume *= (1.0f - hollowVolume);
  452. }
  453. }
  454. break;
  455. case ProfileShape.Circle:
  456. if (_pbs.PathCurve == (byte)Extrusion.Straight)
  457. {
  458. volume *= 0.78539816339f; // elipse base
  459. if (hollowAmount > 0.0)
  460. {
  461. switch (_pbs.HollowShape)
  462. {
  463. case HollowShape.Same:
  464. case HollowShape.Circle:
  465. break;
  466. case HollowShape.Square:
  467. hollowVolume *= 0.5f * 2.5984480504799f;
  468. break;
  469. case HollowShape.Triangle:
  470. hollowVolume *= .5f * 1.27323954473516f;
  471. break;
  472. default:
  473. hollowVolume = 0;
  474. break;
  475. }
  476. volume *= (1.0f - hollowVolume);
  477. }
  478. }
  479. else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
  480. {
  481. volume *= 0.61685027506808491367715568749226e-2f * (float)(200 - _pbs.PathScaleX);
  482. tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
  483. volume *= (1.0f - tmp * tmp);
  484. if (hollowAmount > 0.0)
  485. {
  486. // calculate the hollow volume by it's shape compared to the prim shape
  487. hollowVolume *= hollowAmount;
  488. switch (_pbs.HollowShape)
  489. {
  490. case HollowShape.Same:
  491. case HollowShape.Circle:
  492. break;
  493. case HollowShape.Square:
  494. hollowVolume *= 0.5f * 2.5984480504799f;
  495. break;
  496. case HollowShape.Triangle:
  497. hollowVolume *= .5f * 1.27323954473516f;
  498. break;
  499. default:
  500. hollowVolume = 0;
  501. break;
  502. }
  503. volume *= (1.0f - hollowVolume);
  504. }
  505. }
  506. break;
  507. case ProfileShape.HalfCircle:
  508. if (_pbs.PathCurve == (byte)Extrusion.Curve1)
  509. {
  510. volume *= 0.52359877559829887307710723054658f;
  511. }
  512. break;
  513. case ProfileShape.EquilateralTriangle:
  514. if (_pbs.PathCurve == (byte)Extrusion.Straight)
  515. {
  516. volume *= 0.32475953f;
  517. if (hollowAmount > 0.0)
  518. {
  519. // calculate the hollow volume by it's shape compared to the prim shape
  520. switch (_pbs.HollowShape)
  521. {
  522. case HollowShape.Same:
  523. case HollowShape.Triangle:
  524. hollowVolume *= .25f;
  525. break;
  526. case HollowShape.Square:
  527. hollowVolume *= 0.499849f * 3.07920140172638f;
  528. break;
  529. case HollowShape.Circle:
  530. // Hollow shape is a perfect cyllinder in respect to the cube's scale
  531. // Cyllinder hollow volume calculation
  532. hollowVolume *= 0.1963495f * 3.07920140172638f;
  533. break;
  534. default:
  535. hollowVolume = 0;
  536. break;
  537. }
  538. volume *= (1.0f - hollowVolume);
  539. }
  540. }
  541. else if (_pbs.PathCurve == (byte)Extrusion.Curve1)
  542. {
  543. volume *= 0.32475953f;
  544. volume *= 0.01f * (float)(200 - _pbs.PathScaleX);
  545. tmp = 1.0f - .02f * (float)(200 - _pbs.PathScaleY);
  546. volume *= (1.0f - tmp * tmp);
  547. if (hollowAmount > 0.0)
  548. {
  549. hollowVolume *= hollowAmount;
  550. switch (_pbs.HollowShape)
  551. {
  552. case HollowShape.Same:
  553. case HollowShape.Triangle:
  554. hollowVolume *= .25f;
  555. break;
  556. case HollowShape.Square:
  557. hollowVolume *= 0.499849f * 3.07920140172638f;
  558. break;
  559. case HollowShape.Circle:
  560. hollowVolume *= 0.1963495f * 3.07920140172638f;
  561. break;
  562. default:
  563. hollowVolume = 0;
  564. break;
  565. }
  566. volume *= (1.0f - hollowVolume);
  567. }
  568. }
  569. break;
  570. default:
  571. break;
  572. }
  573. float taperX1;
  574. float taperY1;
  575. float taperX;
  576. float taperY;
  577. float pathBegin;
  578. float pathEnd;
  579. float profileBegin;
  580. float profileEnd;
  581. if (_pbs.PathCurve == (byte)Extrusion.Straight || _pbs.PathCurve == (byte)Extrusion.Flexible)
  582. {
  583. taperX1 = _pbs.PathScaleX * 0.01f;
  584. if (taperX1 > 1.0f)
  585. taperX1 = 2.0f - taperX1;
  586. taperX = 1.0f - taperX1;
  587. taperY1 = _pbs.PathScaleY * 0.01f;
  588. if (taperY1 > 1.0f)
  589. taperY1 = 2.0f - taperY1;
  590. taperY = 1.0f - taperY1;
  591. }
  592. else
  593. {
  594. taperX = _pbs.PathTaperX * 0.01f;
  595. if (taperX < 0.0f)
  596. taperX = -taperX;
  597. taperX1 = 1.0f - taperX;
  598. taperY = _pbs.PathTaperY * 0.01f;
  599. if (taperY < 0.0f)
  600. taperY = -taperY;
  601. taperY1 = 1.0f - taperY;
  602. }
  603. volume *= (taperX1 * taperY1 + 0.5f * (taperX1 * taperY + taperX * taperY1) + 0.3333333333f * taperX * taperY);
  604. pathBegin = (float)_pbs.PathBegin * 2.0e-5f;
  605. pathEnd = 1.0f - (float)_pbs.PathEnd * 2.0e-5f;
  606. volume *= (pathEnd - pathBegin);
  607. // this is crude aproximation
  608. profileBegin = (float)_pbs.ProfileBegin * 2.0e-5f;
  609. profileEnd = 1.0f - (float)_pbs.ProfileEnd * 2.0e-5f;
  610. volume *= (profileEnd - profileBegin);
  611. returnMass = m_density * volume;
  612. if (returnMass <= 0)
  613. returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
  614. // else if (returnMass > _parent_scene.maximumMassObject)
  615. // returnMass = _parent_scene.maximumMassObject;
  616. // Recursively calculate mass
  617. bool HasChildPrim = false;
  618. lock (childrenPrim)
  619. {
  620. if (childrenPrim.Count > 0)
  621. {
  622. HasChildPrim = true;
  623. }
  624. }
  625. if (HasChildPrim)
  626. {
  627. OdePrim[] childPrimArr = new OdePrim[0];
  628. lock (childrenPrim)
  629. childPrimArr = childrenPrim.ToArray();
  630. for (int i = 0; i < childPrimArr.Length; i++)
  631. {
  632. if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
  633. returnMass += childPrimArr[i].CalculateMass();
  634. // failsafe, this shouldn't happen but with OpenSim, you never know :)
  635. if (i > 256)
  636. break;
  637. }
  638. }
  639. if (returnMass > _parent_scene.maximumMassObject)
  640. returnMass = _parent_scene.maximumMassObject;
  641. return returnMass;
  642. }
  643. #endregion
  644. private void setMass()
  645. {
  646. if (Body != (IntPtr) 0)
  647. {
  648. float newmass = CalculateMass();
  649. //m_log.Info("[PHYSICS]: New Mass: " + newmass.ToString());
  650. d.MassSetBoxTotal(out pMass, newmass, _size.X, _size.Y, _size.Z);
  651. d.BodySetMass(Body, ref pMass);
  652. }
  653. }
  654. private void setAngularVelocity(float x, float y, float z)
  655. {
  656. if (Body != (IntPtr)0)
  657. {
  658. d.BodySetAngularVel(Body, x, y, z);
  659. }
  660. }
  661. /// <summary>
  662. /// Stop a prim from being subject to physics.
  663. /// </summary>
  664. internal void disableBody()
  665. {
  666. //this kills the body so things like 'mesh' can re-create it.
  667. lock (this)
  668. {
  669. if (!childPrim)
  670. {
  671. if (Body != IntPtr.Zero)
  672. {
  673. _parent_scene.DeactivatePrim(this);
  674. m_collisionCategories &= ~CollisionCategories.Body;
  675. m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
  676. if (m_assetFailed)
  677. {
  678. d.GeomSetCategoryBits(prim_geom, 0);
  679. d.GeomSetCollideBits(prim_geom, 0);
  680. }
  681. else
  682. {
  683. d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
  684. d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
  685. }
  686. d.BodyDestroy(Body);
  687. lock (childrenPrim)
  688. {
  689. if (childrenPrim.Count > 0)
  690. {
  691. foreach (OdePrim prm in childrenPrim)
  692. {
  693. _parent_scene.DeactivatePrim(prm);
  694. prm.Body = IntPtr.Zero;
  695. }
  696. }
  697. }
  698. Body = IntPtr.Zero;
  699. }
  700. }
  701. else
  702. {
  703. _parent_scene.DeactivatePrim(this);
  704. m_collisionCategories &= ~CollisionCategories.Body;
  705. m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
  706. if (m_assetFailed)
  707. {
  708. d.GeomSetCategoryBits(prim_geom, 0);
  709. d.GeomSetCollideBits(prim_geom, 0);
  710. }
  711. else
  712. {
  713. d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
  714. d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
  715. }
  716. Body = IntPtr.Zero;
  717. }
  718. }
  719. m_disabled = true;
  720. m_collisionscore = 0;
  721. }
  722. private static Dictionary<IMesh, IntPtr> m_MeshToTriMeshMap = new Dictionary<IMesh, IntPtr>();
  723. private void setMesh(OdeScene parent_scene, IMesh mesh)
  724. {
  725. // m_log.DebugFormat("[ODE PRIM]: Setting mesh on {0} to {1}", Name, mesh);
  726. // This sleeper is there to moderate how long it takes between
  727. // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
  728. //Thread.Sleep(10);
  729. //Kill Body so that mesh can re-make the geom
  730. if (IsPhysical && Body != IntPtr.Zero)
  731. {
  732. if (childPrim)
  733. {
  734. if (_parent != null)
  735. {
  736. OdePrim parent = (OdePrim)_parent;
  737. parent.ChildDelink(this);
  738. }
  739. }
  740. else
  741. {
  742. disableBody();
  743. }
  744. }
  745. IntPtr vertices, indices;
  746. int vertexCount, indexCount;
  747. int vertexStride, triStride;
  748. mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount); // Note, that vertices are fixed in unmanaged heap
  749. mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount); // Also fixed, needs release after usage
  750. m_expectedCollisionContacts = indexCount;
  751. mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
  752. // We must lock here since m_MeshToTriMeshMap is static and multiple scene threads may call this method at
  753. // the same time.
  754. lock (m_MeshToTriMeshMap)
  755. {
  756. if (m_MeshToTriMeshMap.ContainsKey(mesh))
  757. {
  758. _triMeshData = m_MeshToTriMeshMap[mesh];
  759. }
  760. else
  761. {
  762. _triMeshData = d.GeomTriMeshDataCreate();
  763. d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount, triStride);
  764. d.GeomTriMeshDataPreprocess(_triMeshData);
  765. m_MeshToTriMeshMap[mesh] = _triMeshData;
  766. }
  767. }
  768. // _parent_scene.waitForSpaceUnlock(m_targetSpace);
  769. try
  770. {
  771. SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null));
  772. }
  773. catch (AccessViolationException)
  774. {
  775. m_log.ErrorFormat("[PHYSICS]: MESH LOCKED FOR {0}", Name);
  776. return;
  777. }
  778. // if (IsPhysical && Body == (IntPtr) 0)
  779. // {
  780. // Recreate the body
  781. // m_interpenetrationcount = 0;
  782. // m_collisionscore = 0;
  783. // enableBody();
  784. // }
  785. }
  786. internal void ProcessTaints()
  787. {
  788. #if SPAM
  789. Console.WriteLine("ZProcessTaints for " + Name);
  790. #endif
  791. // This must be processed as the very first taint so that later operations have a prim_geom to work with
  792. // if this is a new prim.
  793. if (m_taintadd)
  794. changeadd();
  795. if (!_position.ApproxEquals(m_taintposition, 0f))
  796. changemove();
  797. if (m_taintrot != _orientation)
  798. {
  799. if (childPrim && IsPhysical) // For physical child prim...
  800. {
  801. rotate();
  802. // KF: ODE will also rotate the parent prim!
  803. // so rotate the root back to where it was
  804. OdePrim parent = (OdePrim)_parent;
  805. parent.rotate();
  806. }
  807. else
  808. {
  809. //Just rotate the prim
  810. rotate();
  811. }
  812. }
  813. if (m_taintPhysics != IsPhysical && !(m_taintparent != _parent))
  814. changePhysicsStatus();
  815. if (!_size.ApproxEquals(m_taintsize, 0f))
  816. changesize();
  817. if (m_taintshape)
  818. changeshape();
  819. if (m_taintforce)
  820. changeAddForce();
  821. if (m_taintaddangularforce)
  822. changeAddAngularForce();
  823. if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
  824. changeSetTorque();
  825. if (m_taintdisable)
  826. changedisable();
  827. if (m_taintselected != m_isSelected)
  828. changeSelectedStatus();
  829. if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
  830. changevelocity();
  831. if (m_taintparent != _parent)
  832. changelink();
  833. if (m_taintCollidesWater != m_collidesWater)
  834. changefloatonwater();
  835. if (m_taintAngularLock != m_angularlock)
  836. changeAngularLock();
  837. }
  838. /// <summary>
  839. /// Change prim in response to an angular lock taint.
  840. /// </summary>
  841. private void changeAngularLock()
  842. {
  843. // do we have a Physical object?
  844. if (Body != IntPtr.Zero)
  845. {
  846. //Check that we have a Parent
  847. //If we have a parent then we're not authorative here
  848. if (_parent == null)
  849. {
  850. if (m_taintAngularLock != 0)
  851. {
  852. createAMotor(m_taintAngularLock);
  853. }
  854. else
  855. {
  856. if (Amotor != IntPtr.Zero)
  857. {
  858. d.JointDestroy(Amotor);
  859. Amotor = IntPtr.Zero;
  860. }
  861. }
  862. }
  863. }
  864. m_angularlock = m_taintAngularLock;
  865. }
  866. /// <summary>
  867. /// Change prim in response to a link taint.
  868. /// </summary>
  869. private void changelink()
  870. {
  871. // If the newly set parent is not null
  872. // create link
  873. if (_parent == null && m_taintparent != null)
  874. {
  875. if (m_taintparent.PhysicsActorType == (int)ActorTypes.Prim)
  876. {
  877. OdePrim obj = (OdePrim)m_taintparent;
  878. //obj.disableBody();
  879. //Console.WriteLine("changelink calls ParentPrim");
  880. obj.AddChildPrim(this);
  881. /*
  882. if (obj.Body != (IntPtr)0 && Body != (IntPtr)0 && obj.Body != Body)
  883. {
  884. _linkJointGroup = d.JointGroupCreate(0);
  885. m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
  886. d.JointAttach(m_linkJoint, obj.Body, Body);
  887. d.JointSetFixed(m_linkJoint);
  888. }
  889. */
  890. }
  891. }
  892. // If the newly set parent is null
  893. // destroy link
  894. else if (_parent != null && m_taintparent == null)
  895. {
  896. //Console.WriteLine(" changelink B");
  897. if (_parent is OdePrim)
  898. {
  899. OdePrim obj = (OdePrim)_parent;
  900. obj.ChildDelink(this);
  901. childPrim = false;
  902. //_parent = null;
  903. }
  904. /*
  905. if (Body != (IntPtr)0 && _linkJointGroup != (IntPtr)0)
  906. d.JointGroupDestroy(_linkJointGroup);
  907. _linkJointGroup = (IntPtr)0;
  908. m_linkJoint = (IntPtr)0;
  909. */
  910. }
  911. _parent = m_taintparent;
  912. m_taintPhysics = IsPhysical;
  913. }
  914. /// <summary>
  915. /// Add a child prim to this parent prim.
  916. /// </summary>
  917. /// <param name="prim">Child prim</param>
  918. private void AddChildPrim(OdePrim prim)
  919. {
  920. if (LocalID == prim.LocalID)
  921. return;
  922. if (Body == IntPtr.Zero)
  923. {
  924. Body = d.BodyCreate(_parent_scene.world);
  925. setMass();
  926. }
  927. lock (childrenPrim)
  928. {
  929. if (childrenPrim.Contains(prim))
  930. return;
  931. // m_log.DebugFormat(
  932. // "[ODE PRIM]: Linking prim {0} {1} to {2} {3}", prim.Name, prim.LocalID, Name, LocalID);
  933. childrenPrim.Add(prim);
  934. foreach (OdePrim prm in childrenPrim)
  935. {
  936. d.Mass m2;
  937. d.MassSetZero(out m2);
  938. d.MassSetBoxTotal(out m2, prm.CalculateMass(), prm._size.X, prm._size.Y, prm._size.Z);
  939. d.Quaternion quat = new d.Quaternion();
  940. quat.W = prm._orientation.W;
  941. quat.X = prm._orientation.X;
  942. quat.Y = prm._orientation.Y;
  943. quat.Z = prm._orientation.Z;
  944. d.Matrix3 mat = new d.Matrix3();
  945. d.RfromQ(out mat, ref quat);
  946. d.MassRotate(ref m2, ref mat);
  947. d.MassTranslate(ref m2, Position.X - prm.Position.X, Position.Y - prm.Position.Y, Position.Z - prm.Position.Z);
  948. d.MassAdd(ref pMass, ref m2);
  949. }
  950. foreach (OdePrim prm in childrenPrim)
  951. {
  952. prm.m_collisionCategories |= CollisionCategories.Body;
  953. prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
  954. //Console.WriteLine(" GeomSetCategoryBits 1: " + prm.prim_geom + " - " + (int)prm.m_collisionCategories + " for " + Name);
  955. if (prm.m_assetFailed)
  956. {
  957. d.GeomSetCategoryBits(prm.prim_geom, 0);
  958. d.GeomSetCollideBits(prm.prim_geom, (uint)prm.BadMeshAssetCollideBits);
  959. }
  960. else
  961. {
  962. d.GeomSetCategoryBits(prm.prim_geom, (uint)prm.m_collisionCategories);
  963. d.GeomSetCollideBits(prm.prim_geom, (uint)prm.m_collisionFlags);
  964. }
  965. d.Quaternion quat = new d.Quaternion();
  966. quat.W = prm._orientation.W;
  967. quat.X = prm._orientation.X;
  968. quat.Y = prm._orientation.Y;
  969. quat.Z = prm._orientation.Z;
  970. d.Matrix3 mat = new d.Matrix3();
  971. d.RfromQ(out mat, ref quat);
  972. if (Body != IntPtr.Zero)
  973. {
  974. d.GeomSetBody(prm.prim_geom, Body);
  975. prm.childPrim = true;
  976. d.GeomSetOffsetWorldPosition(prm.prim_geom, prm.Position.X , prm.Position.Y, prm.Position.Z);
  977. //d.GeomSetOffsetPosition(prim.prim_geom,
  978. // (Position.X - prm.Position.X) - pMass.c.X,
  979. // (Position.Y - prm.Position.Y) - pMass.c.Y,
  980. // (Position.Z - prm.Position.Z) - pMass.c.Z);
  981. d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat);
  982. //d.GeomSetOffsetRotation(prm.prim_geom, ref mat);
  983. d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
  984. d.BodySetMass(Body, ref pMass);
  985. }
  986. else
  987. {
  988. m_log.DebugFormat("[PHYSICS]: {0} ain't got no boooooooooddy, no body", Name);
  989. }
  990. prm.m_interpenetrationcount = 0;
  991. prm.m_collisionscore = 0;
  992. prm.m_disabled = false;
  993. prm.Body = Body;
  994. _parent_scene.ActivatePrim(prm);
  995. }
  996. m_collisionCategories |= CollisionCategories.Body;
  997. m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
  998. if (m_assetFailed)
  999. {
  1000. d.GeomSetCategoryBits(prim_geom, 0);
  1001. d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
  1002. }
  1003. else
  1004. {
  1005. //Console.WriteLine("GeomSetCategoryBits 2: " + prim_geom + " - " + (int)m_collisionCategories + " for " + Name);
  1006. d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
  1007. //Console.WriteLine(" Post GeomSetCategoryBits 2");
  1008. d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
  1009. }
  1010. d.Quaternion quat2 = new d.Quaternion();
  1011. quat2.W = _orientation.W;
  1012. quat2.X = _orientation.X;
  1013. quat2.Y = _orientation.Y;
  1014. quat2.Z = _orientation.Z;
  1015. d.Matrix3 mat2 = new d.Matrix3();
  1016. d.RfromQ(out mat2, ref quat2);
  1017. d.GeomSetBody(prim_geom, Body);
  1018. d.GeomSetOffsetWorldPosition(prim_geom, Position.X - pMass.c.X, Position.Y - pMass.c.Y, Position.Z - pMass.c.Z);
  1019. //d.GeomSetOffsetPosition(prim.prim_geom,
  1020. // (Position.X - prm.Position.X) - pMass.c.X,
  1021. // (Position.Y - prm.Position.Y) - pMass.c.Y,
  1022. // (Position.Z - prm.Position.Z) - pMass.c.Z);
  1023. //d.GeomSetOffsetRotation(prim_geom, ref mat2);
  1024. d.MassTranslate(ref pMass, -pMass.c.X, -pMass.c.Y, -pMass.c.Z);
  1025. d.BodySetMass(Body, ref pMass);
  1026. d.BodySetAutoDisableFlag(Body, true);
  1027. d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
  1028. m_interpenetrationcount = 0;
  1029. m_collisionscore = 0;
  1030. m_disabled = false;
  1031. // The body doesn't already have a finite rotation mode set here
  1032. // or remove
  1033. if (_parent == null)
  1034. {
  1035. createAMotor(m_angularlock);
  1036. }
  1037. d.BodySetPosition(Body, Position.X, Position.Y, Position.Z);
  1038. if (m_vehicle.Type != Vehicle.TYPE_NONE)
  1039. m_vehicle.Enable(Body, _parent_scene);
  1040. _parent_scene.ActivatePrim(this);
  1041. }
  1042. }
  1043. private void ChildSetGeom(OdePrim odePrim)
  1044. {
  1045. // m_log.DebugFormat(
  1046. // "[ODE PRIM]: ChildSetGeom {0} {1} for {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
  1047. //if (IsPhysical && Body != IntPtr.Zero)
  1048. lock (childrenPrim)
  1049. {
  1050. foreach (OdePrim prm in childrenPrim)
  1051. {
  1052. //prm.childPrim = true;
  1053. prm.disableBody();
  1054. //prm.m_taintparent = null;
  1055. //prm._parent = null;
  1056. //prm.m_taintPhysics = false;
  1057. //prm.m_disabled = true;
  1058. //prm.childPrim = false;
  1059. }
  1060. }
  1061. disableBody();
  1062. // Spurious - Body == IntPtr.Zero after disableBody()
  1063. // if (Body != IntPtr.Zero)
  1064. // {
  1065. // _parent_scene.DeactivatePrim(this);
  1066. // }
  1067. lock (childrenPrim)
  1068. {
  1069. foreach (OdePrim prm in childrenPrim)
  1070. {
  1071. //Console.WriteLine("ChildSetGeom calls ParentPrim");
  1072. AddChildPrim(prm);
  1073. }
  1074. }
  1075. }
  1076. private void ChildDelink(OdePrim odePrim)
  1077. {
  1078. // m_log.DebugFormat(
  1079. // "[ODE PRIM]: Delinking prim {0} {1} from {2} {3}", odePrim.Name, odePrim.LocalID, Name, LocalID);
  1080. // Okay, we have a delinked child.. need to rebuild the body.
  1081. lock (childrenPrim)
  1082. {
  1083. foreach (OdePrim prm in childrenPrim)
  1084. {
  1085. prm.childPrim = true;
  1086. prm.disableBody();
  1087. //prm.m_taintparent = null;
  1088. //prm._parent = null;
  1089. //prm.m_taintPhysics = false;
  1090. //prm.m_disabled = true;
  1091. //prm.childPrim = false;
  1092. }
  1093. }
  1094. disableBody();
  1095. lock (childrenPrim)
  1096. {
  1097. //Console.WriteLine("childrenPrim.Remove " + odePrim);
  1098. childrenPrim.Remove(odePrim);
  1099. }
  1100. // Spurious - Body == IntPtr.Zero after disableBody()
  1101. // if (Body != IntPtr.Zero)
  1102. // {
  1103. // _parent_scene.DeactivatePrim(this);
  1104. // }
  1105. lock (childrenPrim)
  1106. {
  1107. foreach (OdePrim prm in childrenPrim)
  1108. {
  1109. //Console.WriteLine("ChildDelink calls ParentPrim");
  1110. AddChildPrim(prm);
  1111. }
  1112. }
  1113. }
  1114. /// <summary>
  1115. /// Change prim in response to a selection taint.
  1116. /// </summary>
  1117. private void changeSelectedStatus()
  1118. {
  1119. if (m_taintselected)
  1120. {
  1121. m_collisionCategories = CollisionCategories.Selected;
  1122. m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
  1123. // We do the body disable soft twice because 'in theory' a collision could have happened
  1124. // in between the disabling and the collision properties setting
  1125. // which would wake the physical body up from a soft disabling and potentially cause it to fall
  1126. // through the ground.
  1127. // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
  1128. // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
  1129. // so that causes the selected part to wake up and continue moving.
  1130. // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
  1131. // assembly will stop simulating during the selection, because of the lack of atomicity
  1132. // of select operations (their processing could be interrupted by a thread switch, causing
  1133. // simulation to continue before all of the selected object notifications trickle down to
  1134. // the physics engine).
  1135. // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
  1136. // selected and disabled. then, due to a thread switch, the selection processing is
  1137. // interrupted and the physics engine continues to simulate, so the last 50 items, whose
  1138. // selection was not yet processed, continues to simulate. this wakes up ALL of the
  1139. // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
  1140. // up, start simulating again, which in turn wakes up the last 50.
  1141. if (IsPhysical)
  1142. {
  1143. disableBodySoft();
  1144. }
  1145. if (m_assetFailed)
  1146. {
  1147. d.GeomSetCategoryBits(prim_geom, 0);
  1148. d.GeomSetCollideBits(prim_geom, 0);
  1149. }
  1150. else
  1151. {
  1152. d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
  1153. d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
  1154. }
  1155. if (IsPhysical)
  1156. {
  1157. disableBodySoft();
  1158. }
  1159. }
  1160. else
  1161. {
  1162. m_collisionCategories = CollisionCategories.Geom;
  1163. if (IsPhysical)
  1164. m_collisionCategories |= CollisionCategories.Body;
  1165. m_collisionFlags = m_default_collisionFlags;
  1166. if (m_collidesLand)
  1167. m_collisionFlags |= CollisionCategories.Land;
  1168. if (m_collidesWater)
  1169. m_collisionFlags |= CollisionCategories.Water;
  1170. if (m_assetFailed)
  1171. {
  1172. d.GeomSetCategoryBits(prim_geom, 0);
  1173. d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
  1174. }
  1175. else
  1176. {
  1177. d.GeomSetCategoryBits(prim_geom, (uint)m_collisionCategories);
  1178. d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
  1179. }
  1180. if (IsPhysical)
  1181. {
  1182. if (Body != IntPtr.Zero)
  1183. {
  1184. d.BodySetLinearVel(Body, 0f, 0f, 0f);
  1185. d.BodySetForce(Body, 0, 0, 0);
  1186. enableBodySoft();
  1187. }
  1188. }
  1189. }
  1190. resetCollisionAccounting();
  1191. m_isSelected = m_taintselected;
  1192. }//end changeSelectedStatus
  1193. internal void ResetTaints()
  1194. {
  1195. m_taintposition = _position;
  1196. m_taintrot = _orientation;
  1197. m_taintPhysics = IsPhysical;
  1198. m_taintselected = m_isSelected;
  1199. m_taintsize = _size;
  1200. m_taintshape = false;
  1201. m_taintforce = false;
  1202. m_taintdisable = false;
  1203. m_taintVelocity = Vector3.Zero;
  1204. }
  1205. /// <summary>
  1206. /// Create a geometry for the given mesh in the given target space.
  1207. /// </summary>
  1208. /// <param name="m_targetSpace"></param>
  1209. /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
  1210. private void CreateGeom(IntPtr m_targetSpace, IMesh mesh)
  1211. {
  1212. #if SPAM
  1213. Console.WriteLine("CreateGeom:");
  1214. #endif
  1215. if (mesh != null)
  1216. {
  1217. setMesh(_parent_scene, mesh);
  1218. }
  1219. else
  1220. {
  1221. if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
  1222. {
  1223. if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
  1224. {
  1225. if (((_size.X / 2f) > 0f))
  1226. {
  1227. // _parent_scene.waitForSpaceUnlock(m_targetSpace);
  1228. try
  1229. {
  1230. //Console.WriteLine(" CreateGeom 1");
  1231. SetGeom(d.CreateSphere(m_targetSpace, _size.X / 2));
  1232. m_expectedCollisionContacts = 3;
  1233. }
  1234. catch (AccessViolationException)
  1235. {
  1236. m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
  1237. return;
  1238. }
  1239. }
  1240. else
  1241. {
  1242. // _parent_scene.waitForSpaceUnlock(m_targetSpace);
  1243. try
  1244. {
  1245. //Console.WriteLine(" CreateGeom 2");
  1246. SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
  1247. m_expectedCollisionContacts = 4;
  1248. }
  1249. catch (AccessViolationException)
  1250. {
  1251. m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
  1252. return;
  1253. }
  1254. }
  1255. }
  1256. else
  1257. {
  1258. // _parent_scene.waitForSpaceUnlock(m_targetSpace);
  1259. try
  1260. {
  1261. //Console.WriteLine(" CreateGeom 3");
  1262. SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
  1263. m_expectedCollisionContacts = 4;
  1264. }
  1265. catch (AccessViolationException)
  1266. {
  1267. m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
  1268. return;
  1269. }
  1270. }
  1271. }
  1272. else
  1273. {
  1274. // _parent_scene.waitForSpaceUnlock(m_targetSpace);
  1275. try
  1276. {
  1277. //Console.WriteLine(" CreateGeom 4");
  1278. SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
  1279. m_expectedCollisionContacts = 4;
  1280. }
  1281. catch (AccessViolationException)
  1282. {
  1283. m_log.WarnFormat("[PHYSICS]: Unable to create physics proxy for object {0}", Name);
  1284. return;
  1285. }
  1286. }
  1287. }
  1288. }
  1289. /// <summary>
  1290. /// Remove the existing geom from this prim.
  1291. /// </summary>
  1292. /// <param name="m_targetSpace"></param>
  1293. /// <param name="mesh">If null, then a mesh is used that is based on the profile shape data.</param>
  1294. /// <returns>true if the geom was successfully removed, false if it was already gone or the remove failed.</returns>
  1295. internal bool RemoveGeom()
  1296. {
  1297. if (prim_geom != IntPtr.Zero)
  1298. {
  1299. try
  1300. {
  1301. _parent_scene.geom_name_map.Remove(prim_geom);
  1302. _parent_scene.actor_name_map.Remove(prim_geom);
  1303. d.GeomDestroy(prim_geom);
  1304. m_expectedCollisionContacts = 0;
  1305. prim_geom = IntPtr.Zero;
  1306. }
  1307. catch (System.AccessViolationException)
  1308. {
  1309. prim_geom = IntPtr.Zero;
  1310. m_expectedCollisionContacts = 0;
  1311. m_log.ErrorFormat("[PHYSICS]: PrimGeom dead for {0}", Name);
  1312. return false;
  1313. }
  1314. return true;
  1315. }
  1316. else
  1317. {
  1318. m_log.WarnFormat(
  1319. "[ODE PRIM]: Called RemoveGeom() on {0} {1} where geometry was already null.", Name, LocalID);
  1320. return false;
  1321. }
  1322. }
  1323. /// <summary>
  1324. /// Add prim in response to an add taint.
  1325. /// </summary>
  1326. private void changeadd()
  1327. {
  1328. // m_log.DebugFormat("[ODE PRIM]: Adding prim {0}", Name);
  1329. int[] iprimspaceArrItem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
  1330. IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
  1331. if (targetspace == IntPtr.Zero)
  1332. targetspace = _parent_scene.createprimspace(iprimspaceArrItem[0], iprimspaceArrItem[1]);
  1333. m_targetSpace = targetspace;
  1334. IMesh mesh = null;
  1335. if (_parent_scene.needsMeshing(_pbs))
  1336. {
  1337. // Don't need to re-enable body.. it's done in SetMesh
  1338. mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
  1339. // createmesh returns null when it's a shape that isn't a cube.
  1340. // m_log.Debug(m_localID);
  1341. if (mesh == null)
  1342. CheckMeshAsset();
  1343. else
  1344. m_assetFailed = false;
  1345. }
  1346. #if SPAM
  1347. Console.WriteLine("changeadd 1");
  1348. #endif
  1349. CreateGeom(m_targetSpace, mesh);
  1350. d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
  1351. d.Quaternion myrot = new d.Quaternion();
  1352. myrot.X = _orientation.X;
  1353. myrot.Y = _orientation.Y;
  1354. myrot.Z = _orientation.Z;
  1355. myrot.W = _orientation.W;
  1356. d.GeomSetQuaternion(prim_geom, ref myrot);
  1357. if (IsPhysical && Body == IntPtr.Zero)
  1358. enableBody();
  1359. changeSelectedStatus();
  1360. m_taintadd = false;
  1361. }
  1362. /// <summary>
  1363. /// Move prim in response to a move taint.
  1364. /// </summary>
  1365. private void changemove()
  1366. {
  1367. if (IsPhysical)
  1368. {
  1369. if (!m_disabled && !m_taintremove && !childPrim)
  1370. {
  1371. if (Body == IntPtr.Zero)
  1372. enableBody();
  1373. //Prim auto disable after 20 frames,
  1374. //if you move it, re-enable the prim manually.
  1375. if (_parent != null)
  1376. {
  1377. if (m_linkJoint != IntPtr.Zero)
  1378. {
  1379. d.JointDestroy(m_linkJoint);
  1380. m_linkJoint = IntPtr.Zero;
  1381. }
  1382. }
  1383. if (Body != IntPtr.Zero)
  1384. {
  1385. d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
  1386. if (_parent != null)
  1387. {
  1388. OdePrim odParent = (OdePrim)_parent;
  1389. if (Body != (IntPtr)0 && odParent.Body != (IntPtr)0 && Body != odParent.Body)
  1390. {
  1391. // KF: Fixed Joints were removed? Anyway - this Console.WriteLine does not show up, so routine is not used??
  1392. Console.WriteLine(" JointCreateFixed");
  1393. m_linkJoint = d.JointCreateFixed(_parent_scene.world, _linkJointGroup);
  1394. d.JointAttach(m_linkJoint, Body, odParent.Body);
  1395. d.JointSetFixed(m_linkJoint);
  1396. }
  1397. }
  1398. d.BodyEnable(Body);
  1399. if (m_vehicle.Type != Vehicle.TYPE_NONE)
  1400. {
  1401. m_vehicle.Enable(Body, _parent_scene);
  1402. }
  1403. }
  1404. else
  1405. {
  1406. m_log.WarnFormat("[PHYSICS]: Body for {0} still null after enableBody(). This is a crash scenario.", Name);
  1407. }
  1408. }
  1409. //else
  1410. // {
  1411. //m_log.Debug("[BUG]: race!");
  1412. //}
  1413. }
  1414. // string primScenAvatarIn = _parent_scene.whichspaceamIin(_position);
  1415. // int[] arrayitem = _parent_scene.calculateSpaceArrayItemFromPos(_position);
  1416. // _parent_scene.waitForSpaceUnlock(m_targetSpace);
  1417. IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, _position, m_targetSpace);
  1418. m_targetSpace = tempspace;
  1419. // _parent_scene.waitForSpaceUnlock(m_targetSpace);
  1420. d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
  1421. // _parent_scene.waitForSpaceUnlock(m_targetSpace);
  1422. d.SpaceAdd(m_targetSpace, prim_geom);
  1423. changeSelectedStatus();
  1424. resetCollisionAccounting();
  1425. m_taintposition = _position;
  1426. }
  1427. internal void Move(float timestep)
  1428. {
  1429. float fx = 0;
  1430. float fy = 0;
  1431. float fz = 0;
  1432. if (outofBounds)
  1433. return;
  1434. if (IsPhysical && (Body != IntPtr.Zero) && !m_isSelected && !childPrim) // KF: Only move root prims.
  1435. {
  1436. if (m_vehicle.Type != Vehicle.TYPE_NONE)
  1437. {
  1438. // 'VEHICLES' are dealt with in ODEDynamics.cs
  1439. m_vehicle.Step(timestep, _parent_scene);
  1440. }
  1441. else
  1442. {
  1443. //Console.WriteLine("Move " + Name);
  1444. if (!d.BodyIsEnabled (Body)) d.BodyEnable (Body); // KF add 161009
  1445. float m_mass = CalculateMass();
  1446. // fz = 0f;
  1447. //m_log.Info(m_collisionFlags.ToString());
  1448. //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
  1449. // would come from SceneObjectPart.cs, public void SetBuoyancy(float fvalue) , PhysActor.Buoyancy = fvalue; ??
  1450. // m_buoyancy: (unlimited value) <0=Falls fast; 0=1g; 1=0g; >1 = floats up
  1451. // gravityz multiplier = 1 - m_buoyancy
  1452. fz = _parent_scene.gravityz * (1.0f - m_buoyancy) * m_mass;
  1453. if (PIDActive)
  1454. {
  1455. //Console.WriteLine("PID " + Name);
  1456. // KF - this is for object move? eg. llSetPos() ?
  1457. //if (!d.BodyIsEnabled(Body))
  1458. //d.BodySetForce(Body, 0f, 0f, 0f);
  1459. // If we're using the PID controller, then we have no gravity
  1460. //fz = (-1 * _parent_scene.gravityz) * m_mass; //KF: ?? Prims have no global gravity,so simply...
  1461. fz = 0f;
  1462. // no lock; for now it's only called from within Simulate()
  1463. // If the PID Controller isn't active then we set our force
  1464. // calculating base velocity to the current position
  1465. if ((m_PIDTau < 1) && (m_PIDTau != 0))
  1466. {
  1467. //PID_G = PID_G / m_PIDTau;
  1468. m_PIDTau = 1;
  1469. }
  1470. if ((PID_G - m_PIDTau) <= 0)
  1471. {
  1472. PID_G = m_PIDTau + 1;
  1473. }
  1474. //PidStatus = true;
  1475. // PhysicsVector vec = new PhysicsVector();
  1476. d.Vector3 vel = d.BodyGetLinearVel(Body);
  1477. d.Vector3 pos = d.BodyGetPosition(Body);
  1478. _target_velocity =
  1479. new Vector3(
  1480. (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
  1481. (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
  1482. (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
  1483. );
  1484. // if velocity is zero, use position control; otherwise, velocity control
  1485. if (_target_velocity.ApproxEquals(Vector3.Zero,0.1f))
  1486. {
  1487. // keep track of where we stopped. No more slippin' & slidin'
  1488. // We only want to deactivate the PID Controller if we think we want to have our surrogate
  1489. // react to the physics scene by moving it's position.
  1490. // Avatar to Avatar collisions
  1491. // Prim to avatar collisions
  1492. //fx = (_target_velocity.X - vel.X) * (PID_D) + (_zeroPosition.X - pos.X) * (PID_P * 2);
  1493. //fy = (_target_velocity.Y - vel.Y) * (PID_D) + (_zeroPosition.Y - pos.Y) * (PID_P * 2);
  1494. //fz = fz + (_target_velocity.Z - vel.Z) * (PID_D) + (_zeroPosition.Z - pos.Z) * PID_P;
  1495. d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
  1496. d.BodySetLinearVel(Body, 0, 0, 0);
  1497. d.BodyAddForce(Body, 0, 0, fz);
  1498. return;
  1499. }
  1500. else
  1501. {
  1502. _zeroFlag = false;
  1503. // We're flying and colliding with something
  1504. fx = ((_target_velocity.X) - vel.X) * (PID_D);
  1505. fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
  1506. // vec.Z = (_target_velocity.Z - vel.Z) * PID_D + (_zeroPosition.Z - pos.Z) * PID_P;
  1507. fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
  1508. }
  1509. } // end if (PIDActive)
  1510. // Hover PID Controller needs to be mutually exlusive to MoveTo PID controller
  1511. if (m_useHoverPID && !PIDActive)
  1512. {
  1513. //Console.WriteLine("Hover " + Name);
  1514. // If we're using the PID controller, then we have no gravity
  1515. fz = (-1 * _parent_scene.gravityz) * m_mass;
  1516. // no lock; for now it's only called from within Simulate()
  1517. // If the PID Controller isn't active then we set our force
  1518. // calculating base velocity to the current position
  1519. if ((m_PIDTau < 1))
  1520. {
  1521. PID_G = PID_G / m_PIDTau;
  1522. }
  1523. if ((PID_G - m_PIDTau) <= 0)
  1524. {
  1525. PID_G = m_PIDTau + 1;
  1526. }
  1527. // Where are we, and where are we headed?
  1528. d.Vector3 pos = d.BodyGetPosition(Body);
  1529. d.Vector3 vel = d.BodyGetLinearVel(Body);
  1530. // Non-Vehicles have a limited set of Hover options.
  1531. // determine what our target height really is based on HoverType
  1532. switch (m_PIDHoverType)
  1533. {
  1534. case PIDHoverType.Ground:
  1535. m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
  1536. m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
  1537. break;
  1538. case PIDHoverType.GroundAndWater:
  1539. m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
  1540. m_waterHeight = _parent_scene.GetWaterLevel();
  1541. if (m_groundHeight > m_waterHeight)
  1542. {
  1543. m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
  1544. }
  1545. else
  1546. {
  1547. m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
  1548. }
  1549. break;
  1550. } // end switch (m_PIDHoverType)
  1551. _target_velocity =
  1552. new Vector3(0.0f, 0.0f,
  1553. (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
  1554. );
  1555. // if velocity is zero, use position control; otherwise, velocity control
  1556. if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
  1557. {
  1558. // keep track of where we stopped. No more slippin' & slidin'
  1559. // We only want to deactivate the PID Controller if we think we want to have our surrogate
  1560. // react to the physics scene by moving it's position.
  1561. // Avatar to Avatar collisions
  1562. // Prim to avatar collisions
  1563. d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
  1564. d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
  1565. d.BodyAddForce(Body, 0, 0, fz);
  1566. return;
  1567. }
  1568. else
  1569. {
  1570. _zeroFlag = false;
  1571. // We're flying and colliding with something
  1572. fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
  1573. }
  1574. }
  1575. fx *= m_mass;
  1576. fy *= m_mass;
  1577. //fz *= m_mass;
  1578. fx += m_force.X;
  1579. fy += m_force.Y;
  1580. fz += m_force.Z;
  1581. //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
  1582. if (fx != 0 || fy != 0 || fz != 0)
  1583. {
  1584. //m_taintdisable = true;
  1585. //base.RaiseOutOfBounds(Position);
  1586. //d.BodySetLinearVel(Body, fx, fy, 0f);
  1587. if (!d.BodyIsEnabled(Body))
  1588. {
  1589. // A physical body at rest on a surface will auto-disable after a while,
  1590. // this appears to re-enable it incase the surface it is upon vanishes,
  1591. // and the body should fall again.
  1592. d.BodySetLinearVel(Body, 0f, 0f, 0f);
  1593. d.BodySetForce(Body, 0, 0, 0);
  1594. enableBodySoft();
  1595. }
  1596. // 35x10 = 350n times the mass per second applied maximum.
  1597. float nmax = 35f * m_mass;
  1598. float nmin = -35f * m_mass;
  1599. if (fx > nmax)
  1600. fx = nmax;
  1601. if (fx < nmin)
  1602. fx = nmin;
  1603. if (fy > nmax)
  1604. fy = nmax;
  1605. if (fy < nmin)
  1606. fy = nmin;
  1607. d.BodyAddForce(Body, fx, fy, fz);
  1608. //Console.WriteLine("AddForce " + fx + "," + fy + "," + fz);
  1609. }
  1610. }
  1611. }
  1612. else
  1613. { // is not physical, or is not a body or is selected
  1614. // _zeroPosition = d.BodyGetPosition(Body);
  1615. return;
  1616. //Console.WriteLine("Nothing " + Name);
  1617. }
  1618. }
  1619. private void rotate()
  1620. {
  1621. d.Quaternion myrot = new d.Quaternion();
  1622. myrot.X = _orientation.X;
  1623. myrot.Y = _orientation.Y;
  1624. myrot.Z = _orientation.Z;
  1625. myrot.W = _orientation.W;
  1626. if (Body != IntPtr.Zero)
  1627. {
  1628. // KF: If this is a root prim do BodySet
  1629. d.BodySetQuaternion(Body, ref myrot);
  1630. if (IsPhysical)
  1631. {
  1632. // create or remove locks
  1633. createAMotor(m_angularlock);
  1634. }
  1635. }
  1636. else
  1637. {
  1638. // daughter prim, do Geom set
  1639. d.GeomSetQuaternion(prim_geom, ref myrot);
  1640. }
  1641. resetCollisionAccounting();
  1642. m_taintrot = _orientation;
  1643. }
  1644. private void resetCollisionAccounting()
  1645. {
  1646. m_collisionscore = 0;
  1647. m_interpenetrationcount = 0;
  1648. m_disabled = false;
  1649. }
  1650. /// <summary>
  1651. /// Change prim in response to a disable taint.
  1652. /// </summary>
  1653. private void changedisable()
  1654. {
  1655. m_disabled = true;
  1656. if (Body != IntPtr.Zero)
  1657. {
  1658. d.BodyDisable(Body);
  1659. Body = IntPtr.Zero;
  1660. }
  1661. m_taintdisable = false;
  1662. }
  1663. /// <summary>
  1664. /// Change prim in response to a physics status taint
  1665. /// </summary>
  1666. private void changePhysicsStatus()
  1667. {
  1668. if (IsPhysical)
  1669. {
  1670. if (Body == IntPtr.Zero)
  1671. {
  1672. if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
  1673. {
  1674. changeshape();
  1675. }
  1676. else
  1677. {
  1678. enableBody();
  1679. }
  1680. }
  1681. }
  1682. else
  1683. {
  1684. if (Body != IntPtr.Zero)
  1685. {
  1686. if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
  1687. {
  1688. RemoveGeom();
  1689. //Console.WriteLine("changePhysicsStatus for " + Name);
  1690. changeadd();
  1691. }
  1692. if (childPrim)
  1693. {
  1694. if (_parent != null)
  1695. {
  1696. OdePrim parent = (OdePrim)_parent;
  1697. parent.ChildDelink(this);
  1698. }
  1699. }
  1700. else
  1701. {
  1702. disableBody();
  1703. }
  1704. }
  1705. }
  1706. changeSelectedStatus();
  1707. resetCollisionAccounting();
  1708. m_taintPhysics = IsPhysical;
  1709. }
  1710. /// <summary>
  1711. /// Change prim in response to a size taint.
  1712. /// </summary>
  1713. private void changesize()
  1714. {
  1715. #if SPAM
  1716. m_log.DebugFormat("[ODE PRIM]: Called changesize");
  1717. #endif
  1718. if (_size.X <= 0) _size.X = 0.01f;
  1719. if (_size.Y <= 0) _size.Y = 0.01f;
  1720. if (_size.Z <= 0) _size.Z = 0.01f;
  1721. //kill body to rebuild
  1722. if (IsPhysical && Body != IntPtr.Zero)
  1723. {
  1724. if (childPrim)
  1725. {
  1726. if (_parent != null)
  1727. {
  1728. OdePrim parent = (OdePrim)_parent;
  1729. parent.ChildDelink(this);
  1730. }
  1731. }
  1732. else
  1733. {
  1734. disableBody();
  1735. }
  1736. }
  1737. if (d.SpaceQuery(m_targetSpace, prim_geom))
  1738. {
  1739. // _parent_scene.waitForSpaceUnlock(m_targetSpace);
  1740. d.SpaceRemove(m_targetSpace, prim_geom);
  1741. }
  1742. RemoveGeom();
  1743. // we don't need to do space calculation because the client sends a position update also.
  1744. IMesh mesh = null;
  1745. // Construction of new prim
  1746. if (_parent_scene.needsMeshing(_pbs))
  1747. {
  1748. float meshlod = _parent_scene.meshSculptLOD;
  1749. if (IsPhysical)
  1750. meshlod = _parent_scene.MeshSculptphysicalLOD;
  1751. // Don't need to re-enable body.. it's done in SetMesh
  1752. if (_parent_scene.needsMeshing(_pbs))
  1753. {
  1754. mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
  1755. if (mesh == null)
  1756. CheckMeshAsset();
  1757. else
  1758. m_assetFailed = false;
  1759. }
  1760. }
  1761. CreateGeom(m_targetSpace, mesh);
  1762. d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
  1763. d.Quaternion myrot = new d.Quaternion();
  1764. myrot.X = _orientation.X;
  1765. myrot.Y = _orientation.Y;
  1766. myrot.Z = _orientation.Z;
  1767. myrot.W = _orientation.W;
  1768. d.GeomSetQuaternion(prim_geom, ref myrot);
  1769. //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
  1770. if (IsPhysical && Body == IntPtr.Zero && !childPrim)
  1771. {
  1772. // Re creates body on size.
  1773. // EnableBody also does setMass()
  1774. enableBody();
  1775. d.BodyEnable(Body);
  1776. }
  1777. changeSelectedStatus();
  1778. if (childPrim)
  1779. {
  1780. if (_parent is OdePrim)
  1781. {
  1782. OdePrim parent = (OdePrim)_parent;
  1783. parent.ChildSetGeom(this);
  1784. }
  1785. }
  1786. resetCollisionAccounting();
  1787. m_taintsize = _size;
  1788. }
  1789. /// <summary>
  1790. /// Change prim in response to a float on water taint.
  1791. /// </summary>
  1792. /// <param name="timestep"></param>
  1793. private void changefloatonwater()
  1794. {
  1795. m_collidesWater = m_taintCollidesWater;
  1796. if (m_collidesWater)
  1797. {
  1798. m_collisionFlags |= CollisionCategories.Water;
  1799. }
  1800. else
  1801. {
  1802. m_collisionFlags &= ~CollisionCategories.Water;
  1803. }
  1804. if (m_assetFailed)
  1805. d.GeomSetCollideBits(prim_geom, (uint)BadMeshAssetCollideBits);
  1806. else
  1807. d.GeomSetCollideBits(prim_geom, (uint)m_collisionFlags);
  1808. }
  1809. /// <summary>
  1810. /// Change prim in response to a shape taint.
  1811. /// </summary>
  1812. private void changeshape()
  1813. {
  1814. m_taintshape = false;
  1815. // Cleanup of old prim geometry and Bodies
  1816. if (IsPhysical && Body != IntPtr.Zero)
  1817. {
  1818. if (childPrim)
  1819. {
  1820. if (_parent != null)
  1821. {
  1822. OdePrim parent = (OdePrim)_parent;
  1823. parent.ChildDelink(this);
  1824. }
  1825. }
  1826. else
  1827. {
  1828. disableBody();
  1829. }
  1830. }
  1831. RemoveGeom();
  1832. // we don't need to do space calculation because the client sends a position update also.
  1833. if (_size.X <= 0) _size.X = 0.01f;
  1834. if (_size.Y <= 0) _size.Y = 0.01f;
  1835. if (_size.Z <= 0) _size.Z = 0.01f;
  1836. // Construction of new prim
  1837. IMesh mesh = null;
  1838. if (_parent_scene.needsMeshing(_pbs))
  1839. {
  1840. // Don't need to re-enable body.. it's done in CreateMesh
  1841. float meshlod = _parent_scene.meshSculptLOD;
  1842. if (IsPhysical)
  1843. meshlod = _parent_scene.MeshSculptphysicalLOD;
  1844. // createmesh returns null when it doesn't mesh.
  1845. mesh = _parent_scene.mesher.CreateMesh(Name, _pbs, _size, meshlod, IsPhysical);
  1846. if (mesh == null)
  1847. CheckMeshAsset();
  1848. else
  1849. m_assetFailed = false;
  1850. }
  1851. CreateGeom(m_targetSpace, mesh);
  1852. d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
  1853. d.Quaternion myrot = new d.Quaternion();
  1854. //myrot.W = _orientation.w;
  1855. myrot.W = _orientation.W;
  1856. myrot.X = _orientation.X;
  1857. myrot.Y = _orientation.Y;
  1858. myrot.Z = _orientation.Z;
  1859. d.GeomSetQuaternion(prim_geom, ref myrot);
  1860. //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
  1861. if (IsPhysical && Body == IntPtr.Zero)
  1862. {
  1863. // Re creates body on size.
  1864. // EnableBody also does setMass()
  1865. enableBody();
  1866. if (Body != IntPtr.Zero)
  1867. {
  1868. d.BodyEnable(Body);
  1869. }
  1870. }
  1871. changeSelectedStatus();
  1872. if (childPrim)
  1873. {
  1874. if (_parent is OdePrim)
  1875. {
  1876. OdePrim parent = (OdePrim)_parent;
  1877. parent.ChildSetGeom(this);
  1878. }
  1879. }
  1880. resetCollisionAccounting();
  1881. // m_taintshape = false;
  1882. }
  1883. /// <summary>
  1884. /// Change prim in response to an add force taint.
  1885. /// </summary>
  1886. private void changeAddForce()
  1887. {
  1888. if (!m_isSelected)
  1889. {
  1890. lock (m_forcelist)
  1891. {
  1892. //m_log.Info("[PHYSICS]: dequeing forcelist");
  1893. if (IsPhysical)
  1894. {
  1895. Vector3 iforce = Vector3.Zero;
  1896. int i = 0;
  1897. try
  1898. {
  1899. for (i = 0; i < m_forcelist.Count; i++)
  1900. {
  1901. iforce = iforce + (m_forcelist[i] * 100);
  1902. }
  1903. }
  1904. catch (IndexOutOfRangeException)
  1905. {
  1906. m_forcelist = new List<Vector3>();
  1907. m_collisionscore = 0;
  1908. m_interpenetrationcount = 0;
  1909. m_taintforce = false;
  1910. return;
  1911. }
  1912. catch (ArgumentOutOfRangeException)
  1913. {
  1914. m_forcelist = new List<Vector3>();
  1915. m_collisionscore = 0;
  1916. m_interpenetrationcount = 0;
  1917. m_taintforce = false;
  1918. return;
  1919. }
  1920. d.BodyEnable(Body);
  1921. d.BodyAddForce(Body, iforce.X, iforce.Y, iforce.Z);
  1922. }
  1923. m_forcelist.Clear();
  1924. }
  1925. m_collisionscore = 0;
  1926. m_interpenetrationcount = 0;
  1927. }
  1928. m_taintforce = false;
  1929. }
  1930. /// <summary>
  1931. /// Change prim in response to a torque taint.
  1932. /// </summary>
  1933. private void changeSetTorque()
  1934. {
  1935. if (!m_isSelected)
  1936. {
  1937. if (IsPhysical && Body != IntPtr.Zero)
  1938. {
  1939. d.BodySetTorque(Body, m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
  1940. }
  1941. }
  1942. m_taintTorque = Vector3.Zero;
  1943. }
  1944. /// <summary>
  1945. /// Change prim in response to an angular force taint.
  1946. /// </summary>
  1947. private void changeAddAngularForce()
  1948. {
  1949. if (!m_isSelected)
  1950. {
  1951. lock (m_angularforcelist)
  1952. {
  1953. //m_log.Info("[PHYSICS]: dequeing forcelist");
  1954. if (IsPhysical)
  1955. {
  1956. Vector3 iforce = Vector3.Zero;
  1957. for (int i = 0; i < m_angularforcelist.Count; i++)
  1958. {
  1959. iforce = iforce + (m_angularforcelist[i] * 100);
  1960. }
  1961. d.BodyEnable(Body);
  1962. d.BodyAddTorque(Body, iforce.X, iforce.Y, iforce.Z);
  1963. }
  1964. m_angularforcelist.Clear();
  1965. }
  1966. m_collisionscore = 0;
  1967. m_interpenetrationcount = 0;
  1968. }
  1969. m_taintaddangularforce = false;
  1970. }
  1971. /// <summary>
  1972. /// Change prim in response to a velocity taint.
  1973. /// </summary>
  1974. private void changevelocity()
  1975. {
  1976. if (!m_isSelected)
  1977. {
  1978. // Not sure exactly why this sleep is here, but from experimentation it appears to stop an avatar
  1979. // walking through a default rez size prim if it keeps kicking it around - justincc.
  1980. Thread.Sleep(20);
  1981. if (IsPhysical)
  1982. {
  1983. if (Body != IntPtr.Zero)
  1984. {
  1985. d.BodySetLinearVel(Body, m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
  1986. }
  1987. }
  1988. //resetCollisionAccounting();
  1989. }
  1990. m_taintVelocity = Vector3.Zero;
  1991. }
  1992. internal void setPrimForRemoval()
  1993. {
  1994. m_taintremove = true;
  1995. }
  1996. public override bool Flying
  1997. {
  1998. // no flying prims for you
  1999. get { return false; }
  2000. set { }
  2001. }
  2002. public override bool IsColliding
  2003. {
  2004. get { return iscolliding; }
  2005. set { iscolliding = value; }
  2006. }
  2007. public override bool CollidingGround
  2008. {
  2009. get { return false; }
  2010. set { return; }
  2011. }
  2012. public override bool CollidingObj
  2013. {
  2014. get { return false; }
  2015. set { return; }
  2016. }
  2017. public override bool ThrottleUpdates
  2018. {
  2019. get { return m_throttleUpdates; }
  2020. set { m_throttleUpdates = value; }
  2021. }
  2022. public override bool Stopped
  2023. {
  2024. get { return _zeroFlag; }
  2025. }
  2026. public override Vector3 Position
  2027. {
  2028. get { return _position; }
  2029. set { _position = value;
  2030. //m_log.Info("[PHYSICS]: " + _position.ToString());
  2031. }
  2032. }
  2033. public override Vector3 Size
  2034. {
  2035. get { return _size; }
  2036. set
  2037. {
  2038. if (value.IsFinite())
  2039. {
  2040. _size = value;
  2041. // m_log.DebugFormat("[PHYSICS]: Set size on {0} to {1}", Name, value);
  2042. }
  2043. else
  2044. {
  2045. m_log.WarnFormat("[PHYSICS]: Got NaN Size on object {0}", Name);
  2046. }
  2047. }
  2048. }
  2049. public override float Mass
  2050. {
  2051. get { return CalculateMass(); }
  2052. }
  2053. public override Vector3 Force
  2054. {
  2055. //get { return Vector3.Zero; }
  2056. get { return m_force; }
  2057. set
  2058. {
  2059. if (value.IsFinite())
  2060. {
  2061. m_force = value;
  2062. }
  2063. else
  2064. {
  2065. m_log.WarnFormat("[PHYSICS]: NaN in Force Applied to an Object {0}", Name);
  2066. }
  2067. }
  2068. }
  2069. public override int VehicleType
  2070. {
  2071. get { return (int)m_vehicle.Type; }
  2072. set { m_vehicle.ProcessTypeChange((Vehicle)value); }
  2073. }
  2074. public override void VehicleFloatParam(int param, float value)
  2075. {
  2076. m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value);
  2077. }
  2078. public override void VehicleVectorParam(int param, Vector3 value)
  2079. {
  2080. m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value);
  2081. }
  2082. public override void VehicleRotationParam(int param, Quaternion rotation)
  2083. {
  2084. m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
  2085. }
  2086. public override void VehicleFlags(int param, bool remove)
  2087. {
  2088. m_vehicle.ProcessVehicleFlags(param, remove);
  2089. }
  2090. public override void SetVolumeDetect(int param)
  2091. {
  2092. // We have to lock the scene here so that an entire simulate loop either uses volume detect for all
  2093. // possible collisions with this prim or for none of them.
  2094. lock (_parent_scene.OdeLock)
  2095. {
  2096. m_isVolumeDetect = (param != 0);
  2097. }
  2098. }
  2099. public override Vector3 CenterOfMass
  2100. {
  2101. get { return Vector3.Zero; }
  2102. }
  2103. public override Vector3 GeometricCenter
  2104. {
  2105. get { return Vector3.Zero; }
  2106. }
  2107. public override PrimitiveBaseShape Shape
  2108. {
  2109. set
  2110. {
  2111. _pbs = value;
  2112. m_assetFailed = false;
  2113. m_taintshape = true;
  2114. }
  2115. }
  2116. public override Vector3 Velocity
  2117. {
  2118. get
  2119. {
  2120. // Average previous velocity with the new one so
  2121. // client object interpolation works a 'little' better
  2122. if (_zeroFlag)
  2123. return Vector3.Zero;
  2124. Vector3 returnVelocity = Vector3.Zero;
  2125. returnVelocity.X = (m_lastVelocity.X + _velocity.X) * 0.5f; // 0.5f is mathematically equiv to '/ 2'
  2126. returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) * 0.5f;
  2127. returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) * 0.5f;
  2128. return returnVelocity;
  2129. }
  2130. set
  2131. {
  2132. if (value.IsFinite())
  2133. {
  2134. _velocity = value;
  2135. m_taintVelocity = value;
  2136. _parent_scene.AddPhysicsActorTaint(this);
  2137. }
  2138. else
  2139. {
  2140. m_log.WarnFormat("[PHYSICS]: Got NaN Velocity in Object {0}", Name);
  2141. }
  2142. }
  2143. }
  2144. public override Vector3 Torque
  2145. {
  2146. get
  2147. {
  2148. if (!IsPhysical || Body == IntPtr.Zero)
  2149. return Vector3.Zero;
  2150. return _torque;
  2151. }
  2152. set
  2153. {
  2154. if (value.IsFinite())
  2155. {
  2156. m_taintTorque = value;
  2157. _parent_scene.AddPhysicsActorTaint(this);
  2158. }
  2159. else
  2160. {
  2161. m_log.WarnFormat("[PHYSICS]: Got NaN Torque in Object {0}", Name);
  2162. }
  2163. }
  2164. }
  2165. public override float CollisionScore
  2166. {
  2167. get { return m_collisionscore; }
  2168. set { m_collisionscore = value; }
  2169. }
  2170. public override bool Kinematic
  2171. {
  2172. get { return false; }
  2173. set { }
  2174. }
  2175. public override Quaternion Orientation
  2176. {
  2177. get { return _orientation; }
  2178. set
  2179. {
  2180. if (QuaternionIsFinite(value))
  2181. _orientation = value;
  2182. else
  2183. m_log.WarnFormat("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object {0}", Name);
  2184. }
  2185. }
  2186. private static bool QuaternionIsFinite(Quaternion q)
  2187. {
  2188. if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
  2189. return false;
  2190. if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
  2191. return false;
  2192. if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
  2193. return false;
  2194. if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
  2195. return false;
  2196. return true;
  2197. }
  2198. public override Vector3 Acceleration
  2199. {
  2200. get { return _acceleration; }
  2201. set { _acceleration = value; }
  2202. }
  2203. public override void AddForce(Vector3 force, bool pushforce)
  2204. {
  2205. if (force.IsFinite())
  2206. {
  2207. lock (m_forcelist)
  2208. m_forcelist.Add(force);
  2209. m_taintforce = true;
  2210. }
  2211. else
  2212. {
  2213. m_log.WarnFormat("[PHYSICS]: Got Invalid linear force vector from Scene in Object {0}", Name);
  2214. }
  2215. //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
  2216. }
  2217. public override void AddAngularForce(Vector3 force, bool pushforce)
  2218. {
  2219. if (force.IsFinite())
  2220. {
  2221. m_angularforcelist.Add(force);
  2222. m_taintaddangularforce = true;
  2223. }
  2224. else
  2225. {
  2226. m_log.WarnFormat("[PHYSICS]: Got Invalid Angular force vector from Scene in Object {0}", Name);
  2227. }
  2228. }
  2229. public override Vector3 RotationalVelocity
  2230. {
  2231. get
  2232. {
  2233. Vector3 pv = Vector3.Zero;
  2234. if (_zeroFlag)
  2235. return pv;
  2236. m_lastUpdateSent = false;
  2237. if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
  2238. return pv;
  2239. return m_rotationalVelocity;
  2240. }
  2241. set
  2242. {
  2243. if (value.IsFinite())
  2244. {
  2245. m_rotationalVelocity = value;
  2246. setAngularVelocity(value.X, value.Y, value.Z);
  2247. }
  2248. else
  2249. {
  2250. m_log.WarnFormat("[PHYSICS]: Got NaN RotationalVelocity in Object {0}", Name);
  2251. }
  2252. }
  2253. }
  2254. public override void CrossingFailure()
  2255. {
  2256. /*
  2257. m_crossingfailures++;
  2258. if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
  2259. {
  2260. base.RaiseOutOfBounds(_position);
  2261. return;
  2262. }
  2263. else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
  2264. {
  2265. m_log.Warn("[PHYSICS]: Too many crossing failures for: " + Name);
  2266. }
  2267. */
  2268. d.AllocateODEDataForThread(0U);
  2269. _position.X = Util.Clip(_position.X, 0.5f, _parent_scene.WorldExtents.X - 0.5f);
  2270. _position.Y = Util.Clip(_position.Y, 0.5f, _parent_scene.WorldExtents.Y - 0.5f);
  2271. _position.Z = Util.Clip(_position.Z + 0.2f, -100f, 50000f);
  2272. m_lastposition = _position;
  2273. _velocity.X = 0;
  2274. _velocity.Y = 0;
  2275. _velocity.Z = 0;
  2276. m_lastVelocity = _velocity;
  2277. if (Body != IntPtr.Zero)
  2278. {
  2279. d.BodySetLinearVel(Body, 0, 0, 0); // stop it
  2280. d.BodySetPosition(Body, _position.X, _position.Y, _position.Z);
  2281. }
  2282. if(m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE)
  2283. m_vehicle.Stop(); // this also updates vehicle last position from the body position
  2284. enableBodySoft();
  2285. outofBounds = false;
  2286. base.RequestPhysicsterseUpdate();
  2287. }
  2288. public override float Buoyancy
  2289. {
  2290. get { return m_buoyancy; }
  2291. set { m_buoyancy = value; }
  2292. }
  2293. public override void link(PhysicsActor obj)
  2294. {
  2295. m_taintparent = obj;
  2296. }
  2297. public override void delink()
  2298. {
  2299. m_taintparent = null;
  2300. }
  2301. public override void LockAngularMotion(byte axislocks)
  2302. {
  2303. // m_log.DebugFormat("[axislocks]: {0}", axislocks);
  2304. m_taintAngularLock = axislocks;
  2305. }
  2306. internal void UpdatePositionAndVelocity()
  2307. {
  2308. // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
  2309. if (outofBounds)
  2310. return;
  2311. if (_parent == null)
  2312. {
  2313. Vector3 pv = Vector3.Zero;
  2314. bool lastZeroFlag = _zeroFlag;
  2315. float m_minvelocity = 0;
  2316. if (Body != IntPtr.Zero) // FIXME -> or if it is a joint
  2317. {
  2318. d.Vector3 vec = d.BodyGetPosition(Body);
  2319. d.Quaternion ori = d.BodyGetQuaternion(Body);
  2320. d.Vector3 vel = d.BodyGetLinearVel(Body);
  2321. d.Vector3 rotvel = d.BodyGetAngularVel(Body);
  2322. d.Vector3 torque = d.BodyGetTorque(Body);
  2323. _torque = new Vector3(torque.X, torque.Y, torque.Z);
  2324. Vector3 l_position = Vector3.Zero;
  2325. Quaternion l_orientation = Quaternion.Identity;
  2326. m_lastposition = _position;
  2327. m_lastorientation = _orientation;
  2328. l_position.X = vec.X;
  2329. l_position.Y = vec.Y;
  2330. l_position.Z = vec.Z;
  2331. l_orientation.X = ori.X;
  2332. l_orientation.Y = ori.Y;
  2333. l_orientation.Z = ori.Z;
  2334. l_orientation.W = ori.W;
  2335. if (l_position.Z < 0)
  2336. {
  2337. // This is so prim that get lost underground don't fall forever and suck up
  2338. //
  2339. // Sim resources and memory.
  2340. // Disables the prim's movement physics....
  2341. // It's a hack and will generate a console message if it fails.
  2342. //IsPhysical = false;
  2343. _acceleration.X = 0;
  2344. _acceleration.Y = 0;
  2345. _acceleration.Z = 0;
  2346. _velocity.X = 0;
  2347. _velocity.Y = 0;
  2348. _velocity.Z = 0;
  2349. m_rotationalVelocity.X = 0;
  2350. m_rotationalVelocity.Y = 0;
  2351. m_rotationalVelocity.Z = 0;
  2352. if (_parent == null)
  2353. base.RaiseOutOfBounds(_position);
  2354. if (_parent == null)
  2355. base.RequestPhysicsterseUpdate();
  2356. m_throttleUpdates = false;
  2357. throttleCounter = 0;
  2358. _zeroFlag = true;
  2359. //outofBounds = true;
  2360. return;
  2361. }
  2362. if (l_position.X > ((int)_parent_scene.WorldExtents.X - 0.05f) || l_position.X < 0f || l_position.Y > ((int)_parent_scene.WorldExtents.Y - 0.05f) || l_position.Y < 0f)
  2363. {
  2364. //base.RaiseOutOfBounds(l_position);
  2365. /*
  2366. if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
  2367. {
  2368. _position = l_position;
  2369. //_parent_scene.remActivePrim(this);
  2370. if (_parent == null)
  2371. base.RequestPhysicsterseUpdate();
  2372. return;
  2373. }
  2374. else
  2375. {
  2376. if (_parent == null)
  2377. base.RaiseOutOfBounds(l_position);
  2378. return;
  2379. }
  2380. */
  2381. outofBounds = true;
  2382. // part near the border on outside
  2383. if (l_position.X < 0)
  2384. Util.Clamp(l_position.X, -0.1f, -2f);
  2385. else
  2386. Util.Clamp(l_position.X, _parent_scene.WorldExtents.X + 0.1f, _parent_scene.WorldExtents.X + 2f);
  2387. if (l_position.Y < 0)
  2388. Util.Clamp(l_position.Y, -0.1f, -2f);
  2389. else
  2390. Util.Clamp(l_position.Y, _parent_scene.WorldExtents.Y + 0.1f, _parent_scene.WorldExtents.Y + 2f);
  2391. d.BodySetPosition(Body, l_position.X, l_position.Y, l_position.Z);
  2392. // stop it
  2393. d.BodySetAngularVel(Body, 0, 0, 0);
  2394. d.BodySetLinearVel(Body, 0, 0, 0);
  2395. disableBodySoft();
  2396. _position = l_position;
  2397. // tell framework to fix it
  2398. if (_parent == null)
  2399. base.RequestPhysicsterseUpdate();
  2400. return;
  2401. }
  2402. //float Adiff = 1.0f - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation));
  2403. //Console.WriteLine("Adiff " + Name + " = " + Adiff);
  2404. if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
  2405. && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
  2406. && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
  2407. // && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
  2408. && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.0001)) // KF 0.01 is far to large
  2409. {
  2410. _zeroFlag = true;
  2411. //Console.WriteLine("ZFT 2");
  2412. m_throttleUpdates = false;
  2413. }
  2414. else
  2415. {
  2416. //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
  2417. _zeroFlag = false;
  2418. m_lastUpdateSent = false;
  2419. //m_throttleUpdates = false;
  2420. }
  2421. if (_zeroFlag)
  2422. {
  2423. _velocity.X = 0.0f;
  2424. _velocity.Y = 0.0f;
  2425. _velocity.Z = 0.0f;
  2426. _acceleration.X = 0;
  2427. _acceleration.Y = 0;
  2428. _acceleration.Z = 0;
  2429. //_orientation.w = 0f;
  2430. //_orientation.X = 0f;
  2431. //_orientation.Y = 0f;
  2432. //_orientation.Z = 0f;
  2433. m_rotationalVelocity.X = 0;
  2434. m_rotationalVelocity.Y = 0;
  2435. m_rotationalVelocity.Z = 0;
  2436. if (!m_lastUpdateSent)
  2437. {
  2438. m_throttleUpdates = false;
  2439. throttleCounter = 0;
  2440. m_rotationalVelocity = pv;
  2441. if (_parent == null)
  2442. {
  2443. base.RequestPhysicsterseUpdate();
  2444. }
  2445. m_lastUpdateSent = true;
  2446. }
  2447. }
  2448. else
  2449. {
  2450. if (lastZeroFlag != _zeroFlag)
  2451. {
  2452. if (_parent == null)
  2453. {
  2454. base.RequestPhysicsterseUpdate();
  2455. }
  2456. }
  2457. m_lastVelocity = _velocity;
  2458. _position = l_position;
  2459. _velocity.X = vel.X;
  2460. _velocity.Y = vel.Y;
  2461. _velocity.Z = vel.Z;
  2462. _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
  2463. _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f, _velocity.Y - m_lastVelocity.Y / 0.1f, _velocity.Z - m_lastVelocity.Z / 0.1f);
  2464. //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
  2465. // Note here that linearvelocity is affecting angular velocity... so I'm guessing this is a vehicle specific thing...
  2466. // it does make sense to do this for tiny little instabilities with physical prim, however 0.5m/frame is fairly large.
  2467. // reducing this to 0.02m/frame seems to help the angular rubberbanding quite a bit, however, to make sure it doesn't affect elevators and vehicles
  2468. // adding these logical exclusion situations to maintain this where I think it was intended to be.
  2469. if (m_throttleUpdates || PIDActive || (m_vehicle != null && m_vehicle.Type != Vehicle.TYPE_NONE) || (Amotor != IntPtr.Zero))
  2470. {
  2471. m_minvelocity = 0.5f;
  2472. }
  2473. else
  2474. {
  2475. m_minvelocity = 0.02f;
  2476. }
  2477. if (_velocity.ApproxEquals(pv, m_minvelocity))
  2478. {
  2479. m_rotationalVelocity = pv;
  2480. }
  2481. else
  2482. {
  2483. m_rotationalVelocity = new Vector3(rotvel.X, rotvel.Y, rotvel.Z);
  2484. }
  2485. //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
  2486. _orientation.X = ori.X;
  2487. _orientation.Y = ori.Y;
  2488. _orientation.Z = ori.Z;
  2489. _orientation.W = ori.W;
  2490. m_lastUpdateSent = false;
  2491. if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
  2492. {
  2493. if (_parent == null)
  2494. {
  2495. base.RequestPhysicsterseUpdate();
  2496. }
  2497. }
  2498. else
  2499. {
  2500. throttleCounter++;
  2501. }
  2502. }
  2503. m_lastposition = l_position;
  2504. }
  2505. else
  2506. {
  2507. // Not a body.. so Make sure the client isn't interpolating
  2508. _velocity.X = 0;
  2509. _velocity.Y = 0;
  2510. _velocity.Z = 0;
  2511. _acceleration.X = 0;
  2512. _acceleration.Y = 0;
  2513. _acceleration.Z = 0;
  2514. m_rotationalVelocity.X = 0;
  2515. m_rotationalVelocity.Y = 0;
  2516. m_rotationalVelocity.Z = 0;
  2517. _zeroFlag = true;
  2518. }
  2519. }
  2520. }
  2521. public override bool FloatOnWater
  2522. {
  2523. set {
  2524. m_taintCollidesWater = value;
  2525. _parent_scene.AddPhysicsActorTaint(this);
  2526. }
  2527. }
  2528. public override void SetMomentum(Vector3 momentum)
  2529. {
  2530. }
  2531. public override Vector3 PIDTarget
  2532. {
  2533. set
  2534. {
  2535. if (value.IsFinite())
  2536. {
  2537. m_PIDTarget = value;
  2538. }
  2539. else
  2540. m_log.WarnFormat("[PHYSICS]: Got NaN PIDTarget from Scene on Object {0}", Name);
  2541. }
  2542. }
  2543. public override bool PIDActive { get; set; }
  2544. public override float PIDTau { set { m_PIDTau = value; } }
  2545. public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
  2546. public override bool PIDHoverActive { get { return m_useHoverPID;} set { m_useHoverPID = value; } }
  2547. public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
  2548. public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
  2549. public override Quaternion APIDTarget{ set { return; } }
  2550. public override bool APIDActive{ set { return; } }
  2551. public override float APIDStrength{ set { return; } }
  2552. public override float APIDDamping{ set { return; } }
  2553. private void createAMotor(byte axislock)
  2554. {
  2555. if (Body == IntPtr.Zero)
  2556. return;
  2557. if (Amotor != IntPtr.Zero)
  2558. {
  2559. d.JointDestroy(Amotor);
  2560. Amotor = IntPtr.Zero;
  2561. }
  2562. if(axislock == 0)
  2563. return;
  2564. int axisnum = 0;
  2565. bool axisX = false;
  2566. bool axisY = false;
  2567. bool axisZ = false;
  2568. if((axislock & 0x02) != 0)
  2569. {
  2570. axisnum++;
  2571. axisX = true;
  2572. }
  2573. if((axislock & 0x04) != 0)
  2574. {
  2575. axisnum++;
  2576. axisY = true;
  2577. }
  2578. if((axislock & 0x08) != 0)
  2579. {
  2580. axisnum++;
  2581. axisZ = true;
  2582. }
  2583. if(axisnum == 0)
  2584. return;
  2585. // stop it
  2586. d.BodySetTorque(Body, 0, 0, 0);
  2587. d.BodySetAngularVel(Body, 0, 0, 0);
  2588. Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
  2589. d.JointAttach(Amotor, Body, IntPtr.Zero);
  2590. d.JointSetAMotorMode(Amotor, 0);
  2591. d.JointSetAMotorNumAxes(Amotor, axisnum);
  2592. // get current orientation to lock
  2593. d.Quaternion dcur = d.BodyGetQuaternion(Body);
  2594. Quaternion curr; // crap convertion between identical things
  2595. curr.X = dcur.X;
  2596. curr.Y = dcur.Y;
  2597. curr.Z = dcur.Z;
  2598. curr.W = dcur.W;
  2599. Vector3 ax;
  2600. int i = 0;
  2601. int j = 0;
  2602. if (axisX)
  2603. {
  2604. ax = (new Vector3(1, 0, 0)) * curr; // rotate world X to current local X
  2605. d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
  2606. d.JointSetAMotorAngle(Amotor, 0, 0);
  2607. d.JointSetAMotorParam(Amotor, (int)d.JointParam.LoStop, 0f);
  2608. d.JointSetAMotorParam(Amotor, (int)d.JointParam.HiStop, 0f);
  2609. d.JointSetAMotorParam(Amotor, (int)d.JointParam.Vel, 0);
  2610. d.JointSetAMotorParam(Amotor, (int)d.JointParam.FudgeFactor, 0.0001f);
  2611. d.JointSetAMotorParam(Amotor, (int)d.JointParam.Bounce, 0f);
  2612. d.JointSetAMotorParam(Amotor, (int)d.JointParam.CFM, 0f);
  2613. d.JointSetAMotorParam(Amotor, (int)d.JointParam.FMax, 5e8f);
  2614. d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopCFM, 0f);
  2615. d.JointSetAMotorParam(Amotor, (int)d.JointParam.StopERP, 0.8f);
  2616. i++;
  2617. j = 256; // move to next axis set
  2618. }
  2619. if (axisY)
  2620. {
  2621. ax = (new Vector3(0, 1, 0)) * curr;
  2622. d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
  2623. d.JointSetAMotorAngle(Amotor, i, 0);
  2624. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f);
  2625. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f);
  2626. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
  2627. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
  2628. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
  2629. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f);
  2630. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
  2631. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
  2632. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
  2633. i++;
  2634. j += 256;
  2635. }
  2636. if (axisZ)
  2637. {
  2638. ax = (new Vector3(0, 0, 1)) * curr;
  2639. d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
  2640. d.JointSetAMotorAngle(Amotor, i, 0);
  2641. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.LoStop, 0f);
  2642. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.HiStop, 0f);
  2643. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Vel, 0);
  2644. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FudgeFactor, 0.0001f);
  2645. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.Bounce, 0f);
  2646. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.CFM, 0f);
  2647. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.FMax, 5e8f);
  2648. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopCFM, 0f);
  2649. d.JointSetAMotorParam(Amotor, j + (int)d.JointParam.StopERP, 0.8f);
  2650. }
  2651. }
  2652. public override void SubscribeEvents(int ms)
  2653. {
  2654. m_eventsubscription = ms;
  2655. _parent_scene.AddCollisionEventReporting(this);
  2656. }
  2657. public override void UnSubscribeEvents()
  2658. {
  2659. _parent_scene.RemoveCollisionEventReporting(this);
  2660. m_eventsubscription = 0;
  2661. }
  2662. public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
  2663. {
  2664. CollisionEventsThisFrame.AddCollider(CollidedWith, contact);
  2665. }
  2666. public void SendCollisions()
  2667. {
  2668. if (m_collisionsOnPreviousFrame || CollisionEventsThisFrame.Count > 0)
  2669. {
  2670. base.SendCollisionUpdate(CollisionEventsThisFrame);
  2671. if (CollisionEventsThisFrame.Count > 0)
  2672. {
  2673. m_collisionsOnPreviousFrame = true;
  2674. CollisionEventsThisFrame.Clear();
  2675. }
  2676. else
  2677. {
  2678. m_collisionsOnPreviousFrame = false;
  2679. }
  2680. }
  2681. }
  2682. public override bool SubscribedEvents()
  2683. {
  2684. if (m_eventsubscription > 0)
  2685. return true;
  2686. return false;
  2687. }
  2688. public override void SetMaterial(int pMaterial)
  2689. {
  2690. m_material = pMaterial;
  2691. }
  2692. private void CheckMeshAsset()
  2693. {
  2694. if (_pbs.SculptEntry && !m_assetFailed && _pbs.SculptTexture != UUID.Zero)
  2695. {
  2696. m_assetFailed = true;
  2697. Util.FireAndForget(delegate
  2698. {
  2699. RequestAssetDelegate assetProvider = _parent_scene.RequestAssetMethod;
  2700. if (assetProvider != null)
  2701. assetProvider(_pbs.SculptTexture, MeshAssetReceived);
  2702. }, null, "ODEPrim.CheckMeshAsset");
  2703. }
  2704. }
  2705. private void MeshAssetReceived(AssetBase asset)
  2706. {
  2707. if (asset != null && asset.Data != null && asset.Data.Length > 0)
  2708. {
  2709. if (!_pbs.SculptEntry)
  2710. return;
  2711. if (_pbs.SculptTexture.ToString() != asset.ID)
  2712. return;
  2713. _pbs.SculptData = new byte[asset.Data.Length];
  2714. asset.Data.CopyTo(_pbs.SculptData, 0);
  2715. // m_assetFailed = false;
  2716. // m_log.DebugFormat(
  2717. // "[ODE PRIM]: Received mesh/sculpt data asset {0} with {1} bytes for {2} at {3} in {4}",
  2718. // _pbs.SculptTexture, _pbs.SculptData.Length, Name, _position, _parent_scene.Name);
  2719. m_taintshape = true;
  2720. _parent_scene.AddPhysicsActorTaint(this);
  2721. }
  2722. else
  2723. {
  2724. m_log.WarnFormat(
  2725. "[ODE PRIM]: Could not get mesh/sculpt asset {0} for {1} at {2} in {3}",
  2726. _pbs.SculptTexture, Name, _position, _parent_scene.PhysicsSceneName);
  2727. }
  2728. }
  2729. }
  2730. }