1
0

ODEPrim.cs 116 KB

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