デルタの計算部

uint8_t PrintLine::queueNonlinearMove(uint8_t check_endstops,uint8_t pathOptimize, uint8_t softEndstop)
{
	EVENT_CONTRAIN_DESTINATION_COORDINATES
    int32_t difference[E_AXIS_ARRAY];
    float axisDistanceMM[VIRTUAL_AXIS_ARRAY]; // Real cartesian axis movement in mm. Virtual axis in 4;
    uint8_t secondSpeed = Printer::fanSpeed;
    for(fast8_t axis = 0; axis < E_AXIS_ARRAY; axis++)
    {
        difference[axis] = Printer::destinationSteps[axis] - Printer::currentPositionSteps[axis];
        if(axis == E_AXIS)
        {
            if(Printer::mode == PRINTER_MODE_FFF)
            {
                Printer::extrudeMultiplyError += (static_cast<float>(difference[E_AXIS]) * Printer::extrusionFactor);
                difference[E_AXIS] = static_cast<int32_t>(Printer::extrudeMultiplyError);
                Printer::extrudeMultiplyError -= difference[E_AXIS];
                axisDistanceMM[E_AXIS] = difference[E_AXIS] * Printer::invAxisStepsPerMM[E_AXIS];
                Printer::filamentPrinted += axisDistanceMM[E_AXIS];
                axisDistanceMM[E_AXIS] = fabs(axisDistanceMM[E_AXIS]);
            }
        }
        else
            axisDistanceMM[axis] = fabs(difference[axis] * Printer::invAxisStepsPerMM[axis]);
    }

    float cartesianDistance;
    ufast8_t cartesianDir;
    int32_t cartesianDeltaSteps[E_AXIS_ARRAY];
    calculateDirectionAndDelta(difference, &cartesianDir, cartesianDeltaSteps);

    int16_t segmentCount;
    if (cartesianDir & XY_STEP)
    {
        float seconds = cartesianDistance / feedrate;
        float sps = static_cast<float>((cartesianDir & ESTEP) == ESTEP ? Printer::printMovesPerSecond : Printer::travelMovesPerSecond);
        segmentCount = RMath::max(1, static_cast<int16_t>(sps * seconds));
    }
    else
    {
        segmentCount = (cartesianDeltaSteps[Z_AXIS] + (uint32_t)43680) / (uint32_t)43679; // can not go to 65535 for rounding issues causing overflow later in some cases!
    }
    int numLines = (segmentCount + DELTASEGMENTS_PER_PRINTLINE - 1) / DELTASEGMENTS_PER_PRINTLINE;
    int segmentsPerLine = segmentCount / numLines;

    int32_t startPosition[E_AXIS_ARRAY], fractionalSteps[E_AXIS_ARRAY];
    if(numLines > 1)
    {
        for (fast8_t i = 0; i < Z_AXIS_ARRAY; i++)
            startPosition[i] = Printer::currentPositionSteps[i];
        startPosition[E_AXIS] = 0;
        cartesianDistance /= static_cast<float>(numLines);
    }

    Printer::unsetAllSteppersDisabled(); // Motor is enabled now
    waitForXFreeLines(1);

    insertWaitMovesIfNeeded(pathOptimize, RMath::min(PRINTLINE_CACHE_SIZE - 4, numLines));
    uint32_t oldEDestination = Printer::destinationSteps[E_AXIS]; // flow and volumetric extrusion changed virtual target
    Printer::currentPositionSteps[E_AXIS] = 0;

    for (int lineNumber = 1; lineNumber <= numLines; lineNumber++)
    {
        waitForXFreeLines(1);
        PrintLine *p = getNextWriteLine();
        if (numLines == 1)
        {
            p->dir = cartesianDir;
            for (fast8_t i = 0; i < E_AXIS_ARRAY; i++)
            {
                p->delta[i] = cartesianDeltaSteps[i];
                fractionalSteps[i] = difference[i];
            }
            p->distance = cartesianDistance;
        }
        else
        {
            for (fast8_t i = 0; i < E_AXIS_ARRAY; i++)
            {
                Printer::destinationSteps[i] = startPosition[i] + (difference[i] * lineNumber) / numLines;
                fractionalSteps[i] = Printer::destinationSteps[i] - Printer::currentPositionSteps[i];
                axisDistanceMM[i] = fabs(fractionalSteps[i] * Printer::invAxisStepsPerMM[i]);
            }
            calculateDirectionAndDelta(fractionalSteps, &p->dir, p->delta);
            p->distance = cartesianDistance;
        }

        p->joinFlags = 0;
        p->secondSpeed = secondSpeed;
        p->moveID = lastMoveID;

        if (lineNumber == numLines && !pathOptimize)
            p->setEndSpeedFixed(true);

        p->flags = (check_endstops ? FLAG_CHECK_ENDSTOPS : 0);
        p->numNonlinearSegments = segmentsPerLine;

        uint16_t maxStepsPerSegment = p->calculateNonlinearSubSegments(softEndstop);
        int32_t virtualAxisSteps = static_cast<int32_t>(maxStepsPerSegment) * segmentsPerLine;
		fast8_t drivingAxis = X_AXIS;
        p->primaryAxis = VIRTUAL_AXIS; // Virtual axis will lead Bresenham step either way
        if (virtualAxisSteps > p->delta[E_AXIS])   // Is delta move or E axis leading
        {
            p->stepsRemaining = virtualAxisSteps;
            axisDistanceMM[VIRTUAL_AXIS] = p->distance;  //virtual_axis_move * Printer::invAxisStepsPerMM[Z_AXIS]; // Steps/unit same as all the towers
            p->numPrimaryStepPerSegment = maxStepsPerSegment;
        }
        else
        {
            p->numPrimaryStepPerSegment = (p->delta[E_AXIS] + segmentsPerLine - 1) / segmentsPerLine;
            p->stepsRemaining = p->numPrimaryStepPerSegment * segmentsPerLine;
            axisDistanceMM[VIRTUAL_AXIS] = -p->distance; //p->stepsRemaining * Printer::invAxisStepsPerMM[Z_AXIS];
			drivingAxis = E_AXIS;
        }
        p->calculateMove(axisDistanceMM, pathOptimize,drivingAxis);
        for (fast8_t i = 0; i < E_AXIS_ARRAY; i++)
        {
            Printer::currentPositionSteps[i] += fractionalSteps[i];
        }
    }
    Printer::currentPositionSteps[E_AXIS] = Printer::destinationSteps[E_AXIS] = oldEDestination;
    lastMoveID++; // Will wrap at 255

    return true; // flag success
}