Force-Based Algorithm for Motion Planning of Large Agent