1
0

BSLinkset.cs 18 KB

123456789101112131415161718192021222324252627282930313233343536373839404142434445464748495051525354555657585960616263646566676869707172737475767778798081828384858687888990919293949596979899100101102103104105106107108109110111112113114115116117118119120121122123124125126127128129130131132133134135136137138139140141142143144145146147148149150151152153154155156157158159160161162163164165166167168169170171172173174175176177178179180181182183184185186187188189190191192193194195196197198199200201202203204205206207208209210211212213214215216217218219220221222223224225226227228229230231232233234235236237238239240241242243244245246247248249250251252253254255256257258259260261262263264265266267268269270271272273274275276277278279280281282283284285286287288289290291292293294295296297298299300301302303304305306307308309310311312313314315316317318319320321322323324325326327328329330331332333334335336337338339340341342343344345346347348349350351352353354355356357358359360361362363364365366367368369370371372373374375376377378379380381382383384385386387388389390391392393394395396397398399400401402403404405406407408409410411412413414415416417418419420421422423424425426427428429430431432433434435436437438439440441442443444445446447448449450451452453454455456457458459460461462463464465466467468469470471472473474475476477478479480481482483484485486487488489490491492493494495496497498499500501502503
  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 copyrightD
  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.Text;
  30. using OMV = OpenMetaverse;
  31. namespace OpenSim.Region.PhysicsModule.BulletS
  32. {
  33. public abstract class BSLinkset
  34. {
  35. // private static string LogHeader = "[BULLETSIM LINKSET]";
  36. public enum LinksetImplementation
  37. {
  38. Constraint = 0, // linkset tied together with constraints
  39. Compound = 1, // linkset tied together as a compound object
  40. Manual = 2 // linkset tied together manually (code moves all the pieces)
  41. }
  42. // Create the correct type of linkset for this child
  43. public static BSLinkset Factory(BSScene physScene, BSPrimLinkable parent)
  44. {
  45. BSLinkset ret = null;
  46. switch (parent.LinksetType)
  47. {
  48. case LinksetImplementation.Constraint:
  49. ret = new BSLinksetConstraints(physScene, parent);
  50. break;
  51. case LinksetImplementation.Compound:
  52. ret = new BSLinksetCompound(physScene, parent);
  53. break;
  54. case LinksetImplementation.Manual:
  55. // ret = new BSLinksetManual(physScene, parent);
  56. break;
  57. default:
  58. ret = new BSLinksetCompound(physScene, parent);
  59. break;
  60. }
  61. if (ret == null)
  62. {
  63. physScene.Logger.ErrorFormat("[BULLETSIM LINKSET] Factory could not create linkset. Parent name={1}, ID={2}", parent.Name, parent.LocalID);
  64. }
  65. return ret;
  66. }
  67. public class BSLinkInfo
  68. {
  69. public BSPrimLinkable member;
  70. public BSLinkInfo(BSPrimLinkable pMember)
  71. {
  72. member = pMember;
  73. }
  74. public virtual void ResetLink() { }
  75. public virtual void SetLinkParameters(BSConstraint constrain) { }
  76. // Returns 'true' if physical property updates from the child should be reported to the simulator
  77. public virtual bool ShouldUpdateChildProperties() { return false; }
  78. }
  79. public LinksetImplementation LinksetImpl { get; protected set; }
  80. public BSPrimLinkable LinksetRoot { get; protected set; }
  81. protected BSScene m_physicsScene { get; private set; }
  82. static int m_nextLinksetID = 1;
  83. public int LinksetID { get; private set; }
  84. // The children under the root in this linkset.
  85. // protected HashSet<BSPrimLinkable> m_children;
  86. protected Dictionary<BSPrimLinkable, BSLinkInfo> m_children;
  87. // We lock the diddling of linkset classes to prevent any badness.
  88. // This locks the modification of the instances of this class. Changes
  89. // to the physical representation is done via the tainting mechenism.
  90. protected object m_linksetActivityLock = new Object();
  91. // We keep the prim's mass in the linkset structure since it could be dependent on other prims
  92. public float LinksetMass { get; protected set; }
  93. public virtual bool LinksetIsColliding { get { return false; } }
  94. public OMV.Vector3 CenterOfMass
  95. {
  96. get { return ComputeLinksetCenterOfMass(); }
  97. }
  98. public OMV.Vector3 GeometricCenter
  99. {
  100. get { return ComputeLinksetGeometricCenter(); }
  101. }
  102. protected BSLinkset(BSScene scene, BSPrimLinkable parent)
  103. {
  104. // A simple linkset of one (no children)
  105. LinksetID = m_nextLinksetID++;
  106. // We create LOTS of linksets.
  107. if (m_nextLinksetID <= 0)
  108. m_nextLinksetID = 1;
  109. m_physicsScene = scene;
  110. LinksetRoot = parent;
  111. m_children = new Dictionary<BSPrimLinkable, BSLinkInfo>();
  112. LinksetMass = parent.RawMass;
  113. Rebuilding = false;
  114. RebuildScheduled = false;
  115. parent.ClearDisplacement();
  116. }
  117. // Link to a linkset where the child knows the parent.
  118. // Parent changing should not happen so do some sanity checking.
  119. // We return the parent's linkset so the child can track its membership.
  120. // Called at runtime.
  121. public BSLinkset AddMeToLinkset(BSPrimLinkable child)
  122. {
  123. lock (m_linksetActivityLock)
  124. {
  125. // Don't add the root to its own linkset
  126. if (!IsRoot(child))
  127. AddChildToLinkset(child);
  128. LinksetMass = ComputeLinksetMass();
  129. }
  130. return this;
  131. }
  132. // Remove a child from a linkset.
  133. // Returns a new linkset for the child which is a linkset of one (just the
  134. // orphened child).
  135. // Called at runtime.
  136. public BSLinkset RemoveMeFromLinkset(BSPrimLinkable child, bool inTaintTime)
  137. {
  138. lock (m_linksetActivityLock)
  139. {
  140. if (IsRoot(child))
  141. {
  142. // Cannot remove the root from a linkset.
  143. return this;
  144. }
  145. RemoveChildFromLinkset(child, inTaintTime);
  146. LinksetMass = ComputeLinksetMass();
  147. }
  148. // The child is down to a linkset of just itself
  149. return BSLinkset.Factory(m_physicsScene, child);
  150. }
  151. // Return 'true' if the passed object is the root object of this linkset
  152. public bool IsRoot(BSPrimLinkable requestor)
  153. {
  154. return (requestor.LocalID == LinksetRoot.LocalID);
  155. }
  156. public int NumberOfChildren { get { return m_children.Count; } }
  157. // Return 'true' if this linkset has any children (more than the root member)
  158. public bool HasAnyChildren { get { return (m_children.Count > 0); } }
  159. // Return 'true' if this child is in this linkset
  160. public bool HasChild(BSPrimLinkable child)
  161. {
  162. bool ret = false;
  163. lock (m_linksetActivityLock)
  164. {
  165. ret = m_children.ContainsKey(child);
  166. }
  167. return ret;
  168. }
  169. // Perform an action on each member of the linkset including root prim.
  170. // Depends on the action on whether this should be done at taint time.
  171. public delegate bool ForEachMemberAction(BSPrimLinkable obj);
  172. public virtual bool ForEachMember(ForEachMemberAction action)
  173. {
  174. bool ret = false;
  175. lock (m_linksetActivityLock)
  176. {
  177. action(LinksetRoot);
  178. foreach (BSPrimLinkable po in m_children.Keys)
  179. {
  180. if (action(po))
  181. break;
  182. }
  183. }
  184. return ret;
  185. }
  186. public bool TryGetLinkInfo(BSPrimLinkable child, out BSLinkInfo foundInfo)
  187. {
  188. bool ret = false;
  189. BSLinkInfo found = null;
  190. lock (m_linksetActivityLock)
  191. {
  192. ret = m_children.TryGetValue(child, out found);
  193. }
  194. foundInfo = found;
  195. return ret;
  196. }
  197. // Perform an action on each member of the linkset including root prim.
  198. // Depends on the action on whether this should be done at taint time.
  199. public delegate bool ForEachLinkInfoAction(BSLinkInfo obj);
  200. public virtual bool ForEachLinkInfo(ForEachLinkInfoAction action)
  201. {
  202. bool ret = false;
  203. lock (m_linksetActivityLock)
  204. {
  205. foreach (BSLinkInfo po in m_children.Values)
  206. {
  207. if (action(po))
  208. break;
  209. }
  210. }
  211. return ret;
  212. }
  213. // Check the type of the link and return 'true' if the link is flexible and the
  214. // updates from the child should be sent to the simulator so things change.
  215. public virtual bool ShouldReportPropertyUpdates(BSPrimLinkable child)
  216. {
  217. bool ret = false;
  218. BSLinkInfo linkInfo;
  219. if (m_children.TryGetValue(child, out linkInfo))
  220. {
  221. ret = linkInfo.ShouldUpdateChildProperties();
  222. }
  223. return ret;
  224. }
  225. // Called after a simulation step to post a collision with this object.
  226. // Return 'true' if linkset processed the collision. 'false' says the linkset didn't have
  227. // anything to add for the collision and it should be passed through normal processing.
  228. // Default processing for a linkset.
  229. public virtual bool HandleCollide(BSPhysObject collider, BSPhysObject collidee,
  230. OMV.Vector3 contactPoint, OMV.Vector3 contactNormal, float pentrationDepth)
  231. {
  232. bool ret = false;
  233. // prims in the same linkset cannot collide with each other
  234. BSPrimLinkable convCollidee = collidee as BSPrimLinkable;
  235. if (convCollidee != null && (LinksetID == convCollidee.Linkset.LinksetID))
  236. {
  237. // By returning 'true', we tell the caller the collision has been 'handled' so it won't
  238. // do anything about this collision and thus, effectivily, ignoring the collision.
  239. ret = true;
  240. }
  241. else
  242. {
  243. // Not a collision between members of the linkset. Must be a real collision.
  244. // So the linkset root can know if there is a collision anywhere in the linkset.
  245. LinksetRoot.SomeCollisionSimulationStep = m_physicsScene.SimulationStep;
  246. }
  247. return ret;
  248. }
  249. // I am the root of a linkset and a new child is being added
  250. // Called while LinkActivity is locked.
  251. protected abstract void AddChildToLinkset(BSPrimLinkable child);
  252. // I am the root of a linkset and one of my children is being removed.
  253. // Safe to call even if the child is not really in my linkset.
  254. protected abstract void RemoveChildFromLinkset(BSPrimLinkable child, bool inTaintTime);
  255. // When physical properties are changed the linkset needs to recalculate
  256. // its internal properties.
  257. // May be called at runtime or taint-time.
  258. public virtual void Refresh(BSPrimLinkable requestor)
  259. {
  260. LinksetMass = ComputeLinksetMass();
  261. }
  262. // Flag denoting the linkset is in the process of being rebuilt.
  263. // Used to know not the schedule a rebuild in the middle of a rebuild.
  264. // Because of potential update calls that could want to schedule another rebuild.
  265. protected bool Rebuilding { get; set; }
  266. // Flag saying a linkset rebuild has been scheduled.
  267. // This is turned on when the rebuild is requested and turned off when
  268. // the rebuild is complete. Used to limit modifications to the
  269. // linkset parameters while the linkset is in an intermediate state.
  270. // Protected by a "lock(m_linsetActivityLock)" on the BSLinkset object
  271. public bool RebuildScheduled { get; protected set; }
  272. // The object is going dynamic (physical). Do any setup necessary
  273. // for a dynamic linkset.
  274. // Only the state of the passed object can be modified. The rest of the linkset
  275. // has not yet been fully constructed.
  276. // Return 'true' if any properties updated on the passed object.
  277. // Called at taint-time!
  278. public abstract bool MakeDynamic(BSPrimLinkable child);
  279. public virtual bool AllPartsComplete
  280. {
  281. get {
  282. bool ret = true;
  283. this.ForEachMember((member) =>
  284. {
  285. if ((!member.IsInitialized) || member.IsIncomplete || member.PrimAssetState == BSPhysObject.PrimAssetCondition.Waiting)
  286. {
  287. ret = false;
  288. return true; // exit loop
  289. }
  290. return false; // continue loop
  291. });
  292. return ret;
  293. }
  294. }
  295. // The object is going static (non-physical). Do any setup necessary
  296. // for a static linkset.
  297. // Return 'true' if any properties updated on the passed object.
  298. // Called at taint-time!
  299. public abstract bool MakeStatic(BSPrimLinkable child);
  300. // Called when a parameter update comes from the physics engine for any object
  301. // of the linkset is received.
  302. // Passed flag is update came from physics engine (true) or the user (false).
  303. // Called at taint-time!!
  304. public abstract void UpdateProperties(UpdatedProperties whichUpdated, BSPrimLinkable physObject);
  305. // Routine used when rebuilding the body of the root of the linkset
  306. // Destroy all the constraints have have been made to root.
  307. // This is called when the root body is changing.
  308. // Returns 'true' of something was actually removed and would need restoring
  309. // Called at taint-time!!
  310. public abstract bool RemoveDependencies(BSPrimLinkable child);
  311. // ================================================================
  312. // Some physical setting happen to all members of the linkset
  313. public virtual void SetPhysicalFriction(float friction)
  314. {
  315. ForEachMember((member) =>
  316. {
  317. if (member.PhysBody.HasPhysicalBody)
  318. m_physicsScene.PE.SetFriction(member.PhysBody, friction);
  319. return false; // 'false' says to continue looping
  320. }
  321. );
  322. }
  323. public virtual void SetPhysicalRestitution(float restitution)
  324. {
  325. ForEachMember((member) =>
  326. {
  327. if (member.PhysBody.HasPhysicalBody)
  328. m_physicsScene.PE.SetRestitution(member.PhysBody, restitution);
  329. return false; // 'false' says to continue looping
  330. }
  331. );
  332. }
  333. public virtual void SetPhysicalGravity(OMV.Vector3 gravity)
  334. {
  335. ForEachMember((member) =>
  336. {
  337. if (member.PhysBody.HasPhysicalBody)
  338. m_physicsScene.PE.SetGravity(member.PhysBody, gravity);
  339. return false; // 'false' says to continue looping
  340. }
  341. );
  342. }
  343. public virtual void ComputeAndSetLocalInertia(OMV.Vector3 inertiaFactor, float linksetMass)
  344. {
  345. ForEachMember((member) =>
  346. {
  347. if (member.PhysBody.HasPhysicalBody)
  348. {
  349. OMV.Vector3 inertia = m_physicsScene.PE.CalculateLocalInertia(member.PhysShape.physShapeInfo, linksetMass);
  350. member.Inertia = inertia * inertiaFactor;
  351. m_physicsScene.PE.SetMassProps(member.PhysBody, linksetMass, member.Inertia);
  352. m_physicsScene.PE.UpdateInertiaTensor(member.PhysBody);
  353. DetailLog("{0},BSLinkset.ComputeAndSetLocalInertia,m.mass={1}, inertia={2}", member.LocalID, linksetMass, member.Inertia);
  354. }
  355. return false; // 'false' says to continue looping
  356. }
  357. );
  358. }
  359. public virtual void SetPhysicalCollisionFlags(CollisionFlags collFlags)
  360. {
  361. ForEachMember((member) =>
  362. {
  363. if (member.PhysBody.HasPhysicalBody)
  364. m_physicsScene.PE.SetCollisionFlags(member.PhysBody, collFlags);
  365. return false; // 'false' says to continue looping
  366. }
  367. );
  368. }
  369. public virtual void AddToPhysicalCollisionFlags(CollisionFlags collFlags)
  370. {
  371. ForEachMember((member) =>
  372. {
  373. if (member.PhysBody.HasPhysicalBody)
  374. m_physicsScene.PE.AddToCollisionFlags(member.PhysBody, collFlags);
  375. return false; // 'false' says to continue looping
  376. }
  377. );
  378. }
  379. public virtual void RemoveFromPhysicalCollisionFlags(CollisionFlags collFlags)
  380. {
  381. ForEachMember((member) =>
  382. {
  383. if (member.PhysBody.HasPhysicalBody)
  384. m_physicsScene.PE.RemoveFromCollisionFlags(member.PhysBody, collFlags);
  385. return false; // 'false' says to continue looping
  386. }
  387. );
  388. }
  389. // ================================================================
  390. protected virtual float ComputeLinksetMass()
  391. {
  392. float mass = LinksetRoot.RawMass;
  393. if (HasAnyChildren)
  394. {
  395. lock (m_linksetActivityLock)
  396. {
  397. foreach (BSPrimLinkable bp in m_children.Keys)
  398. {
  399. mass += bp.RawMass;
  400. }
  401. }
  402. }
  403. return mass;
  404. }
  405. // Computes linkset's center of mass in world coordinates.
  406. protected virtual OMV.Vector3 ComputeLinksetCenterOfMass()
  407. {
  408. OMV.Vector3 com;
  409. lock (m_linksetActivityLock)
  410. {
  411. com = LinksetRoot.Position * LinksetRoot.RawMass;
  412. float totalMass = LinksetRoot.RawMass;
  413. foreach (BSPrimLinkable bp in m_children.Keys)
  414. {
  415. com += bp.Position * bp.RawMass;
  416. totalMass += bp.RawMass;
  417. }
  418. if (totalMass != 0f)
  419. com /= totalMass;
  420. }
  421. return com;
  422. }
  423. protected virtual OMV.Vector3 ComputeLinksetGeometricCenter()
  424. {
  425. OMV.Vector3 com;
  426. lock (m_linksetActivityLock)
  427. {
  428. com = LinksetRoot.Position;
  429. foreach (BSPrimLinkable bp in m_children.Keys)
  430. {
  431. com += bp.Position;
  432. }
  433. com /= (m_children.Count + 1);
  434. }
  435. return com;
  436. }
  437. #region Extension
  438. public virtual object Extension(string pFunct, params object[] pParams)
  439. {
  440. return null;
  441. }
  442. #endregion // Extension
  443. // Invoke the detailed logger and output something if it's enabled.
  444. protected void DetailLog(string msg, params Object[] args)
  445. {
  446. if (m_physicsScene.PhysicsLogging.Enabled)
  447. m_physicsScene.DetailLog(msg, args);
  448. }
  449. }
  450. }