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