2012-07-26 21:54:57 +00:00
|
|
|
|
/*
|
|
|
|
|
* Copyright (c) Contributors, http://opensimulator.org/
|
|
|
|
|
* See CONTRIBUTORS.TXT for a full list of copyright holders.
|
|
|
|
|
*
|
|
|
|
|
* Redistribution and use in source and binary forms, with or without
|
|
|
|
|
* modification, are permitted provided that the following conditions are met:
|
|
|
|
|
* * Redistributions of source code must retain the above copyright
|
|
|
|
|
* notice, this list of conditions and the following disclaimer.
|
|
|
|
|
* * Redistributions in binary form must reproduce the above copyrightD
|
|
|
|
|
* notice, this list of conditions and the following disclaimer in the
|
|
|
|
|
* documentation and/or other materials provided with the distribution.
|
|
|
|
|
* * Neither the name of the OpenSimulator Project nor the
|
|
|
|
|
* names of its contributors may be used to endorse or promote products
|
|
|
|
|
* derived from this software without specific prior written permission.
|
|
|
|
|
*
|
|
|
|
|
* THIS SOFTWARE IS PROVIDED BY THE DEVELOPERS ``AS IS'' AND ANY
|
|
|
|
|
* EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
|
|
|
|
|
* WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
|
|
|
|
|
* DISCLAIMED. IN NO EVENT SHALL THE CONTRIBUTORS BE LIABLE FOR ANY
|
|
|
|
|
* DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
|
|
|
|
|
* (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
|
|
|
|
|
* LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
|
|
|
|
|
* ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
|
|
|
|
|
* (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
|
|
|
|
|
* SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
|
|
|
|
|
*/
|
|
|
|
|
using System;
|
|
|
|
|
using System.Collections.Generic;
|
|
|
|
|
using System.Text;
|
|
|
|
|
|
|
|
|
|
using OMV = OpenMetaverse;
|
|
|
|
|
|
|
|
|
|
namespace OpenSim.Region.Physics.BulletSPlugin
|
|
|
|
|
{
|
|
|
|
|
public class BSLinkset
|
|
|
|
|
{
|
2012-10-01 15:27:26 +00:00
|
|
|
|
// private static string LogHeader = "[BULLETSIM LINKSET]";
|
2012-07-26 21:54:57 +00:00
|
|
|
|
|
2012-09-18 15:39:52 +00:00
|
|
|
|
public BSPhysObject LinksetRoot { get; protected set; }
|
2012-07-26 21:54:57 +00:00
|
|
|
|
|
2012-09-18 15:39:52 +00:00
|
|
|
|
public BSScene PhysicsScene { get; private set; }
|
2012-07-26 21:54:57 +00:00
|
|
|
|
|
2012-08-15 23:27:30 +00:00
|
|
|
|
static int m_nextLinksetID = 1;
|
|
|
|
|
public int LinksetID { get; private set; }
|
|
|
|
|
|
2012-09-28 05:00:02 +00:00
|
|
|
|
// The children under the root in this linkset.
|
|
|
|
|
// There are two lists of children: the current children at runtime
|
|
|
|
|
// and the children at taint-time. For instance, if you delink a
|
|
|
|
|
// child from the linkset, the child is removed from m_children
|
|
|
|
|
// but the constraint won't be removed until taint time.
|
|
|
|
|
// Two lists lets this track the 'current' children and
|
|
|
|
|
// the physical 'taint' children separately.
|
|
|
|
|
// After taint processing and before the simulation step, these
|
|
|
|
|
// two lists must be the same.
|
2012-10-05 22:33:17 +00:00
|
|
|
|
private HashSet<BSPhysObject> m_children;
|
|
|
|
|
private HashSet<BSPhysObject> m_taintChildren;
|
2012-07-26 21:54:57 +00:00
|
|
|
|
|
|
|
|
|
// We lock the diddling of linkset classes to prevent any badness.
|
|
|
|
|
// This locks the modification of the instances of this class. Changes
|
|
|
|
|
// to the physical representation is done via the tainting mechenism.
|
|
|
|
|
private object m_linksetActivityLock = new Object();
|
|
|
|
|
|
|
|
|
|
// We keep the prim's mass in the linkset structure since it could be dependent on other prims
|
|
|
|
|
private float m_mass;
|
2012-09-27 16:31:33 +00:00
|
|
|
|
public float LinksetMass
|
|
|
|
|
{
|
|
|
|
|
get
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
|
|
|
|
m_mass = ComputeLinksetMass();
|
|
|
|
|
return m_mass;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public OMV.Vector3 CenterOfMass
|
|
|
|
|
{
|
|
|
|
|
get { return ComputeLinksetCenterOfMass(); }
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
public OMV.Vector3 GeometricCenter
|
|
|
|
|
{
|
|
|
|
|
get { return ComputeLinksetGeometricCenter(); }
|
|
|
|
|
}
|
|
|
|
|
|
2012-08-24 19:58:42 +00:00
|
|
|
|
public BSLinkset(BSScene scene, BSPhysObject parent)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
|
|
|
|
// A simple linkset of one (no children)
|
2012-08-15 23:27:30 +00:00
|
|
|
|
LinksetID = m_nextLinksetID++;
|
|
|
|
|
// We create LOTS of linksets.
|
2012-09-27 16:31:33 +00:00
|
|
|
|
if (m_nextLinksetID <= 0)
|
2012-08-15 23:27:30 +00:00
|
|
|
|
m_nextLinksetID = 1;
|
2012-09-18 15:39:52 +00:00
|
|
|
|
PhysicsScene = scene;
|
|
|
|
|
LinksetRoot = parent;
|
2012-10-05 22:33:17 +00:00
|
|
|
|
m_children = new HashSet<BSPhysObject>();
|
|
|
|
|
m_taintChildren = new HashSet<BSPhysObject>();
|
2012-07-26 21:54:57 +00:00
|
|
|
|
m_mass = parent.MassRaw;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Link to a linkset where the child knows the parent.
|
|
|
|
|
// Parent changing should not happen so do some sanity checking.
|
2012-08-10 23:22:44 +00:00
|
|
|
|
// We return the parent's linkset so the child can track its membership.
|
2012-09-28 02:57:35 +00:00
|
|
|
|
// Called at runtime.
|
2012-08-24 19:58:42 +00:00
|
|
|
|
public BSLinkset AddMeToLinkset(BSPhysObject child)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
|
|
|
|
lock (m_linksetActivityLock)
|
|
|
|
|
{
|
2012-09-27 15:23:29 +00:00
|
|
|
|
// Don't add the root to its own linkset
|
|
|
|
|
if (!IsRoot(child))
|
|
|
|
|
AddChildToLinkset(child);
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
2012-08-10 23:22:44 +00:00
|
|
|
|
return this;
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
|
2012-08-15 18:36:21 +00:00
|
|
|
|
// Remove a child from a linkset.
|
|
|
|
|
// Returns a new linkset for the child which is a linkset of one (just the
|
|
|
|
|
// orphened child).
|
2012-09-28 02:57:35 +00:00
|
|
|
|
// Called at runtime.
|
2012-08-24 19:58:42 +00:00
|
|
|
|
public BSLinkset RemoveMeFromLinkset(BSPhysObject child)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
|
|
|
|
lock (m_linksetActivityLock)
|
|
|
|
|
{
|
|
|
|
|
if (IsRoot(child))
|
2012-09-27 15:23:29 +00:00
|
|
|
|
{
|
|
|
|
|
// Cannot remove the root from a linkset.
|
|
|
|
|
return this;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
RemoveChildFromLinkset(child);
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// The child is down to a linkset of just itself
|
2012-08-15 18:36:21 +00:00
|
|
|
|
return new BSLinkset(PhysicsScene, child);
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Return 'true' if the passed object is the root object of this linkset
|
2012-08-24 19:58:42 +00:00
|
|
|
|
public bool IsRoot(BSPhysObject requestor)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
2012-09-18 15:39:52 +00:00
|
|
|
|
return (requestor.LocalID == LinksetRoot.LocalID);
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
|
2012-08-10 23:22:44 +00:00
|
|
|
|
public int NumberOfChildren { get { return m_children.Count; } }
|
|
|
|
|
|
2012-07-26 21:54:57 +00:00
|
|
|
|
// Return 'true' if this linkset has any children (more than the root member)
|
|
|
|
|
public bool HasAnyChildren { get { return (m_children.Count > 0); } }
|
|
|
|
|
|
|
|
|
|
// Return 'true' if this child is in this linkset
|
2012-08-24 19:58:42 +00:00
|
|
|
|
public bool HasChild(BSPhysObject child)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
|
|
|
|
bool ret = false;
|
2012-08-15 18:36:21 +00:00
|
|
|
|
lock (m_linksetActivityLock)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
2012-08-24 19:58:42 +00:00
|
|
|
|
foreach (BSPhysObject bp in m_children)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
2012-08-15 18:36:21 +00:00
|
|
|
|
if (child.LocalID == bp.LocalID)
|
|
|
|
|
{
|
|
|
|
|
ret = true;
|
|
|
|
|
break;
|
|
|
|
|
}
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
return ret;
|
|
|
|
|
}
|
|
|
|
|
|
2012-10-10 15:02:37 +00:00
|
|
|
|
// When physical properties are changed the linkset needs to recalculate
|
|
|
|
|
// its internal properties.
|
|
|
|
|
// May be called at runtime or taint-time (just pass the appropriate flag).
|
|
|
|
|
public void Refresh(BSPhysObject requestor, bool inTaintTime)
|
|
|
|
|
{
|
2012-10-10 20:56:16 +00:00
|
|
|
|
// If there are no children, not physical or not root, I am not the one that recomputes the constraints
|
|
|
|
|
// (For the moment, static linksets do create constraints so remove the test for physical.)
|
|
|
|
|
if (!HasAnyChildren || /*!requestor.IsPhysical ||*/ !IsRoot(requestor))
|
2012-10-10 15:02:37 +00:00
|
|
|
|
return;
|
|
|
|
|
|
|
|
|
|
BSScene.TaintCallback refreshOperation = delegate()
|
|
|
|
|
{
|
|
|
|
|
RecomputeLinksetConstraintVariables();
|
|
|
|
|
DetailLog("{0},BSLinkset.Refresh,complete,rBody={1}",
|
|
|
|
|
LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
|
|
|
|
|
};
|
|
|
|
|
if (inTaintTime)
|
|
|
|
|
refreshOperation();
|
|
|
|
|
else
|
|
|
|
|
PhysicsScene.TaintedObject("BSLinkSet.Refresh", refreshOperation);
|
|
|
|
|
}
|
|
|
|
|
|
2012-09-28 02:57:35 +00:00
|
|
|
|
// The object is going dynamic (physical). Do any setup necessary
|
|
|
|
|
// for a dynamic linkset.
|
|
|
|
|
// Only the state of the passed object can be modified. The rest of the linkset
|
|
|
|
|
// has not yet been fully constructed.
|
|
|
|
|
// Return 'true' if any properties updated on the passed object.
|
|
|
|
|
// Called at taint-time!
|
|
|
|
|
public bool MakeDynamic(BSPhysObject child)
|
|
|
|
|
{
|
|
|
|
|
// What is done for each object in BSPrim is what we want.
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// The object is going static (non-physical). Do any setup necessary
|
|
|
|
|
// for a static linkset.
|
|
|
|
|
// Return 'true' if any properties updated on the passed object.
|
|
|
|
|
// Called at taint-time!
|
|
|
|
|
public bool MakeStatic(BSPhysObject child)
|
|
|
|
|
{
|
|
|
|
|
// What is done for each object in BSPrim is what we want.
|
|
|
|
|
return false;
|
|
|
|
|
}
|
|
|
|
|
|
2012-10-10 15:02:37 +00:00
|
|
|
|
// If the software is handling the movement of all the objects in a linkset
|
|
|
|
|
// (like if one doesn't use constraints for static linksets), this is called
|
|
|
|
|
// when an update for the root of the linkset is received.
|
|
|
|
|
// Called at taint-time!!
|
|
|
|
|
public void UpdateProperties(BSPhysObject physObject)
|
2012-09-28 02:57:35 +00:00
|
|
|
|
{
|
2012-10-10 15:02:37 +00:00
|
|
|
|
// The root local properties have been updated. Apply to the children if appropriate.
|
|
|
|
|
if (IsRoot(physObject) && HasAnyChildren)
|
2012-09-28 02:57:35 +00:00
|
|
|
|
{
|
2012-10-10 15:02:37 +00:00
|
|
|
|
if (!physObject.IsPhysical)
|
2012-09-28 02:57:35 +00:00
|
|
|
|
{
|
2012-10-10 15:02:37 +00:00
|
|
|
|
// TODO: implement software linkset update for static object linksets
|
|
|
|
|
}
|
2012-09-28 02:57:35 +00:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Routine used when rebuilding the body of the root of the linkset
|
|
|
|
|
// Destroy all the constraints have have been made to root.
|
|
|
|
|
// This is called when the root body is changing.
|
2012-09-28 05:00:02 +00:00
|
|
|
|
// Returns 'true' of something eas actually removed and would need restoring
|
2012-09-28 02:57:35 +00:00
|
|
|
|
// Called at taint-time!!
|
2012-09-28 05:00:02 +00:00
|
|
|
|
public bool RemoveBodyDependencies(BSPrim child)
|
2012-09-28 02:57:35 +00:00
|
|
|
|
{
|
2012-09-28 05:00:02 +00:00
|
|
|
|
bool ret = false;
|
|
|
|
|
|
2012-09-28 02:57:35 +00:00
|
|
|
|
lock (m_linksetActivityLock)
|
|
|
|
|
{
|
|
|
|
|
if (IsRoot(child))
|
|
|
|
|
{
|
|
|
|
|
// If the one with the dependency is root, must undo all children
|
2012-10-05 22:33:17 +00:00
|
|
|
|
DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
|
|
|
|
|
child.LocalID, LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
|
|
|
|
|
|
|
|
|
|
ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
|
2012-09-28 02:57:35 +00:00
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
|
2012-09-28 05:00:02 +00:00
|
|
|
|
child.LocalID,
|
2012-09-28 02:57:35 +00:00
|
|
|
|
LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
|
|
|
|
|
child.LocalID, child.BSBody.ptr.ToString("X"));
|
2012-10-05 22:33:17 +00:00
|
|
|
|
// ret = PhysicallyUnlinkAChildFromRoot(LinksetRoot, child);
|
|
|
|
|
// Despite the function name, this removes any link to the specified object.
|
|
|
|
|
ret = PhysicallyUnlinkAllChildrenFromRoot(child);
|
2012-09-28 02:57:35 +00:00
|
|
|
|
}
|
|
|
|
|
}
|
2012-09-28 05:00:02 +00:00
|
|
|
|
return ret;
|
2012-09-28 02:57:35 +00:00
|
|
|
|
}
|
|
|
|
|
|
2012-10-10 15:02:37 +00:00
|
|
|
|
// Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
|
|
|
|
|
// this routine will restore the removed constraints.
|
2012-09-28 02:57:35 +00:00
|
|
|
|
// Called at taint-time!!
|
|
|
|
|
public void RestoreBodyDependencies(BSPrim child)
|
|
|
|
|
{
|
|
|
|
|
lock (m_linksetActivityLock)
|
|
|
|
|
{
|
|
|
|
|
if (IsRoot(child))
|
|
|
|
|
{
|
|
|
|
|
DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreChildrenForRoot,rID={1},numChild={2}",
|
2012-09-28 05:00:02 +00:00
|
|
|
|
child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
|
|
|
|
|
foreach (BSPhysObject bpo in m_taintChildren)
|
2012-09-28 02:57:35 +00:00
|
|
|
|
{
|
2012-10-05 22:33:17 +00:00
|
|
|
|
PhysicallyLinkAChildToRoot(LinksetRoot, bpo);
|
2012-09-28 02:57:35 +00:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
|
2012-09-28 05:00:02 +00:00
|
|
|
|
LinksetRoot.LocalID,
|
2012-09-28 02:57:35 +00:00
|
|
|
|
LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
|
|
|
|
|
child.LocalID, child.BSBody.ptr.ToString("X"));
|
2012-10-05 22:33:17 +00:00
|
|
|
|
PhysicallyLinkAChildToRoot(LinksetRoot, child);
|
2012-09-28 02:57:35 +00:00
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// ================================================================
|
|
|
|
|
// Below this point is internal magic
|
|
|
|
|
|
2012-07-26 21:54:57 +00:00
|
|
|
|
private float ComputeLinksetMass()
|
|
|
|
|
{
|
2012-09-18 18:26:19 +00:00
|
|
|
|
float mass;
|
|
|
|
|
lock (m_linksetActivityLock)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
2012-09-18 18:26:19 +00:00
|
|
|
|
mass = LinksetRoot.MassRaw;
|
2012-09-28 05:00:02 +00:00
|
|
|
|
foreach (BSPhysObject bp in m_taintChildren)
|
2012-09-18 18:26:19 +00:00
|
|
|
|
{
|
|
|
|
|
mass += bp.MassRaw;
|
|
|
|
|
}
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
return mass;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
private OMV.Vector3 ComputeLinksetCenterOfMass()
|
|
|
|
|
{
|
2012-09-18 18:26:19 +00:00
|
|
|
|
OMV.Vector3 com;
|
2012-08-15 18:36:21 +00:00
|
|
|
|
lock (m_linksetActivityLock)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
2012-09-18 18:26:19 +00:00
|
|
|
|
com = LinksetRoot.Position * LinksetRoot.MassRaw;
|
|
|
|
|
float totalMass = LinksetRoot.MassRaw;
|
|
|
|
|
|
2012-09-28 05:00:02 +00:00
|
|
|
|
foreach (BSPhysObject bp in m_taintChildren)
|
2012-08-15 18:36:21 +00:00
|
|
|
|
{
|
|
|
|
|
com += bp.Position * bp.MassRaw;
|
|
|
|
|
totalMass += bp.MassRaw;
|
|
|
|
|
}
|
|
|
|
|
if (totalMass != 0f)
|
|
|
|
|
com /= totalMass;
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return com;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
private OMV.Vector3 ComputeLinksetGeometricCenter()
|
|
|
|
|
{
|
2012-09-18 18:26:19 +00:00
|
|
|
|
OMV.Vector3 com;
|
2012-08-15 18:36:21 +00:00
|
|
|
|
lock (m_linksetActivityLock)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
2012-09-18 18:26:19 +00:00
|
|
|
|
com = LinksetRoot.Position;
|
|
|
|
|
|
2012-09-28 05:00:02 +00:00
|
|
|
|
foreach (BSPhysObject bp in m_taintChildren)
|
2012-08-15 18:36:21 +00:00
|
|
|
|
{
|
|
|
|
|
com += bp.Position * bp.MassRaw;
|
|
|
|
|
}
|
2012-09-28 05:00:02 +00:00
|
|
|
|
com /= (m_taintChildren.Count + 1);
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
return com;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// I am the root of a linkset and a new child is being added
|
2012-08-10 15:33:09 +00:00
|
|
|
|
// Called while LinkActivity is locked.
|
2012-08-24 19:58:42 +00:00
|
|
|
|
private void AddChildToLinkset(BSPhysObject child)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
|
|
|
|
if (!HasChild(child))
|
|
|
|
|
{
|
|
|
|
|
m_children.Add(child);
|
|
|
|
|
|
2012-10-05 22:33:17 +00:00
|
|
|
|
BSPhysObject rootx = LinksetRoot; // capture the root as of now
|
2012-08-24 19:58:42 +00:00
|
|
|
|
BSPhysObject childx = child;
|
2012-09-27 15:23:29 +00:00
|
|
|
|
|
2012-10-05 22:33:17 +00:00
|
|
|
|
DetailLog("{0},AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
|
2012-09-28 02:57:35 +00:00
|
|
|
|
|
2012-09-18 15:39:52 +00:00
|
|
|
|
PhysicsScene.TaintedObject("AddChildToLinkset", delegate()
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
2012-10-05 22:33:17 +00:00
|
|
|
|
DetailLog("{0},AddChildToLinkset,taint,rID={1},rBody={2},cID={3},cBody={4}",
|
|
|
|
|
rootx.LocalID,
|
|
|
|
|
rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
|
|
|
|
|
childx.LocalID, childx.BSBody.ptr.ToString("X"));
|
2012-10-02 17:52:51 +00:00
|
|
|
|
// Since this is taint-time, the body and shape could have changed for the child
|
2012-10-05 22:33:17 +00:00
|
|
|
|
rootx.ForcePosition = rootx.Position; // DEBUG
|
|
|
|
|
childx.ForcePosition = childx.Position; // DEBUG
|
|
|
|
|
PhysicallyLinkAChildToRoot(rootx, childx);
|
|
|
|
|
m_taintChildren.Add(child);
|
2012-07-26 21:54:57 +00:00
|
|
|
|
});
|
|
|
|
|
}
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
2012-08-09 22:01:05 +00:00
|
|
|
|
// Forcefully removing a child from a linkset.
|
|
|
|
|
// This is not being called by the child so we have to make sure the child doesn't think
|
|
|
|
|
// it's still connected to the linkset.
|
|
|
|
|
// Normal OpenSimulator operation will never do this because other SceneObjectPart information
|
2012-09-27 15:23:29 +00:00
|
|
|
|
// also has to be updated (like pointer to prim's parent).
|
2012-08-24 19:58:42 +00:00
|
|
|
|
private void RemoveChildFromOtherLinkset(BSPhysObject pchild)
|
2012-08-09 22:01:05 +00:00
|
|
|
|
{
|
2012-09-18 15:39:52 +00:00
|
|
|
|
pchild.Linkset = new BSLinkset(PhysicsScene, pchild);
|
2012-08-09 22:01:05 +00:00
|
|
|
|
RemoveChildFromLinkset(pchild);
|
|
|
|
|
}
|
|
|
|
|
|
2012-07-26 21:54:57 +00:00
|
|
|
|
// I am the root of a linkset and one of my children is being removed.
|
|
|
|
|
// Safe to call even if the child is not really in my linkset.
|
2012-08-24 19:58:42 +00:00
|
|
|
|
private void RemoveChildFromLinkset(BSPhysObject child)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
|
|
|
|
if (m_children.Remove(child))
|
|
|
|
|
{
|
2012-09-27 15:23:29 +00:00
|
|
|
|
BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
|
2012-08-24 19:58:42 +00:00
|
|
|
|
BSPhysObject childx = child;
|
2012-09-28 02:57:35 +00:00
|
|
|
|
|
2012-09-28 05:00:02 +00:00
|
|
|
|
DetailLog("{0},RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
|
|
|
|
|
childx.LocalID,
|
2012-10-02 17:52:51 +00:00
|
|
|
|
rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
|
|
|
|
|
childx.LocalID, childx.BSBody.ptr.ToString("X"));
|
2012-09-28 02:57:35 +00:00
|
|
|
|
|
2012-09-18 15:39:52 +00:00
|
|
|
|
PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
2012-10-05 22:33:17 +00:00
|
|
|
|
m_taintChildren.Remove(child);
|
|
|
|
|
PhysicallyUnlinkAChildFromRoot(rootx, childx);
|
2012-09-13 15:11:54 +00:00
|
|
|
|
RecomputeLinksetConstraintVariables();
|
2012-07-26 21:54:57 +00:00
|
|
|
|
});
|
2012-08-15 18:36:21 +00:00
|
|
|
|
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
// This will happen if we remove the root of the linkset first. Non-fatal occurance.
|
2012-08-15 18:36:21 +00:00
|
|
|
|
// PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Create a constraint between me (root of linkset) and the passed prim (the child).
|
|
|
|
|
// Called at taint time!
|
2012-10-05 22:33:17 +00:00
|
|
|
|
private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
|
|
|
|
// Zero motion for children so they don't interpolate
|
|
|
|
|
childPrim.ZeroMotion();
|
|
|
|
|
|
2012-08-15 18:36:21 +00:00
|
|
|
|
// Relative position normalized to the root prim
|
|
|
|
|
// Essentually a vector pointing from center of rootPrim to center of childPrim
|
|
|
|
|
OMV.Vector3 childRelativePosition = childPrim.Position - rootPrim.Position;
|
|
|
|
|
|
|
|
|
|
// real world coordinate of midpoint between the two objects
|
|
|
|
|
OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
|
|
|
|
|
|
2012-10-05 22:33:17 +00:00
|
|
|
|
DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
|
2012-09-27 16:31:33 +00:00
|
|
|
|
rootPrim.LocalID,
|
2012-10-05 22:33:17 +00:00
|
|
|
|
rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
|
|
|
|
|
childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"),
|
2012-09-27 15:23:29 +00:00
|
|
|
|
rootPrim.Position, childPrim.Position, midPoint);
|
|
|
|
|
|
2012-09-28 02:57:35 +00:00
|
|
|
|
// create a constraint that allows no freedom of movement between the two objects
|
|
|
|
|
// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
|
|
|
|
|
|
2012-08-15 18:36:21 +00:00
|
|
|
|
BS6DofConstraint constrain = new BS6DofConstraint(
|
2012-10-05 22:33:17 +00:00
|
|
|
|
PhysicsScene.World, rootPrim.BSBody, childPrim.BSBody, midPoint, true, true );
|
2012-09-28 02:57:35 +00:00
|
|
|
|
|
2012-08-17 20:30:46 +00:00
|
|
|
|
/* NOTE: below is an attempt to build constraint with full frame computation, etc.
|
2012-09-27 15:23:29 +00:00
|
|
|
|
* Using the midpoint is easier since it lets the Bullet code manipulate the transforms
|
2012-08-15 18:36:21 +00:00
|
|
|
|
* of the objects.
|
2012-08-17 20:30:46 +00:00
|
|
|
|
* Code left as a warning to future programmers.
|
2012-08-15 18:36:21 +00:00
|
|
|
|
// ==================================================================================
|
2012-07-26 21:54:57 +00:00
|
|
|
|
// relative position normalized to the root prim
|
2012-08-10 15:33:09 +00:00
|
|
|
|
OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
|
|
|
|
|
OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation;
|
2012-07-26 21:54:57 +00:00
|
|
|
|
|
|
|
|
|
// relative rotation of the child to the parent
|
|
|
|
|
OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
|
2012-08-15 18:36:21 +00:00
|
|
|
|
OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
|
2012-07-26 21:54:57 +00:00
|
|
|
|
|
|
|
|
|
// create a constraint that allows no freedom of movement between the two objects
|
|
|
|
|
// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
|
2012-10-05 22:33:17 +00:00
|
|
|
|
DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
|
2012-08-09 22:01:05 +00:00
|
|
|
|
BS6DofConstraint constrain = new BS6DofConstraint(
|
2012-08-15 18:36:21 +00:00
|
|
|
|
PhysicsScene.World, rootPrim.Body, childPrim.Body,
|
2012-07-31 16:23:05 +00:00
|
|
|
|
OMV.Vector3.Zero,
|
2012-08-15 18:36:21 +00:00
|
|
|
|
OMV.Quaternion.Inverse(rootPrim.Orientation),
|
|
|
|
|
OMV.Vector3.Zero,
|
|
|
|
|
OMV.Quaternion.Inverse(childPrim.Orientation),
|
|
|
|
|
// A point half way between the parent and child
|
|
|
|
|
// childRelativePosition/2,
|
|
|
|
|
// childRelativeRotation,
|
|
|
|
|
// childRelativePosition/2,
|
|
|
|
|
// inverseChildRelativeRotation,
|
|
|
|
|
true,
|
|
|
|
|
true
|
2012-07-31 16:23:05 +00:00
|
|
|
|
);
|
2012-08-15 18:36:21 +00:00
|
|
|
|
// ==================================================================================
|
|
|
|
|
*/
|
|
|
|
|
|
2012-09-18 15:39:52 +00:00
|
|
|
|
PhysicsScene.Constraints.AddConstraint(constrain);
|
2012-08-08 20:48:49 +00:00
|
|
|
|
|
|
|
|
|
// zero linear and angular limits makes the objects unable to move in relation to each other
|
2012-07-26 21:54:57 +00:00
|
|
|
|
constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
|
|
|
|
|
constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
|
|
|
|
|
|
|
|
|
|
// tweek the constraint to increase stability
|
2012-08-15 18:36:21 +00:00
|
|
|
|
constrain.UseFrameOffset(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintUseFrameOffset));
|
|
|
|
|
constrain.TranslationalLimitMotor(PhysicsScene.BoolNumeric(PhysicsScene.Params.linkConstraintEnableTransMotor),
|
|
|
|
|
PhysicsScene.Params.linkConstraintTransMotorMaxVel,
|
|
|
|
|
PhysicsScene.Params.linkConstraintTransMotorMaxForce);
|
|
|
|
|
constrain.SetCFMAndERP(PhysicsScene.Params.linkConstraintCFM, PhysicsScene.Params.linkConstraintERP);
|
2012-09-13 15:11:54 +00:00
|
|
|
|
if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
|
|
|
|
|
{
|
|
|
|
|
constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
|
|
|
|
|
}
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// Remove linkage between myself and a particular child
|
2012-09-27 15:23:29 +00:00
|
|
|
|
// The root and child bodies are passed in because we need to remove the constraint between
|
|
|
|
|
// the bodies that were at unlink time.
|
2012-07-26 21:54:57 +00:00
|
|
|
|
// Called at taint time!
|
2012-10-05 22:33:17 +00:00
|
|
|
|
private bool PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
2012-10-05 22:33:17 +00:00
|
|
|
|
bool ret = false;
|
|
|
|
|
DetailLog("{0},BSLinkset.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
|
2012-09-27 16:31:33 +00:00
|
|
|
|
rootPrim.LocalID,
|
2012-10-05 22:33:17 +00:00
|
|
|
|
rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
|
|
|
|
|
childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"));
|
2012-08-09 22:01:05 +00:00
|
|
|
|
|
2012-08-15 18:36:21 +00:00
|
|
|
|
// Find the constraint for this link and get rid of it from the overall collection and from my list
|
2012-10-05 22:33:17 +00:00
|
|
|
|
if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody, childPrim.BSBody))
|
|
|
|
|
{
|
|
|
|
|
// Make the child refresh its location
|
|
|
|
|
BulletSimAPI.PushUpdate2(childPrim.BSBody.ptr);
|
|
|
|
|
ret = true;
|
|
|
|
|
}
|
2012-08-15 18:36:21 +00:00
|
|
|
|
|
2012-10-05 22:33:17 +00:00
|
|
|
|
return ret;
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
|
2012-09-27 15:23:29 +00:00
|
|
|
|
// Remove linkage between myself and any possible children I might have.
|
2012-07-26 21:54:57 +00:00
|
|
|
|
// Called at taint time!
|
2012-10-05 22:33:17 +00:00
|
|
|
|
private bool PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
|
2012-07-26 21:54:57 +00:00
|
|
|
|
{
|
2012-10-05 22:33:17 +00:00
|
|
|
|
DetailLog("{0},BSLinkset.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
|
|
|
|
|
bool ret = false;
|
2012-08-09 22:01:05 +00:00
|
|
|
|
|
2012-10-05 22:33:17 +00:00
|
|
|
|
if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody))
|
|
|
|
|
{
|
|
|
|
|
ret = true;
|
|
|
|
|
}
|
|
|
|
|
return ret;
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
|
2012-09-28 05:00:02 +00:00
|
|
|
|
// Call each of the constraints that make up this linkset and recompute the
|
|
|
|
|
// various transforms and variables. Used when objects are added or removed
|
|
|
|
|
// from a linkset to make sure the constraints know about the new mass and
|
|
|
|
|
// geometry.
|
|
|
|
|
// Must only be called at taint time!!
|
|
|
|
|
private void RecomputeLinksetConstraintVariables()
|
|
|
|
|
{
|
|
|
|
|
float linksetMass = LinksetMass;
|
|
|
|
|
foreach (BSPhysObject child in m_taintChildren)
|
|
|
|
|
{
|
|
|
|
|
BSConstraint constrain;
|
|
|
|
|
if (PhysicsScene.Constraints.TryGetConstraint(LinksetRoot.BSBody, child.BSBody, out constrain))
|
|
|
|
|
{
|
|
|
|
|
// DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,taint,child={1},mass={2},A={3},B={4}",
|
|
|
|
|
// LinksetRoot.LocalID, child.LocalID, linksetMass, constrain.Body1.ID, constrain.Body2.ID);
|
|
|
|
|
constrain.RecomputeConstraintVariables(linksetMass);
|
|
|
|
|
}
|
|
|
|
|
else
|
|
|
|
|
{
|
|
|
|
|
// Non-fatal error that happens when children are being added to the linkset but
|
|
|
|
|
// their constraints have not been created yet.
|
|
|
|
|
break;
|
|
|
|
|
}
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
// If the whole linkset is not here, doesn't make sense to recompute linkset wide values
|
|
|
|
|
if (m_children.Count == m_taintChildren.Count)
|
|
|
|
|
{
|
|
|
|
|
// If this is a multiple object linkset, set everybody's center of mass to the set's center of mass
|
|
|
|
|
OMV.Vector3 centerOfMass = ComputeLinksetCenterOfMass();
|
2012-10-05 22:33:17 +00:00
|
|
|
|
BulletSimAPI.SetCenterOfMassByPosRot2(LinksetRoot.BSBody.ptr,
|
|
|
|
|
centerOfMass, OMV.Quaternion.Identity);
|
|
|
|
|
DetailLog("{0},BSLinkset.RecomputeLinksetConstraintVariables,setCenterOfMass,COM={1},rBody={2}",
|
|
|
|
|
LinksetRoot.LocalID, centerOfMass, LinksetRoot.BSBody.ptr.ToString("X"));
|
2012-09-28 05:00:02 +00:00
|
|
|
|
foreach (BSPhysObject child in m_taintChildren)
|
|
|
|
|
{
|
2012-10-05 22:33:17 +00:00
|
|
|
|
BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr,
|
|
|
|
|
centerOfMass, OMV.Quaternion.Identity);
|
2012-09-28 05:00:02 +00:00
|
|
|
|
}
|
2012-10-05 22:33:17 +00:00
|
|
|
|
|
|
|
|
|
// BulletSimAPI.DumpAllInfo2(PhysicsScene.World.ptr); // DEBUG DEBUG DEBUG
|
2012-09-28 05:00:02 +00:00
|
|
|
|
}
|
|
|
|
|
return;
|
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
|
2012-07-26 21:54:57 +00:00
|
|
|
|
// Invoke the detailed logger and output something if it's enabled.
|
|
|
|
|
private void DetailLog(string msg, params Object[] args)
|
|
|
|
|
{
|
2012-10-05 22:33:17 +00:00
|
|
|
|
if (PhysicsScene.PhysicsLogging.Enabled)
|
|
|
|
|
PhysicsScene.DetailLog(msg, args);
|
2012-07-26 21:54:57 +00:00
|
|
|
|
}
|
|
|
|
|
|
|
|
|
|
}
|
|
|
|
|
}
|