Formation Control Using Vehicle Operational Envelopes and Behavior-Based Dual-Mode Model Predictive Control