Fix out of bounds access

[AnthonyTornetta]

Resolves #477 by checking index is within bounds before accessing data at that index

[sebcrozet]

Thank you for this PR. This fix isn’t ideal as it result in some contacts being ignored by the constraints solver. The case that is being checked here shouldn’t happen, so something must be wrong with disabling bodies as you mentioned in #477.