OpenSim
/
OpenSimMirror
1
0
Fork 0
Code Issues Pull Requests Releases Wiki Activity

BulletSim: Create LinkSet abstract class and sparate constraint based linksets into own subclass. Will eventually add manual movement linkset subclass.

integration
Robert Adams 2012-10-21 16:12:06 -07:00
parent 319ebaca06
commit a61f20ac74
3 changed files with 434 additions and 319 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

355
OpenSim/Region/Physics/BulletSPlugin/BSLinkset.cs
View File

@ -32,10 +32,27 @@ using OMV = OpenMetaverse;
namespace OpenSim.Region.Physics.BulletSPlugin
{
public class BSLinkset
public abstract class BSLinkset
{
// private static string LogHeader = "[BULLETSIM LINKSET]";
// Create the correct type of linkset for this child
public static BSLinkset Factory(BSScene physScene, BSPhysObject parent)
{
BSLinkset ret = null;
/*
if (parent.IsPhysical)
ret = new BSLinksetConstraints(physScene, parent);
else
ret = new BSLinksetManual(physScene, parent);
*/
// at the moment, there is only one
ret = new BSLinksetConstraints(physScene, parent);
return ret;
}
public BSPhysObject LinksetRoot { get; protected set; }
public BSScene PhysicsScene { get; private set; }
@ -52,16 +69,16 @@ public class BSLinkset
// the physical 'taint' children separately.
// After taint processing and before the simulation step, these
// two lists must be the same.
private HashSet<BSPhysObject> m_children;
private HashSet<BSPhysObject> m_taintChildren;
protected HashSet<BSPhysObject> m_children;
protected HashSet<BSPhysObject> m_taintChildren;
// 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();
protected 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;
protected float m_mass;
public float LinksetMass
{
get
@ -81,7 +98,7 @@ public class BSLinkset
get { return ComputeLinksetGeometricCenter(); }
}
public BSLinkset(BSScene scene, BSPhysObject parent)
protected void Initialize(BSScene scene, BSPhysObject parent)
{
// A simple linkset of one (no children)
LinksetID = m_nextLinksetID++;
@ -128,7 +145,7 @@ public class BSLinkset
}
// The child is down to a linkset of just itself
return new BSLinkset(PhysicsScene, child);
return BSLinkset.Factory(PhysicsScene, child);
}
// Return 'true' if the passed object is the root object of this linkset
@ -163,24 +180,7 @@ public class BSLinkset
// 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)
{
// 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))
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);
}
public abstract void Refresh(BSPhysObject requestor, bool inTaintTime);
// The object is going dynamic (physical). Do any setup necessary
// for a dynamic linkset.
@ -188,102 +188,36 @@ public class BSLinkset
// 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;
}
public abstract bool MakeDynamic(BSPhysObject child);
// 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;
}
public abstract bool MakeStatic(BSPhysObject child);
// 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)
{
// The root local properties have been updated. Apply to the children if appropriate.
if (IsRoot(physObject) && HasAnyChildren)
{
if (!physObject.IsPhysical)
{
// TODO: implement software linkset update for static object linksets
}
}
}
public abstract void UpdateProperties(BSPhysObject physObject);
// 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.
// Returns 'true' of something eas actually removed and would need restoring
// Called at taint-time!!
public bool RemoveBodyDependencies(BSPrim child)
{
bool ret = false;
lock (m_linksetActivityLock)
{
if (IsRoot(child))
{
// If the one with the dependency is root, must undo all children
DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
child.LocalID, LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
}
else
{
DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
child.LocalID,
LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
child.LocalID, child.BSBody.ptr.ToString("X"));
// ret = PhysicallyUnlinkAChildFromRoot(LinksetRoot, child);
// Despite the function name, this removes any link to the specified object.
ret = PhysicallyUnlinkAllChildrenFromRoot(child);
}
}
return ret;
}
public abstract bool RemoveBodyDependencies(BSPrim child);
// Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
// this routine will restore the removed constraints.
// Called at taint-time!!
public void RestoreBodyDependencies(BSPrim child)
{
lock (m_linksetActivityLock)
{
if (IsRoot(child))
{
DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreChildrenForRoot,rID={1},numChild={2}",
child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
foreach (BSPhysObject bpo in m_taintChildren)
{
PhysicallyLinkAChildToRoot(LinksetRoot, bpo);
}
}
else
{
DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
LinksetRoot.LocalID,
LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
child.LocalID, child.BSBody.ptr.ToString("X"));
PhysicallyLinkAChildToRoot(LinksetRoot, child);
}
}
}
public abstract void RestoreBodyDependencies(BSPrim child);
// ================================================================
// Below this point is internal magic
private float ComputeLinksetMass()
protected virtual float ComputeLinksetMass()
{
float mass;
lock (m_linksetActivityLock)
@ -297,7 +231,7 @@ public class BSLinkset
return mass;
}
private OMV.Vector3 ComputeLinksetCenterOfMass()
protected virtual OMV.Vector3 ComputeLinksetCenterOfMass()
{
OMV.Vector3 com;
lock (m_linksetActivityLock)
@ -317,7 +251,7 @@ public class BSLinkset
return com;
}
private OMV.Vector3 ComputeLinksetGeometricCenter()
protected virtual OMV.Vector3 ComputeLinksetGeometricCenter()
{
OMV.Vector3 com;
lock (m_linksetActivityLock)
@ -336,236 +270,21 @@ public class BSLinkset
// I am the root of a linkset and a new child is being added
// Called while LinkActivity is locked.
private void AddChildToLinkset(BSPhysObject child)
{
if (!HasChild(child))
{
m_children.Add(child);
BSPhysObject rootx = LinksetRoot; // capture the root as of now
BSPhysObject childx = child;
DetailLog("{0},AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
PhysicsScene.TaintedObject("AddChildToLinkset", delegate()
{
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"));
// Since this is taint-time, the body and shape could have changed for the child
rootx.ForcePosition = rootx.Position; // DEBUG
childx.ForcePosition = childx.Position; // DEBUG
PhysicallyLinkAChildToRoot(rootx, childx);
m_taintChildren.Add(child);
});
}
return;
}
protected abstract void AddChildToLinkset(BSPhysObject child);
// 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
// also has to be updated (like pointer to prim's parent).
private void RemoveChildFromOtherLinkset(BSPhysObject pchild)
{
pchild.Linkset = new BSLinkset(PhysicsScene, pchild);
RemoveChildFromLinkset(pchild);
}
protected abstract void RemoveChildFromOtherLinkset(BSPhysObject pchild);
// 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.
private void RemoveChildFromLinkset(BSPhysObject child)
{
if (m_children.Remove(child))
{
BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
BSPhysObject childx = child;
DetailLog("{0},RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
childx.LocalID,
rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
childx.LocalID, childx.BSBody.ptr.ToString("X"));
PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
{
m_taintChildren.Remove(child);
PhysicallyUnlinkAChildFromRoot(rootx, childx);
RecomputeLinksetConstraintVariables();
});
}
else
{
// This will happen if we remove the root of the linkset first. Non-fatal occurance.
// PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
}
return;
}
// Create a constraint between me (root of linkset) and the passed prim (the child).
// Called at taint time!
private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
{
// Zero motion for children so they don't interpolate
childPrim.ZeroMotion();
// 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);
DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
rootPrim.LocalID,
rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"),
rootPrim.Position, childPrim.Position, midPoint);
// create a constraint that allows no freedom of movement between the two objects
// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
BS6DofConstraint constrain = new BS6DofConstraint(
PhysicsScene.World, rootPrim.BSBody, childPrim.BSBody, midPoint, true, true );
/* NOTE: below is an attempt to build constraint with full frame computation, etc.
* Using the midpoint is easier since it lets the Bullet code manipulate the transforms
* of the objects.
* Code left as a warning to future programmers.
// ==================================================================================
// relative position normalized to the root prim
OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation;
// relative rotation of the child to the parent
OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
// create a constraint that allows no freedom of movement between the two objects
// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
BS6DofConstraint constrain = new BS6DofConstraint(
PhysicsScene.World, rootPrim.Body, childPrim.Body,
OMV.Vector3.Zero,
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
);
// ==================================================================================
*/
PhysicsScene.Constraints.AddConstraint(constrain);
// zero linear and angular limits makes the objects unable to move in relation to each other
constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
// tweek the constraint to increase stability
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);
if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
{
constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
}
}
// Remove linkage between myself and a particular child
// The root and child bodies are passed in because we need to remove the constraint between
// the bodies that were at unlink time.
// Called at taint time!
private bool PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
{
bool ret = false;
DetailLog("{0},BSLinkset.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
rootPrim.LocalID,
rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"));
// Find the constraint for this link and get rid of it from the overall collection and from my list
if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody, childPrim.BSBody))
{
// Make the child refresh its location
BulletSimAPI.PushUpdate2(childPrim.BSBody.ptr);
ret = true;
}
return ret;
}
// Remove linkage between myself and any possible children I might have.
// Called at taint time!
private bool PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
{
DetailLog("{0},BSLinkset.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
bool ret = false;
if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody))
{
ret = true;
}
return ret;
}
// 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();
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"));
foreach (BSPhysObject child in m_taintChildren)
{
BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr,
centerOfMass, OMV.Quaternion.Identity);
}
// BulletSimAPI.DumpAllInfo2(PhysicsScene.World.ptr); // DEBUG DEBUG DEBUG
}
return;
}
protected abstract void RemoveChildFromLinkset(BSPhysObject child);
// Invoke the detailed logger and output something if it's enabled.
private void DetailLog(string msg, params Object[] args)
protected void DetailLog(string msg, params Object[] args)
{
if (PhysicsScene.PhysicsLogging.Enabled)
PhysicsScene.DetailLog(msg, args);

396
OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs Executable file
View File

@ -0,0 +1,396 @@
/*
* 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 BSLinksetConstraints : BSLinkset
{
// private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
public BSLinksetConstraints(BSScene scene, BSPhysObject parent)
{
base.Initialize(scene, parent);
}
// 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 override void Refresh(BSPhysObject requestor, bool inTaintTime)
{
// 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))
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);
}
// 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 override 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 override bool MakeStatic(BSPhysObject child)
{
// What is done for each object in BSPrim is what we want.
return false;
}
// 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 override void UpdateProperties(BSPhysObject physObject)
{
// The root local properties have been updated. Apply to the children if appropriate.
if (IsRoot(physObject) && HasAnyChildren)
{
if (!physObject.IsPhysical)
{
// TODO: implement software linkset update for static object linksets
}
}
}
// 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.
// Returns 'true' of something eas actually removed and would need restoring
// Called at taint-time!!
public override bool RemoveBodyDependencies(BSPrim child)
{
bool ret = false;
lock (m_linksetActivityLock)
{
if (IsRoot(child))
{
// If the one with the dependency is root, must undo all children
DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeChildrenForRoot,rID={1},rBody={2}",
child.LocalID, LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"));
ret = PhysicallyUnlinkAllChildrenFromRoot(LinksetRoot);
}
else
{
DetailLog("{0},BSLinkset.RemoveBodyDependencies,removeSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
child.LocalID,
LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
child.LocalID, child.BSBody.ptr.ToString("X"));
// ret = PhysicallyUnlinkAChildFromRoot(LinksetRoot, child);
// Despite the function name, this removes any link to the specified object.
ret = PhysicallyUnlinkAllChildrenFromRoot(child);
}
}
return ret;
}
// Companion to RemoveBodyDependencies(). If RemoveBodyDependencies() returns 'true',
// this routine will restore the removed constraints.
// Called at taint-time!!
public override void RestoreBodyDependencies(BSPrim child)
{
lock (m_linksetActivityLock)
{
if (IsRoot(child))
{
DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreChildrenForRoot,rID={1},numChild={2}",
child.LocalID, LinksetRoot.LocalID, m_taintChildren.Count);
foreach (BSPhysObject bpo in m_taintChildren)
{
PhysicallyLinkAChildToRoot(LinksetRoot, bpo);
}
}
else
{
DetailLog("{0},BSLinkset.RestoreBodyDependencies,restoreSingleChild,rID={1},rBody={2},cID={3},cBody={4}",
LinksetRoot.LocalID,
LinksetRoot.LocalID, LinksetRoot.BSBody.ptr.ToString("X"),
child.LocalID, child.BSBody.ptr.ToString("X"));
PhysicallyLinkAChildToRoot(LinksetRoot, child);
}
}
}
// ================================================================
// Below this point is internal magic
// I am the root of a linkset and a new child is being added
// Called while LinkActivity is locked.
protected override void AddChildToLinkset(BSPhysObject child)
{
if (!HasChild(child))
{
m_children.Add(child);
BSPhysObject rootx = LinksetRoot; // capture the root as of now
BSPhysObject childx = child;
DetailLog("{0},AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
PhysicsScene.TaintedObject("AddChildToLinkset", delegate()
{
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"));
// Since this is taint-time, the body and shape could have changed for the child
rootx.ForcePosition = rootx.Position; // DEBUG
childx.ForcePosition = childx.Position; // DEBUG
PhysicallyLinkAChildToRoot(rootx, childx);
m_taintChildren.Add(child);
});
}
return;
}
// 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
// also has to be updated (like pointer to prim's parent).
protected override void RemoveChildFromOtherLinkset(BSPhysObject pchild)
{
pchild.Linkset = BSLinkset.Factory(PhysicsScene, pchild);
RemoveChildFromLinkset(pchild);
}
// 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.
protected override void RemoveChildFromLinkset(BSPhysObject child)
{
if (m_children.Remove(child))
{
BSPhysObject rootx = LinksetRoot; // capture the root and body as of now
BSPhysObject childx = child;
DetailLog("{0},RemoveChildFromLinkset,call,rID={1},rBody={2},cID={3},cBody={4}",
childx.LocalID,
rootx.LocalID, rootx.BSBody.ptr.ToString("X"),
childx.LocalID, childx.BSBody.ptr.ToString("X"));
PhysicsScene.TaintedObject("RemoveChildFromLinkset", delegate()
{
m_taintChildren.Remove(child);
PhysicallyUnlinkAChildFromRoot(rootx, childx);
RecomputeLinksetConstraintVariables();
});
}
else
{
// This will happen if we remove the root of the linkset first. Non-fatal occurance.
// PhysicsScene.Logger.ErrorFormat("{0}: Asked to remove child from linkset that was not in linkset", LogHeader);
}
return;
}
// Create a constraint between me (root of linkset) and the passed prim (the child).
// Called at taint time!
private void PhysicallyLinkAChildToRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
{
// Zero motion for children so they don't interpolate
childPrim.ZeroMotion();
// 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);
DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
rootPrim.LocalID,
rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"),
rootPrim.Position, childPrim.Position, midPoint);
// create a constraint that allows no freedom of movement between the two objects
// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
BS6DofConstraint constrain = new BS6DofConstraint(
PhysicsScene.World, rootPrim.BSBody, childPrim.BSBody, midPoint, true, true );
/* NOTE: below is an attempt to build constraint with full frame computation, etc.
* Using the midpoint is easier since it lets the Bullet code manipulate the transforms
* of the objects.
* Code left as a warning to future programmers.
// ==================================================================================
// relative position normalized to the root prim
OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
OMV.Vector3 childRelativePosition = (childPrim.Position - rootPrim.Position) * invThisOrientation;
// relative rotation of the child to the parent
OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
// create a constraint that allows no freedom of movement between the two objects
// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
DetailLog("{0},BSLinkset.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, childPrim.LocalID);
BS6DofConstraint constrain = new BS6DofConstraint(
PhysicsScene.World, rootPrim.Body, childPrim.Body,
OMV.Vector3.Zero,
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
);
// ==================================================================================
*/
PhysicsScene.Constraints.AddConstraint(constrain);
// zero linear and angular limits makes the objects unable to move in relation to each other
constrain.SetLinearLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
constrain.SetAngularLimits(OMV.Vector3.Zero, OMV.Vector3.Zero);
// tweek the constraint to increase stability
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);
if (PhysicsScene.Params.linkConstraintSolverIterations != 0f)
{
constrain.SetSolverIterations(PhysicsScene.Params.linkConstraintSolverIterations);
}
}
// Remove linkage between myself and a particular child
// The root and child bodies are passed in because we need to remove the constraint between
// the bodies that were at unlink time.
// Called at taint time!
private bool PhysicallyUnlinkAChildFromRoot(BSPhysObject rootPrim, BSPhysObject childPrim)
{
bool ret = false;
DetailLog("{0},BSLinkset.PhysicallyUnlinkAChildFromRoot,taint,root={1},rBody={2},child={3},cBody={4}",
rootPrim.LocalID,
rootPrim.LocalID, rootPrim.BSBody.ptr.ToString("X"),
childPrim.LocalID, childPrim.BSBody.ptr.ToString("X"));
// Find the constraint for this link and get rid of it from the overall collection and from my list
if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody, childPrim.BSBody))
{
// Make the child refresh its location
BulletSimAPI.PushUpdate2(childPrim.BSBody.ptr);
ret = true;
}
return ret;
}
// Remove linkage between myself and any possible children I might have.
// Called at taint time!
private bool PhysicallyUnlinkAllChildrenFromRoot(BSPhysObject rootPrim)
{
DetailLog("{0},BSLinkset.PhysicallyUnlinkAllChildren,taint", rootPrim.LocalID);
bool ret = false;
if (PhysicsScene.Constraints.RemoveAndDestroyConstraint(rootPrim.BSBody))
{
ret = true;
}
return ret;
}
// 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();
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"));
foreach (BSPhysObject child in m_taintChildren)
{
BulletSimAPI.SetCenterOfMassByPosRot2(child.BSBody.ptr,
centerOfMass, OMV.Quaternion.Identity);
}
// BulletSimAPI.DumpAllInfo2(PhysicsScene.World.ptr); // DEBUG DEBUG DEBUG
}
return;
}
}
}

2
OpenSim/Region/Physics/BulletSPlugin/BSPhysObject.cs
View File

@ -46,7 +46,7 @@ public abstract class BSPhysObject : PhysicsActor
PhysObjectName = name;
TypeName = typeName;
Linkset = new BSLinkset(PhysicsScene, this);
Linkset = BSLinkset.Factory(PhysicsScene, this);
LastAssetBuildFailed = false;
CollisionCollection = new CollisionEventUpdate();