PageRenderTime 65ms CodeModel.GetById 21ms RepoModel.GetById 1ms app.codeStats 0ms

/Aurora/Physics/AuroraOpenDynamicsEngine/AODEPrim.cs

https://bitbucket.org/VirtualReality/software-testing
C# | 2988 lines | 2302 code | 400 blank | 286 comment | 430 complexity | 0bfd8d4544f79359a43b71071cd95f83 MD5 | raw file
  1. /*
  2. * Copyright (c) Contributors, http://aurora-sim.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 Aurora-Sim 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. using Aurora.Framework;
  41. using Aurora.Framework.ConsoleFramework;
  42. using Aurora.Framework.Physics;
  43. using Aurora.Framework.SceneInfo;
  44. using Aurora.Framework.Utilities;
  45. using OdeAPI;
  46. using OpenMetaverse;
  47. using System;
  48. using System.Collections.Generic;
  49. using System.Linq;
  50. //using Ode.NET;
  51. namespace Aurora.Physics.AuroraOpenDynamicsEngine
  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 AuroraODEPrim : PhysicsObject
  57. {
  58. private const CollisionCategories m_default_collisionFlags = (CollisionCategories.Geom
  59. | CollisionCategories.Space
  60. | CollisionCategories.Body
  61. | CollisionCategories.Character
  62. );
  63. private static readonly Dictionary<ulong, IntPtr> m_MeshToTriMeshMap = new Dictionary<ulong, IntPtr>();
  64. private readonly AuroraODEPhysicsScene _parent_scene;
  65. private readonly Vector3 _torque = Vector3.Zero;
  66. private readonly int body_autodisable_frames = 20;
  67. private readonly AuroraODEDynamics m_vehicle;
  68. private IntPtr Amotor = IntPtr.Zero;
  69. public IntPtr Body { get; private set; }
  70. private CollisionEventUpdate CollisionEventsThisFrame;
  71. private float PID_D = 35f;
  72. private float PID_G = 25f;
  73. private Vector3 _acceleration;
  74. private float _mass; // prim or object mass
  75. private IMesh _mesh;
  76. private Quaternion _orientation;
  77. private PhysicsObject _parent;
  78. private ISceneChildEntity _parent_entity;
  79. private PrimitiveBaseShape _pbs;
  80. private Vector3 _position;
  81. private Vector3 _size;
  82. private IntPtr _triMeshData;
  83. private Vector3 _velocity;
  84. private bool _zeroFlag;
  85. private bool _zeroFlagForceSet;
  86. internal volatile bool childPrim;
  87. internal List<AuroraODEPrim> childrenPrim = new List<AuroraODEPrim>();
  88. private int fakeori; // control the use of above
  89. private int fakepos; // control the use of above
  90. private bool hasOOBoffsetFromMesh; // if true we did compute it form mesh centroid, else from aabb
  91. private bool iscolliding;
  92. private Vector3 m_angularforceacc;
  93. private Vector3 m_angularlock = Vector3.One;
  94. private bool m_blockPhysicalReconstruction;
  95. private bool m_buildingRepresentation;
  96. // KF: These next 7 params apply to llSetHoverHeight(float height, integer water, float tau),
  97. // and are for non-VEHICLES only.
  98. private float m_buoyancy; //KF: m_buoyancy should be set by llSetBuoyancy() for non-vehicle.
  99. // private float m_tensor = 5f;
  100. private bool m_collidesLand = true;
  101. private bool m_collidesWater;
  102. // Default we're a Geometry
  103. private CollisionCategories m_collisionCategories = (CollisionCategories.Geom);
  104. // Default, Collide with Other Geometries, spaces and Bodies
  105. private CollisionCategories m_collisionFlags = m_default_collisionFlags;
  106. internal float m_collisionscore;
  107. private int m_crossingfailures;
  108. internal bool m_disabled;
  109. private bool m_eventsubscription;
  110. //This disables the prim so that it cannot do much anything at all
  111. private Vector3 m_force;
  112. private Vector3 m_forceacc;
  113. internal bool m_frozen;
  114. private bool m_isSelected;
  115. private bool m_isVolumeDetect; // If true, this prim only detects collisions but doesn't collide actively
  116. private bool m_isphysical;
  117. private int m_lastUpdateSent;
  118. private Vector3 m_lastVelocity;
  119. private Quaternion m_lastorientation;
  120. private Vector3 m_lastposition;
  121. private uint m_localID;
  122. private int m_material = (int) Material.Wood;
  123. private bool m_primIsRemoved;
  124. private Vector3 m_pushForce;
  125. private Vector3 m_rotationalVelocity;
  126. internal IntPtr m_targetSpace = IntPtr.Zero;
  127. private bool m_throttleUpdates;
  128. private float primMass; // prim own mass
  129. public Vector3 primOOBoffset; // is centroid out of mesh or rest aabb
  130. public Vector3 primOOBsize; // prim real dimensions from mesh
  131. public IntPtr prim_geom;
  132. private d.Mass primdMass; // prim inertia information on it's own referencial
  133. private Quaternion showorientation; // tmp hack see showposition
  134. private Vector3 showposition; // a temp hack for now rest of code expects position to be changed immediately
  135. public AuroraODEPrim(ISceneChildEntity entity, AuroraODEPhysicsScene parent_scene, bool pisPhysical)
  136. {
  137. m_vehicle = new AuroraODEDynamics();
  138. //gc = GCHandle.Alloc(prim_geom, GCHandleType.Pinned);
  139. // correct for changed timestep
  140. PID_D /= (parent_scene.ODE_STEPSIZE*50f); // original ode fps of 50
  141. PID_G /= (parent_scene.ODE_STEPSIZE*50f);
  142. body_autodisable_frames = parent_scene.bodyFramesAutoDisable;
  143. prim_geom = IntPtr.Zero;
  144. _size = entity.Scale;
  145. _position = entity.AbsolutePosition;
  146. fakepos = 0;
  147. _orientation = entity.GetWorldRotation();
  148. fakeori = 0;
  149. _pbs = entity.Shape;
  150. _parent_entity = entity;
  151. _parent_scene = parent_scene;
  152. m_targetSpace = IntPtr.Zero;
  153. /*
  154. m_isphysical = pisPhysical;
  155. if (m_isphysical)
  156. m_targetSpace = _parent_scene.space;
  157. */
  158. m_isphysical = false;
  159. m_forceacc = Vector3.Zero;
  160. m_angularforceacc = Vector3.Zero;
  161. hasOOBoffsetFromMesh = false;
  162. _triMeshData = IntPtr.Zero;
  163. CalcPrimBodyData();
  164. _parent_scene.AddSimulationChange(() => changeadd());
  165. }
  166. public ISceneChildEntity ParentEntity
  167. {
  168. get { return _parent_entity; }
  169. }
  170. public override bool BuildingRepresentation
  171. {
  172. get { return m_buildingRepresentation; }
  173. set
  174. {
  175. if (value)
  176. m_buildingRepresentation = value;
  177. //else
  178. // _parent_scene.AddSimulationChange(() => m_buildingRepresentation = false);
  179. }
  180. }
  181. public override bool BlockPhysicalReconstruction
  182. {
  183. get { return m_blockPhysicalReconstruction; }
  184. set
  185. {
  186. if (value)
  187. m_blockPhysicalReconstruction = value;
  188. else
  189. _parent_scene.AddSimulationChange(() =>
  190. {
  191. if (value)
  192. DestroyBody();
  193. else
  194. {
  195. m_blockPhysicalReconstruction = false;
  196. if (!childPrim)
  197. MakeBody();
  198. }
  199. if (!childPrim && childrenPrim.Count > 0)
  200. {
  201. foreach (AuroraODEPrim prm in childrenPrim)
  202. prm.BlockPhysicalReconstruction = value;
  203. }
  204. });
  205. }
  206. }
  207. public PhysicsObject Parent
  208. {
  209. get { return _parent; }
  210. }
  211. public override int PhysicsActorType
  212. {
  213. get { return (int) ActorTypes.Prim; }
  214. }
  215. public override uint LocalID
  216. {
  217. get { return m_localID; }
  218. set
  219. {
  220. //MainConsole.Instance.Info("[PHYSICS]: Setting TrackerID: " + value);
  221. m_localID = value;
  222. }
  223. }
  224. public override bool VolumeDetect
  225. {
  226. get { return m_isVolumeDetect; }
  227. set { _parent_scene.AddSimulationChange(() => changevoldtc(value)); }
  228. }
  229. public override bool Selected
  230. {
  231. set
  232. {
  233. // This only makes the object not collidable if the object
  234. // is physical or the object is modified somehow *IN THE FUTURE*
  235. // without this, if an avatar selects prim, they can walk right
  236. // through it while it's selected
  237. if ((IsPhysical && !_zeroFlag) || !value)
  238. _parent_scene.AddSimulationChange(() => changeSelectedStatus(value));
  239. else
  240. m_isSelected = value;
  241. if (m_isSelected)
  242. disableBodySoft();
  243. }
  244. }
  245. public override bool IsPhysical
  246. {
  247. get
  248. {
  249. if (childPrim && _parent != null) // root prim defines if is physical or not
  250. return ((AuroraODEPrim) _parent).m_isphysical;
  251. else
  252. return m_isphysical;
  253. }
  254. set
  255. {
  256. _parent_scene.AddSimulationChange(() => changePhysicsStatus(value));
  257. if (!value) // Zero the remembered last velocity
  258. m_lastVelocity = Vector3.Zero;
  259. }
  260. }
  261. public override bool IsTruelyColliding { get; set; }
  262. public override bool IsColliding
  263. {
  264. get { return iscolliding; }
  265. set
  266. {
  267. if (value && _parent != null)
  268. _parent.LinkSetIsColliding = value;
  269. LinkSetIsColliding = value;
  270. iscolliding = value;
  271. }
  272. }
  273. public override bool LinkSetIsColliding { get; set; }
  274. public override bool ThrottleUpdates
  275. {
  276. get { return m_throttleUpdates; }
  277. set { m_throttleUpdates = value; }
  278. }
  279. public override Vector3 Position
  280. {
  281. get
  282. {
  283. if (fakepos > 0)
  284. return showposition;
  285. else
  286. return _position;
  287. }
  288. set
  289. {
  290. showposition = value;
  291. fakepos++;
  292. _parent_scene.AddSimulationChange(() => changePosition(value));
  293. }
  294. }
  295. public override Vector3 Size
  296. {
  297. get { return _size; }
  298. set
  299. {
  300. if (value.IsFinite())
  301. _parent_scene.AddSimulationChange(() => changesize(value));
  302. else
  303. MainConsole.Instance.Warn("[PHYSICS]: Got NaN Size on object");
  304. }
  305. }
  306. public override float Mass
  307. {
  308. get { return _mass; }
  309. }
  310. public override Vector3 Force
  311. {
  312. //get { return Vector3.Zero; }
  313. get { return m_force; }
  314. set
  315. {
  316. if (value.IsFinite())
  317. {
  318. _parent_scene.AddSimulationChange(() => changeforce(value, false));
  319. }
  320. else
  321. {
  322. MainConsole.Instance.Warn("[PHYSICS]: NaN in Force Applied to an Object");
  323. }
  324. }
  325. }
  326. public override int VehicleType
  327. {
  328. get { return (int) m_vehicle.Type; }
  329. set { _parent_scene.AddSimulationChange(() => changeVehicleType(value)); }
  330. }
  331. public override Vector3 CenterOfMass
  332. {
  333. get
  334. {
  335. d.Vector3 dtmp;
  336. if (IsPhysical && !childPrim && Body != IntPtr.Zero)
  337. {
  338. dtmp = d.BodyGetPosition(Body);
  339. return new Vector3(dtmp.X, dtmp.Y, dtmp.Z);
  340. }
  341. else if (prim_geom != IntPtr.Zero)
  342. {
  343. d.Quaternion dq;
  344. d.GeomCopyQuaternion(prim_geom, out dq);
  345. Quaternion q;
  346. q.X = dq.X;
  347. q.Y = dq.Y;
  348. q.Z = dq.Z;
  349. q.W = dq.W;
  350. Vector3 vtmp = primOOBoffset*q;
  351. dtmp = d.GeomGetPosition(prim_geom);
  352. return new Vector3(dtmp.X + vtmp.X, dtmp.Y + vtmp.Y, dtmp.Z + vtmp.Z);
  353. }
  354. else
  355. return Vector3.Zero;
  356. }
  357. }
  358. public override PrimitiveBaseShape Shape
  359. {
  360. set { _parent_scene.AddSimulationChange(() => changeshape(value)); }
  361. }
  362. public override Vector3 Velocity
  363. {
  364. get
  365. {
  366. // Averate previous velocity with the new one so
  367. // client object interpolation works a 'little' better
  368. if (_zeroFlag)
  369. return Vector3.Zero;
  370. Vector3 returnVelocity = Vector3.Zero;
  371. returnVelocity.X = (m_lastVelocity.X + _velocity.X)/2;
  372. returnVelocity.Y = (m_lastVelocity.Y + _velocity.Y)/2;
  373. returnVelocity.Z = (m_lastVelocity.Z + _velocity.Z)/2;
  374. return returnVelocity;
  375. }
  376. set
  377. {
  378. if (value.IsFinite())
  379. _parent_scene.AddSimulationChange(() => changevelocity(value));
  380. else
  381. {
  382. MainConsole.Instance.Warn("[PHYSICS]: Got NaN Velocity in Object");
  383. }
  384. }
  385. }
  386. public override Vector3 Torque
  387. {
  388. get
  389. {
  390. if (childPrim || !m_isphysical || Body == IntPtr.Zero)
  391. return Vector3.Zero;
  392. return _torque;
  393. }
  394. set
  395. {
  396. if (value.IsFinite())
  397. _parent_scene.AddSimulationChange(() => changeSetTorque(value));
  398. else
  399. MainConsole.Instance.Warn("[PHYSICS]: Got NaN Torque in Object");
  400. }
  401. }
  402. public override float CollisionScore
  403. {
  404. get { return m_collisionscore; }
  405. set { m_collisionscore = value; }
  406. }
  407. public override Quaternion Orientation
  408. {
  409. get
  410. {
  411. if (fakeori > 0)
  412. return showorientation;
  413. else
  414. return _orientation;
  415. }
  416. set
  417. {
  418. if (QuaternionIsFinite(value))
  419. {
  420. showorientation = value;
  421. fakeori++;
  422. _parent_scene.AddSimulationChange(() => changeOrientation(value));
  423. }
  424. else
  425. MainConsole.Instance.Warn("[PHYSICS]: Got NaN quaternion Orientation from Scene in Object");
  426. }
  427. }
  428. public override Vector3 Acceleration
  429. {
  430. get { return _acceleration; }
  431. }
  432. public override Vector3 RotationalVelocity
  433. {
  434. get
  435. {
  436. if (_zeroFlag)
  437. return Vector3.Zero;
  438. if (m_rotationalVelocity.ApproxEquals(Vector3.Zero, 0.00001f))
  439. return Vector3.Zero;
  440. return m_rotationalVelocity;
  441. }
  442. set
  443. {
  444. if (value.IsFinite())
  445. _parent_scene.AddSimulationChange(() => changeangvelocity(value));
  446. else
  447. MainConsole.Instance.Warn("[PHYSICS]: Got NaN RotationalVelocity in Object");
  448. }
  449. }
  450. public override float Buoyancy
  451. {
  452. get { return m_buoyancy; }
  453. set { m_buoyancy = value; }
  454. }
  455. public override bool FloatOnWater
  456. {
  457. set { _parent_scene.AddSimulationChange(() => changefloatonwater(value)); }
  458. }
  459. public override void ClearVelocity()
  460. {
  461. _velocity = Vector3.Zero;
  462. _acceleration = Vector3.Zero;
  463. m_rotationalVelocity = Vector3.Zero;
  464. m_lastorientation = Orientation;
  465. m_lastposition = Position;
  466. m_lastVelocity = _velocity;
  467. if (Body != null && Body != IntPtr.Zero)
  468. d.BodySetLinearVel(Body, 0, 0, 0);
  469. }
  470. public override void ForceSetVelocity(Vector3 velocity)
  471. {
  472. _velocity = velocity;
  473. m_lastVelocity = velocity;
  474. if (Body != IntPtr.Zero)
  475. d.BodySetLinearVel(Body, velocity.X, velocity.Y, velocity.Z);
  476. }
  477. public void ForceSetRotVelocity(Vector3 velocity)
  478. {
  479. m_rotationalVelocity = velocity;
  480. if (Body != IntPtr.Zero)
  481. d.BodySetAngularVel(Body, velocity.X, velocity.Y, velocity.Z);
  482. }
  483. public void SetGeom(IntPtr geom)
  484. {
  485. prim_geom = geom;
  486. //Console.WriteLine("SetGeom to " + prim_geom + " for " + m_primName);
  487. if (prim_geom != IntPtr.Zero)
  488. {
  489. d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories);
  490. d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags);
  491. CalcPrimBodyData();
  492. _parent_scene.actor_name_map[prim_geom] = this;
  493. }
  494. if (childPrim)
  495. {
  496. if (_parent != null && _parent is AuroraODEPrim)
  497. {
  498. AuroraODEPrim parent = (AuroraODEPrim) _parent;
  499. //Console.WriteLine("SetGeom calls ChildSetGeom");
  500. parent.ChildSetGeom(this);
  501. }
  502. }
  503. //MainConsole.Instance.Warn("Setting Geom to: " + prim_geom);
  504. }
  505. public void enableBodySoft()
  506. {
  507. if (!childPrim)
  508. {
  509. if (m_isphysical && Body != IntPtr.Zero)
  510. {
  511. d.BodyEnable(Body);
  512. if (m_vehicle.Type != Vehicle.TYPE_NONE)
  513. m_vehicle.Enable(Body, this, _parent_scene);
  514. }
  515. m_disabled = false;
  516. }
  517. }
  518. public void disableBodySoft()
  519. {
  520. if (!childPrim)
  521. {
  522. m_disabled = true;
  523. m_vehicle.Disable(this);
  524. if (IsPhysical && Body != IntPtr.Zero)
  525. d.BodyDisable(Body);
  526. }
  527. }
  528. private void MakeBody()
  529. {
  530. // d.Vector3 dvtmp;
  531. // d.Vector3 dbtmp;
  532. if (m_blockPhysicalReconstruction) // building is blocked
  533. return;
  534. if (childPrim) // child prims don't get own bodies;
  535. return;
  536. if (prim_geom == IntPtr.Zero)
  537. {
  538. MainConsole.Instance.Warn("[PHYSICS]: Unable to link the linkset. Root has no geom yet");
  539. return;
  540. }
  541. if (!m_isphysical) // only physical things get a body
  542. return;
  543. if (Body != IntPtr.Zero) // who shouldn't have one already ?
  544. {
  545. d.BodyDestroy(Body);
  546. Body = IntPtr.Zero;
  547. MainConsole.Instance.Warn("[PHYSICS]: MakeBody called having a body");
  548. }
  549. d.Mass objdmass = new d.Mass {};
  550. d.Matrix3 mymat = new d.Matrix3();
  551. d.Quaternion myrot = new d.Quaternion();
  552. Body = d.BodyCreate(_parent_scene.world);
  553. DMassDup(ref primdMass, out objdmass);
  554. // rotate inertia
  555. myrot.X = _orientation.X;
  556. myrot.Y = _orientation.Y;
  557. myrot.Z = _orientation.Z;
  558. myrot.W = _orientation.W;
  559. d.RfromQ(out mymat, ref myrot);
  560. d.MassRotate(ref objdmass, ref mymat);
  561. // set the body rotation and position
  562. d.BodySetRotation(Body, ref mymat);
  563. // recompute full object inertia if needed
  564. if (childrenPrim.Count > 0)
  565. {
  566. d.Matrix3 mat = new d.Matrix3();
  567. d.Quaternion quat = new d.Quaternion();
  568. d.Mass tmpdmass = new d.Mass {};
  569. Vector3 rcm;
  570. rcm.X = _position.X + objdmass.c.X;
  571. rcm.Y = _position.Y + objdmass.c.Y;
  572. rcm.Z = _position.Z + objdmass.c.Z;
  573. lock (childrenPrim)
  574. {
  575. foreach (AuroraODEPrim prm in childrenPrim)
  576. {
  577. if (prm.prim_geom == IntPtr.Zero)
  578. {
  579. MainConsole.Instance.Warn(
  580. "[PHYSICS]: Unable to link one of the linkset elements, skipping it. No geom yet");
  581. continue;
  582. }
  583. DMassCopy(ref prm.primdMass, ref tmpdmass);
  584. // apply prim current rotation to inertia
  585. quat.W = prm._orientation.W;
  586. quat.X = prm._orientation.X;
  587. quat.Y = prm._orientation.Y;
  588. quat.Z = prm._orientation.Z;
  589. d.RfromQ(out mat, ref quat);
  590. d.MassRotate(ref tmpdmass, ref mat);
  591. Vector3 ppos = prm._position;
  592. ppos.X += tmpdmass.c.X - rcm.X;
  593. ppos.Y += tmpdmass.c.Y - rcm.Y;
  594. ppos.Z += tmpdmass.c.Z - rcm.Z;
  595. // refer inertia to root prim center of mass position
  596. d.MassTranslate(ref tmpdmass,
  597. ppos.X,
  598. ppos.Y,
  599. ppos.Z);
  600. d.MassAdd(ref objdmass, ref tmpdmass); // add to total object inertia
  601. // fix prim colision cats
  602. d.GeomClearOffset(prm.prim_geom);
  603. d.GeomSetBody(prm.prim_geom, Body);
  604. prm.Body = Body;
  605. d.GeomSetOffsetWorldRotation(prm.prim_geom, ref mat); // set relative rotation
  606. }
  607. }
  608. }
  609. d.GeomClearOffset(prim_geom); // make sure we don't have a hidden offset
  610. // associate root geom with body
  611. d.GeomSetBody(prim_geom, Body);
  612. d.BodySetPosition(Body, _position.X + objdmass.c.X, _position.Y + objdmass.c.Y, _position.Z + objdmass.c.Z);
  613. d.GeomSetOffsetWorldPosition(prim_geom, _position.X, _position.Y, _position.Z);
  614. d.MassTranslate(ref objdmass, -objdmass.c.X, -objdmass.c.Y, -objdmass.c.Z);
  615. // ode wants inertia at center of body
  616. myrot.W = -myrot.W;
  617. d.RfromQ(out mymat, ref myrot);
  618. d.MassRotate(ref objdmass, ref mymat);
  619. d.BodySetMass(Body, ref objdmass);
  620. _mass = objdmass.mass;
  621. m_collisionCategories |= CollisionCategories.Body;
  622. m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
  623. // disconnect from world gravity so we can apply buoyancy
  624. // if (!testRealGravity)
  625. d.BodySetGravityMode(Body, false);
  626. d.BodySetAutoDisableFlag(Body, true);
  627. d.BodySetAutoDisableSteps(Body, body_autodisable_frames);
  628. d.BodySetDamping(Body, .001f, .0002f);
  629. m_disabled = false;
  630. d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories);
  631. d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags);
  632. if (m_targetSpace != _parent_scene.space)
  633. {
  634. if (d.SpaceQuery(m_targetSpace, prim_geom))
  635. d.SpaceRemove(m_targetSpace, prim_geom);
  636. m_targetSpace = _parent_scene.space;
  637. d.SpaceAdd(m_targetSpace, prim_geom);
  638. }
  639. lock (childrenPrim)
  640. {
  641. foreach (AuroraODEPrim prm in childrenPrim)
  642. {
  643. if (prm.prim_geom == IntPtr.Zero)
  644. continue;
  645. Vector3 ppos = prm._position;
  646. d.GeomSetOffsetWorldPosition(prm.prim_geom, ppos.X, ppos.Y, ppos.Z); // set relative position
  647. prm.m_collisionCategories |= CollisionCategories.Body;
  648. prm.m_collisionFlags |= (CollisionCategories.Land | CollisionCategories.Wind);
  649. d.GeomSetCategoryBits(prm.prim_geom, (int) prm.m_collisionCategories);
  650. d.GeomSetCollideBits(prm.prim_geom, (int) prm.m_collisionFlags);
  651. if (prm.m_targetSpace != _parent_scene.space)
  652. {
  653. if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
  654. d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
  655. prm.m_targetSpace = _parent_scene.space;
  656. d.SpaceAdd(m_targetSpace, prm.prim_geom);
  657. }
  658. prm.m_disabled = false;
  659. _parent_scene.addActivePrim(prm);
  660. }
  661. }
  662. // The body doesn't already have a finite rotation mode set here
  663. if ((!m_angularlock.ApproxEquals(Vector3.One, 0.0f)) && _parent == null)
  664. {
  665. createAMotor(m_angularlock);
  666. }
  667. if (m_vehicle.Type != Vehicle.TYPE_NONE)
  668. m_vehicle.Enable(Body, this, _parent_scene);
  669. _parent_scene.addActivePrim(this);
  670. }
  671. private void SetInStaticSpace(AuroraODEPrim prm)
  672. {
  673. if (prm.m_targetSpace != null && prm.m_targetSpace == _parent_scene.space)
  674. {
  675. if (d.SpaceQuery(prm.m_targetSpace, prm.prim_geom))
  676. d.SpaceRemove(prm.m_targetSpace, prm.prim_geom);
  677. }
  678. prm.m_targetSpace = _parent_scene.calculateSpaceForGeom(prm._position);
  679. d.SpaceAdd(prm.m_targetSpace, prm.prim_geom);
  680. }
  681. public void DestroyBody()
  682. // for now removes all colisions etc from childs, full body reconstruction is needed after this
  683. {
  684. //this kills the body so things like 'mesh' can re-create it.
  685. lock (this)
  686. {
  687. if (Body != IntPtr.Zero)
  688. {
  689. _parent_scene.remActivePrim(this);
  690. m_collisionCategories &= ~CollisionCategories.Body;
  691. m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
  692. if (prim_geom != IntPtr.Zero)
  693. {
  694. d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories);
  695. d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags);
  696. UpdateDataFromGeom();
  697. SetInStaticSpace(this);
  698. }
  699. if (!childPrim)
  700. {
  701. lock (childrenPrim)
  702. {
  703. foreach (AuroraODEPrim prm in childrenPrim)
  704. {
  705. _parent_scene.remActivePrim(prm);
  706. prm.m_collisionCategories &= ~CollisionCategories.Body;
  707. prm.m_collisionFlags &= ~(CollisionCategories.Wind | CollisionCategories.Land);
  708. if (prm.prim_geom != IntPtr.Zero)
  709. {
  710. d.GeomSetCategoryBits(prm.prim_geom, (int) m_collisionCategories);
  711. d.GeomSetCollideBits(prm.prim_geom, (int) m_collisionFlags);
  712. prm.UpdateDataFromGeom();
  713. prm.Body = IntPtr.Zero;
  714. SetInStaticSpace(prm);
  715. }
  716. prm._mass = prm.primMass;
  717. }
  718. }
  719. m_vehicle.Disable(this);
  720. d.BodyDestroy(Body);
  721. }
  722. }
  723. Body = IntPtr.Zero;
  724. }
  725. _mass = primMass;
  726. m_disabled = true;
  727. }
  728. public bool setMesh(AuroraODEPhysicsScene parent_scene, IMesh mesh)
  729. {
  730. // This sleeper is there to moderate how long it takes between
  731. // setting up the mesh and pre-processing it when we get rapid fire mesh requests on a single object
  732. //Thread.Sleep(10);
  733. //Kill Body so that mesh can re-make the geom
  734. if (IsPhysical && Body != IntPtr.Zero)
  735. {
  736. if (childPrim)
  737. {
  738. if (_parent != null)
  739. {
  740. AuroraODEPrim parent = (AuroraODEPrim) _parent;
  741. parent.ChildDelink(this);
  742. }
  743. }
  744. else
  745. {
  746. DestroyBody();
  747. }
  748. }
  749. IntPtr vertices, indices;
  750. int vertexCount, indexCount;
  751. int vertexStride, triStride;
  752. mesh.getVertexListAsPtrToFloatArray(out vertices, out vertexStride, out vertexCount);
  753. // Note, that vertices are fixed in unmanaged heap
  754. mesh.getIndexListAsPtrToIntArray(out indices, out triStride, out indexCount);
  755. // Also fixed, needs release after usage
  756. if (vertexCount == 0 || indexCount == 0)
  757. {
  758. MainConsole.Instance.WarnFormat(
  759. "[PHYSICS]: Got invalid mesh on prim at <{0},{1},{2}>. It can be a sculpt with alpha channel in map. Replacing it by a small box.",
  760. _position.X, _position.Y, _position.Z);
  761. _size.X = 0.01f;
  762. _size.Y = 0.01f;
  763. _size.Z = 0.01f;
  764. return false;
  765. }
  766. primOOBoffset = mesh.GetCentroid();
  767. hasOOBoffsetFromMesh = true;
  768. mesh.releaseSourceMeshData(); // free up the original mesh data to save memory
  769. if (m_MeshToTriMeshMap.ContainsKey(mesh.Key))
  770. {
  771. _triMeshData = m_MeshToTriMeshMap[mesh.Key];
  772. }
  773. else
  774. {
  775. _triMeshData = d.GeomTriMeshDataCreate();
  776. d.GeomTriMeshDataBuildSimple(_triMeshData, vertices, vertexStride, vertexCount, indices, indexCount,
  777. triStride);
  778. d.GeomTriMeshDataPreprocess(_triMeshData);
  779. m_MeshToTriMeshMap[mesh.Key] = _triMeshData;
  780. }
  781. try
  782. {
  783. if (prim_geom == IntPtr.Zero)
  784. {
  785. SetGeom(d.CreateTriMesh(m_targetSpace, _triMeshData, null, null, null));
  786. }
  787. }
  788. catch (AccessViolationException)
  789. {
  790. MainConsole.Instance.Error("[PHYSICS]: MESH LOCKED");
  791. return false;
  792. }
  793. return true;
  794. }
  795. private void changeAngularLock(Vector3 newlock)
  796. {
  797. // do we have a Physical object?
  798. if (Body != IntPtr.Zero)
  799. {
  800. //Check that we have a Parent
  801. //If we have a parent then we're not authorative here
  802. if (_parent == null)
  803. {
  804. if (!newlock.ApproxEquals(Vector3.One, 0f))
  805. {
  806. createAMotor(newlock);
  807. }
  808. else
  809. {
  810. if (Amotor != IntPtr.Zero)
  811. {
  812. d.JointDestroy(Amotor);
  813. Amotor = IntPtr.Zero;
  814. }
  815. }
  816. }
  817. }
  818. // Store this for later in case we get turned into a separate body
  819. m_angularlock = newlock;
  820. }
  821. private void changelink(AuroraODEPrim newparent)
  822. {
  823. // If the newly set parent is not null
  824. // create link
  825. if (_parent == null && newparent != null)
  826. {
  827. newparent.ParentPrim(this);
  828. }
  829. // If the newly set parent is null
  830. // destroy link
  831. else if (_parent != null)
  832. {
  833. if (_parent is AuroraODEPrim)
  834. {
  835. if (newparent != _parent)
  836. {
  837. AuroraODEPrim obj = (AuroraODEPrim) _parent;
  838. obj.ChildDelink(this);
  839. childPrim = false;
  840. if (newparent != null)
  841. {
  842. newparent.ParentPrim(this);
  843. }
  844. }
  845. }
  846. }
  847. _parent = newparent;
  848. }
  849. // I'm the parent
  850. // prim is the child
  851. public void ParentPrim(AuroraODEPrim prim)
  852. {
  853. //Console.WriteLine("ParentPrim " + m_primName);
  854. if (this.m_localID != prim.m_localID)
  855. {
  856. DestroyBody();
  857. lock (childrenPrim)
  858. {
  859. #if (!ISWIN)
  860. foreach (AuroraODEPrim prm in prim.childrenPrim)
  861. {
  862. if (!childrenPrim.Contains(prm))
  863. {
  864. childrenPrim.Add(prm);
  865. }
  866. }
  867. #else
  868. foreach (AuroraODEPrim prm in prim.childrenPrim.Where(prm => !childrenPrim.Contains(prm)))
  869. {
  870. childrenPrim.Add(prm);
  871. }
  872. #endif
  873. if (!childrenPrim.Contains(prim)) // must allow full reconstruction
  874. childrenPrim.Add(prim);
  875. }
  876. //Remove old children
  877. prim.childrenPrim.Clear();
  878. prim.childPrim = true;
  879. prim._parent = this;
  880. if (prim.Body != IntPtr.Zero)
  881. {
  882. prim.DestroyBody(); // don't loose bodies around
  883. prim.Body = IntPtr.Zero;
  884. }
  885. MakeBody(); // full nasty reconstruction
  886. }
  887. }
  888. private void ChildSetGeom(AuroraODEPrim odePrim)
  889. {
  890. DestroyBody();
  891. MakeBody();
  892. }
  893. private void UpdateChildsfromgeom()
  894. {
  895. if (childrenPrim.Count > 0)
  896. {
  897. foreach (AuroraODEPrim prm in childrenPrim)
  898. prm.UpdateDataFromGeom();
  899. }
  900. }
  901. private void UpdateDataFromGeom()
  902. {
  903. if (prim_geom != IntPtr.Zero)
  904. {
  905. d.Vector3 lpos = d.GeomGetPosition(prim_geom);
  906. _position.X = lpos.X;
  907. _position.Y = lpos.Y;
  908. _position.Z = lpos.Z;
  909. d.Quaternion qtmp = new d.Quaternion
  910. {
  911. };
  912. d.GeomCopyQuaternion(prim_geom, out qtmp);
  913. _orientation.W = qtmp.W;
  914. _orientation.X = qtmp.X;
  915. _orientation.Y = qtmp.Y;
  916. _orientation.Z = qtmp.Z;
  917. }
  918. }
  919. private void ChildDelink(AuroraODEPrim odePrim)
  920. {
  921. // Okay, we have a delinked child.. destroy all body and remake
  922. if (odePrim != this && !childrenPrim.Contains(odePrim))
  923. return;
  924. DestroyBody();
  925. if (odePrim == this)
  926. {
  927. AuroraODEPrim newroot = null;
  928. lock (childrenPrim)
  929. {
  930. if (childrenPrim.Count > 0)
  931. {
  932. newroot = childrenPrim[0];
  933. childrenPrim.RemoveAt(0);
  934. foreach (AuroraODEPrim prm in childrenPrim)
  935. {
  936. newroot.childrenPrim.Add(prm);
  937. }
  938. childrenPrim.Clear();
  939. }
  940. if (newroot != null)
  941. {
  942. newroot.childPrim = false;
  943. newroot._parent = null;
  944. newroot.MakeBody();
  945. }
  946. }
  947. }
  948. else
  949. {
  950. lock (childrenPrim)
  951. {
  952. childrenPrim.Remove(odePrim);
  953. odePrim.childPrim = false;
  954. odePrim._parent = null;
  955. //odePrim.UpdateDataFromGeom ();
  956. odePrim.MakeBody();
  957. }
  958. }
  959. MakeBody();
  960. }
  961. private void ChildRemove(AuroraODEPrim odePrim)
  962. {
  963. // Okay, we have a delinked child.. destroy all body and remake
  964. if (odePrim != this && !childrenPrim.Contains(odePrim))
  965. return;
  966. DestroyBody();
  967. if (odePrim == this)
  968. {
  969. AuroraODEPrim newroot = null;
  970. lock (childrenPrim)
  971. {
  972. if (childrenPrim.Count > 0)
  973. {
  974. newroot = childrenPrim[0];
  975. childrenPrim.RemoveAt(0);
  976. foreach (AuroraODEPrim prm in childrenPrim)
  977. {
  978. newroot.childrenPrim.Add(prm);
  979. }
  980. childrenPrim.Clear();
  981. }
  982. if (newroot != null)
  983. {
  984. newroot.childPrim = false;
  985. newroot._parent = null;
  986. newroot.MakeBody();
  987. }
  988. }
  989. return;
  990. }
  991. else
  992. {
  993. lock (childrenPrim)
  994. {
  995. childrenPrim.Remove(odePrim);
  996. odePrim.childPrim = false;
  997. odePrim._parent = null;
  998. }
  999. }
  1000. MakeBody();
  1001. }
  1002. private void changeSelectedStatus(bool newsel)
  1003. {
  1004. bool isphys = IsPhysical;
  1005. if (newsel)
  1006. {
  1007. m_collisionCategories = CollisionCategories.Selected;
  1008. m_collisionFlags = (CollisionCategories.Sensor | CollisionCategories.Space);
  1009. // We do the body disable soft twice because 'in theory' a collision could have happened
  1010. // in between the disabling and the collision properties setting
  1011. // which would wake the physical body up from a soft disabling and potentially cause it to fall
  1012. // through the ground.
  1013. // NOTE FOR JOINTS: this doesn't always work for jointed assemblies because if you select
  1014. // just one part of the assembly, the rest of the assembly is non-selected and still simulating,
  1015. // so that causes the selected part to wake up and continue moving.
  1016. // even if you select all parts of a jointed assembly, it is not guaranteed that the entire
  1017. // assembly will stop simulating during the selection, because of the lack of atomicity
  1018. // of select operations (their processing could be interrupted by a thread switch, causing
  1019. // simulation to continue before all of the selected object notifications trickle down to
  1020. // the physics engine).
  1021. // e.g. we select 100 prims that are connected by joints. non-atomically, the first 50 are
  1022. // selected and disabled. then, due to a thread switch, the selection processing is
  1023. // interrupted and the physics engine continues to simulate, so the last 50 items, whose
  1024. // selection was not yet processed, continues to simulate. this wakes up ALL of the
  1025. // first 50 again. then the last 50 are disabled. then the first 50, which were just woken
  1026. // up, start simulating again, which in turn wakes up the last 50.
  1027. if (isphys)
  1028. {
  1029. disableBodySoft();
  1030. }
  1031. if (prim_geom != IntPtr.Zero)
  1032. {
  1033. d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories);
  1034. d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags);
  1035. }
  1036. if (isphys)
  1037. {
  1038. disableBodySoft();
  1039. }
  1040. }
  1041. else
  1042. {
  1043. m_collisionCategories = CollisionCategories.Geom;
  1044. if (isphys)
  1045. m_collisionCategories |= CollisionCategories.Body;
  1046. m_collisionFlags = m_default_collisionFlags;
  1047. if (m_collidesLand)
  1048. m_collisionFlags |= CollisionCategories.Land;
  1049. if (m_collidesWater)
  1050. m_collisionFlags |= CollisionCategories.Water;
  1051. if (prim_geom != IntPtr.Zero)
  1052. {
  1053. d.GeomSetCategoryBits(prim_geom, (int) m_collisionCategories);
  1054. d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags);
  1055. }
  1056. if (isphys)
  1057. {
  1058. if (Body != IntPtr.Zero)
  1059. {
  1060. d.BodySetLinearVel(Body, 0f, 0f, 0f);
  1061. d.BodySetAngularVel(Body, 0f, 0f, 0f);
  1062. d.BodySetForce(Body, 0, 0, 0);
  1063. d.BodySetTorque(Body, 0, 0, 0);
  1064. enableBodySoft();
  1065. }
  1066. }
  1067. }
  1068. resetCollisionAccounting();
  1069. m_isSelected = newsel;
  1070. if (!m_isSelected)
  1071. {
  1072. _zeroFlag = false;
  1073. }
  1074. }
  1075. //end changeSelectedStatus
  1076. public void CreateGeom(IntPtr m_targetSpace, IMesh _mesh)
  1077. {
  1078. hasOOBoffsetFromMesh = false;
  1079. bool havemesh = false;
  1080. //Console.WriteLine("CreateGeom:");
  1081. if (_mesh != null)
  1082. {
  1083. havemesh = setMesh(_parent_scene, _mesh); // this will give a mesh to non trivial known prims
  1084. }
  1085. if (!havemesh)
  1086. {
  1087. if (_pbs.ProfileShape == ProfileShape.HalfCircle && _pbs.PathCurve == (byte) Extrusion.Curve1
  1088. && _size.X == _size.Y && _size.Y == _size.Z)
  1089. {
  1090. // it's a sphere
  1091. try
  1092. {
  1093. SetGeom(d.CreateSphere(m_targetSpace, _size.X*0.5f));
  1094. }
  1095. catch (Exception e)
  1096. {
  1097. MainConsole.Instance.Warn("[PHYSICS]: Create sphere failed: {0}", e);
  1098. return;
  1099. }
  1100. }
  1101. else
  1102. {
  1103. try
  1104. {
  1105. SetGeom(d.CreateBox(m_targetSpace, _size.X, _size.Y, _size.Z));
  1106. }
  1107. catch (Exception e)
  1108. {
  1109. MainConsole.Instance.Warn("[PHYSICS]: Create box failed: {0}", e);
  1110. return;
  1111. }
  1112. }
  1113. }
  1114. }
  1115. private void RemoveGeom()
  1116. {
  1117. if (prim_geom != IntPtr.Zero)
  1118. {
  1119. _parent_scene.actor_name_map.Remove(prim_geom);
  1120. try
  1121. {
  1122. d.GeomDestroy(prim_geom);
  1123. /*
  1124. if (_triMeshData != IntPtr.Zero)
  1125. {
  1126. d.GeomTriMeshDataDestroy(_triMeshData);
  1127. _triMeshData = IntPtr.Zero;
  1128. }
  1129. */
  1130. }
  1131. catch (Exception e)
  1132. {
  1133. MainConsole.Instance.ErrorFormat("[PHYSICS]: PrimGeom destruction failed {1}", e);
  1134. }
  1135. prim_geom = IntPtr.Zero;
  1136. }
  1137. else
  1138. {
  1139. MainConsole.Instance.ErrorFormat("[PHYSICS]: PrimGeom destruction BAD");
  1140. }
  1141. Body = IntPtr.Zero;
  1142. hasOOBoffsetFromMesh = false;
  1143. CalcPrimBodyData();
  1144. }
  1145. public void changeadd()
  1146. {
  1147. // all prims are now created non physical
  1148. IntPtr targetspace = _parent_scene.calculateSpaceForGeom(_position);
  1149. m_targetSpace = targetspace;
  1150. if (_mesh == null)
  1151. {
  1152. if (_parent_scene.needsMeshing(_parent_entity))
  1153. {
  1154. // Don't need to re-enable body.. it's done in SetMesh
  1155. _mesh = _parent_scene.mesher.CreateMesh(_parent_entity.Name, _pbs, _size,
  1156. _parent_scene.meshSculptLOD, true);
  1157. //Tell things above if they want to cache it or something
  1158. if (_mesh != null)
  1159. _parent_entity.ParentEntity.GeneratedMesh(_parent_entity, _mesh);
  1160. // createmesh returns null when it's a shape that isn't a cube.
  1161. // MainConsole.Instance.Debug(m_localID);
  1162. // Remove the reference to any JPEG2000 sculpt data so it can be GCed
  1163. _pbs.SculptData = null;
  1164. }
  1165. }
  1166. //Console.WriteLine("changeadd 1");
  1167. CreateGeom(m_targetSpace, _mesh);
  1168. CalcPrimBodyData();
  1169. if (prim_geom != IntPtr.Zero)
  1170. {
  1171. d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
  1172. d.Quaternion myrot = new d.Quaternion();
  1173. Quaternion fake = _orientation;
  1174. myrot.X = fake.X;
  1175. myrot.Y = fake.Y;
  1176. myrot.Z = fake.Z;
  1177. myrot.W = fake.W;
  1178. d.GeomSetQuaternion(prim_geom, ref myrot);
  1179. // _parent_scene.actor_name_map[prim_geom] = (PhysicsActor)this;
  1180. }
  1181. changeSelectedStatus(m_isSelected);
  1182. }
  1183. public void changePosition(Vector3 newpos)
  1184. {
  1185. if (m_isphysical)
  1186. {
  1187. if (childPrim)
  1188. {
  1189. if (m_blockPhysicalReconstruction) // inertia is messed, must rebuild
  1190. {
  1191. _position = newpos;
  1192. }
  1193. }
  1194. else
  1195. {
  1196. if (newpos != _position)
  1197. {
  1198. d.GeomSetPosition(prim_geom, newpos.X, newpos.Y, newpos.Z);
  1199. _position = newpos;
  1200. }
  1201. }
  1202. if (Body != IntPtr.Zero)
  1203. d.BodyEnable(Body);
  1204. }
  1205. else
  1206. {
  1207. if (newpos != _position)
  1208. {
  1209. IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, newpos, m_targetSpace);
  1210. m_targetSpace = tempspace;
  1211. d.GeomSetPosition(prim_geom, newpos.X, newpos.Y, newpos.Z);
  1212. d.SpaceAdd(m_targetSpace, prim_geom);
  1213. _position = newpos;
  1214. }
  1215. }
  1216. if (--fakepos < 0)
  1217. fakepos = 0;
  1218. changeSelectedStatus(m_isSelected);
  1219. resetCollisionAccounting();
  1220. }
  1221. public void changeOrientation(Quaternion newrot)
  1222. {
  1223. if (m_isphysical)
  1224. {
  1225. if (childPrim)
  1226. {
  1227. if (m_blockPhysicalReconstruction) // inertia is messed, must rebuild
  1228. {
  1229. _orientation = newrot;
  1230. }
  1231. }
  1232. else
  1233. {
  1234. if (newrot != _orientation)
  1235. {
  1236. d.Quaternion myrot = new d.Quaternion();
  1237. Quaternion fake = newrot;
  1238. myrot.X = fake.X;
  1239. myrot.Y = fake.Y;
  1240. myrot.Z = fake.Z;
  1241. myrot.W = fake.W;
  1242. d.GeomSetQuaternion(prim_geom, ref myrot);
  1243. _orientation = newrot;
  1244. if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
  1245. createAMotor(m_angularlock);
  1246. }
  1247. }
  1248. if (Body != IntPtr.Zero)
  1249. d.BodyEnable(Body);
  1250. }
  1251. else
  1252. {
  1253. if (newrot != _orientation)
  1254. {
  1255. d.Quaternion myrot = new d.Quaternion();
  1256. Quaternion fake = newrot;
  1257. myrot.X = fake.X;
  1258. myrot.Y = fake.Y;
  1259. myrot.Z = fake.Z;
  1260. myrot.W = fake.W;
  1261. d.GeomSetQuaternion(prim_geom, ref myrot);
  1262. _orientation = newrot;
  1263. }
  1264. }
  1265. if (--fakeori < 0)
  1266. fakeori = 0;
  1267. changeSelectedStatus(m_isSelected);
  1268. resetCollisionAccounting();
  1269. }
  1270. /*
  1271. public void changemoveandrotate(Vector3 newpos, Quaternion newrot)
  1272. {
  1273. if (m_isphysical)
  1274. {
  1275. if (childPrim)
  1276. {
  1277. if (m_blockPhysicalReconstruction) // inertia is messed, must rebuild
  1278. {
  1279. _orientation = newrot;
  1280. _position = newpos;
  1281. }
  1282. }
  1283. else
  1284. {
  1285. if (newrot != _orientation)
  1286. {
  1287. d.Quaternion myrot = new d.Quaternion();
  1288. Quaternion fake = newrot;
  1289. myrot.X = fake.X;
  1290. myrot.Y = fake.Y;
  1291. myrot.Z = fake.Z;
  1292. myrot.W = fake.W;
  1293. d.GeomSetQuaternion(prim_geom, ref myrot);
  1294. _orientation = newrot;
  1295. if (Body != IntPtr.Zero && !m_angularlock.ApproxEquals(Vector3.One, 0f))
  1296. createAMotor(m_angularlock);
  1297. }
  1298. if (newpos != _position)
  1299. {
  1300. d.GeomSetPosition(prim_geom, newpos.X, newpos.Y, newpos.Z);
  1301. _position = newpos;
  1302. }
  1303. }
  1304. if (Body != IntPtr.Zero)
  1305. d.BodyEnable(Body);
  1306. }
  1307. else
  1308. {
  1309. if (newrot != _orientation)
  1310. {
  1311. d.Quaternion myrot = new d.Quaternion();
  1312. Quaternion fake = newrot;
  1313. myrot.X = fake.X;
  1314. myrot.Y = fake.Y;
  1315. myrot.Z = fake.Z;
  1316. myrot.W = fake.W;
  1317. d.GeomSetQuaternion(prim_geom, ref myrot);
  1318. _orientation = newrot;
  1319. }
  1320. if (newpos != _position)
  1321. {
  1322. _parent_scene.waitForSpaceUnlock(m_targetSpace);
  1323. IntPtr tempspace = _parent_scene.recalculateSpaceForGeom(prim_geom, newpos, m_targetSpace);
  1324. m_targetSpace = tempspace;
  1325. d.GeomSetPosition(prim_geom, newpos.X, newpos.Y, newpos.Z);
  1326. d.SpaceAdd(m_targetSpace, prim_geom);
  1327. _position = newpos;
  1328. }
  1329. }
  1330. if (--fakepos < 0)
  1331. fakepos = 0;
  1332. if (--fakeori < 0)
  1333. fakeori = 0;
  1334. changeSelectedStatus(m_isSelected);
  1335. resetCollisionAccounting();
  1336. }
  1337. */
  1338. public void Move(float timestep)
  1339. {
  1340. if (m_isphysical && Body != IntPtr.Zero && !m_isSelected && !childPrim && !m_blockPhysicalReconstruction)
  1341. // KF: Only move root prims.
  1342. {
  1343. float fx = 0;
  1344. float fy = 0;
  1345. float fz = 0;
  1346. if (m_vehicle.Type != Vehicle.TYPE_NONE)
  1347. {
  1348. // 'VEHICLES' are dealt with in ODEDynamics.cs
  1349. m_vehicle.Step(Body, timestep, _parent_scene, this);
  1350. if (m_disabled || m_frozen)
  1351. {
  1352. d.BodySetForce(Body, 0, 0, 0);
  1353. d.BodySetLinearVel(Body, 0, 0, 0);
  1354. d.BodySetAngularVel(Body, 0, 0, 0);
  1355. _parent_scene.BadPrim(this.childPrim ? (AuroraODEPrim) _parent : this);
  1356. }
  1357. else
  1358. {
  1359. /*
  1360. d.Vector3 vel = d.BodyGetLinearVel (Body);
  1361. m_lastVelocity = _velocity;
  1362. _velocity = new Vector3 ((float)vel.X, (float)vel.Y, (float)vel.Z);
  1363. d.Vector3 pos = d.BodyGetPosition(Body);
  1364. m_lastposition = _position;
  1365. _position = new Vector3 ((float)pos.X, (float)pos.Y, (float)pos.Z);
  1366. d.Quaternion ori;
  1367. _zeroFlag = false;
  1368. _acceleration = ((_velocity - m_lastVelocity) / timestep);
  1369. //MainConsole.Instance.Info ("[PHYSICS]: P1: " + _position + " V2: " + m_lastposition + " Acceleration: " + _acceleration.ToString ());
  1370. d.GeomCopyQuaternion (prim_geom, out ori);
  1371. _orientation.X = ori.X;
  1372. _orientation.Y = ori.Y;
  1373. _orientation.Z = ori.Z;
  1374. _orientation.W = ori.W;
  1375. d.Vector3 rotvel = d.BodyGetAngularVel (Body);
  1376. m_rotationalVelocity.X = (float)rotvel.X;
  1377. m_rotationalVelocity.Y = (float)rotvel.Y;
  1378. m_rotationalVelocity.Z = (float)rotvel.Z;
  1379. */
  1380. }
  1381. }
  1382. else
  1383. {
  1384. Vector3 dcpos = d.BodyGetPosition(Body).ToVector3();
  1385. Vector3 vel = d.BodyGetLinearVel(Body).ToVector3();
  1386. Vector3 gravForce = new Vector3();
  1387. _parent_scene.CalculateGravity(_mass, dcpos, true,
  1388. (1.0f - m_buoyancy)*_parent_entity.GravityMultiplier, ref gravForce);
  1389. fx *= _mass;
  1390. fy *= _mass;
  1391. fz *= _mass;
  1392. fx += gravForce.X;
  1393. fy += gravForce.Y;
  1394. fz += gravForce.Z;
  1395. fx += m_force.X;
  1396. fy += m_force.Y;
  1397. fz += m_force.Z;
  1398. fx += m_pushForce.X*10;
  1399. fy += m_pushForce.Y*10;
  1400. fz += m_pushForce.Z*10;
  1401. m_pushForce = Vector3.Zero;
  1402. #region drag and forces accumulators
  1403. fx += m_forceacc.X;
  1404. fy += m_forceacc.Y;
  1405. fz += m_forceacc.Z;
  1406. m_forceacc = Vector3.Zero;
  1407. Vector3 newtorque;
  1408. newtorque.X = m_angularforceacc.X;
  1409. newtorque.Y = m_angularforceacc.Y;
  1410. newtorque.Z = m_angularforceacc.Z;
  1411. m_angularforceacc = Vector3.Zero;
  1412. #endregion
  1413. if (Math.Abs(fx) < 0.01)
  1414. fx = 0;
  1415. if (Math.Abs(fy) < 0.01)
  1416. fy = 0;
  1417. if (Math.Abs(fz) < 0.01)
  1418. fz = 0;
  1419. if (!d.BodyIsEnabled(Body))
  1420. d.BodyEnable(Body);
  1421. if (fx != 0 || fy != 0 || fz != 0)
  1422. d.BodyAddForce(Body, fx, fy, fz);
  1423. if (newtorque.X != 0 || newtorque.Y != 0 || newtorque.Z != 0)
  1424. d.BodyAddTorque(Body, newtorque.X, newtorque.Y, newtorque.Z);
  1425. }
  1426. }
  1427. }
  1428. public void UpdatePositionAndVelocity(float timestep)
  1429. {
  1430. if (m_frozen)
  1431. return;
  1432. // no lock; called from Simulate() -- if you call this from elsewhere, gotta lock or do Monitor.Enter/Exit!
  1433. if (_parent == null)
  1434. {
  1435. if (Body != IntPtr.Zero && prim_geom != IntPtr.Zero)
  1436. {
  1437. d.Vector3 cpos = d.BodyGetPosition(Body); // object position ( center of mass)
  1438. d.Vector3 lpos = d.GeomGetPosition(prim_geom); // root position that is seem by rest of simulator
  1439. #region Crossing failures
  1440. if (cpos.X > ((int) _parent_scene.WorldExtents.X - 0.05f) ||
  1441. cpos.X < 0f ||
  1442. cpos.Y > ((int) _parent_scene.WorldExtents.Y - 0.05f) ||
  1443. cpos.Y < 0f ||
  1444. cpos.Z < -100 ||
  1445. cpos.Z > 100000)
  1446. {
  1447. if (m_crossingfailures < _parent_scene.geomCrossingFailuresBeforeOutofbounds)
  1448. {
  1449. _position.X = lpos.X;
  1450. _position.Y = lpos.Y;
  1451. _position.Z = lpos.Z;
  1452. m_crossingfailures++;
  1453. m_lastposition = _position;
  1454. m_lastorientation = _orientation;
  1455. base.RequestPhysicsterseUpdate();
  1456. m_crossingfailures = 0;
  1457. return;
  1458. }
  1459. else
  1460. {
  1461. if (m_vehicle.Type == Vehicle.TYPE_NONE)
  1462. {
  1463. m_disabled = true;
  1464. m_frozen = true;
  1465. Vector3 l_position;
  1466. l_position.X = lpos.X;
  1467. l_position.Y = lpos.Y;
  1468. l_position.Z = lpos.Z;
  1469. base.RaiseOutOfBounds(l_position);
  1470. m_crossingfailures = 0;
  1471. return;
  1472. }
  1473. else
  1474. {
  1475. Vector3 newPos = Position;
  1476. newPos.X = Util.Clip(Position.X, 0.75f, _parent_scene.Region.RegionSizeX - 0.75f);
  1477. newPos.Y = Util.Clip(Position.Y, 0.75f, _parent_scene.Region.RegionSizeY - 0.75f);
  1478. Position = newPos;
  1479. d.BodySetPosition(Body, newPos.X, newPos.Y, newPos.Z);
  1480. m_crossingfailures = 0;
  1481. }
  1482. }
  1483. }
  1484. #endregion
  1485. #region Out of bounds
  1486. if (cpos.Z < 0 ||
  1487. (cpos.Z > _parent_scene.m_flightCeilingHeight && _parent_scene.m_useFlightCeilingHeight))
  1488. {
  1489. // This is so prim that get lost underground don't fall forever and suck up
  1490. //
  1491. // Sim resources and memory.
  1492. // Disables the prim's movement physics....
  1493. // It's a hack and will generate a console message if it fails.
  1494. //IsPhysical = false;
  1495. base.RaiseOutOfBounds(_position);
  1496. cpos.Z = cpos.Z < 0 ? 0 : _parent_scene.m_flightCeilingHeight;
  1497. _acceleration.X = 0;
  1498. _acceleration.Y = 0;
  1499. _acceleration.Z = 0;
  1500. _velocity.X = 0;
  1501. _velocity.Y = 0;
  1502. _velocity.Z = 0;
  1503. m_rotationalVelocity.X = 0;
  1504. m_rotationalVelocity.Y = 0;
  1505. m_rotationalVelocity.Z = 0;
  1506. d.BodySetLinearVel(Body, 0, 0, 0); // stop it
  1507. d.BodySetAngularVel(Body, 0, 0, 0); // stop it
  1508. d.BodySetPosition(Body, cpos.X, cpos.Y, cpos.Z); // put it somewhere
  1509. m_lastposition = _position;
  1510. m_lastorientation = _orientation;
  1511. base.RequestPhysicsterseUpdate();
  1512. m_throttleUpdates = true;
  1513. _zeroFlag = true;
  1514. m_frozen = true;
  1515. return;
  1516. }
  1517. #endregion
  1518. d.Quaternion ori;
  1519. d.GeomCopyQuaternion(prim_geom, out ori);
  1520. if ((Math.Abs(m_lastposition.X - lpos.X) < 0.01)
  1521. && (Math.Abs(m_lastposition.Y - lpos.Y) < 0.01)
  1522. && (Math.Abs(m_lastposition.Z - lpos.Z) < 0.01)
  1523. && (Math.Abs(m_lastorientation.X - ori.X) < 0.001)
  1524. && (Math.Abs(m_lastorientation.Y - ori.Y) < 0.001)
  1525. && (Math.Abs(m_lastorientation.Z - ori.Z) < 0.001)
  1526. )
  1527. {
  1528. _zeroFlag = true;
  1529. if (!_zeroFlagForceSet)
  1530. {
  1531. _zeroFlagForceSet = true;
  1532. m_lastUpdateSent = 2;
  1533. }
  1534. }
  1535. else
  1536. {
  1537. _zeroFlagForceSet = false;
  1538. _zeroFlag = false;
  1539. m_lastUpdateSent = 2;
  1540. }
  1541. bool needupdate = false;
  1542. if (_zeroFlag)
  1543. {
  1544. if (m_lastUpdateSent > 0)
  1545. {
  1546. //Keep the velocity, it won't be sent anywhere outside of the physics engine because of the _zeroFlag checks
  1547. //And this allows us to keep the _zeroFlag check a bit more stable
  1548. _velocity.X = 0.0f;
  1549. _velocity.Y = 0.0f;
  1550. _velocity.Z = 0.0f;
  1551. _acceleration.X = 0;
  1552. _acceleration.Y = 0;
  1553. _acceleration.Z = 0;
  1554. m_rotationalVelocity.X = 0;
  1555. m_rotationalVelocity.Y = 0;
  1556. m_rotationalVelocity.Z = 0;
  1557. // better let ode keep dealing with small values --Ubit
  1558. //ODE doesn't deal with them though, it just keeps adding them, never stopping the movement of the prim..
  1559. // its supposed to!
  1560. /*
  1561. d.BodySetLinearVel(Body, 0, 0, 0);
  1562. d.BodySetAngularVel(Body, 0, 0, 0);
  1563. d.BodySetForce(Body, 0, 0, 0);
  1564. */
  1565. needupdate = true;
  1566. m_lastUpdateSent--;
  1567. }
  1568. }
  1569. else
  1570. {
  1571. _position.X = lpos.X;
  1572. _position.Y = lpos.Y;
  1573. _position.Z = lpos.Z;
  1574. _orientation.X = ori.X;
  1575. _orientation.Y = ori.Y;
  1576. _orientation.Z = ori.Z;
  1577. _orientation.W = ori.W;
  1578. d.Vector3 vel = d.BodyGetLinearVel(Body);
  1579. d.Vector3 rotvel = d.BodyGetAngularVel(Body);
  1580. _velocity.X = vel.X;
  1581. _velocity.Y = vel.Y;
  1582. _velocity.Z = vel.Z;
  1583. _acceleration = ((_velocity - m_lastVelocity)/timestep);
  1584. //MainConsole.Instance.Info("[PHYSICS]: V1: " + _velocity + " V2: " + m_lastVelocity + " Acceleration: " + _acceleration.ToString());
  1585. m_rotationalVelocity.X = rotvel.X;
  1586. m_rotationalVelocity.Y = rotvel.Y;
  1587. m_rotationalVelocity.Z = rotvel.Z;
  1588. needupdate = true;
  1589. }
  1590. m_lastVelocity = _velocity; // for accelaration
  1591. if (needupdate)
  1592. {
  1593. m_lastposition = _position;
  1594. m_lastorientation = _orientation;
  1595. base.RequestPhysicsterseUpdate();
  1596. }
  1597. }
  1598. else
  1599. {
  1600. // Not a body.. so Make sure the client isn't interpolating
  1601. _velocity.X = 0;
  1602. _velocity.Y = 0;
  1603. _velocity.Z = 0;
  1604. _acceleration.X = 0;
  1605. _acceleration.Y = 0;
  1606. _acceleration.Z = 0;
  1607. m_rotationalVelocity.X = 0;
  1608. m_rotationalVelocity.Y = 0;
  1609. m_rotationalVelocity.Z = 0;
  1610. _zeroFlag = true;
  1611. m_frozen = true;
  1612. }
  1613. }
  1614. }
  1615. private d.Quaternion ConvertTodQuat(Quaternion q)
  1616. {
  1617. d.Quaternion dq = new d.Quaternion {X = q.X, Y = q.Y, Z = q.Z, W = q.W};
  1618. return dq;
  1619. }
  1620. internal void resetCollisionAccounting()
  1621. {
  1622. m_collisionscore = 0;
  1623. m_disabled = false;
  1624. }
  1625. public void changedisable()
  1626. {
  1627. m_disabled = true;
  1628. if (Body != IntPtr.Zero)
  1629. {
  1630. d.BodyDisable(Body);
  1631. Body = IntPtr.Zero;
  1632. }
  1633. }
  1634. public void changePhysicsStatus(bool newphys)
  1635. {
  1636. m_isphysical = newphys;
  1637. if (!childPrim)
  1638. {
  1639. if (newphys)
  1640. {
  1641. if (Body == IntPtr.Zero)
  1642. {
  1643. if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
  1644. {
  1645. changeshape(_pbs);
  1646. }
  1647. else
  1648. {
  1649. MakeBody();
  1650. }
  1651. }
  1652. }
  1653. else
  1654. {
  1655. if (Body != IntPtr.Zero)
  1656. {
  1657. //UpdateChildsfromgeom ();
  1658. if (_pbs.SculptEntry && _parent_scene.meshSculptedPrim)
  1659. {
  1660. changeshape(_pbs);
  1661. }
  1662. else
  1663. DestroyBody();
  1664. }
  1665. }
  1666. }
  1667. changeSelectedStatus(m_isSelected);
  1668. resetCollisionAccounting();
  1669. FirePhysicalRepresentationChanged();
  1670. }
  1671. public void changefloatonwater(bool arg)
  1672. {
  1673. m_collidesWater = arg;
  1674. if (prim_geom != IntPtr.Zero)
  1675. {
  1676. if (m_collidesWater)
  1677. m_collisionFlags |= CollisionCategories.Water;
  1678. else
  1679. m_collisionFlags &= ~CollisionCategories.Water;
  1680. d.GeomSetCollideBits(prim_geom, (int) m_collisionFlags);
  1681. }
  1682. }
  1683. public void changeprimsizeshape()
  1684. {
  1685. _parent_scene.actor_name_map.Remove(prim_geom);
  1686. AuroraODEPrim parent = null;
  1687. bool chp = childPrim;
  1688. if (chp)
  1689. parent = (AuroraODEPrim) _parent;
  1690. // Cleanup of old prim geometry and Bodies
  1691. if (chp)
  1692. {
  1693. if (parent != null)
  1694. parent.DestroyBody();
  1695. }
  1696. else
  1697. {
  1698. DestroyBody();
  1699. }
  1700. if (prim_geom != IntPtr.Zero)
  1701. {
  1702. try
  1703. {
  1704. d.GeomDestroy(prim_geom);
  1705. }
  1706. catch (AccessViolationException)
  1707. {
  1708. prim_geom = IntPtr.Zero;
  1709. MainConsole.Instance.Error("[PHYSICS]: PrimGeom dead");
  1710. }
  1711. prim_geom = IntPtr.Zero;
  1712. }
  1713. // we don't need to do space calculation because the client sends a position update also.
  1714. if (_size.X <= 0)
  1715. _size.X = 0.01f;
  1716. if (_size.Y <= 0)
  1717. _size.Y = 0.01f;
  1718. if (_size.Z <= 0)
  1719. _size.Z = 0.01f;
  1720. // Construction of new prim
  1721. if (_parent_scene.needsMeshing(_parent_entity))
  1722. {
  1723. float meshlod = _parent_scene.meshSculptLOD;
  1724. IMesh mesh = _parent_scene.mesher.CreateMesh(_parent_entity.Name, _pbs, _size, meshlod, true);
  1725. // createmesh returns null when it doesn't mesh.
  1726. CreateGeom(m_targetSpace, mesh);
  1727. }
  1728. else
  1729. {
  1730. _mesh = null;
  1731. //Console.WriteLine("changeshape");
  1732. CreateGeom(m_targetSpace, null);
  1733. }
  1734. CalcPrimBodyData();
  1735. if (prim_geom != IntPtr.Zero)
  1736. {
  1737. d.GeomSetPosition(prim_geom, _position.X, _position.Y, _position.Z);
  1738. d.Quaternion myrot = new d.Quaternion();
  1739. Quaternion fake = _orientation;
  1740. myrot.X = fake.X;
  1741. myrot.Y = fake.Y;
  1742. myrot.Z = fake.Z;
  1743. myrot.W = fake.W;
  1744. d.GeomSetQuaternion(prim_geom, ref myrot);
  1745. _parent_scene.actor_name_map[prim_geom] = this;
  1746. }
  1747. changeSelectedStatus(m_isSelected);
  1748. if (chp)
  1749. {
  1750. if (parent != null)
  1751. {
  1752. parent.MakeBody();
  1753. }
  1754. }
  1755. else
  1756. MakeBody();
  1757. }
  1758. public void changeshape(PrimitiveBaseShape arg)
  1759. {
  1760. _pbs = arg;
  1761. changeprimsizeshape();
  1762. }
  1763. public void changesize(Vector3 arg)
  1764. {
  1765. _size = arg;
  1766. changeprimsizeshape();
  1767. }
  1768. public void changeAddForce(object arg)
  1769. {
  1770. if (!m_isSelected)
  1771. {
  1772. if (IsPhysical)
  1773. {
  1774. if (m_vehicle.Type == Vehicle.TYPE_NONE)
  1775. m_forceacc += (Vector3) arg*100;
  1776. else
  1777. m_vehicle.ProcessForceTaint((Vector3) arg);
  1778. }
  1779. }
  1780. }
  1781. public void changeSetTorque(Vector3 newtorque)
  1782. {
  1783. if (!m_isSelected)
  1784. {
  1785. if (IsPhysical && Body != IntPtr.Zero)
  1786. {
  1787. d.BodySetTorque(Body, newtorque.X, newtorque.Y, newtorque.Z);
  1788. }
  1789. }
  1790. }
  1791. public void changeAddAngularForce(Vector3 arg)
  1792. {
  1793. if (!m_isSelected && IsPhysical)
  1794. m_angularforceacc += arg*100;
  1795. }
  1796. private void changevelocity(Vector3 arg)
  1797. {
  1798. _velocity = arg;
  1799. if (!m_isSelected)
  1800. {
  1801. if (IsPhysical)
  1802. {
  1803. if (Body != IntPtr.Zero)
  1804. {
  1805. d.BodySetLinearVel(Body, _velocity.X, _velocity.Y, _velocity.Z);
  1806. }
  1807. }
  1808. //resetCollisionAccounting();
  1809. }
  1810. }
  1811. public void setPrimForRemoval()
  1812. {
  1813. if (m_primIsRemoved)
  1814. return; //Already being removed
  1815. m_primIsRemoved = true;
  1816. lock (childrenPrim)
  1817. {
  1818. foreach (AuroraODEPrim prm in childrenPrim)
  1819. {
  1820. prm.m_primIsRemoved = true;
  1821. }
  1822. }
  1823. _parent_scene.AddSimulationChange(() =>
  1824. {
  1825. if (_parent != null)
  1826. {
  1827. AuroraODEPrim parent = (AuroraODEPrim) _parent;
  1828. parent.ChildRemove(this);
  1829. }
  1830. else
  1831. ChildRemove(this);
  1832. RemoveGeom();
  1833. m_targetSpace = IntPtr.Zero;
  1834. _parent_scene.RemovePrimThreadLocked(this);
  1835. });
  1836. }
  1837. public void setPrimForDeletion()
  1838. {
  1839. if (m_primIsRemoved)
  1840. return; //Already being removed
  1841. m_primIsRemoved = true;
  1842. lock (childrenPrim)
  1843. {
  1844. foreach (AuroraODEPrim prm in childrenPrim)
  1845. {
  1846. prm.m_primIsRemoved = true;
  1847. }
  1848. }
  1849. _parent_scene.AddSimulationChange(() => deletePrimLocked());
  1850. }
  1851. private void deletePrimLocked()
  1852. {
  1853. if (_parent != null)
  1854. {
  1855. AuroraODEPrim parent = (AuroraODEPrim) _parent;
  1856. parent.DestroyBody();
  1857. }
  1858. else
  1859. DestroyBody();
  1860. RemoveGeom();
  1861. m_targetSpace = IntPtr.Zero;
  1862. _parent_scene.RemovePrimThreadLocked(this);
  1863. }
  1864. public override void VehicleFloatParam(int param, float value)
  1865. {
  1866. strVehicleFloatParam strf = new strVehicleFloatParam {param = param, value = value};
  1867. _parent_scene.AddSimulationChange(() => changeVehicleFloatParam(strf.param, strf.value));
  1868. }
  1869. public override void VehicleVectorParam(int param, Vector3 value)
  1870. {
  1871. strVehicleVectorParam strv = new strVehicleVectorParam {param = param, value = value};
  1872. _parent_scene.AddSimulationChange(() => changeVehicleVectorParam(strv.param, strv.value));
  1873. }
  1874. public override void VehicleRotationParam(int param, Quaternion rotation)
  1875. {
  1876. strVehicleQuartParam strq = new strVehicleQuartParam {param = param, value = rotation};
  1877. _parent_scene.AddSimulationChange(() => changeVehicleRotationParam(strq.param, strq.value));
  1878. }
  1879. public override void VehicleFlags(int param, bool remove)
  1880. {
  1881. strVehicleBoolParam strb = new strVehicleBoolParam {param = param, value = remove};
  1882. _parent_scene.AddSimulationChange(() => changeVehicleFlags(strb.param, strb.value));
  1883. }
  1884. public override void SetCameraPos(Quaternion CameraRotation)
  1885. {
  1886. _parent_scene.AddSimulationChange(() => changeSetCameraPos(CameraRotation));
  1887. }
  1888. internal static bool QuaternionIsFinite(Quaternion q)
  1889. {
  1890. if (Single.IsNaN(q.X) || Single.IsInfinity(q.X))
  1891. return false;
  1892. if (Single.IsNaN(q.Y) || Single.IsInfinity(q.Y))
  1893. return false;
  1894. if (Single.IsNaN(q.Z) || Single.IsInfinity(q.Z))
  1895. return false;
  1896. if (Single.IsNaN(q.W) || Single.IsInfinity(q.W))
  1897. return false;
  1898. return true;
  1899. }
  1900. public override void AddForce(Vector3 force, bool pushforce)
  1901. {
  1902. if (force.IsFinite())
  1903. {
  1904. _parent_scene.AddSimulationChange(() => changeforce(force, pushforce));
  1905. }
  1906. else
  1907. {
  1908. MainConsole.Instance.Warn("[PHYSICS]: Got Invalid linear force vector from Scene in Object");
  1909. }
  1910. //MainConsole.Instance.Info("[PHYSICS]: Added Force:" + force.ToString() + " to prim at " + Position.ToString());
  1911. }
  1912. public override void AddAngularForce(Vector3 force, bool pushforce)
  1913. {
  1914. if (force.IsFinite())
  1915. _parent_scene.AddSimulationChange(() => changeAddAngularForce(force));
  1916. else
  1917. MainConsole.Instance.Warn("[PHYSICS]: Got Invalid Angular force vector from Scene in Object");
  1918. }
  1919. public override void CrossingFailure()
  1920. {
  1921. m_crossingfailures++;
  1922. if (m_crossingfailures > _parent_scene.geomCrossingFailuresBeforeOutofbounds)
  1923. {
  1924. base.RaiseOutOfBounds(_position);
  1925. return;
  1926. }
  1927. else if (m_crossingfailures == _parent_scene.geomCrossingFailuresBeforeOutofbounds)
  1928. {
  1929. MainConsole.Instance.Warn("[PHYSICS]: Too many crossing failures for: " + _parent_entity.Name + " @ " +
  1930. _parent_entity.AbsolutePosition);
  1931. }
  1932. }
  1933. public override void link(PhysicsObject obj)
  1934. {
  1935. _parent_scene.AddSimulationChange(() => changelink((AuroraODEPrim) obj));
  1936. }
  1937. public override void delink()
  1938. {
  1939. _parent_scene.AddSimulationChange(() => changelink(null));
  1940. }
  1941. public override void LockAngularMotion(Vector3 axis)
  1942. {
  1943. // reverse the zero/non zero values for ODE.
  1944. if (axis.IsFinite())
  1945. {
  1946. axis.X = (axis.X > 0) ? 1f : 0f;
  1947. axis.Y = (axis.Y > 0) ? 1f : 0f;
  1948. axis.Z = (axis.Z > 0) ? 1f : 0f;
  1949. MainConsole.Instance.DebugFormat("[axislock]: <{0},{1},{2}>", axis.X, axis.Y, axis.Z);
  1950. _parent_scene.AddSimulationChange(() => changeAngularLock(axis));
  1951. }
  1952. else
  1953. {
  1954. MainConsole.Instance.Warn("[PHYSICS]: Got NaN locking axis from Scene on Object");
  1955. }
  1956. }
  1957. private void createAMotor(Vector3 axis)
  1958. {
  1959. if (Body == IntPtr.Zero)
  1960. return;
  1961. if (Amotor != IntPtr.Zero)
  1962. {
  1963. d.JointDestroy(Amotor);
  1964. Amotor = IntPtr.Zero;
  1965. }
  1966. int axisnum = 3 - (int) (axis.X + axis.Y + axis.Z);
  1967. if (axisnum <= 0)
  1968. return;
  1969. // stop it
  1970. d.BodySetTorque(Body, 0, 0, 0);
  1971. d.BodySetAngularVel(Body, 0, 0, 0);
  1972. Amotor = d.JointCreateAMotor(_parent_scene.world, IntPtr.Zero);
  1973. d.JointAttach(Amotor, Body, IntPtr.Zero);
  1974. d.JointSetAMotorMode(Amotor, 0);
  1975. d.JointSetAMotorNumAxes(Amotor, axisnum);
  1976. // get current orientation to lock
  1977. d.Quaternion dcur = d.BodyGetQuaternion(Body);
  1978. Quaternion curr; // crap convertion between identical things
  1979. curr.X = dcur.X;
  1980. curr.Y = dcur.Y;
  1981. curr.Z = dcur.Z;
  1982. curr.W = dcur.W;
  1983. Vector3 ax;
  1984. int i = 0;
  1985. int j = 0;
  1986. if (axis.X == 0)
  1987. {
  1988. ax = (new Vector3(1, 0, 0))*curr; // rotate world X to current local X
  1989. // ODE should do this with axis relative to body 1 but seems to fail
  1990. d.JointSetAMotorAxis(Amotor, 0, 0, ax.X, ax.Y, ax.Z);
  1991. d.JointSetAMotorAngle(Amotor, 0, 0);
  1992. d.JointSetAMotorParam(Amotor, (int) d.JointParam.LoStop, -0.000001f);
  1993. d.JointSetAMotorParam(Amotor, (int) d.JointParam.HiStop, 0.000001f);
  1994. d.JointSetAMotorParam(Amotor, (int) d.JointParam.Vel, 0);
  1995. d.JointSetAMotorParam(Amotor, (int) d.JointParam.FudgeFactor, 0.0001f);
  1996. d.JointSetAMotorParam(Amotor, (int) d.JointParam.Bounce, 0f);
  1997. d.JointSetAMotorParam(Amotor, (int) d.JointParam.FMax, 550000000);
  1998. i++;
  1999. j = 256; // aodeplugin.cs doesn't have all parameters so this moves to next axis set
  2000. }
  2001. if (axis.Y == 0)
  2002. {
  2003. ax = (new Vector3(0, 1, 0))*curr;
  2004. d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
  2005. d.JointSetAMotorAngle(Amotor, i, 0);
  2006. d.JointSetAMotorParam(Amotor, j + (int) d.JointParam.LoStop, -0.000001f);
  2007. d.JointSetAMotorParam(Amotor, j + (int) d.JointParam.HiStop, 0.000001f);
  2008. d.JointSetAMotorParam(Amotor, j + (int) d.JointParam.Vel, 0);
  2009. d.JointSetAMotorParam(Amotor, j + (int) d.JointParam.FudgeFactor, 0.0001f);
  2010. d.JointSetAMotorParam(Amotor, j + (int) d.JointParam.Bounce, 0f);
  2011. d.JointSetAMotorParam(Amotor, j + (int) d.JointParam.FMax, 550000000);
  2012. i++;
  2013. j += 256;
  2014. }
  2015. if (axis.Z == 0)
  2016. {
  2017. ax = (new Vector3(0, 0, 1))*curr;
  2018. d.JointSetAMotorAxis(Amotor, i, 0, ax.X, ax.Y, ax.Z);
  2019. d.JointSetAMotorAngle(Amotor, i, 0);
  2020. d.JointSetAMotorParam(Amotor, j + (int) d.JointParam.LoStop, -0.000001f);
  2021. d.JointSetAMotorParam(Amotor, j + (int) d.JointParam.HiStop, 0.000001f);
  2022. d.JointSetAMotorParam(Amotor, j + (int) d.JointParam.Vel, 0);
  2023. d.JointSetAMotorParam(Amotor, j + (int) d.JointParam.FudgeFactor, 0.0001f);
  2024. d.JointSetAMotorParam(Amotor, j + (int) d.JointParam.Bounce, 0f);
  2025. d.JointSetAMotorParam(Amotor, j + (int) d.JointParam.FMax, 550000000);
  2026. }
  2027. d.JointAddAMotorTorques(Amotor, 0.001f, 0.001f, 0.001f);
  2028. }
  2029. public Matrix4 FromDMass(d.Mass pMass)
  2030. {
  2031. Matrix4 obj;
  2032. obj.M11 = pMass.I.M00;
  2033. obj.M12 = pMass.I.M01;
  2034. obj.M13 = pMass.I.M02;
  2035. obj.M14 = 0;
  2036. obj.M21 = pMass.I.M10;
  2037. obj.M22 = pMass.I.M11;
  2038. obj.M23 = pMass.I.M12;
  2039. obj.M24 = 0;
  2040. obj.M31 = pMass.I.M20;
  2041. obj.M32 = pMass.I.M21;
  2042. obj.M33 = pMass.I.M22;
  2043. obj.M34 = 0;
  2044. obj.M41 = 0;
  2045. obj.M42 = 0;
  2046. obj.M43 = 0;
  2047. obj.M44 = 1;
  2048. return obj;
  2049. }
  2050. public d.Mass FromMatrix4(Matrix4 pMat, ref d.Mass obj)
  2051. {
  2052. obj.I.M00 = pMat[0, 0];
  2053. obj.I.M01 = pMat[0, 1];
  2054. obj.I.M02 = pMat[0, 2];
  2055. obj.I.M10 = pMat[1, 0];
  2056. obj.I.M11 = pMat[1, 1];
  2057. obj.I.M12 = pMat[1, 2];
  2058. obj.I.M20 = pMat[2, 0];
  2059. obj.I.M21 = pMat[2, 1];
  2060. obj.I.M22 = pMat[2, 2];
  2061. return obj;
  2062. }
  2063. public override void SubscribeEvents(int ms)
  2064. {
  2065. m_eventsubscription = true;
  2066. _parent_scene.addCollisionEventReporting(this);
  2067. }
  2068. public override void UnSubscribeEvents()
  2069. {
  2070. _parent_scene.remCollisionEventReporting(this);
  2071. m_eventsubscription = false;
  2072. }
  2073. public override void AddCollisionEvent(uint CollidedWith, ContactPoint contact)
  2074. {
  2075. if (base.SubscribedToCollisions() && SubscribedEvents())
  2076. //If we don't have anything that we are going to trigger, don't even add
  2077. {
  2078. if (CollisionEventsThisFrame == null)
  2079. CollisionEventsThisFrame = new CollisionEventUpdate();
  2080. CollisionEventsThisFrame.addCollider(CollidedWith, contact);
  2081. }
  2082. }
  2083. public override void SendCollisions()
  2084. {
  2085. if (CollisionEventsThisFrame == null || m_frozen) //No collisions or frozen, don't mess with it
  2086. return;
  2087. base.SendCollisionUpdate(CollisionEventsThisFrame.Copy());
  2088. CollisionEventsThisFrame = CollisionEventsThisFrame.Count == 0 ? null : new CollisionEventUpdate();
  2089. }
  2090. public override bool SubscribedEvents()
  2091. {
  2092. return m_eventsubscription;
  2093. }
  2094. private static void DMassCopy(ref d.Mass src, ref d.Mass dst)
  2095. {
  2096. dst.c.W = src.c.W;
  2097. dst.c.X = src.c.X;
  2098. dst.c.Y = src.c.Y;
  2099. dst.c.Z = src.c.Z;
  2100. dst.mass = src.mass;
  2101. dst.I.M00 = src.I.M00;
  2102. dst.I.M01 = src.I.M01;
  2103. dst.I.M02 = src.I.M02;
  2104. dst.I.M10 = src.I.M10;
  2105. dst.I.M11 = src.I.M11;
  2106. dst.I.M12 = src.I.M12;
  2107. dst.I.M20 = src.I.M20;
  2108. dst.I.M21 = src.I.M21;
  2109. dst.I.M22 = src.I.M22;
  2110. }
  2111. private static void DMassDup(ref d.Mass src, out d.Mass dst)
  2112. {
  2113. dst = new d.Mass
  2114. {
  2115. };
  2116. dst.c.W = src.c.W;
  2117. dst.c.X = src.c.X;
  2118. dst.c.Y = src.c.Y;
  2119. dst.c.Z = src.c.Z;
  2120. dst.mass = src.mass;
  2121. dst.I.M00 = src.I.M00;
  2122. dst.I.M01 = src.I.M01;
  2123. dst.I.M02 = src.I.M02;
  2124. dst.I.M10 = src.I.M10;
  2125. dst.I.M11 = src.I.M11;
  2126. dst.I.M12 = src.I.M12;
  2127. dst.I.M20 = src.I.M20;
  2128. dst.I.M21 = src.I.M21;
  2129. dst.I.M22 = src.I.M22;
  2130. }
  2131. private void changeacceleration(Object arg)
  2132. {
  2133. _acceleration = (Vector3) arg;
  2134. }
  2135. private void changeangvelocity(Vector3 arg)
  2136. {
  2137. m_rotationalVelocity = arg;
  2138. }
  2139. private void changeforce(Vector3 force, bool pushforce)
  2140. {
  2141. if (pushforce)
  2142. {
  2143. if (IsPhysical && m_vehicle.Type != Vehicle.TYPE_NONE)
  2144. m_vehicle.ProcessForceTaint(force);
  2145. else
  2146. m_pushForce = force;
  2147. }
  2148. else
  2149. m_force = force;
  2150. }
  2151. private void changevoldtc(bool arg)
  2152. {
  2153. m_isVolumeDetect = arg;
  2154. }
  2155. private void donullchange()
  2156. {
  2157. }
  2158. private void changeVehicleType(int value)
  2159. {
  2160. m_vehicle.ProcessTypeChange(this, (Vehicle) value, _parent_scene.ODE_STEPSIZE);
  2161. if (m_vehicle.Type == Vehicle.TYPE_NONE)
  2162. m_vehicle.Enable(Body, this, _parent_scene);
  2163. }
  2164. private void changeVehicleFloatParam(int param, float value)
  2165. {
  2166. m_vehicle.ProcessFloatVehicleParam((Vehicle) param, value, _parent_scene.ODE_STEPSIZE);
  2167. }
  2168. private void changeVehicleVectorParam(int param, Vector3 value)
  2169. {
  2170. m_vehicle.ProcessVectorVehicleParam((Vehicle) param, value, _parent_scene.ODE_STEPSIZE);
  2171. }
  2172. private void changeVehicleRotationParam(int param, Quaternion rotation)
  2173. {
  2174. m_vehicle.ProcessRotationVehicleParam((Vehicle) param, rotation);
  2175. }
  2176. private void changeVehicleFlags(int param, bool remove)
  2177. {
  2178. m_vehicle.ProcessVehicleFlags(param, remove);
  2179. }
  2180. private void changeSetCameraPos(Quaternion CameraRotation)
  2181. {
  2182. m_vehicle.ProcessSetCameraPos(CameraRotation);
  2183. }
  2184. #region Material/Contact setting/getting
  2185. public override void SetMaterial(int pMaterial, bool forceMaterialSettings)
  2186. {
  2187. Material oldMaterial = (Material) m_material;
  2188. m_material = pMaterial;
  2189. Material m = (Material) pMaterial;
  2190. if (!forceMaterialSettings || oldMaterial == m)
  2191. return;
  2192. //Fix restitution and friction values as well
  2193. switch (m)
  2194. {
  2195. case Material.Flesh:
  2196. _parent_entity.Friction = 0.9f;
  2197. _parent_entity.Restitution = 0.3f;
  2198. break;
  2199. case Material.Glass:
  2200. _parent_entity.Friction = 0.2f;
  2201. _parent_entity.Restitution = 0.7f;
  2202. break;
  2203. case Material.Metal:
  2204. _parent_entity.Friction = 0.3f;
  2205. _parent_entity.Restitution = 0.4f;
  2206. break;
  2207. case Material.Plastic:
  2208. _parent_entity.Friction = 0.4f;
  2209. _parent_entity.Restitution = 0.7f;
  2210. break;
  2211. case Material.Rubber:
  2212. _parent_entity.Friction = 0.9f;
  2213. _parent_entity.Restitution = 0.9f;
  2214. break;
  2215. case Material.Stone:
  2216. _parent_entity.Friction = 0.8f;
  2217. _parent_entity.Restitution = 0.4f;
  2218. break;
  2219. case Material.Wood:
  2220. _parent_entity.Friction = 0.6f;
  2221. _parent_entity.Restitution = 0.5f;
  2222. break;
  2223. default:
  2224. //?????
  2225. break;
  2226. }
  2227. }
  2228. public void GetContactParam(PhysicsActor actor, ref d.Contact contact)
  2229. {
  2230. int vehicleType = 0;
  2231. if ((_parent != null && (vehicleType = _parent.VehicleType) != (int) Vehicle.TYPE_NONE) ||
  2232. (vehicleType = VehicleType) != (int) Vehicle.TYPE_NONE ||
  2233. (actor is AuroraODEPrim && ((AuroraODEPrim) actor).Parent != null &&
  2234. (vehicleType = ((AuroraODEPrim) actor).Parent.VehicleType) != (int) Vehicle.TYPE_NONE) ||
  2235. (actor is AuroraODEPrim &&
  2236. (vehicleType = ((AuroraODEPrim) actor).VehicleType) != (int) Vehicle.TYPE_NONE))
  2237. {
  2238. if (vehicleType == (int) Vehicle.TYPE_CAR)
  2239. {
  2240. contact.surface.bounce = 0;
  2241. contact.surface.bounce_vel = 0;
  2242. contact.surface.mu = 2;
  2243. }
  2244. else if (vehicleType == (int) Vehicle.TYPE_SLED)
  2245. {
  2246. contact.surface.bounce = 0;
  2247. contact.surface.bounce_vel = 0;
  2248. contact.surface.mu = 0;
  2249. }
  2250. else if (vehicleType == (int) Vehicle.TYPE_AIRPLANE ||
  2251. vehicleType == (int) Vehicle.TYPE_BALLOON ||
  2252. vehicleType == (int) Vehicle.TYPE_BOAT)
  2253. {
  2254. contact.surface.bounce = 0;
  2255. contact.surface.bounce_vel = 0;
  2256. contact.surface.mu = 100;
  2257. }
  2258. }
  2259. else
  2260. {
  2261. float restSquared = _parent_entity.Restitution*_parent_entity.Restitution*_parent_entity.Restitution;
  2262. float maxVel = Velocity.Z < -1f ? -1f : Velocity.Z > 1f ? 1f : Velocity.Z;
  2263. contact.surface.bounce = (maxVel*-(restSquared));
  2264. //Its about 1:1 surprisingly, even though this constant was for havok
  2265. if (contact.surface.bounce > 1.5f)
  2266. contact.surface.bounce = 0.5f; //Limit the bouncing please...
  2267. if (contact.surface.bounce <= 0)
  2268. {
  2269. contact.surface.bounce = 0;
  2270. contact.surface.bounce_vel = 0;
  2271. }
  2272. else
  2273. contact.surface.bounce_vel = 0.01f*restSquared*(-maxVel*restSquared);
  2274. //give it a good amount of bounce and have it depend on how much velocity is there too
  2275. contact.surface.mu = 800;
  2276. if (contact.surface.bounce_vel != 0)
  2277. contact.surface.mode |= d.ContactFlags.Bounce;
  2278. else
  2279. contact.surface.mode &= d.ContactFlags.Bounce;
  2280. if (actor.PhysicsActorType == (int) ActorTypes.Prim)
  2281. contact.surface.mu *= _parent_entity.Friction;
  2282. else if (actor.PhysicsActorType == (int) ActorTypes.Ground)
  2283. contact.surface.mu *= 2;
  2284. else
  2285. contact.surface.mu /= 2;
  2286. if (m_vehicle.Type != Vehicle.TYPE_NONE && actor.PhysicsActorType != (int) ActorTypes.Agent)
  2287. contact.surface.mu *= 0.05f;
  2288. }
  2289. }
  2290. #endregion
  2291. #region Mass Calculation
  2292. private float CalculatePrimVolume()
  2293. {
  2294. float volume = _size.X*_size.Y*_size.Z; // default
  2295. float tmp;
  2296. float hollowAmount = _pbs.ProfileHollow*2.0e-5f;
  2297. float hollowVolume = hollowAmount*hollowAmount;
  2298. switch (_pbs.ProfileShape)
  2299. {
  2300. case ProfileShape.Square:
  2301. // default box
  2302. if (_pbs.PathCurve == (byte) Extrusion.Straight)
  2303. {
  2304. if (hollowAmount > 0.0)
  2305. {
  2306. switch (_pbs.HollowShape)
  2307. {
  2308. case HollowShape.Square:
  2309. case HollowShape.Same:
  2310. break;
  2311. case HollowShape.Circle:
  2312. hollowVolume *= 0.78539816339f;
  2313. break;
  2314. case HollowShape.Triangle:
  2315. hollowVolume *= (0.5f*.5f);
  2316. break;
  2317. default:
  2318. hollowVolume = 0;
  2319. break;
  2320. }
  2321. volume *= (1.0f - hollowVolume);
  2322. }
  2323. }
  2324. else if (_pbs.PathCurve == (byte) Extrusion.Curve1)
  2325. {
  2326. //a tube
  2327. volume *= 0.78539816339e-2f*(200 - _pbs.PathScaleX);
  2328. tmp = 1.0f - 2.0e-2f*(200 - _pbs.PathScaleY);
  2329. volume -= volume*tmp*tmp;
  2330. if (hollowAmount > 0.0)
  2331. {
  2332. hollowVolume *= hollowAmount;
  2333. switch (_pbs.HollowShape)
  2334. {
  2335. case HollowShape.Square:
  2336. case HollowShape.Same:
  2337. break;
  2338. case HollowShape.Circle:
  2339. hollowVolume *= 0.78539816339f;
  2340. break;
  2341. case HollowShape.Triangle:
  2342. hollowVolume *= 0.5f*0.5f;
  2343. break;
  2344. default:
  2345. hollowVolume = 0;
  2346. break;
  2347. }
  2348. volume *= (1.0f - hollowVolume);
  2349. }
  2350. }
  2351. break;
  2352. case ProfileShape.Circle:
  2353. if (_pbs.PathCurve == (byte) Extrusion.Straight)
  2354. {
  2355. volume *= 0.78539816339f; // elipse base
  2356. if (hollowAmount > 0.0)
  2357. {
  2358. switch (_pbs.HollowShape)
  2359. {
  2360. case HollowShape.Same:
  2361. case HollowShape.Circle:
  2362. break;
  2363. case HollowShape.Square:
  2364. hollowVolume *= 0.5f*2.5984480504799f;
  2365. break;
  2366. case HollowShape.Triangle:
  2367. hollowVolume *= .5f*1.27323954473516f;
  2368. break;
  2369. default:
  2370. hollowVolume = 0;
  2371. break;
  2372. }
  2373. volume *= (1.0f - hollowVolume);
  2374. }
  2375. }
  2376. else if (_pbs.PathCurve == (byte) Extrusion.Curve1)
  2377. {
  2378. volume *= 0.61685027506808491367715568749226e-2f*(200 - _pbs.PathScaleX);
  2379. tmp = 1.0f - .02f*(200 - _pbs.PathScaleY);
  2380. volume *= (1.0f - tmp*tmp);
  2381. if (hollowAmount > 0.0)
  2382. {
  2383. // calculate the hollow volume by it's shape compared to the prim shape
  2384. hollowVolume *= hollowAmount;
  2385. switch (_pbs.HollowShape)
  2386. {
  2387. case HollowShape.Same:
  2388. case HollowShape.Circle:
  2389. break;
  2390. case HollowShape.Square:
  2391. hollowVolume *= 0.5f*2.5984480504799f;
  2392. break;
  2393. case HollowShape.Triangle:
  2394. hollowVolume *= .5f*1.27323954473516f;
  2395. break;
  2396. default:
  2397. hollowVolume = 0;
  2398. break;
  2399. }
  2400. volume *= (1.0f - hollowVolume);
  2401. }
  2402. }
  2403. break;
  2404. case ProfileShape.HalfCircle:
  2405. if (_pbs.PathCurve == (byte) Extrusion.Curve1)
  2406. {
  2407. volume *= 0.52359877559829887307710723054658f;
  2408. }
  2409. break;
  2410. case ProfileShape.EquilateralTriangle:
  2411. if (_pbs.PathCurve == (byte) Extrusion.Straight)
  2412. {
  2413. volume *= 0.32475953f;
  2414. if (hollowAmount > 0.0)
  2415. {
  2416. // calculate the hollow volume by it's shape compared to the prim shape
  2417. switch (_pbs.HollowShape)
  2418. {
  2419. case HollowShape.Same:
  2420. case HollowShape.Triangle:
  2421. hollowVolume *= .25f;
  2422. break;
  2423. case HollowShape.Square:
  2424. hollowVolume *= 0.499849f*3.07920140172638f;
  2425. break;
  2426. case HollowShape.Circle:
  2427. // Hollow shape is a perfect cyllinder in respect to the cube's scale
  2428. // Cyllinder hollow volume calculation
  2429. hollowVolume *= 0.1963495f*3.07920140172638f;
  2430. break;
  2431. default:
  2432. hollowVolume = 0;
  2433. break;
  2434. }
  2435. volume *= (1.0f - hollowVolume);
  2436. }
  2437. }
  2438. else if (_pbs.PathCurve == (byte) Extrusion.Curve1)
  2439. {
  2440. volume *= 0.32475953f;
  2441. volume *= 0.01f*(200 - _pbs.PathScaleX);
  2442. tmp = 1.0f - .02f*(200 - _pbs.PathScaleY);
  2443. volume *= (1.0f - tmp*tmp);
  2444. if (hollowAmount > 0.0)
  2445. {
  2446. hollowVolume *= hollowAmount;
  2447. switch (_pbs.HollowShape)
  2448. {
  2449. case HollowShape.Same:
  2450. case HollowShape.Triangle:
  2451. hollowVolume *= .25f;
  2452. break;
  2453. case HollowShape.Square:
  2454. hollowVolume *= 0.499849f*3.07920140172638f;
  2455. break;
  2456. case HollowShape.Circle:
  2457. hollowVolume *= 0.1963495f*3.07920140172638f;
  2458. break;
  2459. default:
  2460. hollowVolume = 0;
  2461. break;
  2462. }
  2463. volume *= (1.0f - hollowVolume);
  2464. }
  2465. }
  2466. break;
  2467. default:
  2468. break;
  2469. }
  2470. float taperX1;
  2471. float taperY1;
  2472. float taperX;
  2473. float taperY;
  2474. float pathBegin;
  2475. float pathEnd;
  2476. float profileBegin;
  2477. float profileEnd;
  2478. if (_pbs.PathCurve == (byte) Extrusion.Straight || _pbs.PathCurve == (byte) Extrusion.Flexible)
  2479. {
  2480. taperX1 = _pbs.PathScaleX*0.01f;
  2481. if (taperX1 > 1.0f)
  2482. taperX1 = 2.0f - taperX1;
  2483. taperX = 1.0f - taperX1;
  2484. taperY1 = _pbs.PathScaleY*0.01f;
  2485. if (taperY1 > 1.0f)
  2486. taperY1 = 2.0f - taperY1;
  2487. taperY = 1.0f - taperY1;
  2488. }
  2489. else
  2490. {
  2491. taperX = _pbs.PathTaperX*0.01f;
  2492. if (taperX < 0.0f)
  2493. taperX = -taperX;
  2494. taperX1 = 1.0f - taperX;
  2495. taperY = _pbs.PathTaperY*0.01f;
  2496. if (taperY < 0.0f)
  2497. taperY = -taperY;
  2498. taperY1 = 1.0f - taperY;
  2499. }
  2500. volume *= (taperX1*taperY1 + 0.5f*(taperX1*taperY + taperX*taperY1) + 0.3333333333f*taperX*taperY);
  2501. pathBegin = _pbs.PathBegin*2.0e-5f;
  2502. pathEnd = 1.0f - _pbs.PathEnd*2.0e-5f;
  2503. volume *= (pathEnd - pathBegin);
  2504. // this is crude aproximation
  2505. profileBegin = _pbs.ProfileBegin*2.0e-5f;
  2506. profileEnd = 1.0f - _pbs.ProfileEnd*2.0e-5f;
  2507. volume *= (profileEnd - profileBegin);
  2508. return volume;
  2509. }
  2510. // end CalculateMass
  2511. public void CalcPrimBodyData()
  2512. {
  2513. if (prim_geom == IntPtr.Zero)
  2514. {
  2515. // Ubit let's have a initial basic OOB
  2516. primOOBsize.X = _size.X;
  2517. primOOBsize.Y = _size.Y;
  2518. primOOBsize.Z = _size.Z;
  2519. primOOBoffset = Vector3.Zero;
  2520. }
  2521. else
  2522. {
  2523. d.AABB AABB;
  2524. d.GeomGetAABB(prim_geom, out AABB); // get the AABB from engine geom
  2525. primOOBsize.X = (AABB.MaxX - AABB.MinX);
  2526. primOOBsize.Y = (AABB.MaxY - AABB.MinY);
  2527. primOOBsize.Z = (AABB.MaxZ - AABB.MinZ);
  2528. if (!hasOOBoffsetFromMesh)
  2529. {
  2530. primOOBoffset.X = (AABB.MaxX + AABB.MinX)*0.5f;
  2531. primOOBoffset.Y = (AABB.MaxY + AABB.MinY)*0.5f;
  2532. primOOBoffset.Z = (AABB.MaxZ + AABB.MinZ)*0.5f;
  2533. }
  2534. }
  2535. // also its own inertia and mass
  2536. // keep using basic shape mass for now
  2537. float volume = CalculatePrimVolume();
  2538. primMass = _parent_entity.Density*volume*0.01f; //Divide by 100 as its a bit high for ODE....
  2539. if (primMass <= 0)
  2540. primMass = 0.0001f; //ckrinke: Mass must be greater then zero.
  2541. // else if (returnMass > _parent_scene.maximumMassObject)
  2542. // returnMass = _parent_scene.maximumMassObject;
  2543. if (primMass > _parent_scene.maximumMassObject)
  2544. primMass = _parent_scene.maximumMassObject;
  2545. _mass = primMass;
  2546. d.MassSetBoxTotal(out primdMass, primMass, primOOBsize.X, primOOBsize.Y, primOOBsize.Z);
  2547. d.MassTranslate(ref primdMass,
  2548. primOOBoffset.X,
  2549. primOOBoffset.Y,
  2550. primOOBoffset.Z);
  2551. primOOBsize *= 0.5f; // let obb size be a corner coords
  2552. }
  2553. #endregion
  2554. #region Nested type: strVehicleBoolParam
  2555. private struct strVehicleBoolParam
  2556. {
  2557. public int param;
  2558. public bool value;
  2559. }
  2560. #endregion
  2561. #region Nested type: strVehicleFloatParam
  2562. private struct strVehicleFloatParam
  2563. {
  2564. public int param;
  2565. public float value;
  2566. }
  2567. #endregion
  2568. #region Nested type: strVehicleQuartParam
  2569. private struct strVehicleQuartParam
  2570. {
  2571. public int param;
  2572. public Quaternion value;
  2573. }
  2574. #endregion
  2575. #region Nested type: strVehicleVectorParam
  2576. private struct strVehicleVectorParam
  2577. {
  2578. public int param;
  2579. public Vector3 value;
  2580. }
  2581. #endregion
  2582. }
  2583. }