public TrajectoryStatePoint interpolate(TrajectoryStatePoint other, double x) { double new_t = interpolate(get_t(), other.get_t(), x); double delta_t = new_t - get_t(); if (delta_t < 0.0) { return(other.interpolate(this, 1.0 - x)); } bool reversing = get_velocity() < 0.0 || (epsilonEquals(get_velocity(), 0.0) && get_acceleration() < 0.0); double new_v = get_velocity() + get_acceleration() * delta_t; double new_s = (reversing ? -1.0 : 1.0) * (get_velocity() * delta_t + .5 * get_acceleration() * delta_t * delta_t); // System.out.println("x: " + x + " , new_t: " + new_t + ", new_s: " + new_s + " , distance: " + state() // .distance(other.state())); return(new TrajectoryStatePoint(get_state().interpolate(other.get_state(), new_s / get_state().distance(other.get_state())), new_t, new_v, get_acceleration())); }
public double distance(TrajectoryStatePoint other) { return(get_state().distance(other.get_state())); }
public TrajectorySamplePoint(TrajectoryPoint point) { mState = point.mState; mIndex_floor = mIndex_ceil = point.mIndex; }
public TrajectorySamplePoint(TrajectoryStatePoint state, int index_floor, int index_ceil) { mState = state; mIndex_floor = index_floor; mIndex_ceil = index_ceil; }
public TrajectoryPoint(TrajectoryStatePoint state, int index) { mState = state; mIndex = index; }