private Tuple <Speed, Speed> getSpeeds(Point4Dmm[] points, int currentIndex, Speed facetSpeed, PlaneProjector projector)
{
var currentPoint = points[currentIndex % points.Length];
var nextPoint = points[(currentIndex + 1) % points.Length];
getFacetVectors(currentPoint, nextPoint, out var facetUV, out var facetXY);
var facetSpeedConverted = 1.0 * facetSpeed.StepCount / facetSpeed.Ticks;
if (facetUV.Length <= Constants.MilimetersPerStep && facetXY.Length <= Constants.MilimetersPerStep)
{
//TODO this accounts for numerical instability
return(Tuple.Create(facetSpeed, facetSpeed));
}
double ratio;
switch (SpeedAlgorithm)
{
case SpeedAlgorithm.StickToFacetUV:
ratio = facetSpeedConverted / facetUV.Length;
break;
case SpeedAlgorithm.StickToFacetXY:
ratio = facetSpeedConverted / facetXY.Length;
break;
default:
throw new NotImplementedException("SpeedAlgorithm");
}
facetUV = facetUV * ratio;
facetXY = facetXY * ratio;
var speedPoint = new Point4Dmm(currentPoint.U + facetUV.X, currentPoint.V + facetUV.Y, currentPoint.X + facetXY.X, currentPoint.Y + facetXY.Y);
var currentProjected = projector.Project(currentPoint);
var speedProjected = projector.Project(speedPoint);
getFacetVectors(currentProjected, speedProjected, out var speedUV, out var speedXY);
var speedFactor = Constants.TimerFrequency;
var uvSpeed = speedUV.Length * speedFactor;
var xySpeed = speedXY.Length * speedFactor;
return(Tuple.Create(
new Speed((long)(uvSpeed), speedFactor),
new Speed((long)(xySpeed), speedFactor)
));
}
public static PlanBuilder Shape4dTest()
{
var metricShapeThickness = 300;
var points = ShapeDrawing.CircleToSquare();
var projector = new PlaneProjector(metricShapeThickness, Constants.FullWireLength);
var projectedPoints = projector.Project(points);
var planner = new StraightLinePlanner4D(Speed.FromDeltaT(3000));
return(planner.CreateConstantPlan(new Trajectory4D(projectedPoints.As4Dstep())));
}
private Point4Dmm projectToTowers(Point4Dmm p, double wireLength)
{
return(PlaneProjector.Project(p, this.MetricThickness, wireLength));
}