diff options
| -rw-r--r-- | klippy/stepcompress.c | 31 |
1 files changed, 25 insertions, 6 deletions
diff --git a/klippy/stepcompress.c b/klippy/stepcompress.c index 0af0672e..643e0083 100644 --- a/klippy/stepcompress.c +++ b/klippy/stepcompress.c @@ -465,12 +465,31 @@ stepcompress_push_delta_const( clock_offset += 0.5; start_pos += movexy_r*closestxy_d; height += .5 * step_dist; - while (qn < end) { - double relheight = movexy_r*height - movez_r*closestxy_d; - double v = safe_sqrt(closest_height2 - relheight*relheight); - double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); - *qn++ = clock_offset + pos * inv_velocity; - height += step_dist; + if (!movez_r) { + // Optmized case for common XY only moves (no Z movement) + while (qn < end) { + double v = safe_sqrt(closest_height2 - height*height); + double pos = start_pos + (step_dist > 0. ? -v : v); + *qn++ = clock_offset + pos * inv_velocity; + height += step_dist; + } + } else if (!movexy_r) { + // Optmized case for Z only moves + double v = (step_dist > 0. ? -end_height : end_height); + while (qn < end) { + double pos = start_pos + movez_r*height + v; + *qn++ = clock_offset + pos * inv_velocity; + height += step_dist; + } + } else { + // General case (handles XY+Z moves) + while (qn < end) { + double relheight = movexy_r*height - movez_r*closestxy_d; + double v = safe_sqrt(closest_height2 - relheight*relheight); + double pos = start_pos + movez_r*height + (step_dist > 0. ? -v : v); + *qn++ = clock_offset + pos * inv_velocity; + height += step_dist; + } } sc->queue_next = qn; return step_dist > 0. ? count : -count; |