diff options
-rw-r--r-- | src/GCodes/GCodeBuffer/ExpressionParser.cpp | 2 | ||||
-rw-r--r-- | src/GCodes/GCodes.cpp | 8 | ||||
-rw-r--r-- | src/GCodes/GCodes2.cpp | 2 | ||||
-rw-r--r-- | src/GCodes/GCodes3.cpp | 4 | ||||
-rw-r--r-- | src/Movement/BedProbing/Grid.cpp | 4 | ||||
-rw-r--r-- | src/Movement/BedProbing/RandomProbePointSet.cpp | 4 | ||||
-rw-r--r-- | src/Movement/DDA.cpp | 18 | ||||
-rw-r--r-- | src/Movement/DriveMovement.cpp | 18 | ||||
-rw-r--r-- | src/Movement/Kinematics/FiveBarScaraKinematics.cpp | 8 | ||||
-rw-r--r-- | src/Movement/Kinematics/HangprinterKinematics.cpp | 18 | ||||
-rw-r--r-- | src/Movement/Kinematics/LinearDeltaKinematics.cpp | 14 | ||||
-rw-r--r-- | src/Movement/Kinematics/PolarKinematics.cpp | 6 | ||||
-rw-r--r-- | src/Movement/Kinematics/RotaryDeltaKinematics.cpp | 6 | ||||
-rw-r--r-- | src/Movement/Kinematics/RoundBedKinematics.cpp | 2 | ||||
-rw-r--r-- | src/Movement/Kinematics/ScaraKinematics.cpp | 10 |
15 files changed, 62 insertions, 62 deletions
diff --git a/src/GCodes/GCodeBuffer/ExpressionParser.cpp b/src/GCodes/GCodeBuffer/ExpressionParser.cpp index 310c58bd..2a2f0586 100644 --- a/src/GCodes/GCodeBuffer/ExpressionParser.cpp +++ b/src/GCodes/GCodeBuffer/ExpressionParser.cpp @@ -890,7 +890,7 @@ ExpressionValue ExpressionParser::ParseIdentifierExpression(bool evaluate, bool case Function::sqrt: ConvertToFloat(rslt, evaluate); - rslt.fVal = sqrtf(rslt.fVal); + rslt.fVal = fastSqrtf(rslt.fVal); rslt.param = MaxFloatDigitsDisplayedAfterPoint; break; diff --git a/src/GCodes/GCodes.cpp b/src/GCodes/GCodes.cpp index cc59ac68..30dd0825 100644 --- a/src/GCodes/GCodes.cpp +++ b/src/GCodes/GCodes.cpp @@ -2070,7 +2070,7 @@ bool GCodes::DoStraightMove(GCodeBuffer& gb, bool isCoordinated, const char *& e { // This kinematics approximates linear motion by means of segmentation. // We assume that the segments will be smaller than the mesh spacing. - const float xyLength = sqrtf(fsquare(currentUserPosition[X_AXIS] - initialUserPosition[X_AXIS]) + fsquare(currentUserPosition[Y_AXIS] - initialUserPosition[Y_AXIS])); + const float xyLength = fastSqrtf(fsquare(currentUserPosition[X_AXIS] - initialUserPosition[X_AXIS]) + fsquare(currentUserPosition[Y_AXIS] - initialUserPosition[Y_AXIS])); const float moveTime = xyLength/moveBuffer.feedRate; // this is a best-case time, often the move will take longer moveBuffer.totalSegments = (unsigned int)max<long>(1, lrintf(min<float>(xyLength * kin.GetReciprocalMinSegmentLength(), moveTime * kin.GetSegmentsPerSecond()))); } @@ -2196,7 +2196,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err) float hDivD; if (hSquared >= 0.0) { - hDivD = sqrtf(hSquared/dSquared); + hDivD = fastSqrtf(hSquared/dSquared); } else { @@ -2397,7 +2397,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err) moveBuffer.usePressureAdvance = moveBuffer.hasPositiveExtrusion; - moveBuffer.arcRadius = sqrtf(iParam * iParam + jParam * jParam); + moveBuffer.arcRadius = fastSqrtf(iParam * iParam + jParam * jParam); moveBuffer.arcCurrentAngle = atan2(-jParam, -iParam); // Calculate the total angle moved, which depends on which way round we are going @@ -2420,7 +2420,7 @@ bool GCodes::DoArcMove(GCodeBuffer& gb, bool clockwise, const char *& err) // We leave out the square term because it is very small // In CNC applications even very small deviations can be visible, so we use a smaller segment length at low speeds const float arcSegmentLength = constrain<float> - ( min<float>(sqrtf(8 * moveBuffer.arcRadius * MaxArcDeviation), moveBuffer.feedRate * (1.0/MinArcSegmentsPerSec)), + ( min<float>(fastSqrtf(8 * moveBuffer.arcRadius * MaxArcDeviation), moveBuffer.feedRate * (1.0/MinArcSegmentsPerSec)), MinArcSegmentLength, MaxArcSegmentLength ); diff --git a/src/GCodes/GCodes2.cpp b/src/GCodes/GCodes2.cpp index 8149dea8..7f1f39f9 100644 --- a/src/GCodes/GCodes2.cpp +++ b/src/GCodes/GCodes2.cpp @@ -2083,7 +2083,7 @@ bool GCodes::HandleMcode(GCodeBuffer& gb, const StringRef& reply) THROWS(GCodeEx } else { - reply.catf(" %.03f", (double)(2.0/sqrtf(vef * Pi))); + reply.catf(" %.03f", (double)(2.0/fastSqrtf(vef * Pi))); } } } diff --git a/src/GCodes/GCodes3.cpp b/src/GCodes/GCodes3.cpp index 78d9a86f..a30193e1 100644 --- a/src/GCodes/GCodes3.cpp +++ b/src/GCodes/GCodes3.cpp @@ -350,7 +350,7 @@ GCodeResult GCodes::DefineGrid(GCodeBuffer& gb, const StringRef &reply) THROWS(G effectiveXRadius = radius - 0.1; if (numPoints[1] % 2 == 0) { - effectiveXRadius *= sqrtf(1.0 - 1.0/(float)((numPoints[1] - 1) * (numPoints[1] - 1))); + effectiveXRadius *= fastSqrtf(1.0 - 1.0/(float)((numPoints[1] - 1) * (numPoints[1] - 1))); } spacings[0] = (2 * effectiveXRadius)/(numPoints[0] - 1); } @@ -367,7 +367,7 @@ GCodeResult GCodes::DefineGrid(GCodeBuffer& gb, const StringRef &reply) THROWS(G effectiveYRadius = radius - 0.1; if (numPoints[0] % 2 == 0) { - effectiveYRadius *= sqrtf(1.0 - 1.0/(float)((numPoints[0] - 1) * (numPoints[0] - 1))); + effectiveYRadius *= fastSqrtf(1.0 - 1.0/(float)((numPoints[0] - 1) * (numPoints[0] - 1))); } spacings[1] = (2 * effectiveYRadius)/(numPoints[1] - 1); } diff --git a/src/Movement/BedProbing/Grid.cpp b/src/Movement/BedProbing/Grid.cpp index 264833e9..5baaca88 100644 --- a/src/Movement/BedProbing/Grid.cpp +++ b/src/Movement/BedProbing/Grid.cpp @@ -312,7 +312,7 @@ void GridDefinition::PrintError(float originalAxis0range, float originalAxis1ran { const float totalRange = originalAxis0range + originalAxis1range; const float area = originalAxis0range * originalAxis1range; - const float minSpacing = (totalRange + sqrtf(fsquare(totalRange) + 4.0 * (MaxGridProbePoints - 1) * area))/(2.0 * (MaxGridProbePoints - 1)); + const float minSpacing = (totalRange + fastSqrtf(fsquare(totalRange) + 4.0 * (MaxGridProbePoints - 1) * area))/(2.0 * (MaxGridProbePoints - 1)); const float minXspacing = originalAxis0range/(MaxAxis0GridPoints - 1); r.catf("Too many grid points; suggest increase spacing to %.1fmm", (double)max<float>(minSpacing, minXspacing)); } @@ -700,7 +700,7 @@ void HeightMap::ExtrapolateMissing() noexcept // Plane equation: ax+by+cz=d -> z = (d-(ax+by))/c float a = (axis1z*axis0Axis1 - axis0z*axis1Axis1) / detZ; float b = (axis0z*axis0Axis1 - axis1z*axis0Axis0) / detZ; - const float invC = sqrtf(a*a + b*b + 1.0); + const float invC = fastSqrtf(a*a + b*b + 1.0); const float normLenInv = 1.0 / invC; a *= normLenInv; b *= normLenInv; diff --git a/src/Movement/BedProbing/RandomProbePointSet.cpp b/src/Movement/BedProbing/RandomProbePointSet.cpp index dd8b83a7..5bbee02d 100644 --- a/src/Movement/BedProbing/RandomProbePointSet.cpp +++ b/src/Movement/BedProbing/RandomProbePointSet.cpp @@ -131,7 +131,7 @@ void RandomProbePointSet::ReportProbeHeights(size_t numPoints, const StringRef& } const float mean = sum/numPoints; // In the following, if there is only 1 point we may try to take the square root of a negative number due to rounding error, hence the 'max' call - const float stdDev = sqrtf(max<float>(sumOfSquares/numPoints - fsquare(mean), 0.0)); + const float stdDev = fastSqrtf(max<float>(sumOfSquares/numPoints - fsquare(mean), 0.0)); reply.catf(", mean %.3f, deviation from mean %.3f", (double)mean, (double)stdDev); } @@ -147,7 +147,7 @@ void RandomProbePointSet::DebugPrint(size_t numPoints) const noexcept sumOfSquares += fsquare(zBedProbePoints[i]); } const float mean = sum/numPoints; - debugPrintf(", mean %.3f, deviation from mean %.3f\n", (double)mean, (double)sqrtf(sumOfSquares/numPoints - fsquare(mean))); + debugPrintf(", mean %.3f, deviation from mean %.3f\n", (double)mean, (double)fastSqrtf(sumOfSquares/numPoints - fsquare(mean))); } // End diff --git a/src/Movement/DDA.cpp b/src/Movement/DDA.cpp index 038789d6..4fe96db5 100644 --- a/src/Movement/DDA.cpp +++ b/src/Movement/DDA.cpp @@ -487,7 +487,7 @@ bool DDA::InitStandardMove(DDARing& ring, const RawMove &nextMove, bool doMotorM { // Try to meld this move to the previous move to avoid stop/start // Assuming that this move ends with zero speed, calculate the maximum possible starting speed: u^2 = v^2 - 2as - prev->beforePrepare.targetNextSpeed = min<float>(sqrtf(deceleration * totalDistance * 2.0), requestedSpeed); + prev->beforePrepare.targetNextSpeed = min<float>(fastSqrtf(deceleration * totalDistance * 2.0), requestedSpeed); DoLookahead(ring, prev); startSpeed = prev->endSpeed; } @@ -800,7 +800,7 @@ pre(state == provisional) ) { laDDA->MatchSpeeds(); - const float maxStartSpeed = sqrtf(fsquare(laDDA->beforePrepare.targetNextSpeed) + (2 * laDDA->deceleration * laDDA->totalDistance)); + const float maxStartSpeed = fastSqrtf(fsquare(laDDA->beforePrepare.targetNextSpeed) + (2 * laDDA->deceleration * laDDA->totalDistance)); laDDA->prev->beforePrepare.targetNextSpeed = min<float>(maxStartSpeed, laDDA->requestedSpeed); // leave 'goingUp' true } @@ -811,7 +811,7 @@ pre(state == provisional) { laDDA->flags.hadLookaheadUnderrun = true; } - const float maxReachableSpeed = sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->deceleration * laDDA->totalDistance)); + const float maxReachableSpeed = fastSqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->deceleration * laDDA->totalDistance)); if (laDDA->beforePrepare.targetNextSpeed > maxReachableSpeed) { laDDA->beforePrepare.targetNextSpeed = maxReachableSpeed; @@ -824,7 +824,7 @@ pre(state == provisional) { // This move doesn't reach its requested speed, but it isn't a deceleration-only move // Set its end speed to the minimum of the requested speed and the highest we can reach - const float maxReachableSpeed = sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)); + const float maxReachableSpeed = fastSqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)); if (laDDA->beforePrepare.targetNextSpeed > maxReachableSpeed) { // Looks like this is an acceleration segment, so to ensure smooth acceleration we should reduce targetNextSpeed to endSpeed as well @@ -839,7 +839,7 @@ pre(state == provisional) // Going back down the list // We have adjusted the end speed of the previous move as much as is possible. Adjust this move to match it. laDDA->startSpeed = laDDA->prev->endSpeed; - const float maxEndSpeed = sqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)); + const float maxEndSpeed = fastSqrtf(fsquare(laDDA->startSpeed) + (2 * laDDA->acceleration * laDDA->totalDistance)); if (maxEndSpeed < laDDA->beforePrepare.targetNextSpeed) { laDDA->beforePrepare.targetNextSpeed = maxEndSpeed; @@ -983,7 +983,7 @@ void DDA::RecalculateMove(DDARing& ring) noexcept // It's an accelerate-decelerate move. Calculate accelerate distance from: V^2 = u^2 + 2as. beforePrepare.accelDistance = (vsquared - fsquare(startSpeed))/twoA; beforePrepare.decelDistance = (vsquared - fsquare(endSpeed))/twoD; - topSpeed = sqrtf(vsquared); + topSpeed = fastSqrtf(vsquared); } else { @@ -1663,7 +1663,7 @@ void DDA::Prepare(uint8_t simMode, float extrusionPending[]) noexcept { magnitudeSquared += fsquare(v[d]); } - return sqrtf(magnitudeSquared); + return fastSqrtf(magnitudeSquared); } // Normalise a vector with dim1 dimensions to unit length over the specified axes, and also return its previous magnitude in dim2 dimensions @@ -1726,7 +1726,7 @@ float DDA::NormaliseLinearMotion(AxesBitmap linearAxes) noexcept { yMagSquared /= numYaxes; } - const float magnitude = sqrtf(xMagSquared + yMagSquared + magSquared); + const float magnitude = fastSqrtf(xMagSquared + yMagSquared + magSquared); if (magnitude <= 0.0) { return 0.0; @@ -1742,7 +1742,7 @@ float DDA::NormaliseLinearMotion(AxesBitmap linearAxes) noexcept { float magnitude = 0.0; axes.Iterate([&magnitude, v](unsigned int axis, unsigned int count) { magnitude += fsquare(v[axis]); }); - return sqrtf(magnitude); + return fastSqrtf(magnitude); } // Multiply a vector by a scalar diff --git a/src/Movement/DriveMovement.cpp b/src/Movement/DriveMovement.cpp index 65b2bc9d..27f32b71 100644 --- a/src/Movement/DriveMovement.cpp +++ b/src/Movement/DriveMovement.cpp @@ -125,7 +125,7 @@ bool DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params) n const float B = params.initialY - params.dparams->GetTowerY(drive); const float aAplusbB = A * dda.directionVector[X_AXIS] + B * dda.directionVector[Y_AXIS]; const float dSquaredMinusAsquaredMinusBsquared = params.dparams->GetDiagonalSquared(drive) - fsquare(A) - fsquare(B); - const float h0MinusZ0 = sqrtf(dSquaredMinusAsquaredMinusBsquared); + const float h0MinusZ0 = fastSqrtf(dSquaredMinusAsquaredMinusBsquared); #if DM_USE_FPU mp.delta.fHmz0s = h0MinusZ0 * stepsPerMm; mp.delta.fMinusAaPlusBbTimesS = -(aAplusbB * stepsPerMm); @@ -151,12 +151,12 @@ bool DriveMovement::PrepareDeltaAxis(const DDA& dda, const PrepParams& params) n { // The distance to reversal is the solution to a quadratic equation. One root corresponds to the carriages being below the bed, // the other root corresponds to the carriages being above the bed. - const float drev = ((dda.directionVector[Z_AXIS] * sqrtf(params.a2plusb2 * params.dparams->GetDiagonalSquared(drive) - fsquare(A * dda.directionVector[Y_AXIS] - B * dda.directionVector[X_AXIS]))) + const float drev = ((dda.directionVector[Z_AXIS] * fastSqrtf(params.a2plusb2 * params.dparams->GetDiagonalSquared(drive) - fsquare(A * dda.directionVector[Y_AXIS] - B * dda.directionVector[X_AXIS]))) - aAplusbB)/params.a2plusb2; if (drev > 0.0 && drev < dda.totalDistance) // if the reversal point is within range { // Calculate how many steps we need to move up before reversing - const float hrev = dda.directionVector[Z_AXIS] * drev + sqrtf(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - params.a2plusb2 * fsquare(drev)); + const float hrev = dda.directionVector[Z_AXIS] * drev + fastSqrtf(dSquaredMinusAsquaredMinusBsquared - 2 * drev * aAplusbB - params.a2plusb2 * fsquare(drev)); const int32_t numStepsUp = (int32_t)((hrev - h0MinusZ0) * stepsPerMm); // We may be almost at the peak height already, in which case we don't really have a reversal. @@ -610,7 +610,7 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0) const uint32_t nextCalcStep = nextStep + stepsTillRecalc; #if DM_USE_FPU const float adjustedStartSpeedTimesCdivA = (float)(dda.afterPrepare.startSpeedTimesCdivA + mp.cart.compensationClocks); - nextCalcStepTime = lrintf(sqrtf(fsquare(adjustedStartSpeedTimesCdivA) + (fTwoCsquaredTimesMmPerStepDivA * nextCalcStep)) - adjustedStartSpeedTimesCdivA); + nextCalcStepTime = lrintf(fastSqrtf(fsquare(adjustedStartSpeedTimesCdivA) + (fTwoCsquaredTimesMmPerStepDivA * nextCalcStep)) - adjustedStartSpeedTimesCdivA); #else const uint32_t adjustedStartSpeedTimesCdivA = dda.afterPrepare.startSpeedTimesCdivA + mp.cart.compensationClocks; nextCalcStepTime = isqrt64(isquare64(adjustedStartSpeedTimesCdivA) + (twoCsquaredTimesMmPerStepDivA * nextCalcStep)) - adjustedStartSpeedTimesCdivA; @@ -689,7 +689,7 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0) const float temp = fTwoCsquaredTimesMmPerStepDivD * nextCalcStep; // Allow for possible rounding error when the end speed is zero or very small nextCalcStepTime = (temp < fTwoDistanceToStopTimesCsquaredDivD) - ? adjustedTopSpeedTimesCdivDPlusDecelStartClocks - lrintf(sqrtf(fTwoDistanceToStopTimesCsquaredDivD - temp)) + ? adjustedTopSpeedTimesCdivDPlusDecelStartClocks - lrintf(fastSqrtf(fTwoDistanceToStopTimesCsquaredDivD - temp)) : adjustedTopSpeedTimesCdivDPlusDecelStartClocks; #else const uint64_t temp = twoCsquaredTimesMmPerStepDivD * nextCalcStep; @@ -730,7 +730,7 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0) const uint32_t adjustedTopSpeedTimesCdivDPlusDecelStartClocks = dda.afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks - mp.cart.compensationClocks; nextCalcStepTime = adjustedTopSpeedTimesCdivDPlusDecelStartClocks #if DM_USE_FPU - + lrintf(sqrtf((fTwoCsquaredTimesMmPerStepDivD * nextCalcStep) - mp.cart.fFourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD)); + + lrintf(fastSqrtf((fTwoCsquaredTimesMmPerStepDivD * nextCalcStep) - mp.cart.fFourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD)); #else + isqrt64((int64_t)(twoCsquaredTimesMmPerStepDivD * nextCalcStep) - mp.cart.fourMaxStepDistanceMinusTwoDistanceToStopTimesCsquaredDivD); #endif @@ -839,7 +839,7 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0) const float t1 = mp.delta.fMinusAaPlusBbTimesS + hmz0sc; // Due to rounding error we can end up trying to take the square root of a negative number if we do not take precautions here const float t2a = mp.delta.fDSquaredMinusAsquaredMinusBsquaredTimesSsquared - fsquare(mp.delta.fHmz0s) + fsquare(t1); - const float t2 = (t2a > 0.0) ? sqrtf(t2a) : 0.0; + const float t2 = (t2a > 0.0) ? fastSqrtf(t2a) : 0.0; const float ds = (direction) ? t1 - t2 : t1 + t2; #else // In the following, cKc is the Z movement fraction of the total move scaled by Kc @@ -868,7 +868,7 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0) if (ds < mp.delta.fAccelStopDs) { // Acceleration phase - nextCalcStepTime = lrintf(sqrtf(fsquare((float)dda.afterPrepare.startSpeedTimesCdivA) + (fTwoCsquaredTimesMmPerStepDivA * ds))) - dda.afterPrepare.startSpeedTimesCdivA; + nextCalcStepTime = lrintf(fastSqrtf(fsquare((float)dda.afterPrepare.startSpeedTimesCdivA) + (fTwoCsquaredTimesMmPerStepDivA * ds))) - dda.afterPrepare.startSpeedTimesCdivA; } else if (ds < mp.delta.fDecelStartDs) { @@ -882,7 +882,7 @@ pre(nextStep < totalSteps; stepsTillRecalc == 0) const float temp = fTwoCsquaredTimesMmPerStepDivD * ds; // Because of possible rounding error when the end speed is zero or very small, we need to check that the square root will work OK nextCalcStepTime = (temp < fTwoDistanceToStopTimesCsquaredDivD) - ? dda.afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks - lrintf(sqrtf(fTwoDistanceToStopTimesCsquaredDivD - temp)) + ? dda.afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks - lrintf(fastSqrtf(fTwoDistanceToStopTimesCsquaredDivD - temp)) : dda.afterPrepare.topSpeedTimesCdivDPlusDecelStartClocks; } #else diff --git a/src/Movement/Kinematics/FiveBarScaraKinematics.cpp b/src/Movement/Kinematics/FiveBarScaraKinematics.cpp index 7f8964b8..ce686ed5 100644 --- a/src/Movement/Kinematics/FiveBarScaraKinematics.cpp +++ b/src/Movement/Kinematics/FiveBarScaraKinematics.cpp @@ -177,7 +177,7 @@ bool FiveBarScaraKinematics::isCantilevered(int mode) const noexcept // get angle between -90 and 270 for given origin and destination coordinates float FiveBarScaraKinematics::getAbsoluteAngle(float xOrig, float yOrig, float xDest, float yDest) const noexcept { - const float length = sqrtf(fsquare(xOrig - xDest) + fsquare(yOrig - yDest)); + const float length = fastSqrtf(fsquare(xOrig - xDest) + fsquare(yOrig - yDest)); const float y = fabs(yOrig - yDest); float angle = asinf(y / length) * 180.0f / Pi; @@ -208,9 +208,9 @@ void FiveBarScaraKinematics::getIntersec(float result[], float firstRadius, floa const float secondRadius2 = fsquare(secondRadius); const float distance2 = fsquare(firstX - secondX) + fsquare(firstY - secondY); - const float distance = sqrtf(distance2); + const float distance = fastSqrtf(distance2); - const float delta = 0.25 * sqrtf( + const float delta = 0.25 * fastSqrtf( (distance + firstRadius + secondRadius) * (distance + firstRadius - secondRadius) * (distance - firstRadius + secondRadius) @@ -927,7 +927,7 @@ void FiveBarScaraKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *no //TODO should we make use of numVisibleAxes and/or continuousRotationShortcut? // For now we limit the speed in the XY plane to the lower of the X and Y maximum speeds, and similarly for the acceleration. // Limiting the angular rates of the arms would be better. - const float xyFactor = sqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS])); + const float xyFactor = fastSqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS])); if (xyFactor > 0.01) { const Platform& platform = reprap.GetPlatform(); diff --git a/src/Movement/Kinematics/HangprinterKinematics.cpp b/src/Movement/Kinematics/HangprinterKinematics.cpp index 769b8138..ebef38be 100644 --- a/src/Movement/Kinematics/HangprinterKinematics.cpp +++ b/src/Movement/Kinematics/HangprinterKinematics.cpp @@ -199,10 +199,10 @@ bool HangprinterKinematics::CartesianToMotorSteps(const float machinePos[], cons + fsquare(anchorDz - machinePos[Z_AXIS]); if (aSquared > 0.0 && bSquared > 0.0 && cSquared > 0.0 && dSquared > 0.0) { - motorPos[A_AXIS] = lrintf(sqrtf(aSquared) * stepsPerMm[A_AXIS]); - motorPos[B_AXIS] = lrintf(sqrtf(bSquared) * stepsPerMm[B_AXIS]); - motorPos[C_AXIS] = lrintf(sqrtf(cSquared) * stepsPerMm[C_AXIS]); - motorPos[D_AXIS] = lrintf(sqrtf(dSquared) * stepsPerMm[D_AXIS]); + motorPos[A_AXIS] = lrintf(fastSqrtf(aSquared) * stepsPerMm[A_AXIS]); + motorPos[B_AXIS] = lrintf(fastSqrtf(bSquared) * stepsPerMm[B_AXIS]); + motorPos[C_AXIS] = lrintf(fastSqrtf(cSquared) * stepsPerMm[C_AXIS]); + motorPos[D_AXIS] = lrintf(fastSqrtf(dSquared) * stepsPerMm[D_AXIS]); return true; } return false; @@ -227,7 +227,7 @@ LimitPositionResult HangprinterKinematics::LimitPosition(float finalCoords[], co const float diagonalSquared = fsquare(finalCoords[X_AXIS]) + fsquare(finalCoords[Y_AXIS]); if (diagonalSquared > printRadiusSquared) { - const float factor = sqrtf(printRadiusSquared / diagonalSquared); + const float factor = fastSqrtf(printRadiusSquared / diagonalSquared); finalCoords[X_AXIS] *= factor; finalCoords[Y_AXIS] *= factor; limited = true; @@ -352,7 +352,7 @@ void HangprinterKinematics::InverseTransform(float La, float Lb, float Lc, float const float C = fsquare(S) + fsquare(T) + (anchorA[1] * T - anchorA[0] * S) * P * 2 + (Da2 - fsquare(La)) * P2; // Solve the quadratic equation for z - machinePos[2] = (- halfB - sqrtf(fsquare(halfB) - A * C))/A; + machinePos[2] = (- halfB - fastSqrtf(fsquare(halfB) - A * C))/A; // Substitute back for X and Y machinePos[0] = (Q * machinePos[2] + S)/P; @@ -403,9 +403,9 @@ bool HangprinterKinematics::DoAutoCalibration(size_t numFactors, const RandomPro const floatc_t zp = reprap.GetMove().GetProbeCoordinates(i, machinePos[X_AXIS], machinePos[Y_AXIS], probePoints.PointWasCorrected(i)); machinePos[Z_AXIS] = 0.0; - probeMotorPositions(i, A_AXIS) = sqrtf(LineLengthSquared(machinePos, anchorA)); - probeMotorPositions(i, B_AXIS) = sqrtf(LineLengthSquared(machinePos, anchorB)); - probeMotorPositions(i, C_AXIS) = sqrtf(LineLengthSquared(machinePos, anchorC)); + probeMotorPositions(i, A_AXIS) = fastSqrtf(LineLengthSquared(machinePos, anchorA)); + probeMotorPositions(i, B_AXIS) = fastSqrtf(LineLengthSquared(machinePos, anchorB)); + probeMotorPositions(i, C_AXIS) = fastSqrtf(LineLengthSquared(machinePos, anchorC)); initialSumOfSquares += fcsquare(zp); } initialDeviation.Set(initialSumOfSquares, initialSum, numPoints); diff --git a/src/Movement/Kinematics/LinearDeltaKinematics.cpp b/src/Movement/Kinematics/LinearDeltaKinematics.cpp index d3a68e28..b0127e16 100644 --- a/src/Movement/Kinematics/LinearDeltaKinematics.cpp +++ b/src/Movement/Kinematics/LinearDeltaKinematics.cpp @@ -115,7 +115,7 @@ void LinearDeltaKinematics::Recalc() noexcept { D2[axis] = fsquare(diagonals[axis]); homedCarriageHeights[axis] = homedHeight - + sqrtf(D2[axis] - ((axis < UsualNumTowers) ? fsquare(radius) : fsquare(towerX[axis]) + fsquare(towerY[axis]))) + + fastSqrtf(D2[axis] - ((axis < UsualNumTowers) ? fsquare(radius) : fsquare(towerX[axis]) + fsquare(towerY[axis]))) + endstopAdjustments[axis]; const float heightLimit = homedCarriageHeights[axis] - diagonals[axis]; if (heightLimit < alwaysReachableHeight) @@ -161,7 +161,7 @@ float LinearDeltaKinematics::Transform(const float machinePos[], size_t axis) co { if (axis < numTowers) { - return sqrtf(D2[axis] - fsquare(machinePos[X_AXIS] - towerX[axis]) - fsquare(machinePos[Y_AXIS] - towerY[axis])) + return fastSqrtf(D2[axis] - fsquare(machinePos[X_AXIS] - towerX[axis]) - fsquare(machinePos[Y_AXIS] - towerY[axis])) + machinePos[Z_AXIS] + (machinePos[X_AXIS] * xTilt) + (machinePos[Y_AXIS] * yTilt); @@ -193,7 +193,7 @@ void LinearDeltaKinematics::ForwardTransform(float Ha, float Hb, float Hc, float - (R * T + U * S); const float C = fsquare(towerX[DELTA_A_AXIS] * Q - S) + fsquare(towerY[DELTA_A_AXIS] * Q + T) + (fsquare(Ha) - D2[DELTA_A_AXIS]) * Q2; - const float z = (minusHalfB - sqrtf(fsquare(minusHalfB) - A * C)) / A; + const float z = (minusHalfB - fastSqrtf(fsquare(minusHalfB) - A * C)) / A; machinePos[X_AXIS] = (U * z + S) / Q; machinePos[Y_AXIS] = -(R * z + T) / Q; machinePos[Z_AXIS] = z - ((machinePos[X_AXIS] * xTilt) + (machinePos[Y_AXIS] * yTilt)); @@ -254,7 +254,7 @@ LimitPositionResult LinearDeltaKinematics::LimitPosition(float finalCoords[], co const float diagonalSquared = fsquare(finalCoords[X_AXIS]) + fsquare(finalCoords[Y_AXIS]); if (applyM208Limits && diagonalSquared > printRadiusSquared) { - const float factor = sqrtf(printRadiusSquared / diagonalSquared); + const float factor = fastSqrtf(printRadiusSquared / diagonalSquared); finalCoords[X_AXIS] *= factor; finalCoords[Y_AXIS] *= factor; limited = true; @@ -314,7 +314,7 @@ LimitPositionResult LinearDeltaKinematics::LimitPosition(float finalCoords[], co } else { - const float tP2Q2 = dz * sqrtf(discriminant * Q2) - ((tx * dx) + (ty * dy)) * Q2; + const float tP2Q2 = dz * fastSqrtf(discriminant * Q2) - ((tx * dx) + (ty * dy)) * Q2; const float P2Q2 = P2 * Q2; if (tP2Q2 >= P2Q2) { @@ -734,12 +734,12 @@ void LinearDeltaKinematics::Adjust(size_t numFactors, const floatc_t v[]) noexce { if (numFactors >= 4) { - const float oldCarriageHeight = sqrtf(fsquare(diagonals[tower]) - fsquare(oldRadius)); + const float oldCarriageHeight = fastSqrtf(fsquare(diagonals[tower]) - fsquare(oldRadius)); if (numFactors == 7 || numFactors == 9) { diagonals[tower] += (float)v[6]; } - const float newCarriageHeight = sqrtf(fsquare(diagonals[tower]) - fsquare(radius)); + const float newCarriageHeight = fastSqrtf(fsquare(diagonals[tower]) - fsquare(radius)); endstopAdjustments[tower] += oldCarriageHeight - newCarriageHeight; } endstopAdjustments[tower] += (float)v[tower]; diff --git a/src/Movement/Kinematics/PolarKinematics.cpp b/src/Movement/Kinematics/PolarKinematics.cpp index df07199c..289870e1 100644 --- a/src/Movement/Kinematics/PolarKinematics.cpp +++ b/src/Movement/Kinematics/PolarKinematics.cpp @@ -116,7 +116,7 @@ bool PolarKinematics::Configure(unsigned int mCode, GCodeBuffer& gb, const Strin // Return true if successful, false if we were unable to convert bool PolarKinematics::CartesianToMotorSteps(const float machinePos[], const float stepsPerMm[], size_t numVisibleAxes, size_t numTotalAxes, int32_t motorPos[], bool isCoordinated) const noexcept { - motorPos[0] = lrintf(sqrtf(fsquare(machinePos[0]) + fsquare(machinePos[1])) * stepsPerMm[0]); + motorPos[0] = lrintf(fastSqrtf(fsquare(machinePos[0]) + fsquare(machinePos[1])) * stepsPerMm[0]); motorPos[1] = (motorPos[0] == 0.0) ? 0 : lrintf(atan2f(machinePos[1], machinePos[0]) * RadiansToDegrees * stepsPerMm[1]); // Transform remaining axes linearly @@ -174,7 +174,7 @@ LimitPositionResult PolarKinematics::LimitPosition(float finalCoords[], const fl if (r2 < minRadiusSquared) { radiusLimited = true; - const float r = sqrtf(r2); + const float r = fastSqrtf(r2); if (r < 0.01) { finalCoords[X_AXIS] = minRadius; @@ -189,7 +189,7 @@ LimitPositionResult PolarKinematics::LimitPosition(float finalCoords[], const fl else if (r2 > maxRadiusSquared) { radiusLimited = true; - const float r = sqrtf(r2); + const float r = fastSqrtf(r2); finalCoords[X_AXIS] *= maxRadius/r; finalCoords[Y_AXIS] *= maxRadius/r; } diff --git a/src/Movement/Kinematics/RotaryDeltaKinematics.cpp b/src/Movement/Kinematics/RotaryDeltaKinematics.cpp index 710c2f86..24b81644 100644 --- a/src/Movement/Kinematics/RotaryDeltaKinematics.cpp +++ b/src/Movement/Kinematics/RotaryDeltaKinematics.cpp @@ -614,7 +614,7 @@ LimitPositionResult RotaryDeltaKinematics::LimitPosition(float finalCoords[], co const float diagonalSquared = fsquare(finalCoords[X_AXIS]) + fsquare(finalCoords[Y_AXIS]); if (diagonalSquared > printRadiusSquared) { - const float factor = sqrtf(printRadiusSquared / diagonalSquared); + const float factor = fastSqrtf(printRadiusSquared / diagonalSquared); finalCoords[X_AXIS] *= factor; finalCoords[Y_AXIS] *= factor; limited = true; @@ -743,7 +743,7 @@ float RotaryDeltaKinematics::Transform(const float machinePos[], size_t axis) co const float c = rodSquaredMinusArmSquared[axis] - (fsquare(hMinusZ) + fsquare(rMinusX) + fsquare(y)); // 3. Solve the quadratic equation, taking the lower root - const float sinTheta = (b * c - a * sqrtf(fsquare(a) + fsquare(b) - fsquare(c)))/(fsquare(a) + fsquare(b)); + const float sinTheta = (b * c - a * fastSqrtf(fsquare(a) + fsquare(b) - fsquare(c)))/(fsquare(a) + fsquare(b)); // 4. Take the arc sine and convert to degrees return asinf(sinTheta) * RadiansToDegrees; @@ -797,7 +797,7 @@ void RotaryDeltaKinematics::ForwardTransform(float Ha, float Hb, float Hc, float const float C = fsquare(S) + fsquare(T) + (T * posAY - S * posAX) * P * 2 + (Da2 - rodSquared[DELTA_A_AXIS]) * P2; // Solve the quadratic equation for z - const float z = (- halfB - sqrtf(fsquare(halfB) - A * C))/A; + const float z = (- halfB - fastSqrtf(fsquare(halfB) - A * C))/A; // Substitute back for X and Y machinePos[X_AXIS] = (Q * z + S)/P; diff --git a/src/Movement/Kinematics/RoundBedKinematics.cpp b/src/Movement/Kinematics/RoundBedKinematics.cpp index 308c2ae6..bbfeb7b6 100644 --- a/src/Movement/Kinematics/RoundBedKinematics.cpp +++ b/src/Movement/Kinematics/RoundBedKinematics.cpp @@ -32,7 +32,7 @@ bool RoundBedKinematics::IsReachable(float axesCoords[MaxAxes], AxesBitmap axes, void RoundBedKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalisedDirectionVector, size_t numVisibleAxes, bool continuousRotationShortcut) const noexcept { // Limit the speed in the XY plane to the lower of the X and Y maximum speeds, and similarly for the acceleration - const float xyFactor = sqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS])); + const float xyFactor = fastSqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS])); if (xyFactor > 0.01) { const Platform& platform = reprap.GetPlatform(); diff --git a/src/Movement/Kinematics/ScaraKinematics.cpp b/src/Movement/Kinematics/ScaraKinematics.cpp index fce124ed..3f133006 100644 --- a/src/Movement/Kinematics/ScaraKinematics.cpp +++ b/src/Movement/Kinematics/ScaraKinematics.cpp @@ -73,7 +73,7 @@ bool ScaraKinematics::CalculateThetaAndPsi(const float machinePos[], bool isCoor } psi = acosf(cosPsi) * RadiansToDegrees; - const float sinPsi = sqrtf(square); + const float sinPsi = fastSqrtf(square); const float SCARA_K1 = proximalArmLength + distalArmLength * cosPsi; const float SCARA_K2 = distalArmLength * sinPsi; @@ -277,7 +277,7 @@ LimitPositionResult ScaraKinematics::LimitPosition(float finalCoords[], const fl // We are radius-limited float x = finalCoords[X_AXIS] + xOffset; float y = finalCoords[Y_AXIS] + yOffset; - const float r = sqrtf(fsquare(x) + fsquare(y)); + const float r = fastSqrtf(fsquare(x) + fsquare(y)); if (r < minRadius) { // Radius is too small. The user may have specified x=0 y=0 so allow for this. @@ -476,7 +476,7 @@ void ScaraKinematics::LimitSpeedAndAcceleration(DDA& dda, const float *normalise { // For now we limit the speed in the XY plane to the lower of the X and Y maximum speeds, and similarly for the acceleration. // Limiting the angular rates of the arms would be better. - const float xyFactor = sqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS])); + const float xyFactor = fastSqrtf(fsquare(normalisedDirectionVector[X_AXIS]) + fsquare(normalisedDirectionVector[Y_AXIS])); if (xyFactor > 0.01) { const Platform& platform = reprap.GetPlatform(); @@ -506,7 +506,7 @@ void ScaraKinematics::Recalc() noexcept distalArmLengthSquared = fsquare(distalArmLength); twoPd = proximalArmLength * distalArmLength * 2; - minRadius = max<float>(sqrtf(proximalArmLengthSquared + distalArmLengthSquared + minRadius = max<float>(fastSqrtf(proximalArmLengthSquared + distalArmLengthSquared + twoPd * min<float>(cosf(psiLimits[0] * DegreesToRadians), cosf(psiLimits[1] * DegreesToRadians))) * 1.005, requestedMinRadius); minRadiusSquared = fsquare(minRadius); @@ -523,7 +523,7 @@ void ScaraKinematics::Recalc() noexcept else { const float minAngle = min<float>(fabsf(psiLimits[0]), fabsf(psiLimits[1])) * DegreesToRadians; - maxRadius = sqrtf(proximalArmLengthSquared + distalArmLengthSquared + (twoPd * cosf(minAngle))); + maxRadius = fastSqrtf(proximalArmLengthSquared + distalArmLengthSquared + (twoPd * cosf(minAngle))); } maxRadius *= 0.995; |