internal GearData(Joint gearTeethJoint, int gearTeethIndex, Joint gearCenter1, int gearCenter1Index,
int gear1LinkIndex,
Joint gearCenter2, int gearCenter2Index, int gear2LinkIndex,
int connectingRodIndex, double initialGearAngle)
{
this.gearTeethIndex = gearTeethIndex;
this.gearCenter1Index = gearCenter1Index;
this.gear1LinkIndex = gear1LinkIndex;
this.gearCenter2Index = gearCenter2Index;
this.gear2LinkIndex = gear2LinkIndex;
this.connectingRodIndex = connectingRodIndex;
var dx1 = gearCenter1.xInitial - gearTeethJoint.xInitial;
var dy1 = gearCenter1.yInitial - gearTeethJoint.yInitial;
var dx2 = gearCenter2.xInitial - gearTeethJoint.xInitial;
var dy2 = gearCenter2.yInitial - gearTeethJoint.yInitial;
radius1 = Constants.distance(gearTeethJoint, gearCenter1);
radius2 = Constants.distance(gearTeethJoint, gearCenter2);
gearCenter1.OffsetSlideAngle = initialGearAngle;
radius1 = Math.Sqrt(dx1 * dx1 + dy1 * dy1);
radius2 = Math.Sqrt(dx2 * dx2 + dy2 * dy2);
if (dx1 * dx2 >= 0 && dy1 * dy2 >= 0)
{
//then they're both on the same side of the gear teeth
// and one is inverted (the bigger one to be precise).
if (radius1 > radius2)
{
radius1 *= -1;
}
else
{
radius2 *= -1;
}
}
}
/// <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
}
