/// <summary>
/// Adds the block angle dictionary entry.
/// </summary>
/// <param name="pJoint">The p joint.</param>
/// <param name="refJoint">The reference joint.</param>
/// <param name="pIndex">Index of the p.</param>
/// <param name="refIndex">Index of the reference.</param>
private void addBlockAngleDictionaryEntry(Joint pJoint, Joint refJoint, int pIndex, int refIndex)
{
var key = numJoints * pIndex + refIndex;
var result = (pJoint.TypeOfJoint == JointType.G)
? Math.PI / 2
: pJoint.SlideAngleInitial -
Constants.angle(pJoint.xInitial, pJoint.yInitial, refJoint.xInitial, refJoint.yInitial);
angleFromBlockToJoint.Add(key, result);
}
/// <summary>
/// Sets the gear rotation only from setLinkPositionFromRotate
/// </summary>
/// <param name="knownGearLink">The known gear link.</param>
/// <param name="unknownGearLink">The unknown gear link.</param>
/// <param name="links">The links.</param>
/// <param name="joints">The joints.</param>
internal Boolean SetGearRotation(Link knownGearLink, Link unknownGearLink, List <Link> links, List <Joint> joints)
{
if (unknownGearLink.AngleIsKnown == KnownState.Fully)
{
return(false);
}
var rKnownGear = radiusOfLink(links.IndexOf(knownGearLink));
var rUnkGear = radiusOfOtherLink(links.IndexOf(knownGearLink));
var gearAngleChange = -(rKnownGear / rUnkGear) * (knownGearLink.Angle - knownGearLink.AngleLast);
if (joints[gearCenter1Index].positionKnown == KnownState.Fully &&
joints[gearCenter2Index].positionKnown == KnownState.Fully)
{
var From = joints[gearCenter1Index];
var To = joints[gearCenter2Index];
var connectingRodAngleChange = Constants.angle(From.x, From.y, To.x, To.y) -
Constants.angle(From.xLast, From.yLast, To.xLast, To.yLast);
unknownGearLink.Angle += connectingRodAngleChange * (1 + rKnownGear / rUnkGear) +
gearAngleChange;
unknownGearLink.AngleIsKnown = KnownState.Fully;
return(true);
}
if (unknownGearLink.AngleIsKnown == KnownState.Unknown)
{
unknownGearLink.Angle = gearAngleChange + unknownGearLink.AngleLast;
unknownGearLink.AngleIsKnown = KnownState.Partially;
return(false);
}
else
{
var angleTemp = gearAngleChange + unknownGearLink.AngleLast;
unknownGearLink.Angle = (unknownGearLink.Angle + angleTemp) / 2.0;
unknownGearLink.AngleIsKnown = KnownState.Fully;
return(true);
}
}
/// <summary>
/// Determines the lengths and references.
/// </summary>
internal void DetermineLengthsAndReferences()
{
numJoints = joints.Count;
#region Define Initial Link Angle
var fixedJoints = joints.Where(j => j.FixedWithRespectTo(this)).
OrderBy(j => (j.IsGround) ? 0 : 1).
ThenBy(j => (j.Link2 != null) ? 0 : 1).ThenBy(j => j.xInitial).ToList();
ReferenceJoint1 = fixedJoints[0];
/* the linkAngle is defined from "the joint with the lowest initial x value
* that is fixed to this link" to "the joint with the highest initial x value
* that is fixed to this link. If the link has only one fixed joint (and
* it will necessarily have one due to the addition of a joint added in
* Simulator.addReferencePivotsToSlideOnlyLinks()) or is ground, then the
* initial angle is zero. */
if (fixedJoints.Count == 2 && !IsGround)
{
AngleInitial = Constants.angle(ReferenceJoint1, fixedJoints[1]);
}
else
{
AngleInitial = 0.0;
}
Angle = AngleNumerical = AngleLast = AngleInitial;
foreach (var j in joints.Where(j => j.SlidingWithRespectTo(this)))
{
j.OffsetSlideAngle -= AngleInitial;
}
#endregion
#region Joint-to-Joint Dictionaries
lengths = new Dictionary <int, double>();
distanceToSlideLine = new Dictionary <int, double>();
angleFromBlockToJoint = new Dictionary <int, double>();
for (var i = 0; i < joints.Count - 1; i++)
{
for (var j = i + 1; j < joints.Count; j++)
{
var iJoint = joints[i];
var jJoint = joints[j];
if (iJoint.SlidingWithRespectTo(this) && jJoint.SlidingWithRespectTo(this))
{
var key = numJoints * i + j;
angleFromBlockToJoint.Add(key, 0.0);
distanceToSlideLine.Add(key, 0.0);
key = numJoints * j + i;
angleFromBlockToJoint.Add(key, 0.0);
distanceToSlideLine.Add(key, 0.0);
}
else if (iJoint.SlidingWithRespectTo(this) && !jJoint.SlidingWithRespectTo(this))
{
addSlideDictionaryEntry(iJoint, jJoint, i, j);
addBlockAngleDictionaryEntry(iJoint, jJoint, i, j);
if (jJoint.TypeOfJoint == JointType.P)
{
addBlockAngleDictionaryEntry(jJoint, iJoint, j, i);
addSlideDictionaryEntry(jJoint, iJoint, j, i);
}
}
else if (!iJoint.SlidingWithRespectTo(this) && jJoint.SlidingWithRespectTo(this))
{
addSlideDictionaryEntry(jJoint, iJoint, j, i);
addBlockAngleDictionaryEntry(jJoint, iJoint, j, i);
if (iJoint.TypeOfJoint == JointType.P)
{
addBlockAngleDictionaryEntry(iJoint, jJoint, i, j);
addSlideDictionaryEntry(iJoint, jJoint, i, j);
}
}
else // if (!iJoint.SlidingWithRespectTo(this) && !jJoint.SlidingWithRespectTo(this))
{
lengths.Add(numJoints * i + j,
Constants.distance(iJoint.xInitial, iJoint.yInitial, jJoint.xInitial, jJoint.yInitial));
if (iJoint.TypeOfJoint == JointType.P)
{
addBlockAngleDictionaryEntry(iJoint, jJoint, i, j);
addSlideDictionaryEntry(iJoint, jJoint, i, j);
}
if (jJoint.TypeOfJoint == JointType.P)
{
addBlockAngleDictionaryEntry(jJoint, iJoint, j, i);
addSlideDictionaryEntry(jJoint, iJoint, j, i);
}
}
}
}
#endregion
}
internal static double findAngleChangeBetweenOfConnectingRod(Joint From, Joint To)
{
return(Constants.angle(From.x, From.y, To.x, To.y) -
Constants.angle(From.xLast, From.yLast, To.xLast, To.yLast));
}
