Finished the Ndof SAP-vp implementation.