private void assignJointPosition(Joint j, double xNew, double yNew, Link thisLink)
{
if (double.IsInfinity(xNew) || double.IsInfinity(yNew) ||
double.IsNaN(xNew) || double.IsNaN(yNew))
{
posResult = PositionAnalysisResults.InvalidPosition;
}
else
{
j.x = xNew;
j.y = yNew;
posResult = PositionAnalysisResults.Normal;
if (j.FixedWithRespectTo(thisLink))
{
j.positionKnown = KnownState.Fully;
xNew -= j.xNumerical;
yNew -= j.yNumerical;
PositionError = Math.Sqrt(xNew * xNew + yNew * yNew);
}
else
{
j.positionKnown = KnownState.Partially;
}
}
}
internal Boolean DefineNewPositions(double positionChange)
{
posResult = PositionAnalysisResults.Normal;
InitializeJointsAndLinks(positionChange);
if (posResult == PositionAnalysisResults.InvalidPosition)
{
return(false);
}
var numUnknownJoints = joints.Count(j => j.positionKnown != KnownState.Fully);
while (posResult != PositionAnalysisResults.NoSolvableDyadFound && numUnknownJoints > 0)
{
posResult = PositionAnalysisResults.NoSolvableDyadFound;
foreach (var j in joints)
{
if (j.positionKnown == KnownState.Fully || j.JustATracer)
{
continue;
}
Joint knownJoint1;
Joint knownJoint2;
GearData gData;
double angleChange;
switch (j.TypeOfJoint)
{
case JointType.R:
#region R-R-R
if (FindKnownSlopeOnLink(j, j.Link1, out knownJoint1) &&
FindKnownSlopeOnLink(j, j.Link1, out knownJoint2, knownJoint1))
{
var sJPoint = solveViaIntersectingLines(j, knownJoint1, knownJoint2);
assignJointPosition(j, sJPoint, j.Link1);
setLinkPositionFromRotate(j, j.Link1);
setLinkPositionFromRotate(j, j.Link2);
}
else if (FindKnownPositionOnLink(j, j.Link1, out knownJoint1) &&
FindKnownPositionOnLink(j, j.Link2, out knownJoint2))
{
var sJPoint = solveViaCircleIntersection(j, knownJoint1, knownJoint2);
assignJointPosition(j, sJPoint, j.Link1);
setLinkPositionFromRotate(j, j.Link1);
setLinkPositionFromRotate(j, j.Link2);
}
#endregion
#region R-R-P
else if (FindKnownPositionOnLink(j, j.Link1, out knownJoint1) &&
FindKnownSlopeOnLink(j, j.Link2, out knownJoint2))
{
var sJPoint = solveViaCircleAndLineIntersection(j, knownJoint1, knownJoint2,
out angleChange);
assignJointPosition(j, sJPoint, j.Link1);
setLinkPositionFromRotate(j, j.Link1, angleChange);
setLinkPositionFromRotate(j, j.Link2);
}
#endregion
#region P-R-R
else if (FindKnownSlopeOnLink(j, j.Link1, out knownJoint1) &&
FindKnownPositionOnLink(j, j.Link2, out knownJoint2))
{
var sJPoint = solveViaCircleAndLineIntersection(j, knownJoint2, knownJoint1,
out angleChange);
assignJointPosition(j, sJPoint, j.Link2);
setLinkPositionFromRotate(j, j.Link2, angleChange);
setLinkPositionFromRotate(j, j.Link1);
}
#endregion
#region P-R-P
else if (FindKnownSlopeOnLink(j, j.Link1, out knownJoint1) &&
FindKnownSlopeOnLink(j, j.Link2, out knownJoint2))
{
var sJPoint = solveViaIntersectingLines(j, knownJoint1, knownJoint2);
assignJointPosition(j, sJPoint, j.Link1);
setLinkPositionFromRotate(j, j.Link1);
setLinkPositionFromRotate(j, j.Link2);
}
#endregion
#region R-R-GG
else if (FindKnownPositionOnLink(j, j.Link1, out knownJoint1) &&
GearData.FindKnownGearAngleOnLink(j, j.Link1, j.Link2, joints, links, gearsData,
out gData))
{
gData.SolveGearCenterFromKnownGearAngle(j, knownJoint1, links, out angleChange);
setLinkPositionFromRotate(knownJoint1, j.Link1, angleChange);
}
#endregion
#region GG-R-R
else if (FindKnownPositionOnLink(j, j.Link2, out knownJoint1) &&
GearData.FindKnownGearAngleOnLink(j, j.Link2, j.Link1, joints, links, gearsData,
out gData))
{
gData.SolveGearCenterFromKnownGearAngle(j, knownJoint1, links, out angleChange);
setLinkPositionFromRotate(knownJoint1, j.Link2, angleChange);
}
#endregion
break;
case JointType.P:
#region R-P-R
if (FindKnownPositionOnLink(j, j.Link1, out knownJoint1) &&
FindKnownPositionOnLink(j, j.Link2, out knownJoint2))
{
var sJPoint = solveRPRIntersection(j, knownJoint1, knownJoint2, out angleChange);
assignJointPosition(j, sJPoint, j.Link2);
setLinkPositionFromRotate(knownJoint1, j.Link1, angleChange);
setLinkPositionFromRotate(j, j.Link2, angleChange);
}
#endregion
#region P-P-R
else if (FindKnownSlopeOnLink(j, j.Link1, out knownJoint1) &&
FindKnownPositionOnLink(j, j.Link2, out knownJoint2))
{
/* in this case, the block is on the rotating link and the slide is on the sliding link */
var sJPoint = solveViaSlopeToCircleIntersectionPPR(j, knownJoint1, knownJoint2);
assignJointPosition(j, sJPoint, j.Link2);
setLinkPositionFromRotate(j, j.Link1);
setLinkPositionFromRotate(j, j.Link2);
//setLinkPositionFromTranslation(knownJoint1, j.Link1, sJPoint.x - j.xLast, sJPoint.y - j.yLast,
// j.SlideAngle);
//setLinkPositionFromTranslation(j, j.Link2, sJPoint.x - j.xLast, sJPoint.y - j.yLast);
}
#endregion
#region R-P-P
else if (FindKnownPositionOnLink(j, j.Link1, out knownJoint1) &&
FindKnownSlopeOnLink(j, j.Link2, out knownJoint2))
{
/* in this case, the slide is on the rotating link and the block is on the sliding link */
Point sJPoint = solveViaSlopeToCircleIntersectionRPP(j, knownJoint1, knownJoint2);
assignJointPosition(j, sJPoint, j.Link2);
setLinkPositionFromRotate(j, j.Link1);
setLinkPositionFromRotate(j, j.Link2);
//setLinkPositionFromTranslation(j, j.Link2, sJPoint.x - j.xLast, sJPoint.y - j.yLast);
}
#endregion
#region P-P-P
else if (FindKnownSlopeOnLink(j, j.Link1, out knownJoint1) &&
FindKnownSlopeOnLink(j, j.Link2, out knownJoint2))
{
var sJPoint = solveViaIntersectingLines(j, knownJoint1, knownJoint2);
assignJointPosition(j, sJPoint, j.Link1);
setLinkPositionFromRotate(j, j.Link1);
setLinkPositionFromRotate(j, j.Link2);
}
#endregion
break;
case JointType.RP:
#region R-RP-R&P
//if (FindKnownPositionOnLink(j.Link1, out knownJoint1) &&
// FindKnownPositionAndSlopeOnLink(j.Link2, out knownJoint2))
//{
// throw new Exception("I didn't think it was possible to have an unknown joint for R-RP-R&P");
// /* why is it not possible?
// * because j.Link2 would be the fixed pivot within the joint and since it was known fully,
// * j would have j.knownState = KnownState.Fully and would not be cycled over. */
//}
#endregion
#region P-RP-R&P
//else if (FindKnownSlopeOnLink(j.Link1, out knownJoint1)
// &&
// FindKnownPositionAndSlopeOnLink(j.Link2,
// out knownJoint2))
//{
// throw new Exception("I didn't think it was possible to have an unknown joint for R-RP-R&P");
// /* same as above. */
//}
#endregion
#region R&P-RP-R
if (FindKnownPositionAndSlopeOnLink(j, j.Link1, out knownJoint1) &&
FindKnownPositionOnLink(j, j.Link2, out knownJoint2))
{
var sJPoint = solveRotatePinToSlot(j, knownJoint2, out angleChange);
assignJointPosition(j, sJPoint, j.Link2);
setLinkPositionFromRotate(j, j.Link2, angleChange);
}
#endregion
#region R&P-RP-P
else if (FindKnownPositionAndSlopeOnLink(j, j.Link1, out knownJoint1) &&
FindKnownSlopeOnLink(j, j.Link2, out knownJoint2))
{
/* not sure this is right, but j must be partially known so it has enough
* information to define the first line. */
var sJPoint = solveViaIntersectingLines(j, j, knownJoint2);
assignJointPosition(j, sJPoint, j.Link2);
setLinkPositionFromRotate(j, j.Link2);
//setLinkPositionFromTranslation(j, j.Link2, sJPoint.x - j.xLast, sJPoint.y - j.yLast);
}
#endregion
break;
case JointType.G:
gData = gearsData[joints.IndexOf(j)];
#region R-G-R
if (gData.IsGearSolvableRGR(joints, links))
{
gData.SolveGearPositionAndAnglesRGR(joints, links);
posResult = PositionAnalysisResults.Normal;
}
#endregion
#region P-G-R
else if (gData.IsGearSolvablePGR(joints, links))
{
gData.SolveGearPositionAndAnglesPGR(joints, links);
posResult = PositionAnalysisResults.Normal;
}
#endregion
#region R-G-P
else if (gData.IsGearSolvableRGP(joints, links))
{
gData.SolveGearPositionAndAnglesRGP(joints, links);
posResult = PositionAnalysisResults.Normal;
}
#endregion
break;
}
if (posResult == PositionAnalysisResults.InvalidPosition)
{
return(false);
}
}
numUnknownJoints = joints.Count(j => j.positionKnown != KnownState.Fully);
}
if (posResult == PositionAnalysisResults.NoSolvableDyadFound && numUnknownJoints > 0)
{
try
{
if (NDPS == null)
{
NDPS = new NonDyadicPositionSolver(this);
}
var NDPSError = 0.0;
NDPS.Run_PositionsAreClose(out NDPSError);
PositionError = Math.Sqrt(NDPSError);
}
catch (Exception e)
{
Debug.WriteLine("Error in setting up and running NonDyadicPositionSolver.");
}
}
if (posResult == PositionAnalysisResults.InvalidPosition)
{
return(false);
}
if (joints.Any(j => Math.Abs(j.x - j.xInitial) > maximumDeltaX || Math.Abs(j.y - j.yInitial) > maximumDeltaY))
{
return(false);
}
if (joints.All(j => Math.Abs(j.x - j.xLast) < minimumDeltaX && Math.Abs(j.y - j.yLast) < minimumDeltaY &&
links.All(c => Math.Abs(c.Angle - c.AngleLast) < Constants.AngleMinimumFactor)))
{
return(false);
}
UpdateSliderPosition();
return(true);
}
private double solveRotateSlotToPin(Joint fixedJoint, Joint slideJoint, Link thisLink, double j2j_angle)
{
var dist2Slide = thisLink.DistanceBetweenSlides(slideJoint, fixedJoint);
if (Constants.sameCloseZero(dist2Slide))
{
return(j2j_angle);
}
var distanceBetweenJoints = Constants.distance(fixedJoint, slideJoint);
if (Math.Abs(dist2Slide) > distanceBetweenJoints)
{
posResult = PositionAnalysisResults.InvalidPosition;
return(double.NaN);
}
var changeInSlideAngle = Math.Asin(dist2Slide / distanceBetweenJoints);
var slideAnglePos = j2j_angle + changeInSlideAngle;
var slideAngleNeg = j2j_angle - changeInSlideAngle;
var lastSlideAngle = slideJoint.OffsetSlideAngle + thisLink.AngleLast;
var deltaAnglePos = slideAnglePos - lastSlideAngle;
while (deltaAnglePos > Math.PI / 2)
{
deltaAnglePos -= Math.PI;
}
while (deltaAnglePos < -Math.PI / 2)
{
deltaAnglePos += Math.PI;
}
var deltaAngleNeg = slideAngleNeg - lastSlideAngle;
while (deltaAngleNeg > Math.PI / 2)
{
deltaAngleNeg -= Math.PI;
}
while (deltaAngleNeg < -Math.PI / 2)
{
deltaAngleNeg += Math.PI;
}
var deltaAngleNum = thisLink.AngleNumerical - thisLink.AngleLast;
while (deltaAngleNum > Math.PI)
{
deltaAngleNum -= Constants.FullCircle;
}
while (deltaAngleNum < -Math.PI)
{
deltaAngleNum += Constants.FullCircle;
}
var errorPos = Math.Abs(deltaAnglePos - deltaAngleNum);
var errorNeg = Math.Abs(deltaAngleNeg - deltaAngleNum);
if (errorPos < errorNeg)
{
PositionError = distanceBetweenJoints * errorPos;
return(deltaAnglePos);
}
PositionError = distanceBetweenJoints * errorNeg;
return(deltaAngleNeg);
}
