Changed the contact point finding code to use standard indexing, which may have fixed a contact computation bug