158210cde8
* start again * need that too * this actually works * not needed * do properly * still works * still works * still good * all G without ll * still works * all still good * cleanup building * cleanup sconscript * new lane planner * how on earth is this silent too.... * update * add rotation radius * update * pathplanner first pass * misc fixes * fix * need deep_interp * local again * fix * fix test * very old * new replay * interp properly * correct length * another horrible silent bug * like master * fix that * do doubles * different delay compensation * make robust to empty msg * make pass with hack for now * add some extra * update ref for increased leg * test cpu usage on this pr * tiny bit faster * purge numpy * update ref * not needed * ready for merge * try again after recompile Co-authored-by: Adeeb Shihadeh <adeebshihadeh@gmail.com>
23 lines
690 B
Python
23 lines
690 B
Python
import numpy as np
|
|
|
|
|
|
def deep_interp_np(x, xp, fp, axis=None):
|
|
if axis is not None:
|
|
fp = fp.swapaxes(0,axis)
|
|
x = np.atleast_1d(x)
|
|
xp = np.array(xp)
|
|
if len(xp) < 2:
|
|
return np.repeat(fp, len(x), axis=0)
|
|
if min(np.diff(xp)) < 0:
|
|
raise RuntimeError('Bad x array for interpolation')
|
|
j = np.searchsorted(xp, x) - 1
|
|
j = np.clip(j, 0, len(xp)-2)
|
|
d = np.divide(x - xp[j], xp[j + 1] - xp[j], out=np.ones_like(x, dtype=np.float64), where=xp[j + 1] - xp[j] != 0)
|
|
vals_interp = (fp[j].T*(1 - d)).T + (fp[j + 1].T*d).T
|
|
if axis is not None:
|
|
vals_interp = vals_interp.swapaxes(0,axis)
|
|
if len(vals_interp) == 1:
|
|
return vals_interp[0]
|
|
else:
|
|
return vals_interp
|