def dynamics(x, u, dt):
    """
    Returns next state given last state x, wrench u, and timestep dt.

    """
    # Rotation matrix (orientation, converts body to world)
    R = np.array([
                  [np.cos(x[2]), -np.sin(x[2]), 0],
                  [np.sin(x[2]),  np.cos(x[2]), 0],
                  [           0,             0, 1]
                ])

    # Construct drag coefficients based on our motion signs
    D = np.copy(D_neg)
    for i, v in enumerate(x[3:]):
        if v >= 0:
            D[i] = D_pos[i]

    # Heading controller trying to keep us car-like
    vw = R[:2, :2].dot(x[3:5])
    ang = np.arctan2(vw[1], vw[0])
    c = np.cos(x[2])
    s = np.sin(x[2])
    cg = np.cos(ang)
    sg = np.sin(ang)
    u[2] = magic_rudder*np.arctan2(sg*c - cg*s, cg*c + sg*s)

    # Actuator saturation
    u = B.dot(np.clip(invB.dot(u), -thrust_max, thrust_max))

    # M*vdot + D*v = u  and  pdot = R*v
    xdot = np.concatenate((R.dot(x[3:]), invM*(u - D*x[3:])))

    # First-order integrate
    xnext = x + xdot*dt

    # Impose not driving backwards
    if xnext[3] < 0:
        xnext[3] = abs(x[3])

    # # Impose not turning in place
    # xnext[5] = np.clip(np.abs(xnext[3]/velmax_pos[0]), 0, 1) * xnext[5]

    return xnext