Complete zmp estimator to handle more points
Solve problems for collision detection src