1
0

BulletDotNETPrim.cs 96 KB

1234567891011121314151617181920212223242526272829303132333435363738394041424344454647484950515253545556575859606162636465666768697071727374757677787980818283848586878889909192939495969798991001011021031041051061071081091101111121131141151161171181191201211221231241251261271281291301311321331341351361371381391401411421431441451461471481491501511521531541551561571581591601611621631641651661671681691701711721731741751761771781791801811821831841851861871881891901911921931941951961971981992002012022032042052062072082092102112122132142152162172182192202212222232242252262272282292302312322332342352362372382392402412422432442452462472482492502512522532542552562572582592602612622632642652662672682692702712722732742752762772782792802812822832842852862872882892902912922932942952962972982993003013023033043053063073083093103113123133143153163173183193203213223233243253263273283293303313323333343353363373383393403413423433443453463473483493503513523533543553563573583593603613623633643653663673683693703713723733743753763773783793803813823833843853863873883893903913923933943953963973983994004014024034044054064074084094104114124134144154164174184194204214224234244254264274284294304314324334344354364374384394404414424434444454464474484494504514524534544554564574584594604614624634644654664674684694704714724734744754764774784794804814824834844854864874884894904914924934944954964974984995005015025035045055065075085095105115125135145155165175185195205215225235245255265275285295305315325335345355365375385395405415425435445455465475485495505515525535545555565575585595605615625635645655665675685695705715725735745755765775785795805815825835845855865875885895905915925935945955965975985996006016026036046056066076086096106116126136146156166176186196206216226236246256266276286296306316326336346356366376386396406416426436446456466476486496506516526536546556566576586596606616626636646656666676686696706716726736746756766776786796806816826836846856866876886896906916926936946956966976986997007017027037047057067077087097107117127137147157167177187197207217227237247257267277287297307317327337347357367377387397407417427437447457467477487497507517527537547557567577587597607617627637647657667677687697707717727737747757767777787797807817827837847857867877887897907917927937947957967977987998008018028038048058068078088098108118128138148158168178188198208218228238248258268278288298308318328338348358368378388398408418428438448458468478488498508518528538548558568578588598608618628638648658668678688698708718728738748758768778788798808818828838848858868878888898908918928938948958968978988999009019029039049059069079089099109119129139149159169179189199209219229239249259269279289299309319329339349359369379389399409419429439449459469479489499509519529539549559569579589599609619629639649659669679689699709719729739749759769779789799809819829839849859869879889899909919929939949959969979989991000100110021003100410051006100710081009101010111012101310141015101610171018101910201021102210231024102510261027102810291030103110321033103410351036103710381039104010411042104310441045104610471048104910501051105210531054105510561057105810591060106110621063106410651066106710681069107010711072107310741075107610771078107910801081108210831084108510861087108810891090109110921093109410951096109710981099110011011102110311041105110611071108110911101111111211131114111511161117111811191120112111221123112411251126112711281129113011311132113311341135113611371138113911401141114211431144114511461147114811491150115111521153115411551156115711581159116011611162116311641165116611671168116911701171117211731174117511761177117811791180118111821183118411851186118711881189119011911192119311941195119611971198119912001201120212031204120512061207120812091210121112121213121412151216121712181219122012211222122312241225122612271228122912301231123212331234123512361237123812391240124112421243124412451246124712481249125012511252125312541255125612571258125912601261126212631264126512661267126812691270127112721273127412751276127712781279128012811282128312841285128612871288128912901291129212931294129512961297129812991300130113021303130413051306130713081309131013111312131313141315131613171318131913201321132213231324132513261327132813291330133113321333133413351336133713381339134013411342134313441345134613471348134913501351135213531354135513561357135813591360136113621363136413651366136713681369137013711372137313741375137613771378137913801381138213831384138513861387138813891390139113921393139413951396139713981399140014011402140314041405140614071408140914101411141214131414141514161417141814191420142114221423142414251426142714281429143014311432143314341435143614371438143914401441144214431444144514461447144814491450145114521453145414551456145714581459146014611462146314641465146614671468146914701471147214731474147514761477147814791480148114821483148414851486148714881489149014911492149314941495149614971498149915001501150215031504150515061507150815091510151115121513151415151516151715181519152015211522152315241525152615271528152915301531153215331534153515361537153815391540154115421543154415451546154715481549155015511552155315541555155615571558155915601561156215631564156515661567156815691570157115721573157415751576157715781579158015811582158315841585158615871588158915901591159215931594159515961597159815991600160116021603160416051606160716081609161016111612161316141615161616171618161916201621162216231624162516261627162816291630163116321633163416351636163716381639164016411642164316441645164616471648164916501651165216531654165516561657165816591660166116621663166416651666166716681669167016711672167316741675167616771678167916801681168216831684168516861687168816891690169116921693169416951696169716981699170017011702170317041705170617071708170917101711171217131714171517161717171817191720172117221723172417251726172717281729173017311732173317341735173617371738173917401741174217431744174517461747174817491750175117521753175417551756175717581759176017611762176317641765176617671768176917701771177217731774177517761777177817791780178117821783178417851786178717881789179017911792179317941795179617971798179918001801180218031804180518061807180818091810181118121813181418151816181718181819182018211822182318241825182618271828182918301831183218331834183518361837183818391840184118421843184418451846184718481849185018511852185318541855185618571858185918601861186218631864186518661867186818691870187118721873187418751876187718781879188018811882188318841885188618871888188918901891189218931894189518961897189818991900190119021903190419051906190719081909191019111912191319141915191619171918191919201921192219231924192519261927192819291930193119321933193419351936193719381939194019411942194319441945194619471948194919501951195219531954195519561957195819591960196119621963196419651966196719681969197019711972197319741975197619771978197919801981198219831984198519861987198819891990199119921993199419951996199719981999200020012002200320042005200620072008200920102011201220132014201520162017201820192020202120222023202420252026202720282029203020312032203320342035203620372038203920402041204220432044204520462047204820492050205120522053205420552056205720582059206020612062206320642065206620672068206920702071207220732074207520762077207820792080208120822083208420852086208720882089209020912092209320942095209620972098209921002101210221032104210521062107210821092110211121122113211421152116211721182119212021212122212321242125212621272128212921302131213221332134213521362137213821392140214121422143214421452146214721482149215021512152215321542155215621572158215921602161216221632164216521662167216821692170217121722173217421752176217721782179218021812182218321842185218621872188218921902191219221932194219521962197219821992200220122022203220422052206220722082209221022112212221322142215221622172218221922202221222222232224222522262227222822292230223122322233223422352236223722382239224022412242224322442245224622472248224922502251225222532254225522562257225822592260226122622263226422652266226722682269227022712272227322742275227622772278227922802281228222832284228522862287228822892290229122922293229422952296229722982299230023012302230323042305230623072308230923102311231223132314231523162317231823192320232123222323232423252326232723282329233023312332233323342335233623372338233923402341234223432344234523462347234823492350235123522353235423552356235723582359236023612362236323642365236623672368236923702371237223732374237523762377237823792380238123822383238423852386238723882389239023912392239323942395239623972398239924002401240224032404240524062407240824092410241124122413241424152416241724182419242024212422242324242425242624272428242924302431243224332434243524362437243824392440244124422443244424452446244724482449245024512452245324542455245624572458245924602461246224632464246524662467246824692470247124722473247424752476247724782479248024812482248324842485248624872488248924902491249224932494249524962497249824992500250125022503250425052506250725082509251025112512251325142515251625172518251925202521252225232524252525262527252825292530253125322533253425352536253725382539254025412542254325442545254625472548254925502551255225532554255525562557255825592560256125622563256425652566256725682569257025712572257325742575257625772578257925802581258225832584258525862587258825892590259125922593259425952596259725982599260026012602260326042605260626072608260926102611261226132614261526162617261826192620262126222623262426252626262726282629263026312632263326342635263626372638263926402641264226432644264526462647264826492650265126522653265426552656265726582659266026612662266326642665266626672668266926702671267226732674267526762677267826792680268126822683268426852686268726882689269026912692269326942695269626972698269927002701270227032704270527062707270827092710271127122713271427152716271727182719272027212722272327242725272627272728272927302731273227332734273527362737273827392740274127422743274427452746274727482749275027512752275327542755275627572758275927602761276227632764276527662767
  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. using System;
  28. using System.Collections.Generic;
  29. using System.Reflection;
  30. using System.Runtime.InteropServices;
  31. using System.Threading;
  32. using log4net;
  33. using OpenMetaverse;
  34. using BulletDotNET;
  35. using OpenSim.Framework;
  36. using OpenSim.Region.Physics.Manager;
  37. namespace OpenSim.Region.Physics.BulletDotNETPlugin
  38. {
  39. public class BulletDotNETPrim : PhysicsActor
  40. {
  41. private static readonly ILog m_log = LogManager.GetLogger(MethodBase.GetCurrentMethod().DeclaringType);
  42. private Vector3 _position;
  43. private Vector3 m_zeroPosition;
  44. private Vector3 _velocity;
  45. private Vector3 _torque;
  46. private Vector3 m_lastVelocity;
  47. private Vector3 m_lastposition;
  48. private Quaternion m_lastorientation = Quaternion.Identity;
  49. private Vector3 m_rotationalVelocity;
  50. private Vector3 _size;
  51. private Vector3 _acceleration;
  52. // private d.Vector3 _zeroPosition = new d.Vector3(0.0f, 0.0f, 0.0f);
  53. private Quaternion _orientation;
  54. private Vector3 m_taintposition;
  55. private Vector3 m_taintsize;
  56. private Vector3 m_taintVelocity;
  57. private Vector3 m_taintTorque;
  58. private Quaternion m_taintrot;
  59. private Vector3 m_angularlock = Vector3.One;
  60. private Vector3 m_taintAngularLock = Vector3.One;
  61. // private btGeneric6DofConstraint Amotor;
  62. private Vector3 m_PIDTarget;
  63. private float m_PIDTau;
  64. private float m_PIDHoverHeight;
  65. private float m_PIDHoverTau;
  66. private bool m_useHoverPID;
  67. private PIDHoverType m_PIDHoverType = PIDHoverType.Ground;
  68. private float m_targetHoverHeight;
  69. private float m_groundHeight;
  70. private float m_waterHeight;
  71. private float PID_D = 35f;
  72. private float PID_G = 25f;
  73. // private float m_tensor = 5f;
  74. // private int body_autodisable_frames = 20;
  75. private IMesh primMesh;
  76. private bool m_usePID;
  77. private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
  78. | CollisionCategories.Space
  79. | CollisionCategories.Body
  80. | CollisionCategories.Character
  81. );
  82. private bool m_taintshape;
  83. private bool m_taintPhysics;
  84. // private bool m_collidesLand = true;
  85. private bool m_collidesWater;
  86. public bool m_returnCollisions;
  87. // Default we're a Geometry
  88. // private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
  89. // Default, Collide with Other Geometries, spaces and Bodies
  90. // private CollisionCategories m_collisionFlags = m_default_collisionFlags;
  91. public bool m_taintremove;
  92. public bool m_taintdisable;
  93. public bool m_disabled;
  94. public bool m_taintadd;
  95. public bool m_taintselected;
  96. public bool m_taintCollidesWater;
  97. public uint m_localID;
  98. //public GCHandle gc;
  99. // private CollisionLocker ode;
  100. private bool m_taintforce;
  101. private bool m_taintaddangularforce;
  102. private Vector3 m_force;
  103. private List<Vector3> m_forcelist = new List<Vector3>();
  104. private List<Vector3> m_angularforcelist = new List<Vector3>();
  105. private IMesh _mesh;
  106. private PrimitiveBaseShape _pbs;
  107. private BulletDotNETScene _parent_scene;
  108. public btCollisionShape prim_geom;
  109. public IntPtr _triMeshData;
  110. private PhysicsActor _parent;
  111. private PhysicsActor m_taintparent;
  112. private List<BulletDotNETPrim> childrenPrim = new List<BulletDotNETPrim>();
  113. private bool iscolliding;
  114. private bool m_isphysical;
  115. private bool m_isSelected;
  116. internal bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
  117. private bool m_throttleUpdates;
  118. // private int throttleCounter;
  119. public int m_interpenetrationcount;
  120. public float m_collisionscore;
  121. public int m_roundsUnderMotionThreshold;
  122. private int m_crossingfailures;
  123. public float m_buoyancy;
  124. public bool outofBounds;
  125. private float m_density = 10.000006836f; // Aluminum g/cm3;
  126. public bool _zeroFlag;
  127. private bool m_lastUpdateSent;
  128. private String m_primName;
  129. private Vector3 _target_velocity;
  130. public int m_eventsubscription;
  131. private int m_requestedUpdateFrequency = 0;
  132. private CollisionEventUpdate CollisionEventsThisFrame = null;
  133. public volatile bool childPrim;
  134. private btVector3 tempPosition1;
  135. private btVector3 tempPosition2;
  136. private btVector3 tempPosition3;
  137. private btVector3 tempSize1;
  138. private btVector3 tempSize2;
  139. private btVector3 tempLinearVelocity1;
  140. private btVector3 tempLinearVelocity2;
  141. private btVector3 tempAngularVelocity1;
  142. private btVector3 tempAngularVelocity2;
  143. private btVector3 tempInertia1;
  144. private btVector3 tempInertia2;
  145. private btVector3 tempAddForce;
  146. private btQuaternion tempOrientation1;
  147. private btQuaternion tempOrientation2;
  148. private btMotionState tempMotionState1;
  149. private btMotionState tempMotionState2;
  150. private btMotionState tempMotionState3;
  151. private btTransform tempTransform1;
  152. private btTransform tempTransform2;
  153. private btTransform tempTransform3;
  154. private btTransform tempTransform4;
  155. private btTriangleIndexVertexArray btshapeArray;
  156. private btVector3 AxisLockAngleHigh;
  157. private btVector3 AxisLockLinearLow;
  158. private btVector3 AxisLockLinearHigh;
  159. private bool forceenable = false;
  160. private btGeneric6DofConstraint m_aMotor;
  161. public btRigidBody Body;
  162. public BulletDotNETPrim(String primName, BulletDotNETScene parent_scene, Vector3 pos, Vector3 size,
  163. Quaternion rotation, IMesh mesh, PrimitiveBaseShape pbs, bool pisPhysical)
  164. {
  165. tempPosition1 = new btVector3(0, 0, 0);
  166. tempPosition2 = new btVector3(0, 0, 0);
  167. tempPosition3 = new btVector3(0, 0, 0);
  168. tempSize1 = new btVector3(0, 0, 0);
  169. tempSize2 = new btVector3(0, 0, 0);
  170. tempLinearVelocity1 = new btVector3(0, 0, 0);
  171. tempLinearVelocity2 = new btVector3(0, 0, 0);
  172. tempAngularVelocity1 = new btVector3(0, 0, 0);
  173. tempAngularVelocity2 = new btVector3(0, 0, 0);
  174. tempInertia1 = new btVector3(0, 0, 0);
  175. tempInertia2 = new btVector3(0, 0, 0);
  176. tempOrientation1 = new btQuaternion(0, 0, 0, 1);
  177. tempOrientation2 = new btQuaternion(0, 0, 0, 1);
  178. _parent_scene = parent_scene;
  179. tempTransform1 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero);
  180. tempTransform2 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
  181. tempTransform3 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
  182. tempTransform4 = new btTransform(_parent_scene.QuatIdentity, _parent_scene.VectorZero); ;
  183. tempMotionState1 = new btDefaultMotionState(_parent_scene.TransZero);
  184. tempMotionState2 = new btDefaultMotionState(_parent_scene.TransZero);
  185. tempMotionState3 = new btDefaultMotionState(_parent_scene.TransZero);
  186. AxisLockLinearLow = new btVector3(-1 * (int)Constants.RegionSize, -1 * (int)Constants.RegionSize, -1 * (int)Constants.RegionSize);
  187. int regionsize = (int)Constants.RegionSize;
  188. if (regionsize == 256)
  189. regionsize = 512;
  190. AxisLockLinearHigh = new btVector3((int)Constants.RegionSize, (int)Constants.RegionSize, (int)Constants.RegionSize);
  191. _target_velocity = Vector3.Zero;
  192. _velocity = Vector3.Zero;
  193. _position = pos;
  194. m_taintposition = pos;
  195. PID_D = parent_scene.bodyPIDD;
  196. PID_G = parent_scene.bodyPIDG;
  197. m_density = parent_scene.geomDefaultDensity;
  198. // m_tensor = parent_scene.bodyMotorJointMaxforceTensor;
  199. // body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
  200. prim_geom = null;
  201. Body = null;
  202. if (size.X <= 0) size.X = 0.01f;
  203. if (size.Y <= 0) size.Y = 0.01f;
  204. if (size.Z <= 0) size.Z = 0.01f;
  205. _size = size;
  206. m_taintsize = _size;
  207. _acceleration = Vector3.Zero;
  208. m_rotationalVelocity = Vector3.Zero;
  209. _orientation = rotation;
  210. m_taintrot = _orientation;
  211. _mesh = mesh;
  212. _pbs = pbs;
  213. _parent_scene = parent_scene;
  214. if (pos.Z < 0)
  215. m_isphysical = false;
  216. else
  217. {
  218. m_isphysical = pisPhysical;
  219. // If we're physical, we need to be in the master space for now.
  220. // linksets *should* be in a space together.. but are not currently
  221. }
  222. m_primName = primName;
  223. m_taintadd = true;
  224. _parent_scene.AddPhysicsActorTaint(this);
  225. }
  226. #region PhysicsActor overrides
  227. public override bool Stopped
  228. {
  229. get { return _zeroFlag; }
  230. }
  231. public override Vector3 Size
  232. {
  233. get { return _size; }
  234. set { _size = value; }
  235. }
  236. public override PrimitiveBaseShape Shape
  237. {
  238. set
  239. {
  240. _pbs = value;
  241. m_taintshape = true;
  242. }
  243. }
  244. public override uint LocalID
  245. {
  246. set
  247. {
  248. //m_log.Info("[PHYSICS]: Setting TrackerID: " + value);
  249. m_localID = value;
  250. }
  251. }
  252. public override bool Grabbed
  253. {
  254. set { return; }
  255. }
  256. public override bool Selected
  257. {
  258. set
  259. {
  260. // This only makes the object not collidable if the object
  261. // is physical or the object is modified somehow *IN THE FUTURE*
  262. // without this, if an avatar selects prim, they can walk right
  263. // through it while it's selected
  264. m_collisionscore = 0;
  265. if ((m_isphysical && !_zeroFlag) || !value)
  266. {
  267. m_taintselected = value;
  268. _parent_scene.AddPhysicsActorTaint(this);
  269. }
  270. else
  271. {
  272. m_taintselected = value;
  273. m_isSelected = value;
  274. }
  275. }
  276. }
  277. public override void CrossingFailure()
  278. {
  279. m_crossingfailures++;
  280. if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
  281. {
  282. base.RaiseOutOfBounds(_position);
  283. return;
  284. }
  285. else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
  286. {
  287. m_log.Warn("[PHYSICS]: Too many crossing failures for: " + m_primName);
  288. }
  289. }
  290. public override void link(PhysicsActor obj)
  291. {
  292. m_taintparent = obj;
  293. }
  294. public override void delink()
  295. {
  296. m_taintparent = null;
  297. }
  298. public override void LockAngularMotion(Vector3 axis)
  299. {
  300. m_log.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
  301. m_taintAngularLock = axis;
  302. }
  303. public override Vector3 Position
  304. {
  305. get { return _position; }
  306. set
  307. {
  308. _position = value;
  309. //m_log.Info("[PHYSICS]: " + _position.ToString());
  310. }
  311. }
  312. public override float Mass
  313. {
  314. get { return CalculateMass(); }
  315. }
  316. public override Vector3 Force
  317. {
  318. //get { return Vector3.Zero; }
  319. get { return m_force; }
  320. set { m_force = value; }
  321. }
  322. public override int VehicleType
  323. {
  324. get { return 0; }
  325. set { return; }
  326. }
  327. public override void VehicleFloatParam(int param, float value)
  328. {
  329. //TODO:
  330. }
  331. public override void VehicleVectorParam(int param, Vector3 value)
  332. {
  333. //TODO:
  334. }
  335. public override void VehicleRotationParam(int param, Quaternion rotation)
  336. {
  337. //TODO:
  338. }
  339. public override void VehicleFlags(int param, bool remove)
  340. {
  341. }
  342. public override void SetVolumeDetect(int param)
  343. {
  344. //TODO: GhostObject
  345. m_isVolumeDetect = (param != 0);
  346. }
  347. public override Vector3 GeometricCenter
  348. {
  349. get { return Vector3.Zero; }
  350. }
  351. public override Vector3 CenterOfMass
  352. {
  353. get { return Vector3.Zero; }
  354. }
  355. public override Vector3 Velocity
  356. {
  357. get
  358. {
  359. // Averate previous velocity with the new one so
  360. // client object interpolation works a 'little' better
  361. Vector3 returnVelocity;
  362. returnVelocity.X = (m_lastVelocity.X + _velocity.X) / 2;
  363. returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y) / 2;
  364. returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z) / 2;
  365. return returnVelocity;
  366. }
  367. set
  368. {
  369. _velocity = value;
  370. m_taintVelocity = value;
  371. _parent_scene.AddPhysicsActorTaint(this);
  372. }
  373. }
  374. public override Vector3 Torque
  375. {
  376. get
  377. {
  378. if (!m_isphysical || Body.Handle == IntPtr.Zero)
  379. return Vector3.Zero;
  380. return _torque;
  381. }
  382. set
  383. {
  384. m_taintTorque = value;
  385. _parent_scene.AddPhysicsActorTaint(this);
  386. }
  387. }
  388. public override float CollisionScore
  389. {
  390. get { return m_collisionscore; }
  391. set { m_collisionscore = value; }
  392. }
  393. public override Vector3 Acceleration
  394. {
  395. get { return _acceleration; }
  396. }
  397. public override Quaternion Orientation
  398. {
  399. get { return _orientation; }
  400. set { _orientation = value; }
  401. }
  402. public override int PhysicsActorType
  403. {
  404. get { return (int)ActorTypes.Prim; }
  405. set { return; }
  406. }
  407. public override bool IsPhysical
  408. {
  409. get { return m_isphysical; }
  410. set { m_isphysical = value; }
  411. }
  412. public override bool Flying
  413. {
  414. // no flying prims for you
  415. get { return false; }
  416. set { }
  417. }
  418. public override bool SetAlwaysRun
  419. {
  420. get { return false; }
  421. set { return; }
  422. }
  423. public override bool ThrottleUpdates
  424. {
  425. get { return m_throttleUpdates; }
  426. set { m_throttleUpdates = value; }
  427. }
  428. public override bool IsColliding
  429. {
  430. get { return iscolliding; }
  431. set { iscolliding = value; }
  432. }
  433. public override bool CollidingGround
  434. {
  435. get { return false; }
  436. set { return; }
  437. }
  438. public override bool CollidingObj
  439. {
  440. get { return false; }
  441. set { return; }
  442. }
  443. public override bool FloatOnWater
  444. {
  445. set
  446. {
  447. m_taintCollidesWater = value;
  448. _parent_scene.AddPhysicsActorTaint(this);
  449. }
  450. }
  451. public override Vector3 RotationalVelocity
  452. {
  453. get
  454. {
  455. Vector3 pv = Vector3.Zero;
  456. if (_zeroFlag)
  457. return pv;
  458. m_lastUpdateSent = false;
  459. if (m_rotationalVelocity.ApproxEquals(pv, 0.2f))
  460. return pv;
  461. return m_rotationalVelocity;
  462. }
  463. set { m_rotationalVelocity = value; }
  464. }
  465. public override bool Kinematic
  466. {
  467. get { return false; }
  468. set { }
  469. }
  470. public override float Buoyancy
  471. {
  472. get { return m_buoyancy; }
  473. set { m_buoyancy = value; }
  474. }
  475. public override Vector3 PIDTarget { set { m_PIDTarget = value; ; } }
  476. public override bool PIDActive { set { m_usePID = value; } }
  477. public override float PIDTau { set { m_PIDTau = value; } }
  478. public override float PIDHoverHeight { set { m_PIDHoverHeight = value; ; } }
  479. public override bool PIDHoverActive { set { m_useHoverPID = value; } }
  480. public override PIDHoverType PIDHoverType { set { m_PIDHoverType = value; } }
  481. public override float PIDHoverTau { set { m_PIDHoverTau = value; } }
  482. public override Quaternion APIDTarget { set { return; } }
  483. public override bool APIDActive { set { return; } }
  484. public override float APIDStrength { set { return; } }
  485. public override float APIDDamping { set { return; } }
  486. public override void AddForce(Vector3 force, bool pushforce)
  487. {
  488. m_forcelist.Add(force);
  489. m_taintforce = true;
  490. //m_log.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
  491. }
  492. public override void AddAngularForce(Vector3 force, bool pushforce)
  493. {
  494. m_angularforcelist.Add(force);
  495. m_taintaddangularforce = true;
  496. }
  497. public override void SetMomentum(Vector3 momentum)
  498. {
  499. }
  500. public override void SubscribeEvents(int ms)
  501. {
  502. m_eventsubscription = ms;
  503. m_requestedUpdateFrequency = ms;
  504. _parent_scene.addCollisionEventReporting(this);
  505. }
  506. public override void UnSubscribeEvents()
  507. {
  508. _parent_scene.remCollisionEventReporting(this);
  509. m_eventsubscription = 0;
  510. m_requestedUpdateFrequency = 0;
  511. }
  512. public override bool SubscribedEvents()
  513. {
  514. return (m_eventsubscription > 0);
  515. }
  516. #endregion
  517. public void AddCollision(uint collideWith, ContactPoint contact)
  518. {
  519. if (CollisionEventsThisFrame == null)
  520. {
  521. CollisionEventsThisFrame = new CollisionEventUpdate();
  522. }
  523. CollisionEventsThisFrame.addCollider(collideWith, contact);
  524. }
  525. public void SendCollisions()
  526. {
  527. if (m_eventsubscription >= m_requestedUpdateFrequency)
  528. {
  529. if (CollisionEventsThisFrame != null)
  530. {
  531. base.SendCollisionUpdate(CollisionEventsThisFrame);
  532. }
  533. CollisionEventsThisFrame = null;
  534. // m_eventsubscription = 0;
  535. }
  536. return;
  537. }
  538. internal void Dispose()
  539. {
  540. //TODO:
  541. DisableAxisMotor();
  542. DisposeOfBody();
  543. SetCollisionShape(null);
  544. if (tempMotionState3 != null && tempMotionState3.Handle != IntPtr.Zero)
  545. {
  546. tempMotionState3.Dispose();
  547. tempMotionState3 = null;
  548. }
  549. if (tempMotionState2 != null && tempMotionState2.Handle != IntPtr.Zero)
  550. {
  551. tempMotionState2.Dispose();
  552. tempMotionState2 = null;
  553. }
  554. if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
  555. {
  556. tempMotionState1.Dispose();
  557. tempMotionState1 = null;
  558. }
  559. if (tempTransform4 != null && tempTransform4.Handle != IntPtr.Zero)
  560. {
  561. tempTransform4.Dispose();
  562. tempTransform4 = null;
  563. }
  564. if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)
  565. {
  566. tempTransform3.Dispose();
  567. tempTransform3 = null;
  568. }
  569. if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
  570. {
  571. tempTransform2.Dispose();
  572. tempTransform2 = null;
  573. }
  574. if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero)
  575. {
  576. tempTransform1.Dispose();
  577. tempTransform1 = null;
  578. }
  579. if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
  580. {
  581. tempOrientation2.Dispose();
  582. tempOrientation2 = null;
  583. }
  584. if (tempOrientation1 != null && tempOrientation1.Handle != IntPtr.Zero)
  585. {
  586. tempOrientation1.Dispose();
  587. tempOrientation1 = null;
  588. }
  589. if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
  590. {
  591. tempInertia1.Dispose();
  592. tempInertia1 = null;
  593. }
  594. if (tempInertia2 != null && tempInertia2.Handle != IntPtr.Zero)
  595. {
  596. tempInertia2.Dispose();
  597. tempInertia1 = null;
  598. }
  599. if (tempAngularVelocity2 != null && tempAngularVelocity2.Handle != IntPtr.Zero)
  600. {
  601. tempAngularVelocity2.Dispose();
  602. tempAngularVelocity2 = null;
  603. }
  604. if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)
  605. {
  606. tempAngularVelocity1.Dispose();
  607. tempAngularVelocity1 = null;
  608. }
  609. if (tempLinearVelocity2 != null && tempLinearVelocity2.Handle != IntPtr.Zero)
  610. {
  611. tempLinearVelocity2.Dispose();
  612. tempLinearVelocity2 = null;
  613. }
  614. if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)
  615. {
  616. tempLinearVelocity1.Dispose();
  617. tempLinearVelocity1 = null;
  618. }
  619. if (tempSize2 != null && tempSize2.Handle != IntPtr.Zero)
  620. {
  621. tempSize2.Dispose();
  622. tempSize2 = null;
  623. }
  624. if (tempSize1 != null && tempSize1.Handle != IntPtr.Zero)
  625. {
  626. tempSize1.Dispose();
  627. tempSize1 = null;
  628. }
  629. if (tempPosition3 != null && tempPosition3.Handle != IntPtr.Zero)
  630. {
  631. tempPosition3.Dispose();
  632. tempPosition3 = null;
  633. }
  634. if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
  635. {
  636. tempPosition2.Dispose();
  637. tempPosition2 = null;
  638. }
  639. if (tempPosition1 != null && tempPosition1.Handle != IntPtr.Zero)
  640. {
  641. tempPosition1.Dispose();
  642. tempPosition1 = null;
  643. }
  644. if (AxisLockLinearLow != null && AxisLockLinearLow.Handle != IntPtr.Zero)
  645. {
  646. AxisLockLinearLow.Dispose();
  647. AxisLockLinearLow = null;
  648. }
  649. if (AxisLockLinearHigh != null && AxisLockLinearHigh.Handle != IntPtr.Zero)
  650. {
  651. AxisLockLinearHigh.Dispose();
  652. AxisLockLinearHigh = null;
  653. }
  654. }
  655. public void ProcessTaints(float timestep)
  656. {
  657. if (m_taintadd)
  658. {
  659. // m_log.Debug("[PHYSICS]: TaintAdd");
  660. changeadd(timestep);
  661. }
  662. if (prim_geom == null)
  663. {
  664. CreateGeom(IntPtr.Zero, primMesh);
  665. if (IsPhysical)
  666. SetBody(Mass);
  667. else
  668. SetBody(0);
  669. // m_log.Debug("[PHYSICS]: GEOM_DOESNT_EXSIT");
  670. }
  671. if (prim_geom.Handle == IntPtr.Zero)
  672. {
  673. CreateGeom(IntPtr.Zero, primMesh);
  674. if (IsPhysical)
  675. SetBody(Mass);
  676. else
  677. SetBody(0);
  678. // m_log.Debug("[PHYSICS]: GEOM_DOESNT_EXSIT");
  679. }
  680. if (!_position.ApproxEquals(m_taintposition, 0f))
  681. {
  682. // m_log.Debug("[PHYSICS]: TaintMove");
  683. changemove(timestep);
  684. }
  685. if (m_taintrot != _orientation)
  686. {
  687. // m_log.Debug("[PHYSICS]: TaintRotate");
  688. rotate(timestep);
  689. } //
  690. if (m_taintPhysics != m_isphysical && !(m_taintparent != _parent))
  691. {
  692. // m_log.Debug("[PHYSICS]: TaintPhysics");
  693. changePhysicsStatus(timestep);
  694. }
  695. //
  696. if (!_size.ApproxEquals(m_taintsize, 0f))
  697. {
  698. // m_log.Debug("[PHYSICS]: TaintSize");
  699. changesize(timestep);
  700. }
  701. //
  702. if (m_taintshape)
  703. {
  704. // m_log.Debug("[PHYSICS]: TaintShape");
  705. changeshape(timestep);
  706. } //
  707. if (m_taintforce)
  708. {
  709. // m_log.Debug("[PHYSICS]: TaintForce");
  710. changeAddForce(timestep);
  711. }
  712. if (m_taintaddangularforce)
  713. {
  714. // m_log.Debug("[PHYSICS]: TaintAngularForce");
  715. changeAddAngularForce(timestep);
  716. }
  717. if (!m_taintTorque.ApproxEquals(Vector3.Zero, 0.001f))
  718. {
  719. // m_log.Debug("[PHYSICS]: TaintTorque");
  720. changeSetTorque(timestep);
  721. }
  722. if (m_taintdisable)
  723. {
  724. // m_log.Debug("[PHYSICS]: TaintDisable");
  725. changedisable(timestep);
  726. }
  727. if (m_taintselected != m_isSelected)
  728. {
  729. // m_log.Debug("[PHYSICS]: TaintSelected");
  730. changeSelectedStatus(timestep);
  731. }
  732. if (!m_taintVelocity.ApproxEquals(Vector3.Zero, 0.001f))
  733. {
  734. // m_log.Debug("[PHYSICS]: TaintVelocity");
  735. changevelocity(timestep);
  736. }
  737. if (m_taintparent != _parent)
  738. {
  739. // m_log.Debug("[PHYSICS]: TaintLink");
  740. changelink(timestep);
  741. }
  742. if (m_taintCollidesWater != m_collidesWater)
  743. {
  744. changefloatonwater(timestep);
  745. }
  746. if (!m_angularlock.ApproxEquals(m_taintAngularLock, 0))
  747. {
  748. // m_log.Debug("[PHYSICS]: TaintAngularLock");
  749. changeAngularLock(timestep);
  750. }
  751. if (m_taintremove)
  752. {
  753. DisposeOfBody();
  754. Dispose();
  755. }
  756. }
  757. #region Physics Scene Change Action routines
  758. private void changeadd(float timestep)
  759. {
  760. //SetCollisionShape(null);
  761. // Construction of new prim
  762. if (Body != null)
  763. {
  764. if (Body.Handle != IntPtr.Zero)
  765. {
  766. DisableAxisMotor();
  767. _parent_scene.removeFromWorld(this, Body);
  768. //Body.Dispose();
  769. }
  770. //Body = null;
  771. // TODO: dispose parts that make up body
  772. }
  773. if (_parent_scene.needsMeshing(_pbs))
  774. {
  775. // Don't need to re-enable body.. it's done in SetMesh
  776. float meshlod = _parent_scene.meshSculptLOD;
  777. if (IsPhysical)
  778. meshlod = _parent_scene.MeshSculptphysicalLOD;
  779. IMesh mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical);
  780. // createmesh returns null when it doesn't mesh.
  781. CreateGeom(IntPtr.Zero, mesh);
  782. }
  783. else
  784. {
  785. _mesh = null;
  786. CreateGeom(IntPtr.Zero, null);
  787. }
  788. if (IsPhysical)
  789. SetBody(Mass);
  790. else
  791. SetBody(0);
  792. //changeSelectedStatus(timestep);
  793. m_taintadd = false;
  794. }
  795. private void changemove(float timestep)
  796. {
  797. // m_log.Debug("[PHYSICS]: _________ChangeMove");
  798. if (!m_isphysical)
  799. {
  800. tempTransform2 = Body.getWorldTransform();
  801. btQuaternion quat = tempTransform2.getRotation();
  802. tempPosition2.setValue(_position.X, _position.Y, _position.Z);
  803. tempTransform2.Dispose();
  804. tempTransform2 = new btTransform(quat, tempPosition2);
  805. Body.setWorldTransform(tempTransform2);
  806. changeSelectedStatus(timestep);
  807. resetCollisionAccounting();
  808. }
  809. else
  810. {
  811. if (Body != null)
  812. {
  813. if (Body.Handle != IntPtr.Zero)
  814. {
  815. DisableAxisMotor();
  816. _parent_scene.removeFromWorld(this, Body);
  817. //Body.Dispose();
  818. }
  819. //Body = null;
  820. // TODO: dispose parts that make up body
  821. }
  822. /*
  823. if (_parent_scene.needsMeshing(_pbs))
  824. {
  825. // Don't need to re-enable body.. it's done in SetMesh
  826. float meshlod = _parent_scene.meshSculptLOD;
  827. if (IsPhysical)
  828. meshlod = _parent_scene.MeshSculptphysicalLOD;
  829. IMesh mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical);
  830. // createmesh returns null when it doesn't mesh.
  831. CreateGeom(IntPtr.Zero, mesh);
  832. }
  833. else
  834. {
  835. _mesh = null;
  836. CreateGeom(IntPtr.Zero, null);
  837. }
  838. SetCollisionShape(prim_geom);
  839. */
  840. if (m_isphysical)
  841. SetBody(Mass);
  842. else
  843. SetBody(0);
  844. changeSelectedStatus(timestep);
  845. resetCollisionAccounting();
  846. }
  847. m_taintposition = _position;
  848. }
  849. private void rotate(float timestep)
  850. {
  851. // m_log.Debug("[PHYSICS]: _________ChangeRotate");
  852. tempTransform2 = Body.getWorldTransform();
  853. tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
  854. tempTransform2.setRotation(tempOrientation2);
  855. Body.setWorldTransform(tempTransform2);
  856. resetCollisionAccounting();
  857. m_taintrot = _orientation;
  858. }
  859. private void changePhysicsStatus(float timestep)
  860. {
  861. if (Body != null)
  862. {
  863. if (Body.Handle != IntPtr.Zero)
  864. {
  865. DisableAxisMotor();
  866. _parent_scene.removeFromWorld(this, Body);
  867. //Body.Dispose();
  868. }
  869. //Body = null;
  870. // TODO: dispose parts that make up body
  871. }
  872. // m_log.Debug("[PHYSICS]: _________ChangePhysics");
  873. ProcessGeomCreation();
  874. if (m_isphysical)
  875. SetBody(Mass);
  876. else
  877. SetBody(0);
  878. changeSelectedStatus(timestep);
  879. resetCollisionAccounting();
  880. m_taintPhysics = m_isphysical;
  881. }
  882. internal void ProcessGeomCreation()
  883. {
  884. if (_parent_scene.needsMeshing(_pbs))
  885. {
  886. ProcessGeomCreationAsTriMesh(Vector3.Zero, Quaternion.Identity);
  887. // createmesh returns null when it doesn't mesh.
  888. CreateGeom(IntPtr.Zero, _mesh);
  889. }
  890. else
  891. {
  892. _mesh = null;
  893. CreateGeom(IntPtr.Zero, null);
  894. }
  895. SetCollisionShape(prim_geom);
  896. }
  897. internal bool NeedsMeshing()
  898. {
  899. return _parent_scene.needsMeshing(_pbs);
  900. }
  901. internal void ProcessGeomCreationAsTriMesh(Vector3 positionOffset, Quaternion orientation)
  902. {
  903. // Don't need to re-enable body.. it's done in SetMesh
  904. float meshlod = _parent_scene.meshSculptLOD;
  905. if (IsPhysical)
  906. meshlod = _parent_scene.MeshSculptphysicalLOD;
  907. IMesh mesh = _parent_scene.mesher.CreateMesh(SOPName, _pbs, _size, meshlod, IsPhysical);
  908. if (!positionOffset.ApproxEquals(Vector3.Zero, 0.001f) || orientation != Quaternion.Identity)
  909. {
  910. float[] xyz = new float[3];
  911. xyz[0] = positionOffset.X;
  912. xyz[1] = positionOffset.Y;
  913. xyz[2] = positionOffset.Z;
  914. Matrix4 m4 = Matrix4.CreateFromQuaternion(orientation);
  915. float[,] matrix = new float[3, 3];
  916. matrix[0, 0] = m4.M11;
  917. matrix[0, 1] = m4.M12;
  918. matrix[0, 2] = m4.M13;
  919. matrix[1, 0] = m4.M21;
  920. matrix[1, 1] = m4.M22;
  921. matrix[1, 2] = m4.M23;
  922. matrix[2, 0] = m4.M31;
  923. matrix[2, 1] = m4.M32;
  924. matrix[2, 2] = m4.M33;
  925. mesh.TransformLinear(matrix, xyz);
  926. }
  927. _mesh = mesh;
  928. }
  929. private void changesize(float timestep)
  930. {
  931. if (Body != null)
  932. {
  933. if (Body.Handle != IntPtr.Zero)
  934. {
  935. DisableAxisMotor();
  936. _parent_scene.removeFromWorld(this, Body);
  937. //Body.Dispose();
  938. }
  939. //Body = null;
  940. // TODO: dispose parts that make up body
  941. }
  942. // m_log.Debug("[PHYSICS]: _________ChangeSize");
  943. SetCollisionShape(null);
  944. // Construction of new prim
  945. ProcessGeomCreation();
  946. if (IsPhysical)
  947. SetBody(Mass);
  948. else
  949. SetBody(0);
  950. m_taintsize = _size;
  951. }
  952. private void changeshape(float timestep)
  953. {
  954. if (Body != null)
  955. {
  956. if (Body.Handle != IntPtr.Zero)
  957. {
  958. DisableAxisMotor();
  959. _parent_scene.removeFromWorld(this, Body);
  960. //Body.Dispose();
  961. }
  962. //Body = null;
  963. // TODO: dispose parts that make up body
  964. }
  965. // Cleanup of old prim geometry and Bodies
  966. if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero)
  967. {
  968. if (childPrim)
  969. {
  970. if (_parent != null)
  971. {
  972. BulletDotNETPrim parent = (BulletDotNETPrim)_parent;
  973. parent.ChildDelink(this);
  974. }
  975. }
  976. else
  977. {
  978. //disableBody();
  979. }
  980. }
  981. try
  982. {
  983. //SetCollisionShape(null);
  984. }
  985. catch (System.AccessViolationException)
  986. {
  987. //prim_geom = IntPtr.Zero;
  988. m_log.Error("[PHYSICS]: PrimGeom dead");
  989. }
  990. // we don't need to do space calculation because the client sends a position update also.
  991. if (_size.X <= 0) _size.X = 0.01f;
  992. if (_size.Y <= 0) _size.Y = 0.01f;
  993. if (_size.Z <= 0) _size.Z = 0.01f;
  994. // Construction of new prim
  995. ProcessGeomCreation();
  996. tempPosition1.setValue(_position.X, _position.Y, _position.Z);
  997. if (tempOrientation1.Handle != IntPtr.Zero)
  998. tempOrientation1.Dispose();
  999. tempOrientation1 = new btQuaternion(_orientation.X, Orientation.Y, _orientation.Z, _orientation.W);
  1000. if (tempTransform1 != null && tempTransform1.Handle != IntPtr.Zero)
  1001. tempTransform1.Dispose();
  1002. tempTransform1 = new btTransform(tempOrientation1, tempPosition1);
  1003. //d.GeomBoxSetLengths(prim_geom, _size.X, _size.Y, _size.Z);
  1004. if (IsPhysical)
  1005. {
  1006. SetBody(Mass);
  1007. // Re creates body on size.
  1008. // EnableBody also does setMass()
  1009. }
  1010. else
  1011. {
  1012. SetBody(0);
  1013. }
  1014. changeSelectedStatus(timestep);
  1015. if (childPrim)
  1016. {
  1017. if (_parent is BulletDotNETPrim)
  1018. {
  1019. BulletDotNETPrim parent = (BulletDotNETPrim)_parent;
  1020. parent.ChildSetGeom(this);
  1021. }
  1022. }
  1023. resetCollisionAccounting();
  1024. m_taintshape = false;
  1025. }
  1026. private void resetCollisionAccounting()
  1027. {
  1028. m_collisionscore = 0;
  1029. }
  1030. private void ChildSetGeom(BulletDotNETPrim bulletDotNETPrim)
  1031. {
  1032. // TODO: throw new NotImplementedException();
  1033. }
  1034. private void changeAddForce(float timestep)
  1035. {
  1036. if (!m_isSelected)
  1037. {
  1038. lock (m_forcelist)
  1039. {
  1040. //m_log.Info("[PHYSICS]: dequeing forcelist");
  1041. if (IsPhysical)
  1042. {
  1043. Vector3 iforce = Vector3.Zero;
  1044. for (int i = 0; i < m_forcelist.Count; i++)
  1045. {
  1046. iforce = iforce + m_forcelist[i];
  1047. }
  1048. if (Body != null && Body.Handle != IntPtr.Zero)
  1049. {
  1050. if (tempAddForce != null && tempAddForce.Handle != IntPtr.Zero)
  1051. tempAddForce.Dispose();
  1052. enableBodySoft();
  1053. tempAddForce = new btVector3(iforce.X, iforce.Y, iforce.Z);
  1054. Body.applyCentralImpulse(tempAddForce);
  1055. }
  1056. }
  1057. m_forcelist.Clear();
  1058. }
  1059. m_collisionscore = 0;
  1060. m_interpenetrationcount = 0;
  1061. }
  1062. m_taintforce = false;
  1063. }
  1064. private void changeAddAngularForce(float timestep)
  1065. {
  1066. if (!m_isSelected)
  1067. {
  1068. lock (m_angularforcelist)
  1069. {
  1070. //m_log.Info("[PHYSICS]: dequeing forcelist");
  1071. if (IsPhysical)
  1072. {
  1073. Vector3 iforce = Vector3.Zero;
  1074. for (int i = 0; i < m_angularforcelist.Count; i++)
  1075. {
  1076. iforce = iforce + m_angularforcelist[i];
  1077. }
  1078. if (Body != null && Body.Handle != IntPtr.Zero)
  1079. {
  1080. if (tempAddForce != null && tempAddForce.Handle != IntPtr.Zero)
  1081. tempAddForce.Dispose();
  1082. enableBodySoft();
  1083. tempAddForce = new btVector3(iforce.X, iforce.Y, iforce.Z);
  1084. Body.applyTorqueImpulse(tempAddForce);
  1085. }
  1086. }
  1087. m_angularforcelist.Clear();
  1088. }
  1089. m_collisionscore = 0;
  1090. m_interpenetrationcount = 0;
  1091. }
  1092. m_taintaddangularforce = false;
  1093. }
  1094. private void changeSetTorque(float timestep)
  1095. {
  1096. if (!m_isSelected)
  1097. {
  1098. if (IsPhysical)
  1099. {
  1100. if (Body != null && Body.Handle != IntPtr.Zero)
  1101. {
  1102. tempAngularVelocity2.setValue(m_taintTorque.X, m_taintTorque.Y, m_taintTorque.Z);
  1103. Body.applyTorque(tempAngularVelocity2);
  1104. }
  1105. }
  1106. }
  1107. m_taintTorque = Vector3.Zero;
  1108. }
  1109. private void changedisable(float timestep)
  1110. {
  1111. // TODO: throw new NotImplementedException();
  1112. }
  1113. private void changeSelectedStatus(float timestep)
  1114. {
  1115. // TODO: throw new NotImplementedException();
  1116. if (m_taintselected)
  1117. {
  1118. // Body.setCollisionFlags((int)ContactFlags.CF_NO_CONTACT_RESPONSE);
  1119. disableBodySoft();
  1120. }
  1121. else
  1122. {
  1123. // Body.setCollisionFlags(0 | (int)ContactFlags.CF_CUSTOM_MATERIAL_CALLBACK);
  1124. enableBodySoft();
  1125. }
  1126. m_isSelected = m_taintselected;
  1127. }
  1128. private void changevelocity(float timestep)
  1129. {
  1130. if (!m_isSelected)
  1131. {
  1132. if (IsPhysical)
  1133. {
  1134. if (Body != null && Body.Handle != IntPtr.Zero)
  1135. {
  1136. tempLinearVelocity2.setValue(m_taintVelocity.X, m_taintVelocity.Y, m_taintVelocity.Z);
  1137. Body.setLinearVelocity(tempLinearVelocity2);
  1138. }
  1139. }
  1140. //resetCollisionAccounting();
  1141. }
  1142. m_taintVelocity = Vector3.Zero;
  1143. }
  1144. private void changelink(float timestep)
  1145. {
  1146. if (IsPhysical)
  1147. {
  1148. // Construction of new prim
  1149. if (Body != null)
  1150. {
  1151. if (Body.Handle != IntPtr.Zero)
  1152. {
  1153. DisableAxisMotor();
  1154. _parent_scene.removeFromWorld(this, Body);
  1155. //Body.Dispose();
  1156. }
  1157. //Body = null;
  1158. // TODO: dispose parts that make up body
  1159. }
  1160. if (_parent == null && m_taintparent != null)
  1161. {
  1162. if (m_taintparent is BulletDotNETPrim)
  1163. {
  1164. BulletDotNETPrim obj = (BulletDotNETPrim)m_taintparent;
  1165. obj.ParentPrim(this);
  1166. childPrim = true;
  1167. }
  1168. }
  1169. else if (_parent != null && m_taintparent == null)
  1170. {
  1171. if (_parent is BulletDotNETPrim)
  1172. {
  1173. BulletDotNETPrim obj = (BulletDotNETPrim)_parent;
  1174. obj.ChildDelink(obj);
  1175. childPrim = false;
  1176. }
  1177. }
  1178. if (m_taintparent != null)
  1179. {
  1180. Vector3 taintparentPosition = m_taintparent.Position;
  1181. taintparentPosition.Z = m_taintparent.Position.Z + 0.02f;
  1182. m_taintparent.Position = taintparentPosition;
  1183. _parent_scene.AddPhysicsActorTaint(m_taintparent);
  1184. }
  1185. }
  1186. _parent = m_taintparent;
  1187. m_taintPhysics = m_isphysical;
  1188. }
  1189. private void changefloatonwater(float timestep)
  1190. {
  1191. // TODO: throw new NotImplementedException();
  1192. }
  1193. private void changeAngularLock(float timestep)
  1194. {
  1195. if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero)
  1196. {
  1197. if (_parent == null)
  1198. {
  1199. if (!m_taintAngularLock.ApproxEquals(Vector3.One, 0f))
  1200. {
  1201. //d.BodySetFiniteRotationMode(Body, 0);
  1202. //d.BodySetFiniteRotationAxis(Body,m_taintAngularLock.X,m_taintAngularLock.Y,m_taintAngularLock.Z);
  1203. EnableAxisMotor(m_taintAngularLock);
  1204. }
  1205. else
  1206. {
  1207. DisableAxisMotor();
  1208. }
  1209. }
  1210. }
  1211. m_angularlock = m_taintAngularLock;
  1212. }
  1213. #endregion
  1214. internal void Move(float timestep)
  1215. {
  1216. //TODO:
  1217. float fx = 0;
  1218. float fy = 0;
  1219. float fz = 0;
  1220. if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero && !m_isSelected)
  1221. {
  1222. float m_mass = CalculateMass();
  1223. fz = 0f;
  1224. //m_log.Info(m_collisionFlags.ToString());
  1225. if (m_buoyancy != 0)
  1226. {
  1227. if (m_buoyancy > 0)
  1228. {
  1229. fz = (((-1 * _parent_scene.gravityz) * m_buoyancy) * m_mass) * 0.035f;
  1230. //d.Vector3 l_velocity = d.BodyGetLinearVel(Body);
  1231. //m_log.Info("Using Buoyancy: " + buoyancy + " G: " + (_parent_scene.gravityz * m_buoyancy) + "mass:" + m_mass + " Pos: " + Position.ToString());
  1232. }
  1233. else
  1234. {
  1235. fz = (-1 * (((-1 * _parent_scene.gravityz) * (-1 * m_buoyancy)) * m_mass) * 0.035f);
  1236. }
  1237. }
  1238. if (m_usePID)
  1239. {
  1240. PID_D = 61f;
  1241. PID_G = 65f;
  1242. //if (!d.BodyIsEnabled(Body))
  1243. //d.BodySetForce(Body, 0f, 0f, 0f);
  1244. // If we're using the PID controller, then we have no gravity
  1245. fz = ((-1 * _parent_scene.gravityz) * m_mass) * 1.025f;
  1246. // no lock; for now it's only called from within Simulate()
  1247. // If the PID Controller isn't active then we set our force
  1248. // calculating base velocity to the current position
  1249. if ((m_PIDTau < 1) && (m_PIDTau != 0))
  1250. {
  1251. //PID_G = PID_G / m_PIDTau;
  1252. m_PIDTau = 1;
  1253. }
  1254. if ((PID_G - m_PIDTau) <= 0)
  1255. {
  1256. PID_G = m_PIDTau + 1;
  1257. }
  1258. // TODO: NEED btVector3 for Linear Velocity
  1259. // NEED btVector3 for Position
  1260. Vector3 pos = _position; //TODO: Insert values gotten from bullet
  1261. Vector3 vel = _velocity;
  1262. _target_velocity =
  1263. new Vector3(
  1264. (m_PIDTarget.X - pos.X) * ((PID_G - m_PIDTau) * timestep),
  1265. (m_PIDTarget.Y - pos.Y) * ((PID_G - m_PIDTau) * timestep),
  1266. (m_PIDTarget.Z - pos.Z) * ((PID_G - m_PIDTau) * timestep)
  1267. );
  1268. if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
  1269. {
  1270. /* TODO: Do Bullet equiv
  1271. *
  1272. d.BodySetPosition(Body, m_PIDTarget.X, m_PIDTarget.Y, m_PIDTarget.Z);
  1273. d.BodySetLinearVel(Body, 0, 0, 0);
  1274. d.BodyAddForce(Body, 0, 0, fz);
  1275. return;
  1276. */
  1277. }
  1278. else
  1279. {
  1280. _zeroFlag = false;
  1281. fx = ((_target_velocity.X) - vel.X) * (PID_D);
  1282. fy = ((_target_velocity.Y) - vel.Y) * (PID_D);
  1283. fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
  1284. }
  1285. }
  1286. if (m_useHoverPID && !m_usePID)
  1287. {
  1288. // If we're using the PID controller, then we have no gravity
  1289. fz = (-1 * _parent_scene.gravityz) * m_mass;
  1290. // no lock; for now it's only called from within Simulate()
  1291. // If the PID Controller isn't active then we set our force
  1292. // calculating base velocity to the current position
  1293. if ((m_PIDTau < 1))
  1294. {
  1295. PID_G = PID_G / m_PIDTau;
  1296. }
  1297. if ((PID_G - m_PIDTau) <= 0)
  1298. {
  1299. PID_G = m_PIDTau + 1;
  1300. }
  1301. Vector3 pos = Vector3.Zero; //TODO: Insert values gotten from bullet
  1302. Vector3 vel = Vector3.Zero;
  1303. // determine what our target height really is based on HoverType
  1304. switch (m_PIDHoverType)
  1305. {
  1306. case PIDHoverType.Absolute:
  1307. m_targetHoverHeight = m_PIDHoverHeight;
  1308. break;
  1309. case PIDHoverType.Ground:
  1310. m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
  1311. m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
  1312. break;
  1313. case PIDHoverType.GroundAndWater:
  1314. m_groundHeight = _parent_scene.GetTerrainHeightAtXY(pos.X, pos.Y);
  1315. m_waterHeight = _parent_scene.GetWaterLevel();
  1316. if (m_groundHeight > m_waterHeight)
  1317. {
  1318. m_targetHoverHeight = m_groundHeight + m_PIDHoverHeight;
  1319. }
  1320. else
  1321. {
  1322. m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
  1323. }
  1324. break;
  1325. case PIDHoverType.Water:
  1326. m_waterHeight = _parent_scene.GetWaterLevel();
  1327. m_targetHoverHeight = m_waterHeight + m_PIDHoverHeight;
  1328. break;
  1329. }
  1330. _target_velocity =
  1331. new Vector3(0.0f, 0.0f,
  1332. (m_targetHoverHeight - pos.Z) * ((PID_G - m_PIDHoverTau) * timestep)
  1333. );
  1334. // if velocity is zero, use position control; otherwise, velocity control
  1335. if (_target_velocity.ApproxEquals(Vector3.Zero, 0.1f))
  1336. {
  1337. /* TODO: Do Bullet Equiv
  1338. d.BodySetPosition(Body, pos.X, pos.Y, m_targetHoverHeight);
  1339. d.BodySetLinearVel(Body, vel.X, vel.Y, 0);
  1340. d.BodyAddForce(Body, 0, 0, fz);
  1341. */
  1342. if (Body != null && Body.Handle != IntPtr.Zero)
  1343. {
  1344. Body.setLinearVelocity(_parent_scene.VectorZero);
  1345. Body.clearForces();
  1346. }
  1347. return;
  1348. }
  1349. else
  1350. {
  1351. _zeroFlag = false;
  1352. // We're flying and colliding with something
  1353. fz = fz + ((_target_velocity.Z - vel.Z) * (PID_D) * m_mass);
  1354. }
  1355. }
  1356. fx *= m_mass;
  1357. fy *= m_mass;
  1358. //fz *= m_mass;
  1359. fx += m_force.X;
  1360. fy += m_force.Y;
  1361. fz += m_force.Z;
  1362. //m_log.Info("[OBJPID]: X:" + fx.ToString() + " Y:" + fy.ToString() + " Z:" + fz.ToString());
  1363. if (fx != 0 || fy != 0 || fz != 0)
  1364. {
  1365. /*
  1366. * TODO: Do Bullet Equiv
  1367. if (!d.BodyIsEnabled(Body))
  1368. {
  1369. d.BodySetLinearVel(Body, 0f, 0f, 0f);
  1370. d.BodySetForce(Body, 0, 0, 0);
  1371. enableBodySoft();
  1372. }
  1373. */
  1374. if (!Body.isActive())
  1375. {
  1376. Body.clearForces();
  1377. enableBodySoft();
  1378. }
  1379. // 35x10 = 350n times the mass per second applied maximum.
  1380. float nmax = 35f * m_mass;
  1381. float nmin = -35f * m_mass;
  1382. if (fx > nmax)
  1383. fx = nmax;
  1384. if (fx < nmin)
  1385. fx = nmin;
  1386. if (fy > nmax)
  1387. fy = nmax;
  1388. if (fy < nmin)
  1389. fy = nmin;
  1390. // TODO: Do Bullet Equiv
  1391. // d.BodyAddForce(Body, fx, fy, fz);
  1392. if (Body != null && Body.Handle != IntPtr.Zero)
  1393. {
  1394. Body.activate(true);
  1395. if (tempAddForce != null && tempAddForce.Handle != IntPtr.Zero)
  1396. tempAddForce.Dispose();
  1397. tempAddForce = new btVector3(fx * 0.01f, fy * 0.01f, fz * 0.01f);
  1398. Body.applyCentralImpulse(tempAddForce);
  1399. }
  1400. }
  1401. else
  1402. {
  1403. // if no forces on the prim, make sure everything is zero
  1404. Body.clearForces();
  1405. enableBodySoft();
  1406. }
  1407. }
  1408. else
  1409. {
  1410. if (m_zeroPosition == null)
  1411. m_zeroPosition = Vector3.Zero;
  1412. m_zeroPosition = _position;
  1413. return;
  1414. }
  1415. }
  1416. #region Mass Calculation
  1417. private float CalculateMass()
  1418. {
  1419. float volume = 0;
  1420. // No material is passed to the physics engines yet.. soo..
  1421. // we're using the m_density constant in the class definition
  1422. float returnMass = 0;
  1423. switch (_pbs.ProfileShape)
  1424. {
  1425. case ProfileShape.Square:
  1426. // Profile Volume
  1427. volume = _size.X * _size.Y * _size.Z;
  1428. // If the user has 'hollowed out'
  1429. // ProfileHollow is one of those 0 to 50000 values :P
  1430. // we like percentages better.. so turning into a percentage
  1431. if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
  1432. {
  1433. float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
  1434. // calculate the hollow volume by it's shape compared to the prim shape
  1435. float hollowVolume = 0;
  1436. switch (_pbs.HollowShape)
  1437. {
  1438. case HollowShape.Square:
  1439. case HollowShape.Same:
  1440. // Cube Hollow volume calculation
  1441. float hollowsizex = _size.X * hollowAmount;
  1442. float hollowsizey = _size.Y * hollowAmount;
  1443. float hollowsizez = _size.Z * hollowAmount;
  1444. hollowVolume = hollowsizex * hollowsizey * hollowsizez;
  1445. break;
  1446. case HollowShape.Circle:
  1447. // Hollow shape is a perfect cyllinder in respect to the cube's scale
  1448. // Cyllinder hollow volume calculation
  1449. float hRadius = _size.X / 2;
  1450. float hLength = _size.Z;
  1451. // pi * r2 * h
  1452. hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
  1453. break;
  1454. case HollowShape.Triangle:
  1455. // Equilateral Triangular Prism volume hollow calculation
  1456. // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
  1457. float aLength = _size.Y;
  1458. // 1/2 abh
  1459. hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
  1460. break;
  1461. default:
  1462. hollowVolume = 0;
  1463. break;
  1464. }
  1465. volume = volume - hollowVolume;
  1466. }
  1467. break;
  1468. case ProfileShape.Circle:
  1469. if (_pbs.PathCurve == (byte)Extrusion.Straight)
  1470. {
  1471. // Cylinder
  1472. float volume1 = (float)(Math.PI * Math.Pow(_size.X / 2, 2) * _size.Z);
  1473. float volume2 = (float)(Math.PI * Math.Pow(_size.Y / 2, 2) * _size.Z);
  1474. // Approximating the cylinder's irregularity.
  1475. if (volume1 > volume2)
  1476. {
  1477. volume = (float)volume1 - (volume1 - volume2);
  1478. }
  1479. else if (volume2 > volume1)
  1480. {
  1481. volume = (float)volume2 - (volume2 - volume1);
  1482. }
  1483. else
  1484. {
  1485. // Regular cylinder
  1486. volume = volume1;
  1487. }
  1488. }
  1489. else
  1490. {
  1491. // We don't know what the shape is yet, so use default
  1492. volume = _size.X * _size.Y * _size.Z;
  1493. }
  1494. // If the user has 'hollowed out'
  1495. // ProfileHollow is one of those 0 to 50000 values :P
  1496. // we like percentages better.. so turning into a percentage
  1497. if (((float)_pbs.ProfileHollow / 50000f) > 0.0)
  1498. {
  1499. float hollowAmount = (float)_pbs.ProfileHollow / 50000f;
  1500. // calculate the hollow volume by it's shape compared to the prim shape
  1501. float hollowVolume = 0;
  1502. switch (_pbs.HollowShape)
  1503. {
  1504. case HollowShape.Same:
  1505. case HollowShape.Circle:
  1506. // Hollow shape is a perfect cyllinder in respect to the cube's scale
  1507. // Cyllinder hollow volume calculation
  1508. float hRadius = _size.X / 2;
  1509. float hLength = _size.Z;
  1510. // pi * r2 * h
  1511. hollowVolume = ((float)(Math.PI * Math.Pow(hRadius, 2) * hLength) * hollowAmount);
  1512. break;
  1513. case HollowShape.Square:
  1514. // Cube Hollow volume calculation
  1515. float hollowsizex = _size.X * hollowAmount;
  1516. float hollowsizey = _size.Y * hollowAmount;
  1517. float hollowsizez = _size.Z * hollowAmount;
  1518. hollowVolume = hollowsizex * hollowsizey * hollowsizez;
  1519. break;
  1520. case HollowShape.Triangle:
  1521. // Equilateral Triangular Prism volume hollow calculation
  1522. // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
  1523. float aLength = _size.Y;
  1524. // 1/2 abh
  1525. hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
  1526. break;
  1527. default:
  1528. hollowVolume = 0;
  1529. break;
  1530. }
  1531. volume = volume - hollowVolume;
  1532. }
  1533. break;
  1534. case ProfileShape.HalfCircle:
  1535. if (_pbs.PathCurve == (byte)Extrusion.Curve1)
  1536. {
  1537. if (_size.X == _size.Y && _size.Z == _size.X)
  1538. {
  1539. // regular sphere
  1540. // v = 4/3 * pi * r^3
  1541. float sradius3 = (float)Math.Pow((_size.X / 2), 3);
  1542. volume = (float)((4 / 3f) * Math.PI * sradius3);
  1543. }
  1544. else
  1545. {
  1546. // we treat this as a box currently
  1547. volume = _size.X * _size.Y * _size.Z;
  1548. }
  1549. }
  1550. else
  1551. {
  1552. // We don't know what the shape is yet, so use default
  1553. volume = _size.X * _size.Y * _size.Z;
  1554. }
  1555. break;
  1556. case ProfileShape.EquilateralTriangle:
  1557. /*
  1558. v = (abs((xB*yA-xA*yB)+(xC*yB-xB*yC)+(xA*yC-xC*yA))/2) * h
  1559. // seed mesh
  1560. Vertex MM = new Vertex(-0.25f, -0.45f, 0.0f);
  1561. Vertex PM = new Vertex(+0.5f, 0f, 0.0f);
  1562. Vertex PP = new Vertex(-0.25f, +0.45f, 0.0f);
  1563. */
  1564. float xA = -0.25f * _size.X;
  1565. float yA = -0.45f * _size.Y;
  1566. float xB = 0.5f * _size.X;
  1567. float yB = 0;
  1568. float xC = -0.25f * _size.X;
  1569. float yC = 0.45f * _size.Y;
  1570. volume = (float)((Math.Abs((xB * yA - xA * yB) + (xC * yB - xB * yC) + (xA * yC - xC * yA)) / 2) * _size.Z);
  1571. // If the user has 'hollowed out'
  1572. // ProfileHollow is one of those 0 to 50000 values :P
  1573. // we like percentages better.. so turning into a percentage
  1574. float fhollowFactor = ((float)_pbs.ProfileHollow / 1.9f);
  1575. if (((float)fhollowFactor / 50000f) > 0.0)
  1576. {
  1577. float hollowAmount = (float)fhollowFactor / 50000f;
  1578. // calculate the hollow volume by it's shape compared to the prim shape
  1579. float hollowVolume = 0;
  1580. switch (_pbs.HollowShape)
  1581. {
  1582. case HollowShape.Same:
  1583. case HollowShape.Triangle:
  1584. // Equilateral Triangular Prism volume hollow calculation
  1585. // Triangle is an Equilateral Triangular Prism with aLength = to _size.Y
  1586. float aLength = _size.Y;
  1587. // 1/2 abh
  1588. hollowVolume = (float)((0.5 * aLength * _size.X * _size.Z) * hollowAmount);
  1589. break;
  1590. case HollowShape.Square:
  1591. // Cube Hollow volume calculation
  1592. float hollowsizex = _size.X * hollowAmount;
  1593. float hollowsizey = _size.Y * hollowAmount;
  1594. float hollowsizez = _size.Z * hollowAmount;
  1595. hollowVolume = hollowsizex * hollowsizey * hollowsizez;
  1596. break;
  1597. case HollowShape.Circle:
  1598. // Hollow shape is a perfect cyllinder in respect to the cube's scale
  1599. // Cyllinder hollow volume calculation
  1600. float hRadius = _size.X / 2;
  1601. float hLength = _size.Z;
  1602. // pi * r2 * h
  1603. hollowVolume = ((float)((Math.PI * Math.Pow(hRadius, 2) * hLength) / 2) * hollowAmount);
  1604. break;
  1605. default:
  1606. hollowVolume = 0;
  1607. break;
  1608. }
  1609. volume = volume - hollowVolume;
  1610. }
  1611. break;
  1612. default:
  1613. // we don't have all of the volume formulas yet so
  1614. // use the common volume formula for all
  1615. volume = _size.X * _size.Y * _size.Z;
  1616. break;
  1617. }
  1618. // Calculate Path cut effect on volume
  1619. // Not exact, in the triangle hollow example
  1620. // They should never be zero or less then zero..
  1621. // we'll ignore it if it's less then zero
  1622. // ProfileEnd and ProfileBegin are values
  1623. // from 0 to 50000
  1624. // Turning them back into percentages so that I can cut that percentage off the volume
  1625. float PathCutEndAmount = _pbs.ProfileEnd;
  1626. float PathCutStartAmount = _pbs.ProfileBegin;
  1627. if (((PathCutStartAmount + PathCutEndAmount) / 50000f) > 0.0f)
  1628. {
  1629. float pathCutAmount = ((PathCutStartAmount + PathCutEndAmount) / 50000f);
  1630. // Check the return amount for sanity
  1631. if (pathCutAmount >= 0.99f)
  1632. pathCutAmount = 0.99f;
  1633. volume = volume - (volume * pathCutAmount);
  1634. }
  1635. UInt16 taperX = _pbs.PathScaleX;
  1636. UInt16 taperY = _pbs.PathScaleY;
  1637. float taperFactorX = 0;
  1638. float taperFactorY = 0;
  1639. // Mass = density * volume
  1640. if (taperX != 100)
  1641. {
  1642. if (taperX > 100)
  1643. {
  1644. taperFactorX = 1.0f - ((float)taperX / 200);
  1645. //m_log.Warn("taperTopFactorX: " + extr.taperTopFactorX.ToString());
  1646. }
  1647. else
  1648. {
  1649. taperFactorX = 1.0f - ((100 - (float)taperX) / 100);
  1650. //m_log.Warn("taperBotFactorX: " + extr.taperBotFactorX.ToString());
  1651. }
  1652. volume = (float)volume * ((taperFactorX / 3f) + 0.001f);
  1653. }
  1654. if (taperY != 100)
  1655. {
  1656. if (taperY > 100)
  1657. {
  1658. taperFactorY = 1.0f - ((float)taperY / 200);
  1659. //m_log.Warn("taperTopFactorY: " + extr.taperTopFactorY.ToString());
  1660. }
  1661. else
  1662. {
  1663. taperFactorY = 1.0f - ((100 - (float)taperY) / 100);
  1664. //m_log.Warn("taperBotFactorY: " + extr.taperBotFactorY.ToString());
  1665. }
  1666. volume = (float)volume * ((taperFactorY / 3f) + 0.001f);
  1667. }
  1668. returnMass = m_density * volume;
  1669. if (returnMass <= 0) returnMass = 0.0001f;//ckrinke: Mass must be greater then zero.
  1670. // Recursively calculate mass
  1671. bool HasChildPrim = false;
  1672. lock (childrenPrim)
  1673. {
  1674. if (childrenPrim.Count > 0)
  1675. {
  1676. HasChildPrim = true;
  1677. }
  1678. }
  1679. if (HasChildPrim)
  1680. {
  1681. BulletDotNETPrim[] childPrimArr = new BulletDotNETPrim[0];
  1682. lock (childrenPrim)
  1683. childPrimArr = childrenPrim.ToArray();
  1684. for (int i = 0; i < childPrimArr.Length; i++)
  1685. {
  1686. if (childPrimArr[i] != null && !childPrimArr[i].m_taintremove)
  1687. returnMass += childPrimArr[i].CalculateMass();
  1688. // failsafe, this shouldn't happen but with OpenSim, you never know :)
  1689. if (i > 256)
  1690. break;
  1691. }
  1692. }
  1693. return returnMass;
  1694. }
  1695. #endregion
  1696. public void CreateGeom(IntPtr m_targetSpace, IMesh p_mesh)
  1697. {
  1698. // m_log.Debug("[PHYSICS]: _________CreateGeom");
  1699. if (p_mesh != null)
  1700. {
  1701. //_mesh = _parent_scene.mesher.CreateMesh(m_primName, _pbs, _size, _parent_scene.meshSculptLOD, IsPhysical);
  1702. _mesh = p_mesh;
  1703. setMesh(_parent_scene, _mesh);
  1704. }
  1705. else
  1706. {
  1707. if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte)Extrusion.Curve1)
  1708. {
  1709. if (_size.X == _size.Y && _size.Y == _size.Z && _size.X == _size.Z)
  1710. {
  1711. if (((_size.X / 2f) > 0f))
  1712. {
  1713. //SetGeom to a Regular Sphere
  1714. if (tempSize1 == null)
  1715. tempSize1 = new btVector3(0, 0, 0);
  1716. tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);
  1717. SetCollisionShape(new btSphereShape(_size.X * 0.5f));
  1718. }
  1719. else
  1720. {
  1721. // uses halfextents
  1722. if (tempSize1 == null)
  1723. tempSize1 = new btVector3(0, 0, 0);
  1724. tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);
  1725. SetCollisionShape(new btBoxShape(tempSize1));
  1726. }
  1727. }
  1728. else
  1729. {
  1730. // uses halfextents
  1731. if (tempSize1 == null)
  1732. tempSize1 = new btVector3(0, 0, 0);
  1733. tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);
  1734. SetCollisionShape(new btBoxShape(tempSize1));
  1735. }
  1736. }
  1737. else
  1738. {
  1739. if (tempSize1 == null)
  1740. tempSize1 = new btVector3(0, 0, 0);
  1741. // uses halfextents
  1742. tempSize1.setValue(_size.X * 0.5f, _size.Y * 0.5f, _size.Z * 0.5f);
  1743. SetCollisionShape(new btBoxShape(tempSize1));
  1744. }
  1745. }
  1746. }
  1747. private void setMesh(BulletDotNETScene _parent_scene, IMesh mesh)
  1748. {
  1749. // TODO: Set Collision Body Mesh
  1750. // This sleeper is there to moderate how long it takes between
  1751. // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
  1752. // m_log.Debug("_________SetMesh");
  1753. Thread.Sleep(10);
  1754. //Kill Body so that mesh can re-make the geom
  1755. if (IsPhysical && Body != null && Body.Handle != IntPtr.Zero)
  1756. {
  1757. if (childPrim)
  1758. {
  1759. if (_parent != null)
  1760. {
  1761. BulletDotNETPrim parent = (BulletDotNETPrim)_parent;
  1762. parent.ChildDelink(this);
  1763. }
  1764. }
  1765. else
  1766. {
  1767. //disableBody();
  1768. }
  1769. }
  1770. //IMesh oldMesh = primMesh;
  1771. //primMesh = mesh;
  1772. //float[] vertexList = primMesh.getVertexListAsFloatLocked(); // Note, that vertextList is pinned in memory
  1773. //int[] indexList = primMesh.getIndexListAsIntLocked(); // Also pinned, needs release after usage
  1774. ////Array.Reverse(indexList);
  1775. //primMesh.releaseSourceMeshData(); // free up the original mesh data to save memory
  1776. IMesh oldMesh = primMesh;
  1777. primMesh = mesh;
  1778. float[] vertexList = mesh.getVertexListAsFloatLocked(); // Note, that vertextList is pinned in memory
  1779. int[] indexList = mesh.getIndexListAsIntLocked(); // Also pinned, needs release after usage
  1780. //Array.Reverse(indexList);
  1781. mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
  1782. int VertexCount = vertexList.GetLength(0) / 3;
  1783. int IndexCount = indexList.GetLength(0);
  1784. if (btshapeArray != null && btshapeArray.Handle != IntPtr.Zero)
  1785. btshapeArray.Dispose();
  1786. //Array.Reverse(indexList);
  1787. btshapeArray = new btTriangleIndexVertexArray(IndexCount / 3, indexList, (3 * sizeof(int)),
  1788. VertexCount, vertexList, 3 * sizeof(float));
  1789. SetCollisionShape(new btGImpactMeshShape(btshapeArray));
  1790. //((btGImpactMeshShape) prim_geom).updateBound();
  1791. ((btGImpactMeshShape)prim_geom).setLocalScaling(new btVector3(1, 1, 1));
  1792. ((btGImpactMeshShape)prim_geom).updateBound();
  1793. _parent_scene.SetUsingGImpact();
  1794. //if (oldMesh != null)
  1795. //{
  1796. // oldMesh.releasePinned();
  1797. // oldMesh = null;
  1798. //}
  1799. }
  1800. private void SetCollisionShape(btCollisionShape shape)
  1801. {
  1802. /*
  1803. if (shape == null)
  1804. m_log.Debug("[PHYSICS]:SetShape!Null");
  1805. else
  1806. m_log.Debug("[PHYSICS]:SetShape!");
  1807. if (Body != null)
  1808. {
  1809. DisposeOfBody();
  1810. }
  1811. if (prim_geom != null)
  1812. {
  1813. prim_geom.Dispose();
  1814. prim_geom = null;
  1815. }
  1816. */
  1817. prim_geom = shape;
  1818. //Body.set
  1819. }
  1820. public void SetBody(float mass)
  1821. {
  1822. if (!IsPhysical || childrenPrim.Count == 0)
  1823. {
  1824. if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
  1825. tempMotionState1.Dispose();
  1826. if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
  1827. tempTransform2.Dispose();
  1828. if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
  1829. tempOrientation2.Dispose();
  1830. if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
  1831. tempPosition2.Dispose();
  1832. tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
  1833. tempPosition2 = new btVector3(_position.X, _position.Y, _position.Z);
  1834. tempTransform2 = new btTransform(tempOrientation2, tempPosition2);
  1835. tempMotionState1 = new btDefaultMotionState(tempTransform2, _parent_scene.TransZero);
  1836. if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
  1837. tempInertia1.Dispose();
  1838. tempInertia1 = new btVector3(0, 0, 0);
  1839. prim_geom.calculateLocalInertia(mass, tempInertia1);
  1840. if (mass != 0)
  1841. _parent_scene.addActivePrim(this);
  1842. else
  1843. _parent_scene.remActivePrim(this);
  1844. // Body = new btRigidBody(mass, tempMotionState1, prim_geom);
  1845. //else
  1846. // Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
  1847. if (Body == null)
  1848. {
  1849. Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
  1850. // add localID so we can later map bullet object back to OpenSim object
  1851. Body.setUserPointer(new IntPtr((int)m_localID));
  1852. }
  1853. if (prim_geom is btGImpactMeshShape)
  1854. {
  1855. ((btGImpactMeshShape)prim_geom).setLocalScaling(new btVector3(1, 1, 1));
  1856. ((btGImpactMeshShape)prim_geom).updateBound();
  1857. }
  1858. //Body.setCollisionFlags(Body.getCollisionFlags() | (int)ContactFlags.CF_CUSTOM_MATERIAL_CALLBACK);
  1859. //Body.setUserPointer((IntPtr) (int)m_localID);
  1860. _parent_scene.AddPrimToScene(this);
  1861. }
  1862. else
  1863. {
  1864. // bool hasTrimesh = false;
  1865. lock (childrenPrim)
  1866. {
  1867. foreach (BulletDotNETPrim chld in childrenPrim)
  1868. {
  1869. if (chld == null)
  1870. continue;
  1871. // if (chld.NeedsMeshing())
  1872. // hasTrimesh = true;
  1873. }
  1874. }
  1875. //if (hasTrimesh)
  1876. //{
  1877. ProcessGeomCreationAsTriMesh(Vector3.Zero, Quaternion.Identity);
  1878. // createmesh returns null when it doesn't mesh.
  1879. /*
  1880. if (_mesh is Mesh)
  1881. {
  1882. }
  1883. else
  1884. {
  1885. m_log.Warn("[PHYSICS]: Can't link a OpenSim.Region.Physics.Meshing.Mesh object");
  1886. return;
  1887. }
  1888. */
  1889. foreach (BulletDotNETPrim chld in childrenPrim)
  1890. {
  1891. if (chld == null)
  1892. continue;
  1893. Vector3 offset = chld.Position - Position;
  1894. Vector3 pos = new Vector3(offset.X, offset.Y, offset.Z);
  1895. pos *= Quaternion.Inverse(Orientation);
  1896. //pos *= Orientation;
  1897. offset = pos;
  1898. chld.ProcessGeomCreationAsTriMesh(offset, chld.Orientation);
  1899. _mesh.Append(chld._mesh);
  1900. }
  1901. setMesh(_parent_scene, _mesh);
  1902. //}
  1903. if (tempMotionState1 != null && tempMotionState1.Handle != IntPtr.Zero)
  1904. tempMotionState1.Dispose();
  1905. if (tempTransform2 != null && tempTransform2.Handle != IntPtr.Zero)
  1906. tempTransform2.Dispose();
  1907. if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
  1908. tempOrientation2.Dispose();
  1909. if (tempPosition2 != null && tempPosition2.Handle != IntPtr.Zero)
  1910. tempPosition2.Dispose();
  1911. tempOrientation2 = new btQuaternion(_orientation.X, _orientation.Y, _orientation.Z, _orientation.W);
  1912. tempPosition2 = new btVector3(_position.X, _position.Y, _position.Z);
  1913. tempTransform2 = new btTransform(tempOrientation2, tempPosition2);
  1914. tempMotionState1 = new btDefaultMotionState(tempTransform2, _parent_scene.TransZero);
  1915. if (tempInertia1 != null && tempInertia1.Handle != IntPtr.Zero)
  1916. tempInertia1.Dispose();
  1917. tempInertia1 = new btVector3(0, 0, 0);
  1918. prim_geom.calculateLocalInertia(mass, tempInertia1);
  1919. if (mass != 0)
  1920. _parent_scene.addActivePrim(this);
  1921. else
  1922. _parent_scene.remActivePrim(this);
  1923. // Body = new btRigidBody(mass, tempMotionState1, prim_geom);
  1924. //else
  1925. // Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
  1926. if (Body == null)
  1927. {
  1928. Body = new btRigidBody(mass, tempMotionState1, prim_geom, tempInertia1);
  1929. // each body has the localID stored into it so we can identify collision objects
  1930. Body.setUserPointer(new IntPtr((int)m_localID));
  1931. }
  1932. if (prim_geom is btGImpactMeshShape)
  1933. {
  1934. ((btGImpactMeshShape)prim_geom).setLocalScaling(new btVector3(1, 1, 1));
  1935. ((btGImpactMeshShape)prim_geom).updateBound();
  1936. }
  1937. _parent_scene.AddPrimToScene(this);
  1938. }
  1939. if (IsPhysical)
  1940. changeAngularLock(0);
  1941. }
  1942. private void DisposeOfBody()
  1943. {
  1944. if (Body != null)
  1945. {
  1946. if (Body.Handle != IntPtr.Zero)
  1947. {
  1948. DisableAxisMotor();
  1949. _parent_scene.removeFromWorld(this, Body);
  1950. Body.Dispose();
  1951. }
  1952. Body = null;
  1953. // TODO: dispose parts that make up body
  1954. }
  1955. }
  1956. private void ChildDelink(BulletDotNETPrim pPrim)
  1957. {
  1958. // Okay, we have a delinked child.. need to rebuild the body.
  1959. lock (childrenPrim)
  1960. {
  1961. foreach (BulletDotNETPrim prm in childrenPrim)
  1962. {
  1963. prm.childPrim = true;
  1964. prm.disableBody();
  1965. }
  1966. }
  1967. disableBody();
  1968. lock (childrenPrim)
  1969. {
  1970. childrenPrim.Remove(pPrim);
  1971. }
  1972. if (Body != null && Body.Handle != IntPtr.Zero)
  1973. {
  1974. _parent_scene.remActivePrim(this);
  1975. }
  1976. lock (childrenPrim)
  1977. {
  1978. foreach (BulletDotNETPrim prm in childrenPrim)
  1979. {
  1980. ParentPrim(prm);
  1981. }
  1982. }
  1983. }
  1984. internal void ParentPrim(BulletDotNETPrim prm)
  1985. {
  1986. if (prm == null)
  1987. return;
  1988. lock (childrenPrim)
  1989. {
  1990. if (!childrenPrim.Contains(prm))
  1991. {
  1992. childrenPrim.Add(prm);
  1993. }
  1994. }
  1995. }
  1996. public void disableBody()
  1997. {
  1998. //this kills the body so things like 'mesh' can re-create it.
  1999. /*
  2000. lock (this)
  2001. {
  2002. if (!childPrim)
  2003. {
  2004. if (Body != null && Body.Handle != IntPtr.Zero)
  2005. {
  2006. _parent_scene.remActivePrim(this);
  2007. m_collisionCategories &= ~CollisionCategories.Body;
  2008. m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
  2009. if (prim_geom != null && prim_geom.Handle != IntPtr.Zero)
  2010. {
  2011. // TODO: Set Category bits and Flags
  2012. }
  2013. // TODO: destroy body
  2014. DisposeOfBody();
  2015. lock (childrenPrim)
  2016. {
  2017. if (childrenPrim.Count > 0)
  2018. {
  2019. foreach (BulletDotNETPrim prm in childrenPrim)
  2020. {
  2021. _parent_scene.remActivePrim(prm);
  2022. prm.DisposeOfBody();
  2023. prm.SetCollisionShape(null);
  2024. }
  2025. }
  2026. }
  2027. DisposeOfBody();
  2028. }
  2029. }
  2030. else
  2031. {
  2032. _parent_scene.remActivePrim(this);
  2033. m_collisionCategories &= ~CollisionCategories.Body;
  2034. m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
  2035. if (prim_geom != null && prim_geom.Handle != IntPtr.Zero)
  2036. {
  2037. // TODO: Set Category bits and Flags
  2038. }
  2039. DisposeOfBody();
  2040. }
  2041. }
  2042. */
  2043. DisableAxisMotor();
  2044. m_disabled = true;
  2045. m_collisionscore = 0;
  2046. }
  2047. public void disableBodySoft()
  2048. {
  2049. m_disabled = true;
  2050. if (m_isphysical && Body.Handle != IntPtr.Zero)
  2051. {
  2052. Body.clearForces();
  2053. Body.forceActivationState(0);
  2054. }
  2055. }
  2056. public void enableBodySoft()
  2057. {
  2058. if (!childPrim)
  2059. {
  2060. if (m_isphysical && Body.Handle != IntPtr.Zero)
  2061. {
  2062. Body.clearForces();
  2063. Body.forceActivationState(4);
  2064. forceenable = true;
  2065. }
  2066. m_disabled = false;
  2067. }
  2068. }
  2069. public void enableBody()
  2070. {
  2071. if (!childPrim)
  2072. {
  2073. //SetCollisionShape(prim_geom);
  2074. if (IsPhysical)
  2075. SetBody(Mass);
  2076. else
  2077. SetBody(0);
  2078. // TODO: Set Collision Category Bits and Flags
  2079. // TODO: Set Auto Disable data
  2080. m_interpenetrationcount = 0;
  2081. m_collisionscore = 0;
  2082. m_disabled = false;
  2083. // The body doesn't already have a finite rotation mode set here
  2084. if ((!m_angularlock.ApproxEquals(Vector3.Zero, 0f)) && _parent == null)
  2085. {
  2086. // TODO: Create Angular Motor on Axis Lock!
  2087. }
  2088. _parent_scene.addActivePrim(this);
  2089. }
  2090. }
  2091. public void UpdatePositionAndVelocity()
  2092. {
  2093. if (!m_isSelected)
  2094. {
  2095. if (_parent == null)
  2096. {
  2097. Vector3 pv = Vector3.Zero;
  2098. bool lastZeroFlag = _zeroFlag;
  2099. if (tempPosition3 != null && tempPosition3.Handle != IntPtr.Zero)
  2100. tempPosition3.Dispose();
  2101. if (tempTransform3 != null && tempTransform3.Handle != IntPtr.Zero)
  2102. tempTransform3.Dispose();
  2103. if (tempOrientation2 != null && tempOrientation2.Handle != IntPtr.Zero)
  2104. tempOrientation2.Dispose();
  2105. if (tempAngularVelocity1 != null && tempAngularVelocity1.Handle != IntPtr.Zero)
  2106. tempAngularVelocity1.Dispose();
  2107. if (tempLinearVelocity1 != null && tempLinearVelocity1.Handle != IntPtr.Zero)
  2108. tempLinearVelocity1.Dispose();
  2109. tempTransform3 = Body.getInterpolationWorldTransform();
  2110. tempPosition3 = tempTransform3.getOrigin(); // vec
  2111. tempOrientation2 = tempTransform3.getRotation(); // ori
  2112. tempAngularVelocity1 = Body.getInterpolationAngularVelocity(); //rotvel
  2113. tempLinearVelocity1 = Body.getInterpolationLinearVelocity(); // vel
  2114. _torque = new Vector3(tempAngularVelocity1.getX(), tempAngularVelocity1.getX(),
  2115. tempAngularVelocity1.getZ());
  2116. Vector3 l_position = Vector3.Zero;
  2117. Quaternion l_orientation = Quaternion.Identity;
  2118. m_lastposition = _position;
  2119. m_lastorientation = _orientation;
  2120. l_position.X = tempPosition3.getX();
  2121. l_position.Y = tempPosition3.getY();
  2122. l_position.Z = tempPosition3.getZ();
  2123. l_orientation.X = tempOrientation2.getX();
  2124. l_orientation.Y = tempOrientation2.getY();
  2125. l_orientation.Z = tempOrientation2.getZ();
  2126. l_orientation.W = tempOrientation2.getW();
  2127. if (l_position.X > ((int)Constants.RegionSize - 0.05f) || l_position.X < 0f || l_position.Y > ((int)Constants.RegionSize - 0.05f) || l_position.Y < 0f)
  2128. {
  2129. //base.RaiseOutOfBounds(l_position);
  2130. if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
  2131. {
  2132. _position = l_position;
  2133. //_parent_scene.remActivePrim(this);
  2134. if (_parent == null)
  2135. base.RequestPhysicsterseUpdate();
  2136. return;
  2137. }
  2138. else
  2139. {
  2140. if (_parent == null)
  2141. base.RaiseOutOfBounds(l_position);
  2142. return;
  2143. }
  2144. }
  2145. if (l_position.Z < -200000f)
  2146. {
  2147. // This is so prim that get lost underground don't fall forever and suck up
  2148. //
  2149. // Sim resources and memory.
  2150. // Disables the prim's movement physics....
  2151. // It's a hack and will generate a console message if it fails.
  2152. //IsPhysical = false;
  2153. //if (_parent == null)
  2154. //base.RaiseOutOfBounds(_position);
  2155. _acceleration.X = 0;
  2156. _acceleration.Y = 0;
  2157. _acceleration.Z = 0;
  2158. _velocity.X = 0;
  2159. _velocity.Y = 0;
  2160. _velocity.Z = 0;
  2161. m_rotationalVelocity.X = 0;
  2162. m_rotationalVelocity.Y = 0;
  2163. m_rotationalVelocity.Z = 0;
  2164. if (_parent == null)
  2165. base.RequestPhysicsterseUpdate();
  2166. m_throttleUpdates = false;
  2167. // throttleCounter = 0;
  2168. _zeroFlag = true;
  2169. //outofBounds = true;
  2170. }
  2171. if ((Math.Abs(m_lastposition.X - l_position.X) < 0.02)
  2172. && (Math.Abs(m_lastposition.Y - l_position.Y) < 0.02)
  2173. && (Math.Abs(m_lastposition.Z - l_position.Z) < 0.02)
  2174. && (1.0 - Math.Abs(Quaternion.Dot(m_lastorientation, l_orientation)) < 0.01))
  2175. {
  2176. _zeroFlag = true;
  2177. m_throttleUpdates = false;
  2178. }
  2179. else
  2180. {
  2181. //m_log.Debug(Math.Abs(m_lastposition.X - l_position.X).ToString());
  2182. _zeroFlag = false;
  2183. }
  2184. if (_zeroFlag)
  2185. {
  2186. _velocity.X = 0.0f;
  2187. _velocity.Y = 0.0f;
  2188. _velocity.Z = 0.0f;
  2189. _acceleration.X = 0;
  2190. _acceleration.Y = 0;
  2191. _acceleration.Z = 0;
  2192. //_orientation.w = 0f;
  2193. //_orientation.X = 0f;
  2194. //_orientation.Y = 0f;
  2195. //_orientation.Z = 0f;
  2196. m_rotationalVelocity.X = 0;
  2197. m_rotationalVelocity.Y = 0;
  2198. m_rotationalVelocity.Z = 0;
  2199. if (!m_lastUpdateSent)
  2200. {
  2201. m_throttleUpdates = false;
  2202. // throttleCounter = 0;
  2203. m_rotationalVelocity = pv;
  2204. if (_parent == null)
  2205. base.RequestPhysicsterseUpdate();
  2206. m_lastUpdateSent = true;
  2207. }
  2208. }
  2209. else
  2210. {
  2211. if (lastZeroFlag != _zeroFlag)
  2212. {
  2213. if (_parent == null)
  2214. base.RequestPhysicsterseUpdate();
  2215. }
  2216. m_lastVelocity = _velocity;
  2217. _position = l_position;
  2218. _velocity.X = tempLinearVelocity1.getX();
  2219. _velocity.Y = tempLinearVelocity1.getY();
  2220. _velocity.Z = tempLinearVelocity1.getZ();
  2221. _acceleration = ((_velocity - m_lastVelocity) / 0.1f);
  2222. _acceleration = new Vector3(_velocity.X - m_lastVelocity.X / 0.1f,
  2223. _velocity.Y - m_lastVelocity.Y / 0.1f,
  2224. _velocity.Z - m_lastVelocity.Z / 0.1f);
  2225. //m_log.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
  2226. if (_velocity.ApproxEquals(pv, 0.5f))
  2227. {
  2228. m_rotationalVelocity = pv;
  2229. }
  2230. else
  2231. {
  2232. m_rotationalVelocity = new Vector3(tempAngularVelocity1.getX(), tempAngularVelocity1.getY(), tempAngularVelocity1.getZ());
  2233. }
  2234. //m_log.Debug("ODE: " + m_rotationalVelocity.ToString());
  2235. _orientation.X = l_orientation.X;
  2236. _orientation.Y = l_orientation.Y;
  2237. _orientation.Z = l_orientation.Z;
  2238. _orientation.W = l_orientation.W;
  2239. m_lastUpdateSent = false;
  2240. //if (!m_throttleUpdates || throttleCounter > _parent_scene.geomUpdatesPerThrottledUpdate)
  2241. //{
  2242. if (_parent == null)
  2243. base.RequestPhysicsterseUpdate();
  2244. // }
  2245. // else
  2246. // {
  2247. // throttleCounter++;
  2248. //}
  2249. }
  2250. m_lastposition = l_position;
  2251. if (forceenable)
  2252. {
  2253. Body.forceActivationState(1);
  2254. forceenable = false;
  2255. }
  2256. }
  2257. else
  2258. {
  2259. // Not a body.. so Make sure the client isn't interpolating
  2260. _velocity.X = 0;
  2261. _velocity.Y = 0;
  2262. _velocity.Z = 0;
  2263. _acceleration.X = 0;
  2264. _acceleration.Y = 0;
  2265. _acceleration.Z = 0;
  2266. m_rotationalVelocity.X = 0;
  2267. m_rotationalVelocity.Y = 0;
  2268. m_rotationalVelocity.Z = 0;
  2269. _zeroFlag = true;
  2270. }
  2271. }
  2272. }
  2273. internal void setPrimForRemoval()
  2274. {
  2275. m_taintremove = true;
  2276. }
  2277. internal void EnableAxisMotor(Vector3 axislock)
  2278. {
  2279. if (m_aMotor != null)
  2280. DisableAxisMotor();
  2281. if (Body == null)
  2282. return;
  2283. if (Body.Handle == IntPtr.Zero)
  2284. return;
  2285. if (AxisLockAngleHigh != null && AxisLockAngleHigh.Handle != IntPtr.Zero)
  2286. AxisLockAngleHigh.Dispose();
  2287. m_aMotor = new btGeneric6DofConstraint(Body, _parent_scene.TerrainBody, _parent_scene.TransZero,
  2288. _parent_scene.TransZero, false);
  2289. float endNoLock = (360 * Utils.DEG_TO_RAD);
  2290. AxisLockAngleHigh = new btVector3((axislock.X == 0) ? 0 : endNoLock, (axislock.Y == 0) ? 0 : endNoLock, (axislock.Z == 0) ? 0 : endNoLock);
  2291. m_aMotor.setAngularLowerLimit(_parent_scene.VectorZero);
  2292. m_aMotor.setAngularUpperLimit(AxisLockAngleHigh);
  2293. m_aMotor.setLinearLowerLimit(AxisLockLinearLow);
  2294. m_aMotor.setLinearUpperLimit(AxisLockLinearHigh);
  2295. _parent_scene.getBulletWorld().addConstraint((btTypedConstraint)m_aMotor);
  2296. //m_aMotor.
  2297. }
  2298. internal void DisableAxisMotor()
  2299. {
  2300. if (m_aMotor != null && m_aMotor.Handle != IntPtr.Zero)
  2301. {
  2302. _parent_scene.getBulletWorld().removeConstraint(m_aMotor);
  2303. m_aMotor.Dispose();
  2304. m_aMotor = null;
  2305. }
  2306. }
  2307. }
  2308. }