diff options
| -rw-r--r-- | klippy/chelper/trapq.c | 21 | ||||
| -rw-r--r-- | klippy/chelper/trapq.h | 1 |
2 files changed, 22 insertions, 0 deletions
diff --git a/klippy/chelper/trapq.c b/klippy/chelper/trapq.c index 5d5073c1..838fcdec 100644 --- a/klippy/chelper/trapq.c +++ b/klippy/chelper/trapq.c @@ -130,6 +130,27 @@ trapq_integrate(struct move *m, int axis, double start, double end) return res; } +// Find a move associated with a given time +struct move * +trapq_find_move(struct move *m, double *ptime) +{ + double move_time = *ptime; + for (;;) { + if (unlikely(move_time < 0.)) { + // Check previous move in list + m = list_prev_entry(m, node); + move_time += m->move_t; + } else if (unlikely(move_time > m->move_t)) { + // Check next move in list + move_time -= m->move_t; + m = list_next_entry(m, node); + } else { + *ptime = move_time; + return m; + } + } +} + #define NEVER_TIME 9999999999999999.9 // Allocate a new 'trapq' object diff --git a/klippy/chelper/trapq.h b/klippy/chelper/trapq.h index fa55faaf..14da9ee9 100644 --- a/klippy/chelper/trapq.h +++ b/klippy/chelper/trapq.h @@ -33,6 +33,7 @@ void trapq_append(struct trapq *tq, double print_time double move_get_distance(struct move *m, double move_time); struct coord move_get_coord(struct move *m, double move_time); double trapq_integrate(struct move *m, int axis, double start, double end); +struct move *trapq_find_move(struct move *m, double *ptime); struct trapq *trapq_alloc(void); void trapq_free(struct trapq *tq); void trapq_check_sentinels(struct trapq *tq); |