This paper presents a fast numerical algorithm for velocity optimization based on Pontryagin's minimum principle (PMP). Considering the difficulties associated with the application of the PMP when state constraints exist, a penalty function approach is proposed to convert the state-constrained problem into an unconstrained problem. Then, an iterative numerical algorithm in which the explicit solution is used to find the optimal solution is proposed. The proposed numerical algorithm is applied to velocity trajectory optimization for energy-efficient control of connected and automated vehicles (CAVs). The simulation results indicate that the algorithm can generate the optimal inputs in milliseconds and achieve a significant improvement in computational efficiency compared with traditional methods in a few seconds. A hardware-in-the-loop test for experimental validation is implemented to further verify the real-time performance of the proposed algorithm.