/// <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);
}
var angleTemp = gearAngleChange + unknownGearLink.AngleLast;
unknownGearLink.Angle = (unknownGearLink.Angle + angleTemp) / 2.0;
unknownGearLink.AngleIsKnown = KnownState.Fully;
return(true);
}
/// <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)
? Constants.QuarterCircle
: pJoint.SlideAngleInitial -
Constants.Angle(pJoint.XInitial, pJoint.YInitial, refJoint.XInitial, refJoint.YInitial);
angleFromBlockToJoint.Add(key, result);
}
/// <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));
}
