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;
}