public void SetPrintLevelingEquation(Vector3 position0, Vector3 position1, Vector3 position2, Vector2 bedCenter) { if (position0 == position1 || position1 == position2 || position2 == position0) { return; } Plane planeOfPoints = new Plane(position0, position1, position2); Ray ray = new Ray(new Vector3(bedCenter, 0), Vector3.UnitZ); bool inFront; double distanceToPlaneAtBedCenter = planeOfPoints.GetDistanceToIntersection(ray, out inFront); Matrix4X4 makePointsFlatMatrix = Matrix4X4.CreateTranslation(-bedCenter.x, -bedCenter.y, -distanceToPlaneAtBedCenter); makePointsFlatMatrix *= Matrix4X4.CreateRotation(planeOfPoints.planeNormal, Vector3.UnitZ); makePointsFlatMatrix *= Matrix4X4.CreateTranslation(bedCenter.x, bedCenter.y, 0);//distanceToPlaneAtBedCenter); bedLevelMatrix = Matrix4X4.Invert(makePointsFlatMatrix); { // test that the points come back as 0 zs Vector3 outPosition0 = Vector3.TransformPosition(position0, makePointsFlatMatrix); Vector3 outPosition1 = Vector3.TransformPosition(position1, makePointsFlatMatrix); Vector3 outPosition2 = Vector3.TransformPosition(position2, makePointsFlatMatrix); Vector3 printPosition0 = new Vector3(LevelWizardBase.GetPrintLevelPositionToSample(0), 0); Vector3 printPosition1 = new Vector3(LevelWizardBase.GetPrintLevelPositionToSample(1), 0); Vector3 printPosition2 = new Vector3(LevelWizardBase.GetPrintLevelPositionToSample(2), 0); Vector3 leveledPositon0 = Vector3.TransformPosition(printPosition0, bedLevelMatrix); Vector3 leveledPositon1 = Vector3.TransformPosition(printPosition1, bedLevelMatrix); Vector3 leveledPositon2 = Vector3.TransformPosition(printPosition2, bedLevelMatrix); } }
public void SetPrintLevelingEquation(Vector3 position0, Vector3 position1, Vector3 position2, Vector2 bedCenter) { if (position0 == position1 || position1 == position2 || position2 == position0) { return; } Plane planeOfPoints = new Plane(position0, position1, position2); Ray ray = new Ray(new Vector3(bedCenter, 0), Vector3.UnitZ); bool inFront; double distanceToPlaneAtBedCenter = planeOfPoints.GetDistanceToIntersection(ray, out inFront); Matrix4X4 makePointsFlatMatrix = Matrix4X4.CreateTranslation(-bedCenter.x, -bedCenter.y, -distanceToPlaneAtBedCenter); makePointsFlatMatrix *= Matrix4X4.CreateRotation(planeOfPoints.PlaneNormal, Vector3.UnitZ); makePointsFlatMatrix *= Matrix4X4.CreateTranslation(bedCenter.x, bedCenter.y, 0);//distanceToPlaneAtBedCenter); bedLevelMatrix = Matrix4X4.Invert(makePointsFlatMatrix); }
public Vector3 GetPositionWithZOffset(Vector3 currentDestination) { if (LevelingData.SampledPositions.Count == NumberOfRadialSamples+1) { Vector2 destinationFromCenter = new Vector2(currentDestination) - BedCenter; double angleToPoint = Math.Atan2(destinationFromCenter.y, destinationFromCenter.x); if (angleToPoint < 0) { angleToPoint += MathHelper.Tau; } double oneSegmentAngle = MathHelper.Tau / NumberOfRadialSamples; int firstIndex = (int)(angleToPoint / oneSegmentAngle); int lastIndex = firstIndex + 1; if (lastIndex == NumberOfRadialSamples) { lastIndex = 0; } Plane currentPlane = new Plane(LevelingData.SampledPositions[firstIndex], LevelingData.SampledPositions[lastIndex], LevelingData.SampledPositions[NumberOfRadialSamples]); double hitDistance = currentPlane.GetDistanceToIntersection(new Vector3(currentDestination.x, currentDestination.y, 0), Vector3.UnitZ); currentDestination.z += hitDistance; } return currentDestination; }
public static Vector3 GetPositionWithZOffset(Vector3 currentDestination, PrintLevelingData levelingData) { Vector2 destinationFromCenter = new Vector2(currentDestination) - ActiveSliceSettings.Instance.BedCenter; double angleToPoint = Math.Atan2(destinationFromCenter.y, destinationFromCenter.x); if (angleToPoint < 0) { angleToPoint += MathHelper.Tau; } double oneSegmentAngle = MathHelper.Tau / 6; int firstIndex = (int)(angleToPoint / oneSegmentAngle); int lastIndex = firstIndex + 1; if (lastIndex == 6) { lastIndex = 0; } Plane currentPlane = new Plane(levelingData.SampledPositions[firstIndex], levelingData.SampledPositions[lastIndex], levelingData.SampledPositions[6]); double hitDistance = currentPlane.GetDistanceToIntersection(new Vector3(currentDestination.x, currentDestination.y, 0), Vector3.UnitZ); currentDestination.z += hitDistance; return currentDestination; }