internal MechanismBase[] GetMechanismsForBody(RigidBodyBase body)
{
ArrayList list = new ArrayList();
foreach (MechanismBase mech in Mechanisms)
{
BindingMechanismBase bmech = mech as BindingMechanismBase;
ForceMechanismBase fmech = mech as ForceMechanismBase;
if (bmech != null)
{
if (bmech.EndpointA.strokeref == body.strokeid ||
bmech.EndpointB.strokeref == body.strokeid)
{
list.Add(bmech);
}
}
else if (fmech != null)
{
if (fmech.Body.strokeref == body.strokeid)
{
list.Add(fmech);
}
}
}
return(list.ToArray(typeof(MechanismBase)) as MechanismBase[]);
}
// Stabilize rods, joints, and ropes.
private void StabilizeBindingMechanisms()
{
foreach (MechanismBase mech in doc.Mechanisms)
{
if (mech is BindingMechanismBase)
{
BindingMechanismBase binder = (BindingMechanismBase)mech;
// Synch the velocity and position of the lighter body to the heavier one.
BodyRef bodyRefToHeavy, bodyRefLight;
if (binder.EndpointA.Object.anchored)
{
bodyRefToHeavy = binder.EndpointA;
bodyRefLight = binder.EndpointB;
}
else if (binder.EndpointB.Object.anchored)
{
bodyRefToHeavy = binder.EndpointB;
bodyRefLight = binder.EndpointA;
}
else if (binder.EndpointA.Object.Mass > binder.EndpointB.Object.Mass)
{
bodyRefToHeavy = binder.EndpointA;
bodyRefLight = binder.EndpointB;
}
else
{
bodyRefToHeavy = binder.EndpointB;
bodyRefLight = binder.EndpointA;
}
// Calculate endpoint mismatch.
Point pointHeavy = bodyRefToHeavy.Object.CG +
Vector.Transform(Vector.FromPoint(bodyRefToHeavy.attachloc), bodyRefToHeavy.Object.displacement);
Point pointLight = bodyRefLight.Object.CG +
Vector.Transform(Vector.FromPoint(bodyRefLight.attachloc), bodyRefLight.Object.displacement);
// Move light object half way to match up correct distance to heavy object.
double distance = Geometry.DistanceBetween(pointHeavy, pointLight);
if (distance > 0)
{
// Get the length you are supposed to be at.
double length;
if (binder is RodMech)
{
length = ((RodMech)binder).length;
}
else if (binder is RopeMech)
{
length = ((RopeMech)binder).length;
// Don't need to do anything if you are shorter than the length.
if (distance < length)
{
continue;
}
}
else
{
continue;
}
// Find the ideal position given that length.
Vector heavyToLight = Vector.FromPoints(pointHeavy, pointLight);
Vector heavyToIdeal = heavyToLight * (length / heavyToLight.Length);
Point idealPosition =
new Point(MathEx.Round(pointHeavy.X + heavyToIdeal.DX),
MathEx.Round(pointHeavy.Y + heavyToIdeal.DY));
Vector moveVector = new Vector((idealPosition.X - pointLight.X) / 2,
(idealPosition.Y - pointLight.Y) / 2);
bodyRefLight.Object.MoveNoStore(moveVector.DX, moveVector.DY, 0);
// Also, move the forces on the body.
foreach (MechanismBase forceMech in doc.GetMechanismsForBody(bodyRefLight.Object))
{
if (forceMech is ForceMechanismBase)
{
forceMech.Move(moveVector.DX, moveVector.DY, 0);
}
}
}
}
}
}
