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