/// <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)); }