public State2D_Difference(State2D currentState, State2D goalState)
{
double dX = goalState.X - currentState.X;
double dY = goalState.Y - currentState.Y;
Rho = Math.Sqrt(dX * dX + dY * dY);
Alpha = Normalise(Math.Atan2(dY, dX) - currentState.Theta);
}
public State2D Propagate(State2D currentState, double dT, double vl, double vr)
{
double dTheta_dT = 0.0;
double dX_dT = ((vr + vl) / 2) * Math.Cos(currentState.Theta);
double dY_dT = ((vr + vl) / 2) * Math.Sin(currentState.Theta);
if (WheelSeparation > 0.0)
{
dTheta_dT = (vr - vl) / WheelSeparation;
}
State2D newState = new State2D(
currentState.X + dX_dT * dT,
currentState.Y + dY_dT * dT,
currentState.Theta + dTheta_dT * dT);
_currentState = newState;
return(newState);
}