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

BulletSim: add BSLinkInfo structure to remember link specific information 

for each link in a linkset.
Extend BSLinksetConstraint to create and use BSLinkInfo with the default
static constraint.
TeleportWork
Robert Adams 2013-08-01 12:35:22 -07:00
parent 87ee0c395e
commit 5bcccfc305
3 changed files with 167 additions and 79 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -70,6 +70,15 @@ public abstract class BSLinkset
return ret;
}
public class BSLinkInfo
{
public BSPrimLinkable member;
public BSLinkInfo(BSPrimLinkable pMember)
{
member = pMember;
}
}
public BSPrimLinkable LinksetRoot { get; protected set; }
protected BSScene m_physicsScene { get; private set; }
@ -78,7 +87,8 @@ public abstract class BSLinkset
public int LinksetID { get; private set; }
// The children under the root in this linkset.
protected HashSet<BSPrimLinkable> m_children;
// protected HashSet<BSPrimLinkable> m_children;
protected Dictionary<BSPrimLinkable, BSLinkInfo> m_children;
// We lock the diddling of linkset classes to prevent any badness.
// This locks the modification of the instances of this class. Changes
@ -109,7 +119,7 @@ public abstract class BSLinkset
m_nextLinksetID = 1;
m_physicsScene = scene;
LinksetRoot = parent;
m_children = new HashSet<BSPrimLinkable>();
m_children = new Dictionary<BSPrimLinkable, BSLinkInfo>();
LinksetMass = parent.RawMass;
Rebuilding = false;
@ -170,17 +180,7 @@ public abstract class BSLinkset
bool ret = false;
lock (m_linksetActivityLock)
{
ret = m_children.Contains(child);
/* Safer version but the above should work
foreach (BSPrimLinkable bp in m_children)
{
if (child.LocalID == bp.LocalID)
{
ret = true;
break;
}
}
*/
ret = m_children.ContainsKey(child);
}
return ret;
}
@ -194,7 +194,24 @@ public abstract class BSLinkset
lock (m_linksetActivityLock)
{
action(LinksetRoot);
foreach (BSPrimLinkable po in m_children)
foreach (BSPrimLinkable po in m_children.Keys)
{
if (action(po))
break;
}
}
return ret;
}
// Perform an action on each member of the linkset including root prim.
// Depends on the action on whether this should be done at taint time.
public delegate bool ForEachLinkInfoAction(BSLinkInfo obj);
public virtual bool ForEachLinkInfo(ForEachLinkInfoAction action)
{
bool ret = false;
lock (m_linksetActivityLock)
{
foreach (BSLinkInfo po in m_children.Values)
{
if (action(po))
break;
@ -364,7 +381,7 @@ public abstract class BSLinkset
{
lock (m_linksetActivityLock)
{
foreach (BSPrimLinkable bp in m_children)
foreach (BSPrimLinkable bp in m_children.Keys)
{
mass += bp.RawMass;
}
@ -382,7 +399,7 @@ public abstract class BSLinkset
com = LinksetRoot.Position * LinksetRoot.RawMass;
float totalMass = LinksetRoot.RawMass;
foreach (BSPrimLinkable bp in m_children)
foreach (BSPrimLinkable bp in m_children.Keys)
{
com += bp.Position * bp.RawMass;
totalMass += bp.RawMass;
@ -401,7 +418,7 @@ public abstract class BSLinkset
{
com = LinksetRoot.Position;
foreach (BSPrimLinkable bp in m_children)
foreach (BSPrimLinkable bp in m_children.Keys)
{
com += bp.Position;
}

4
OpenSim/Region/Physics/BulletSPlugin/BSLinksetCompound.cs
View File

@ -257,7 +257,7 @@ public sealed class BSLinksetCompound : BSLinkset
{
if (!HasChild(child))
{
m_children.Add(child);
m_children.Add(child, new BSLinkInfo(child));
DetailLog("{0},BSLinksetCompound.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
@ -353,7 +353,7 @@ public sealed class BSLinksetCompound : BSLinkset
// Add the shapes of all the components of the linkset
int memberIndex = 1;
ForEachMember(delegate(BSPrimLinkable cPrim)
ForEachMember((cPrim) =>
{
if (IsRoot(cPrim))
{

191
OpenSim/Region/Physics/BulletSPlugin/BSLinksetConstraints.cs
View File

@ -36,6 +36,75 @@ public sealed class BSLinksetConstraints : BSLinkset
{
// private static string LogHeader = "[BULLETSIM LINKSET CONSTRAINTS]";
public class BSLinkInfoConstraint : BSLinkInfo
{
public ConstraintType constraintType;
public BSConstraint constraint;
public OMV.Vector3 linearLimitLow;
public OMV.Vector3 linearLimitHigh;
public OMV.Vector3 angularLimitLow;
public OMV.Vector3 angularLimitHigh;
public bool useFrameOffset;
public bool enableTransMotor;
public float transMotorMaxVel;
public float transMotorMaxForce;
public float cfm;
public float erp;
public float solverIterations;
public BSLinkInfoConstraint(BSPrimLinkable pMember)
: base(pMember)
{
constraint = null;
ResetToFixedConstraint();
}
// Set all the parameters for this constraint to a fixed, non-movable constraint.
public void ResetToFixedConstraint()
{
constraintType = ConstraintType.D6_CONSTRAINT_TYPE;
linearLimitLow = OMV.Vector3.Zero;
linearLimitHigh = OMV.Vector3.Zero;
angularLimitLow = OMV.Vector3.Zero;
angularLimitHigh = OMV.Vector3.Zero;
useFrameOffset = BSParam.LinkConstraintUseFrameOffset;
enableTransMotor = BSParam.LinkConstraintEnableTransMotor;
transMotorMaxVel = BSParam.LinkConstraintTransMotorMaxVel;
transMotorMaxForce = BSParam.LinkConstraintTransMotorMaxForce;
cfm = BSParam.LinkConstraintCFM;
erp = BSParam.LinkConstraintERP;
solverIterations = BSParam.LinkConstraintSolverIterations;
}
// Given a constraint, apply the current constraint parameters to same.
public void SetConstraintParameters(BSConstraint constrain)
{
switch (constraintType)
{
case ConstraintType.D6_CONSTRAINT_TYPE:
BSConstraint6Dof constrain6dof = constrain as BSConstraint6Dof;
if (constrain6dof != null)
{
// zero linear and angular limits makes the objects unable to move in relation to each other
constrain6dof.SetLinearLimits(linearLimitLow, linearLimitHigh);
constrain6dof.SetAngularLimits(angularLimitLow, angularLimitHigh);
// tweek the constraint to increase stability
constrain6dof.UseFrameOffset(useFrameOffset);
constrain6dof.TranslationalLimitMotor(enableTransMotor, transMotorMaxVel, transMotorMaxForce);
constrain6dof.SetCFMAndERP(cfm, erp);
if (solverIterations != 0f)
{
constrain6dof.SetSolverIterations(solverIterations);
}
}
break;
default:
break;
}
}
}
public BSLinksetConstraints(BSScene scene, BSPrimLinkable parent) : base(scene, parent)
{
}
@ -142,7 +211,7 @@ public sealed class BSLinksetConstraints : BSLinkset
{
if (!HasChild(child))
{
m_children.Add(child);
m_children.Add(child, new BSLinkInfoConstraint(child));
DetailLog("{0},BSLinksetConstraints.AddChildToLinkset,call,child={1}", LinksetRoot.LocalID, child.LocalID);
@ -190,73 +259,74 @@ public sealed class BSLinksetConstraints : BSLinkset
}
// Create a static constraint between the two passed objects
private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSPrimLinkable childPrim)
private BSConstraint BuildConstraint(BSPrimLinkable rootPrim, BSLinkInfo li)
{
BSLinkInfoConstraint liConstraint = li as BSLinkInfoConstraint;
if (liConstraint == null)
return null;
// Zero motion for children so they don't interpolate
childPrim.ZeroMotion(true);
li.member.ZeroMotion(true);
// 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;
BSConstraint constrain = null;
// real world coordinate of midpoint between the two objects
OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
switch (liConstraint.constraintType)
{
case ConstraintType.D6_CONSTRAINT_TYPE:
// Relative position normalized to the root prim
// Essentually a vector pointing from center of rootPrim to center of li.member
OMV.Vector3 childRelativePosition = liConstraint.member.Position - rootPrim.Position;
DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
rootPrim.LocalID,
rootPrim.LocalID, rootPrim.PhysBody.AddrString,
childPrim.LocalID, childPrim.PhysBody.AddrString,
rootPrim.Position, childPrim.Position, midPoint);
// real world coordinate of midpoint between the two objects
OMV.Vector3 midPoint = rootPrim.Position + (childRelativePosition / 2);
// create a constraint that allows no freedom of movement between the two objects
// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
DetailLog("{0},BSLinksetConstraint.BuildConstraint,taint,root={1},rBody={2},child={3},cBody={4},rLoc={5},cLoc={6},midLoc={7}",
rootPrim.LocalID,
rootPrim.LocalID, rootPrim.PhysBody.AddrString,
liConstraint.member.LocalID, liConstraint.member.PhysBody.AddrString,
rootPrim.Position, liConstraint.member.Position, midPoint);
BSConstraint6Dof constrain = new BSConstraint6Dof(
m_physicsScene.World, rootPrim.PhysBody, childPrim.PhysBody, midPoint, true, true );
// PhysicsScene.World, childPrim.BSBody, rootPrim.BSBody, midPoint, true, true );
// create a constraint that allows no freedom of movement between the two objects
// http://bulletphysics.org/Bullet/phpBB3/viewtopic.php?t=4818
/* 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 for 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;
constrain = new BSConstraint6Dof(
m_physicsScene.World, rootPrim.PhysBody, liConstraint.member.PhysBody, midPoint, true, true );
// relative rotation of the child to the parent
OMV.Quaternion childRelativeRotation = invThisOrientation * childPrim.Orientation;
OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
/* 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 for future programmers.
// ==================================================================================
// relative position normalized to the root prim
OMV.Quaternion invThisOrientation = OMV.Quaternion.Inverse(rootPrim.Orientation);
OMV.Vector3 childRelativePosition = (liConstraint.member.Position - rootPrim.Position) * invThisOrientation;
DetailLog("{0},BSLinksetConstraint.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),
true,
true
);
// ==================================================================================
*/
// relative rotation of the child to the parent
OMV.Quaternion childRelativeRotation = invThisOrientation * liConstraint.member.Orientation;
OMV.Quaternion inverseChildRelativeRotation = OMV.Quaternion.Inverse(childRelativeRotation);
DetailLog("{0},BSLinksetConstraint.PhysicallyLinkAChildToRoot,taint,root={1},child={2}", rootPrim.LocalID, rootPrim.LocalID, liConstraint.member.LocalID);
constrain = new BS6DofConstraint(
PhysicsScene.World, rootPrim.Body, liConstraint.member.Body,
OMV.Vector3.Zero,
OMV.Quaternion.Inverse(rootPrim.Orientation),
OMV.Vector3.Zero,
OMV.Quaternion.Inverse(liConstraint.member.Orientation),
true,
true
);
// ==================================================================================
*/
break;
default:
break;
}
liConstraint.SetConstraintParameters(constrain);
m_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(BSParam.LinkConstraintUseFrameOffset);
constrain.TranslationalLimitMotor(BSParam.LinkConstraintEnableTransMotor,
BSParam.LinkConstraintTransMotorMaxVel,
BSParam.LinkConstraintTransMotorMaxForce);
constrain.SetCFMAndERP(BSParam.LinkConstraintCFM, BSParam.LinkConstraintERP);
if (BSParam.LinkConstraintSolverIterations != 0f)
{
constrain.SetSolverIterations(BSParam.LinkConstraintSolverIterations);
}
return constrain;
}
@ -317,23 +387,24 @@ public sealed class BSLinksetConstraints : BSLinkset
return; // Note the 'finally' clause at the botton which will get executed.
}
foreach (BSPrimLinkable child in m_children)
ForEachLinkInfo((li) =>
{
// A child in the linkset physically shows the mass of the whole linkset.
// This allows Bullet to apply enough force on the child to move the whole linkset.
// (Also do the mass stuff before recomputing the constraint so mass is not zero.)
child.UpdatePhysicalMassProperties(linksetMass, true);
li.member.UpdatePhysicalMassProperties(linksetMass, true);
BSConstraint constrain;
if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, child.PhysBody, out constrain))
if (!m_physicsScene.Constraints.TryGetConstraint(LinksetRoot.PhysBody, li.member.PhysBody, out constrain))
{
// If constraint doesn't exist yet, create it.
constrain = BuildConstraint(LinksetRoot, child);
constrain = BuildConstraint(LinksetRoot, li);
}
constrain.RecomputeConstraintVariables(linksetMass);
// PhysicsScene.PE.DumpConstraint(PhysicsScene.World, constrain.Constraint); // DEBUG DEBUG
}
return false; // 'false' says to keep processing other members
});
}
finally
{